mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-12-05 01:39:13 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #21189 from DumDereDum:new_volume

Browse Source 
New Volume pipeline
This commit is contained in:
Artem Saratovtsev 2022-02-18 17:50:26 +03:00 committed by GitHub
parent 4ba2b05df8
commit 9c87d8bf9c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
25 changed files with 8024 additions and 5386 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
modules/3d/include/opencv2/3d/detail/submap.hpp
View File

@ -43,25 +43,25 @@ public:
};
typedef std::map<int, PoseConstraint> Constraints;
Submap(int _id, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(),
Submap(int _id, const VolumeSettings& settings, const cv::Affine3f& _pose = cv::Affine3f::Identity(),
int _startFrameId = 0)
: id(_id), pose(_pose), cameraPose(Affine3f::Identity()), startFrameId(_startFrameId)
{
VolumeParams vp = volumeParams;
vp.kind = VolumeParams::VolumeKind::HASHTSDF;
Ptr<VolumeParams> pvp = makePtr<VolumeParams>(vp);
volume = makeVolume(pvp);
volume = Volume(VolumeType::HashTSDF, settings);
}
virtual ~Submap() = default;
virtual void integrate(InputArray _depth, float depthFactor, const cv::Matx33f& intrinsics, const int currframeId);
virtual void raycast(const Odometry& icp, const cv::Affine3f& cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
virtual void integrate(InputArray _depth, const int currframeId);
virtual void raycast(const Odometry& icp, const cv::Affine3f& cameraPose, cv::Size frameSize,
OutputArray points = noArray(), OutputArray normals = noArray());
virtual int getTotalAllocatedBlocks() const { return int(volume->getTotalVolumeUnits()); };
virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); };
virtual int getVisibleBlocks(int currFrameId) const
{
return volume->getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD);
CV_Assert(currFrameId >= startFrameId);
//return volume.getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD);
return volume.getVisibleBlocks();
}
float calcVisibilityRatio(int currFrameId) const
@ -100,20 +100,19 @@ public:
OdometryFrame frame;
OdometryFrame renderFrame;
std::shared_ptr<Volume> volume;
Volume volume;
};
template<typename MatType>
void Submap<MatType>::integrate(InputArray _depth, float depthFactor, const cv::Matx33f& intrinsics,
const int currFrameId)
void Submap<MatType>::integrate(InputArray _depth, const int currFrameId)
{
CV_Assert(currFrameId >= startFrameId);
volume->integrate(_depth, depthFactor, cameraPose.matrix, intrinsics, currFrameId);
volume.integrate(_depth, cameraPose.matrix);
}
template<typename MatType>
void Submap<MatType>::raycast(const Odometry& icp, const cv::Affine3f& _cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
void Submap<MatType>::raycast(const Odometry& icp, const cv::Affine3f& _cameraPose, cv::Size frameSize,
OutputArray points, OutputArray normals)
{
if (!points.needed() && !normals.needed())
@ -122,20 +121,20 @@ void Submap<MatType>::raycast(const Odometry& icp, const cv::Affine3f& _cameraPo
frame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
frame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
volume->raycast(_cameraPose.matrix, intrinsics, frameSize, pts, nrm);
volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.height, pts, nrm);
frame.setPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
frame.setPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
renderFrame = frame;
Mat depth;
frame.getDepth(depth);
frame.getScaledDepth(depth);
frame = icp.createOdometryFrame();
frame.setDepth(depth);
}
else
{
volume->raycast(_cameraPose.matrix, intrinsics, frameSize, points, normals);
volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.height, points, normals);
}
}
@ -188,7 +187,7 @@ public:
typedef std::map<int, Ptr<SubmapT>> IdToSubmapPtr;
typedef std::unordered_map<int, ActiveSubmapData> IdToActiveSubmaps;
SubmapManager(const VolumeParams& _volumeParams) : volumeParams(_volumeParams) {}
explicit SubmapManager(const VolumeSettings& _volumeSettings) : volumeSettings(_volumeSettings) {}
virtual ~SubmapManager() = default;
void reset() { submapList.clear(); };
@ -214,7 +213,7 @@ public:
Ptr<detail::PoseGraph> MapToPoseGraph();
void PoseGraphToMap(const Ptr<detail::PoseGraph>& updatedPoseGraph);
VolumeParams volumeParams;
VolumeSettings volumeSettings;
std::vector<Ptr<SubmapT>> submapList;
IdToActiveSubmaps activeSubmaps;
@ -227,7 +226,7 @@ int SubmapManager<MatType>::createNewSubmap(bool isCurrentMap, int currFrameId,
{
int newId = int(submapList.size());
Ptr<SubmapT> newSubmap = cv::makePtr<SubmapT>(newId, volumeParams, pose, currFrameId);
Ptr<SubmapT> newSubmap = cv::makePtr<SubmapT>(newId, volumeSettings, pose, currFrameId);
submapList.push_back(newSubmap);
ActiveSubmapData newSubmapData;

1
modules/3d/include/opencv2/3d/odometry_frame.hpp
View File

@ -52,6 +52,7 @@ public:
void getGrayImage(OutputArray image) const;
void setDepth(InputArray depth);
void getDepth(OutputArray depth) const;
void getScaledDepth(OutputArray depth) const;
void setMask(InputArray mask);
void getMask(OutputArray mask) const;
void setNormals(InputArray normals);

225
modules/3d/include/opencv2/3d/volume.hpp
View File

@ -5,124 +5,135 @@
#ifndef OPENCV_3D_VOLUME_HPP
#define OPENCV_3D_VOLUME_HPP
#include "volume_settings.hpp"
#include "opencv2/core/affine.hpp"
namespace cv
{
class CV_EXPORTS_W Volume
{
public:
Volume(float _voxelSize, Matx44f _pose, float _raycastStepFactor) :
voxelSize(_voxelSize),
voxelSizeInv(1.0f / voxelSize),
pose(_pose),
raycastStepFactor(_raycastStepFactor)
{ }
public:
/** @brief Constructor of default volume - TSDF.
*/
Volume();
/** @brief Constructor of custom volume.
* @param vtype the volume type [TSDF, HashTSDF, ColorTSDF].
* @param settings the custom settings for volume.
*/
Volume(VolumeType vtype, const VolumeSettings& settings);
~Volume();
virtual ~Volume(){};
/** @brief Integrates the input data to the volume.
CV_WRAP
virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose,
const Matx33f& intrinsics, const int frameId = 0) = 0;
virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor,
const Matx44f& cameraPose, const Matx33f& intrinsics,
const Matx33f& rgb_intrinsics, const int frameId = 0) = 0;
CV_WRAP
virtual void raycast(const Matx44f& cameraPose, const Matx33f& intrinsics,
const Size& frameSize, OutputArray points, OutputArray normals) const = 0;
virtual void raycast(const Matx44f& cameraPose, const Matx33f& intrinsics, const Size& frameSize,
OutputArray points, OutputArray normals, OutputArray colors) const = 0;
CV_WRAP
virtual void fetchNormals(InputArray points, OutputArray _normals) const = 0;
CV_WRAP
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0;
CV_WRAP
virtual void reset() = 0;
Camera intrinsics are taken from volume settings structure.
// Works for hash-based volumes only, otherwise returns 1
virtual int getVisibleBlocks(int /*currFrameId*/, int /*frameThreshold*/) const { return 1; }
virtual size_t getTotalVolumeUnits() const { return 1; }
* @param frame the object from which to take depth and image data.
For color TSDF a depth data should be registered with color data, i.e. have the same intrinsics & camera pose.
This can be done using function registerDepth() from 3d module.
* @param pose the pose of camera in global coordinates.
*/
void integrate(const OdometryFrame& frame, InputArray pose);
public:
const float voxelSize;
const float voxelSizeInv;
const Affine3f pose;
const float raycastStepFactor;
/** @brief Integrates the input data to the volume.
Camera intrinsics are taken from volume settings structure.
* @param depth the depth image.
* @param pose the pose of camera in global coordinates.
*/
void integrate(InputArray depth, InputArray pose);
/** @brief Integrates the input data to the volume.
Camera intrinsics are taken from volume settings structure.
* @param depth the depth image.
* @param image the color image (only for ColorTSDF).
For color TSDF a depth data should be registered with color data, i.e. have the same intrinsics & camera pose.
This can be done using function registerDepth() from 3d module.
* @param pose the pose of camera in global coordinates.
*/
void integrate(InputArray depth, InputArray image, InputArray pose);
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
Rendered image size and camera intrinsics are taken from volume settings structure.
* @param cameraPose the pose of camera in global coordinates.
* @param outFrame the object where to store rendered points and normals.
*/
void raycast(InputArray cameraPose, OdometryFrame& outFrame) const;
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
Rendered image size and camera intrinsics are taken from volume settings structure.
* @param cameraPose the pose of camera in global coordinates.
* @param points image to store rendered points.
* @param normals image to store rendered normals corresponding to points.
* @param colors image to store rendered colors corresponding to points (only for ColorTSDF).
*/
void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const;
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
Rendered image size and camera intrinsics are taken from volume settings structure.
* @param cameraPose the pose of camera in global coordinates.
* @param height the height of result image.
* @param width the width of result image.
* @param outFrame the object where to store rendered points and normals.
*/
void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const;
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
Rendered image size and camera intrinsics are taken from volume settings structure.
* @param cameraPose the pose of camera in global coordinates.
* @param height the height of result image.
* @param width the width of result image.
* @param points image to store rendered points.
* @param normals image to store rendered normals corresponding to points.
* @param colors image to store rendered colors corresponding to points (only for ColorTSDF).
*/
void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const;
/** @brief Extract the all data from volume.
* @param points the input exist point.
* @param normals the storage of normals (corresponding to input points) in the image.
*/
void fetchNormals(InputArray points, OutputArray normals) const;
/** @brief Extract the all data from volume.
* @param points the storage of all points.
* @param normals the storage of all normals, corresponding to points.
*/
void fetchPointsNormals(OutputArray points, OutputArray normals) const;
/** @brief Extract the all data from volume.
* @param points the storage of all points.
* @param normals the storage of all normals, corresponding to points.
* @param colors the storage of all colors, corresponding to points (only for ColorTSDF).
*/
void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const;
/** @brief clear all data in volume.
*/
void reset();
/** @brief return visible blocks in volume.
*/
int getVisibleBlocks() const;
/** @brief return number of vulmeunits in volume.
*/
size_t getTotalVolumeUnits() const;
class Impl;
private:
Ptr<Impl> impl;
};
struct CV_EXPORTS_W VolumeParams
{
enum VolumeKind
{
TSDF = 0,
HASHTSDF = 1,
COLOREDTSDF = 2
};
/** @brief Kind of Volume
Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units)
*/
CV_PROP_RW int kind;
/** @brief Resolution of voxel space
Number of voxels in each dimension.
Applicable only for TSDF Volume.
HashTSDF volume only supports equal resolution in all three dimensions
*/
CV_PROP_RW int resolutionX;
CV_PROP_RW int resolutionY;
CV_PROP_RW int resolutionZ;
/** @brief Resolution of volumeUnit in voxel space
Number of voxels in each dimension for volumeUnit
Applicable only for hashTSDF.
*/
CV_PROP_RW int unitResolution = {0};
/** @brief Initial pose of the volume in meters, should be 4x4 float or double matrix */
CV_PROP_RW Mat pose;
/** @brief Length of voxels in meters */
CV_PROP_RW float voxelSize;
/** @brief TSDF truncation distance
Distances greater than value from surface will be truncated to 1.0
*/
CV_PROP_RW float tsdfTruncDist;
/** @brief Max number of frames to integrate per voxel
Represents the max number of frames over which a running average
of the TSDF is calculated for a voxel
*/
CV_PROP_RW int maxWeight;
/** @brief Threshold for depth truncation in meters
Truncates the depth greater than threshold to 0
*/
CV_PROP_RW float depthTruncThreshold;
/** @brief Length of single raycast step
Describes the percentage of voxel length that is skipped per march
*/
CV_PROP_RW float raycastStepFactor;
/** @brief Default set of parameters that provide higher quality reconstruction
at the cost of slow performance.
*/
CV_WRAP static Ptr<VolumeParams> defaultParams(int _volumeType);
/** @brief Coarse set of parameters that provides relatively higher performance
at the cost of reconstrution quality.
*/
CV_WRAP static Ptr<VolumeParams> coarseParams(int _volumeType);
};
CV_EXPORTS_W Ptr<Volume> makeVolume(const Ptr<VolumeParams>& _volumeParams);
CV_EXPORTS_W Ptr<Volume> makeVolume(int _volumeType, float _voxelSize, Matx44f _pose,
float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold,
int _resolutionX, int _resolutionY, int _resolutionZ);
} // namespace cv
#endif // include guard

213
modules/3d/include/opencv2/3d/volume_settings.hpp Normal file
View File

@ -0,0 +1,213 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#ifndef OPENCV_3D_VOLUME_SETTINGS_HPP
#define OPENCV_3D_VOLUME_SETTINGS_HPP
#include <opencv2/core.hpp>
#include <opencv2/3d/volume.hpp>
namespace cv
{
enum class VolumeType
{
TSDF = 0,
HashTSDF = 1,
ColorTSDF = 2
};
class CV_EXPORTS_W VolumeSettings
{
public:
/** @brief Constructor of settings for common TSDF volume type.
*/
VolumeSettings();
/** @brief Constructor of settings for custom Volume type.
* @param volumeType volume type.
*/
VolumeSettings(VolumeType volumeType);
~VolumeSettings();
/** @brief Sets the width of the image for integration.
* @param val input value.
*/
void setIntegrateWidth(int val);
/** @brief Returns the width of the image for integration.
*/
int getIntegrateWidth() const;
/** @brief Sets the height of the image for integration.
* @param val input value.
*/
void setIntegrateHeight(int val);
/** @brief Returns the height of the image for integration.
*/
int getIntegrateHeight() const;
/** @brief Sets the width of the raycasted image.
* @param val input value.
*/
void setRaycastWidth(int val);
/** @brief Returns the width of the raycasted image.
*/
int getRaycastWidth() const;
/** @brief Sets the height of the raycasted image.
* @param val input value.
*/
void setRaycastHeight(int val);
/** @brief Returns the height of the raycasted image.
*/
int getRaycastHeight() const;
/** @brief Sets depth factor, witch is the number for depth scaling.
* @param val input value.
*/
void setDepthFactor(float val);
/** @brief Returns depth factor, witch is the number for depth scaling.
*/
float getDepthFactor() const;
/** @brief Sets the size of voxel.
* @param val input value.
*/
void setVoxelSize(float val);
/** @brief Returns the size of voxel.
*/
float getVoxelSize() const;
/** @brief Sets TSDF truncation distance. Distances greater than value from surface will be truncated to 1.0.
* @param val input value.
*/
void setTsdfTruncateDistance(float val);
/** @brief Returns TSDF truncation distance. Distances greater than value from surface will be truncated to 1.0.
*/
float getTsdfTruncateDistance() const;
/** @brief Sets threshold for depth truncation in meters. Truncates the depth greater than threshold to 0.
* @param val input value.
*/
void setMaxDepth(float val);
/** @brief Returns threshold for depth truncation in meters. Truncates the depth greater than threshold to 0.
*/
float getMaxDepth() const;
/** @brief Sets max number of frames to integrate per voxel.
Represents the max number of frames over which a running average of the TSDF is calculated for a voxel.
* @param val input value.
*/
void setMaxWeight(int val);
/** @brief Returns max number of frames to integrate per voxel.
Represents the max number of frames over which a running average of the TSDF is calculated for a voxel.
*/
int getMaxWeight() const;
/** @brief Sets length of single raycast step.
Describes the percentage of voxel length that is skipped per march.
* @param val input value.
*/
void setRaycastStepFactor(float val);
/** @brief Returns length of single raycast step.
Describes the percentage of voxel length that is skipped per march.
*/
float getRaycastStepFactor() const;
/** @brief Sets volume pose.
* @param val input value.
*/
void setVolumePose(InputArray val);
/** @brief Sets volume pose.
* @param val output value.
*/
void getVolumePose(OutputArray val) const;
/** @brief Resolution of voxel space.
Number of voxels in each dimension.
Applicable only for TSDF Volume.
HashTSDF volume only supports equal resolution in all three dimensions.
* @param val input value.
*/
void setVolumeResolution(InputArray val);
/** @brief Resolution of voxel space.
Number of voxels in each dimension.
Applicable only for TSDF Volume.
HashTSDF volume only supports equal resolution in all three dimensions.
* @param val output value.
*/
void getVolumeResolution(OutputArray val) const;
/** @brief Returns 3 integers representing strides by x, y and z dimension.
Can be used to iterate over volume unit raw data.
* @param val output value.
*/
void getVolumeDimensions(OutputArray val) const;
/** @brief Sets intrinsics of camera for integrations.
* Format of input:
* [ fx 0 cx ]
* [ 0 fy cy ]
* [ 0 0 1 ]
* where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
* @param val input value.
*/
void setCameraIntegrateIntrinsics(InputArray val);
/** @brief Returns intrinsics of camera for integrations.
* Format of output:
* [ fx 0 cx ]
* [ 0 fy cy ]
* [ 0 0 1 ]
* where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
* @param val output value.
*/
void getCameraIntegrateIntrinsics(OutputArray val) const;
/** @brief Sets intrinsics of camera for raycast image.
* Format of input:
* [ fx 0 cx ]
* [ 0 fy cy ]
* [ 0 0 1 ]
* where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
* @param val input value.
*/
void setCameraRaycastIntrinsics(InputArray val);
/** @brief Returns intrinsics of camera for raycast image.
* Format of output:
* [ fx 0 cx ]
* [ 0 fy cy ]
* [ 0 0 1 ]
* where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
* @param val output value.
*/
void getCameraRaycastIntrinsics(OutputArray val) const;
class Impl;
private:
Ptr<Impl> impl;
};
}
#endif // !OPENCV_3D_VOLUME_SETTINGS_HPP

997
modules/3d/perf/perf_tsdf.cpp
View File

File diff suppressed because it is too large Load Diff

1259
modules/3d/src/rgbd/color_tsdf_functions.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

43
modules/3d/src/rgbd/color_tsdf_functions.hpp Normal file
View File

@ -0,0 +1,43 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// Partially rewritten from https://github.com/Nerei/kinfu_remake
// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
#ifndef OPENCV_3D_COLORED_TSDF_FUNCTIONS_HPP
#define OPENCV_3D_COLORED_TSDF_FUNCTIONS_HPP
#include <unordered_set>
#include "utils.hpp"
#include "tsdf_functions.hpp"
#define USE_INTERPOLATION_IN_GETNORMAL 1
namespace cv
{
void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose,
InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume);
void integrateColorTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose,
InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume);
void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width,
InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors);
void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
InputArray _points, OutputArray _normals);
void fetchPointsNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
OutputArray _points, OutputArray _normals);
void fetchPointsNormalsColorsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
OutputArray _points, OutputArray _normals, OutputArray _colors);
} // namespace cv
#endif

1024
modules/3d/src/rgbd/colored_tsdf.cpp
View File

File diff suppressed because it is too large Load Diff

39
modules/3d/src/rgbd/colored_tsdf.hpp
View File

@ -1,39 +0,0 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#ifndef OPENCV_3D_COLORED_TSDF_HPP
#define OPENCV_3D_COLORED_TSDF_HPP
#include "../precomp.hpp"
#include "tsdf_functions.hpp"
namespace cv
{
class ColoredTSDFVolume : public Volume
{
public:
// dimension in voxels, size in meters
ColoredTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist,
int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true);
virtual ~ColoredTSDFVolume() = default;
public:
Point3i volResolution;
WeightType maxWeight;
Point3f volSize;
float truncDist;
Vec4i volDims;
Vec8i neighbourCoords;
};
Ptr<ColoredTSDFVolume> makeColoredTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor,
float _truncDist, int _maxWeight, Point3i _resolution);
Ptr<ColoredTSDFVolume> makeColoredTSDFVolume(const VolumeParams& _params);
} // namespace cv
#endif // include guard

1795
modules/3d/src/rgbd/hash_tsdf.cpp
View File

File diff suppressed because it is too large Load Diff

47
modules/3d/src/rgbd/hash_tsdf.hpp
View File

@ -1,47 +0,0 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#ifndef OPENCV_3D_HASH_TSDF_HPP
#define OPENCV_3D_HASH_TSDF_HPP
#include "../precomp.hpp"
#include "tsdf_functions.hpp"
namespace cv
{
class HashTSDFVolume : public Volume
{
public:
// dimension in voxels, size in meters
//! Use fixed volume cuboid
HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist,
int _maxWeight, float _truncateThreshold, int _volumeUnitRes,
bool zFirstMemOrder = true);
virtual ~HashTSDFVolume() = default;
virtual int getVisibleBlocks(int currFrameId, int frameThreshold) const override = 0;
virtual size_t getTotalVolumeUnits() const override = 0;
public:
int maxWeight;
float truncDist;
float truncateThreshold;
int volumeUnitResolution;
int volumeUnitDegree;
float volumeUnitSize;
bool zFirstMemOrder;
Vec4i volStrides;
};
//template<typename T>
Ptr<HashTSDFVolume> makeHashTSDFVolume(const VolumeParams& _volumeParams);
//template<typename T>
Ptr<HashTSDFVolume> makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist,
int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16);
} // namespace cv
#endif // include guard

1534
modules/3d/src/rgbd/hash_tsdf_functions.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

325
modules/3d/src/rgbd/hash_tsdf_functions.hpp Normal file
View File

@ -0,0 +1,325 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// Partially rewritten from https://github.com/Nerei/kinfu_remake
// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
#ifndef OPENCV_3D_HASH_TSDF_FUNCTIONS_HPP
#define OPENCV_3D_HASH_TSDF_FUNCTIONS_HPP
#include <unordered_set>
#include "utils.hpp"
#include "tsdf_functions.hpp"
#define USE_INTERPOLATION_IN_GETNORMAL 1
#define VOLUMES_SIZE 8192
namespace cv
{
//! Spatial hashing
struct tsdf_hash
{
size_t operator()(const Vec3i& x) const noexcept
{
size_t seed = 0;
constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
for (uint16_t i = 0; i < 3; i++)
{
seed ^= std::hash<int>()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
}
return seed;
}
};
struct VolumeUnit
{
cv::Vec3i coord;
int index;
cv::Matx44f pose;
int lastVisibleIndex = 0;
bool isActive;
};
class CustomHashSet
{
public:
static const int hashDivisor = 32768;
static const int startCapacity = 2048;
std::vector<int> hashes;
// 0-3 for key, 4th for internal use
// don't keep keep value
std::vector<Vec4i> data;
int capacity;
int last;
CustomHashSet()
{
hashes.resize(hashDivisor);
for (int i = 0; i < hashDivisor; i++)
hashes[i] = -1;
capacity = startCapacity;
data.resize(capacity);
for (int i = 0; i < capacity; i++)
data[i] = { 0, 0, 0, -1 };
last = 0;
}
~CustomHashSet() { }
inline size_t calc_hash(Vec3i x) const
{
uint32_t seed = 0;
constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
for (int i = 0; i < 3; i++)
{
seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
}
return seed;
}
// should work on existing elements too
// 0 - need resize
// 1 - idx is inserted
// 2 - idx already exists
int insert(Vec3i idx)
{
if (last < capacity)
{
int hash = int(calc_hash(idx) % hashDivisor);
int place = hashes[hash];
if (place >= 0)
{
int oldPlace = place;
while (place >= 0)
{
if (data[place][0] == idx[0] &&
data[place][1] == idx[1] &&
data[place][2] == idx[2])
return 2;
else
{
oldPlace = place;
place = data[place][3];
//std::cout << "place=" << place << std::endl;
}
}
// found, create here
data[oldPlace][3] = last;
}
else
{
// insert at last
hashes[hash] = last;
}
data[last][0] = idx[0];
data[last][1] = idx[1];
data[last][2] = idx[2];
data[last][3] = -1;
last++;
return 1;
}
else
return 0;
}
int find(Vec3i idx) const
{
int hash = int(calc_hash(idx) % hashDivisor);
int place = hashes[hash];
// search a place
while (place >= 0)
{
if (data[place][0] == idx[0] &&
data[place][1] == idx[1] &&
data[place][2] == idx[2])
break;
else
{
place = data[place][3];
}
}
return place;
}
};
// TODO: remove this structure as soon as HashTSDFGPU data is completely on GPU;
// until then CustomHashTable can be replaced by this one if needed
const int NAN_ELEMENT = -2147483647;
struct Volume_NODE
{
Vec4i idx = Vec4i(NAN_ELEMENT);
int32_t row = -1;
int32_t nextVolumeRow = -1;
int32_t dummy = 0;
int32_t dummy2 = 0;
};
const int _hash_divisor = 32768;
const int _list_size = 4;
class VolumesTable
{
public:
const int hash_divisor = _hash_divisor;
const int list_size = _list_size;
const int32_t free_row = -1;
const int32_t free_isActive = 0;
const cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT);
int bufferNums;
cv::Mat volumes;
VolumesTable() : bufferNums(1)
{
this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType<Volume_NODE>());
for (int i = 0; i < volumes.size().height; i++)
{
Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
v->idx = nan4;
v->row = -1;
v->nextVolumeRow = -1;
}
}
const VolumesTable& operator=(const VolumesTable& vt)
{
this->volumes = vt.volumes;
this->bufferNums = vt.bufferNums;
return *this;
}
~VolumesTable() {};
bool insert(Vec3i idx, int row)
{
CV_Assert(row >= 0);
int bufferNum = 0;
int hash = int(calc_hash(idx) % hash_divisor);
int start = getPos(idx, bufferNum);
int i = start;
while (i >= 0)
{
Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
if (v->idx[0] == NAN_ELEMENT)
{
Vec4i idx4(idx[0], idx[1], idx[2], 0);
bool extend = false;
if (i != start && i % list_size == 0)
{
if (bufferNum >= bufferNums - 1)
{
extend = true;
volumes.resize(hash_divisor * bufferNums);
bufferNums++;
}
bufferNum++;
v->nextVolumeRow = (bufferNum * hash_divisor + hash) * list_size;
}
else
{
v->nextVolumeRow = i + 1;
}
v->idx = idx4;
v->row = row;
return extend;
}
i = v->nextVolumeRow;
}
return false;
}
int findRow(Vec3i idx) const
{
int bufferNum = 0;
int i = getPos(idx, bufferNum);
while (i >= 0)
{
const Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0))
return v->row;
else
i = v->nextVolumeRow;
}
return -1;
}
inline int getPos(Vec3i idx, int bufferNum) const
{
int hash = int(calc_hash(idx) % hash_divisor);
return (bufferNum * hash_divisor + hash) * list_size;
}
inline size_t calc_hash(Vec3i x) const
{
uint32_t seed = 0;
constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
for (int i = 0; i < 3; i++)
{
seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
}
return seed;
}
};
int calcVolumeUnitDegree(Point3i volumeResolution);
typedef std::unordered_set<cv::Vec3i, tsdf_hash> VolumeUnitIndexSet;
typedef std::unordered_map<cv::Vec3i, VolumeUnit, tsdf_hash> VolumeUnitIndexes;
void integrateHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree,
InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits);
void raycastHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree,
InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals);
void fetchNormalsFromHashTsdfVolumeUnit(
const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
const int volumeUnitDegree, InputArray _points, OutputArray _normals);
void fetchPointsNormalsFromHashTsdfVolumeUnit(
const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
const int volumeUnitDegree, OutputArray _points, OutputArray _normals);
#ifdef HAVE_OPENCL
void ocl_integrateHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree,
InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputArray _volUnitsDataCopy, InputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags);
void ocl_raycastHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree,
const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals);
void olc_fetchNormalsFromHashTsdfVolumeUnit(
const VolumeSettings& settings, const int volumeUnitDegree, InputArray _volUnitsData, InputArray _volUnitsDataCopy,
const CustomHashSet& hashTable, InputArray _points, OutputArray _normals);
void ocl_fetchPointsNormalsFromHashTsdfVolumeUnit(
const VolumeSettings& settings, const int volumeUnitDegree, InputArray _volUnitsData, InputArray _volUnitsDataCopy,
const CustomHashSet& hashTable, OutputArray _points, OutputArray _normals);
#endif
} // namespace cv
#endif

26
modules/3d/src/rgbd/odometry_frame_impl.cpp
View File

@ -21,6 +21,7 @@ public:
virtual void getGrayImage(OutputArray image) const = 0;
virtual void setDepth(InputArray depth) = 0;
virtual void getDepth(OutputArray depth) const = 0;
virtual void getScaledDepth(OutputArray depth) const = 0;
virtual void setMask(InputArray mask) = 0;
virtual void getMask(OutputArray mask) const = 0;
virtual void setNormals(InputArray normals) = 0;
@ -46,6 +47,7 @@ public:
virtual void getGrayImage(OutputArray image) const override;
virtual void setDepth(InputArray depth) override;
virtual void getDepth(OutputArray depth) const override;
virtual void getScaledDepth(OutputArray depth) const override;
virtual void setMask(InputArray mask) override;
virtual void getMask(OutputArray mask) const override;
virtual void setNormals(InputArray normals) override;
@ -64,6 +66,7 @@ private:
TMat image;
TMat imageGray;
TMat depth;
TMat scaledDepth;
TMat mask;
TMat normals;
std::vector< std::vector<TMat> > pyramids;
@ -87,6 +90,7 @@ void OdometryFrame::getImage(OutputArray image) const { this->impl->getImage(ima
void OdometryFrame::getGrayImage(OutputArray image) const { this->impl->getGrayImage(image); }
void OdometryFrame::setDepth(InputArray depth) { this->impl->setDepth(depth); }
void OdometryFrame::getDepth(OutputArray depth) const { this->impl->getDepth(depth); }
void OdometryFrame::getScaledDepth(OutputArray depth) const { this->impl->getScaledDepth(depth); }
void OdometryFrame::setMask(InputArray mask) { this->impl->setMask(mask); }
void OdometryFrame::getMask(OutputArray mask) const { this->impl->getMask(mask); }
void OdometryFrame::setNormals(InputArray normals) { this->impl->setNormals(normals); }
@ -140,7 +144,9 @@ void OdometryFrameImplTMat<TMat>::getGrayImage(OutputArray _image) const
template<typename TMat>
void OdometryFrameImplTMat<TMat>::setDepth(InputArray _depth)
{
TMat depth_tmp, depth_flt;
TMat depth_tmp;
Mat depth_flt;
depth_tmp = getTMat<TMat>(_depth);
// Odometry works well with depth values in range [0, 10)
// If it's bigger, let's scale it down by 5000, a typical depth factor
@ -149,12 +155,14 @@ void OdometryFrameImplTMat<TMat>::setDepth(InputArray _depth)
if (max > 10)
{
depth_tmp.convertTo(depth_flt, CV_32FC1, 1.f / 5000.f);
TMat depthMask;
cv::compare(depth_flt, Scalar(FLT_EPSILON), depthMask, cv::CMP_LT);
depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depthMask);
depth_tmp = depth_flt;
// getTMat<Mat>(depth_flt) < FLT_EPSILON dont work with UMat
// depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), getTMat<Mat>(depth_flt) < FLT_EPSILON);
depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth_flt < FLT_EPSILON);
depth_tmp = getTMat<TMat>(depth_flt);
}
this->depth = depth_tmp;
this->depth = getTMat<TMat>(_depth);
this->scaledDepth = depth_tmp;
this->findMask(_depth);
}
@ -164,6 +172,12 @@ void OdometryFrameImplTMat<TMat>::getDepth(OutputArray _depth) const
_depth.assign(this->depth);
}
template<typename TMat>
void OdometryFrameImplTMat<TMat>::getScaledDepth(OutputArray _depth) const
{
_depth.assign(this->scaledDepth);
}
template<typename TMat>
void OdometryFrameImplTMat<TMat>::setMask(InputArray _mask)
{

9
modules/3d/src/rgbd/odometry_functions.cpp
View File

@ -71,6 +71,7 @@ void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings, bool u
checkImage(image);
TMat depth;
if (useDepth)
{
frame.getDepth(depth);
@ -177,7 +178,7 @@ void prepareICPFrameBase(OdometryFrame& frame, OdometrySettings settings)
typedef Mat TMat;
TMat depth;
frame.getDepth(depth);
frame.getScaledDepth(depth);
if (depth.empty())
{
if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0)
@ -256,7 +257,7 @@ void prepareICPFrameDst(OdometryFrame& frame, OdometrySettings settings)
settings.getCameraMatrix(cameraMatrix);
TMat depth, mask, normals;
frame.getDepth(depth);
frame.getScaledDepth(depth);
frame.getMask(mask);
frame.getNormals(normals);
@ -886,9 +887,9 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt,
for (int u1 = 0; u1 < depth1.cols; u1++)
{
float d1 = depth1_row[u1];
if (mask1_row[u1])
if (mask1_row[u1] && !cvIsNaN(d1))
{
CV_DbgAssert(!cvIsNaN(d1));
//CV_DbgAssert(!cvIsNaN(d1));
float transformed_d1 = static_cast<float>(d1 * (KRK_inv6_u1[u1] + KRK_inv7_v1_plus_KRK_inv8[v1]) +
Kt_ptr[2]);

1228
modules/3d/src/rgbd/tsdf.cpp
View File

File diff suppressed because it is too large Load Diff

41
modules/3d/src/rgbd/tsdf.hpp
View File

@ -1,41 +0,0 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// Partially rewritten from https://github.com/Nerei/kinfu_remake
// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
#ifndef OPENCV_3D_TSDF_HPP
#define OPENCV_3D_TSDF_HPP
#include "../precomp.hpp"
#include "tsdf_functions.hpp"
namespace cv
{
class TSDFVolume : public Volume
{
public:
// dimension in voxels, size in meters
TSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist,
int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true);
virtual ~TSDFVolume() = default;
public:
Point3i volResolution;
WeightType maxWeight;
Point3f volSize;
float truncDist;
Vec4i volDims;
Vec8i neighbourCoords;
};
Ptr<TSDFVolume> makeTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor,
float _truncDist, int _maxWeight, Point3i _resolution);
Ptr<TSDFVolume> makeTSDFVolume(const VolumeParams& _params);
} // namespace cv
#endif // include guard

1419
modules/3d/src/rgbd/tsdf_functions.cpp
View File

File diff suppressed because it is too large Load Diff

262
modules/3d/src/rgbd/tsdf_functions.hpp
View File

@ -74,8 +74,10 @@ inline void colorFix(Point3f& c)
if (c.z > 255) c.z = 255;
}
cv::Mat preCalculationPixNorm(Size size, const Intr& intrinsics);
cv::UMat preCalculationPixNormGPU(Size size, const Intr& intrinsics);
void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm);
#ifdef HAVE_OPENCL
void ocl_preCalculationPixNorm(Size size, const Intr& intrinsics, UMat& pixNorm);
#endif
inline depthType bilinearDepth(const Depth& m, cv::Point2f pt)
{
@ -151,256 +153,56 @@ inline depthType bilinearDepth(const Depth& m, cv::Point2f pt)
}
}
void integrateVolumeUnit(
void _integrateVolumeUnit(
float truncDist, float voxelSize, int maxWeight,
cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides,
InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose,
const cv::Intr& intrinsics, InputArray _pixNorms, InputArray _volume);
void integrateRGBVolumeUnit(
void _integrateRGBVolumeUnit(
float truncDist, float voxelSize, int maxWeight,
cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides,
InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose,
const cv::Intr& depth_intrinsics, const cv::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume);
class CustomHashSet
{
public:
static const int hashDivisor = 32768;
static const int startCapacity = 2048;
void integrateTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose,
InputArray _depth, InputArray _pixNorms, InputArray _volume);
std::vector<int> hashes;
// 0-3 for key, 4th for internal use
// don't keep keep value
std::vector<Vec4i> data;
int capacity;
int last;
void integrateTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose,
InputArray _depth, InputArray _pixNorms, InputArray _volume);
CustomHashSet()
{
hashes.resize(hashDivisor);
for (int i = 0; i < hashDivisor; i++)
hashes[i] = -1;
capacity = startCapacity;
data.resize(capacity);
for (int i = 0; i < capacity; i++)
data[i] = { 0, 0, 0, -1 };
void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width,
InputArray _volume, OutputArray _points, OutputArray _normals);
last = 0;
}
void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
InputArray _points, OutputArray _normals);
~CustomHashSet() { }
void fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
OutputArray points, OutputArray normals);
inline size_t calc_hash(Vec3i x) const
{
uint32_t seed = 0;
constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
for (int i = 0; i < 3; i++)
{
seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
}
return seed;
}
// should work on existing elements too
// 0 - need resize
// 1 - idx is inserted
// 2 - idx already exists
int insert(Vec3i idx)
{
if (last < capacity)
{
int hash = int(calc_hash(idx) % hashDivisor);
int place = hashes[hash];
if (place >= 0)
{
int oldPlace = place;
while (place >= 0)
{
if (data[place][0] == idx[0] &&
data[place][1] == idx[1] &&
data[place][2] == idx[2])
return 2;
else
{
oldPlace = place;
place = data[place][3];
//std::cout << "place=" << place << std::endl;
}
}
#ifdef HAVE_OPENCL
void ocl_integrateTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose,
InputArray _depth, InputArray _pixNorms, InputArray _volume);
// found, create here
data[oldPlace][3] = last;
}
else
{
// insert at last
hashes[hash] = last;
}
void ocl_raycastTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width,
InputArray _volume, OutputArray _points, OutputArray _normals);
data[last][0] = idx[0];
data[last][1] = idx[1];
data[last][2] = idx[2];
data[last][3] = -1;
last++;
void ocl_fetchNormalsFromTsdfVolumeUnit(
const VolumeSettings& settings, InputArray _volume,
InputArray _points, OutputArray _normals);
return 1;
}
else
return 0;
}
void ocl_fetchPointsNormalsFromTsdfVolumeUnit(
const VolumeSettings& settings, InputArray _volume,
OutputArray _points, OutputArray _normals);
#endif
int find(Vec3i idx) const
{
int hash = int(calc_hash(idx) % hashDivisor);
int place = hashes[hash];
// search a place
while (place >= 0)
{
if (data[place][0] == idx[0] &&
data[place][1] == idx[1] &&
data[place][2] == idx[2])
break;
else
{
place = data[place][3];
}
}
return place;
}
};
// TODO: remove this structure as soon as HashTSDFGPU data is completely on GPU;
// until then CustomHashTable can be replaced by this one if needed
const int NAN_ELEMENT = -2147483647;
struct Volume_NODE
{
Vec4i idx = Vec4i(NAN_ELEMENT);
int32_t row = -1;
int32_t nextVolumeRow = -1;
int32_t dummy = 0;
int32_t dummy2 = 0;
};
const int _hash_divisor = 32768;
const int _list_size = 4;
class VolumesTable
{
public:
const int hash_divisor = _hash_divisor;
const int list_size = _list_size;
const int32_t free_row = -1;
const int32_t free_isActive = 0;
const cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT);
int bufferNums;
cv::Mat volumes;
VolumesTable() : bufferNums(1)
{
this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType<Volume_NODE>());
for (int i = 0; i < volumes.size().height; i++)
{
Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
v->idx = nan4;
v->row = -1;
v->nextVolumeRow = -1;
}
}
const VolumesTable& operator=(const VolumesTable& vt)
{
this->volumes = vt.volumes;
this->bufferNums = vt.bufferNums;
return *this;
}
~VolumesTable() {};
bool insert(Vec3i idx, int row)
{
CV_Assert(row >= 0);
int bufferNum = 0;
int hash = int(calc_hash(idx) % hash_divisor);
int start = getPos(idx, bufferNum);
int i = start;
while (i >= 0)
{
Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
if (v->idx[0] == NAN_ELEMENT)
{
Vec4i idx4(idx[0], idx[1], idx[2], 0);
bool extend = false;
if (i != start && i % list_size == 0)
{
if (bufferNum >= bufferNums - 1)
{
extend = true;
volumes.resize(hash_divisor * bufferNums);
bufferNums++;
}
bufferNum++;
v->nextVolumeRow = (bufferNum * hash_divisor + hash) * list_size;
}
else
{
v->nextVolumeRow = i + 1;
}
v->idx = idx4;
v->row = row;
return extend;
}
i = v->nextVolumeRow;
}
return false;
}
int findRow(Vec3i idx) const
{
int bufferNum = 0;
int i = getPos(idx, bufferNum);
while (i >= 0)
{
const Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0))
return v->row;
else
i = v->nextVolumeRow;
}
return -1;
}
inline int getPos(Vec3i idx, int bufferNum) const
{
int hash = int(calc_hash(idx) % hash_divisor);
return (bufferNum * hash_divisor + hash) * list_size;
}
inline size_t calc_hash(Vec3i x) const
{
uint32_t seed = 0;
constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
for (int i = 0; i < 3; i++)
{
seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
}
return seed;
}
};
} // namespace cv

120
modules/3d/src/rgbd/volume.cpp
View File

@ -1,120 +0,0 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#include "../precomp.hpp"
#include "tsdf.hpp"
#include "hash_tsdf.hpp"
#include "colored_tsdf.hpp"
namespace cv
{
Ptr<VolumeParams> VolumeParams::defaultParams(int _volumeKind)
{
VolumeParams params;
params.kind = _volumeKind;
params.maxWeight = 64;
params.raycastStepFactor = 0.25f;
params.unitResolution = 0; // unitResolution not used for TSDF
float volumeSize = 3.0f;
Matx44f pose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)).matrix;
params.pose = Mat(pose);
if(params.kind == VolumeKind::TSDF)
{
params.resolutionX = 512;
params.resolutionY = 512;
params.resolutionZ = 512;
params.voxelSize = volumeSize / 512.f;
params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF
params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters
return makePtr<VolumeParams>(params);
}
else if(params.kind == VolumeKind::HASHTSDF)
{
params.unitResolution = 16;
params.voxelSize = volumeSize / 512.f;
params.depthTruncThreshold = 4.f;
params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters
return makePtr<VolumeParams>(params);
}
else if (params.kind == VolumeKind::COLOREDTSDF)
{
params.resolutionX = 512;
params.resolutionY = 512;
params.resolutionZ = 512;
params.voxelSize = volumeSize / 512.f;
params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF
params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters
return makePtr<VolumeParams>(params);
}
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
}
Ptr<VolumeParams> VolumeParams::coarseParams(int _volumeKind)
{
Ptr<VolumeParams> params = defaultParams(_volumeKind);
params->raycastStepFactor = 0.75f;
float volumeSize = 3.0f;
if(params->kind == VolumeKind::TSDF)
{
params->resolutionX = 128;
params->resolutionY = 128;
params->resolutionZ = 128;
params->voxelSize = volumeSize / 128.f;
params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters
return params;
}
else if(params->kind == VolumeKind::HASHTSDF)
{
params->voxelSize = volumeSize / 128.f;
params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters
return params;
}
else if (params->kind == VolumeKind::COLOREDTSDF)
{
params->resolutionX = 128;
params->resolutionY = 128;
params->resolutionZ = 128;
params->voxelSize = volumeSize / 128.f;
params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters
return params;
}
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
}
Ptr<Volume> makeVolume(const Ptr<VolumeParams>& _volumeParams)
{
int kind = _volumeParams->kind;
if(kind == VolumeParams::VolumeKind::TSDF)
return makeTSDFVolume(*_volumeParams);
else if(kind == VolumeParams::VolumeKind::HASHTSDF)
return makeHashTSDFVolume(*_volumeParams);
else if(kind == VolumeParams::VolumeKind::COLOREDTSDF)
return makeColoredTSDFVolume(*_volumeParams);
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
}
Ptr<Volume> makeVolume(int _volumeKind, float _voxelSize, Matx44f _pose,
float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold,
int _resolutionX, int _resolutionY, int _resolutionZ)
{
Point3i _presolution(_resolutionX, _resolutionY, _resolutionZ);
if (_volumeKind == VolumeParams::VolumeKind::TSDF)
{
return makeTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _presolution);
}
else if (_volumeKind == VolumeParams::VolumeKind::HASHTSDF)
{
return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold);
}
else if (_volumeKind == VolumeParams::VolumeKind::COLOREDTSDF)
{
return makeColoredTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _presolution);
}
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
}
} // namespace cv

540
modules/3d/src/rgbd/volume_impl.cpp Normal file
View File

@ -0,0 +1,540 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#include <iostream>
#include "volume_impl.hpp"
#include "tsdf_functions.hpp"
#include "hash_tsdf_functions.hpp"
#include "color_tsdf_functions.hpp"
#include "opencv2/imgproc.hpp"
namespace cv
{
Volume::Impl::Impl(const VolumeSettings& _settings) :
settings(_settings)
#ifdef HAVE_OPENCL
, useGPU(ocl::useOpenCL())
#endif
{}
// TSDF
TsdfVolume::TsdfVolume(const VolumeSettings& _settings) :
Volume::Impl(_settings)
{
Vec3i volResolution;
settings.getVolumeResolution(volResolution);
#ifndef HAVE_OPENCL
volume = Mat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<TsdfVoxel>());
#else
if (useGPU)
gpu_volume = UMat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<TsdfVoxel>());
else
cpu_volume = Mat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<TsdfVoxel>());
#endif
reset();
}
TsdfVolume::~TsdfVolume() {}
void TsdfVolume::integrate(const OdometryFrame& frame, InputArray _cameraPose)
{
CV_TRACE_FUNCTION();
#ifndef HAVE_OPENCL
Mat depth;
#else
UMat depth;
#endif
frame.getDepth(depth);
integrate(depth, _cameraPose);
}
void TsdfVolume::integrate(InputArray _depth, InputArray _cameraPose)
{
CV_TRACE_FUNCTION();
#ifndef HAVE_OPENCL
Mat depth = _depth.getMat();
#else
UMat depth = _depth.getUMat();
#endif
CV_Assert(!depth.empty());
Matx33f intr;
settings.getCameraIntegrateIntrinsics(intr);
Intr intrinsics(intr);
Vec6f newParams((float)depth.rows, (float)depth.cols,
intrinsics.fx, intrinsics.fy,
intrinsics.cx, intrinsics.cy);
if (!(frameParams == newParams))
{
frameParams = newParams;
#ifndef HAVE_OPENCL
preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
#else
if (useGPU)
ocl_preCalculationPixNorm(depth.size(), intrinsics, gpu_pixNorms);
else
preCalculationPixNorm(depth.size(), intrinsics, cpu_pixNorms);
#endif
}
const Matx44f cameraPose = _cameraPose.getMat();
#ifndef HAVE_OPENCL
integrateTsdfVolumeUnit(settings, cameraPose, depth, pixNorms, volume);
#else
if (useGPU)
ocl_integrateTsdfVolumeUnit(settings, cameraPose, depth, gpu_pixNorms, gpu_volume);
else
integrateTsdfVolumeUnit(settings, cameraPose, depth, cpu_pixNorms, cpu_volume);
#endif
}
void TsdfVolume::integrate(InputArray, InputArray, InputArray)
{
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
}
void TsdfVolume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const
{
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), outFrame);
}
void TsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const
{
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors);
}
void TsdfVolume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const
{
#ifndef HAVE_OPENCL
Mat points, normals;
raycast(cameraPose, height, width, points, normals, noArray());
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
#else
if (useGPU)
{
UMat points, normals;
raycast(cameraPose, height, width, points, normals, noArray());
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
}
else
{
Mat points, normals;
raycast(cameraPose, height, width, points, normals, noArray());
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
}
#endif
}
void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const
{
if (_colors.needed())
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
CV_Assert(height > 0);
CV_Assert(width > 0);
const Matx44f cameraPose = _cameraPose.getMat();
#ifndef HAVE_OPENCL
raycastTsdfVolumeUnit(settings, cameraPose, height, width, volume, _points, _normals);
#else
if (useGPU)
ocl_raycastTsdfVolumeUnit(settings, cameraPose, height, width, gpu_volume, _points, _normals);
else
raycastTsdfVolumeUnit(settings, cameraPose, height, width, cpu_volume, _points, _normals);
#endif
}
void TsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
{
#ifndef HAVE_OPENCL
fetchNormalsFromTsdfVolumeUnit(settings, volume, points, normals);
#else
if (useGPU)
ocl_fetchNormalsFromTsdfVolumeUnit(settings, gpu_volume, points, normals);
else
fetchNormalsFromTsdfVolumeUnit(settings, cpu_volume, points, normals);
#endif
}
void TsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
{
#ifndef HAVE_OPENCL
fetchPointsNormalsFromTsdfVolumeUnit(settings, volume, points, normals);
#else
if (useGPU)
ocl_fetchPointsNormalsFromTsdfVolumeUnit(settings, gpu_volume, points, normals);
else
fetchPointsNormalsFromTsdfVolumeUnit(settings, cpu_volume, points, normals);
#endif
}
void TsdfVolume::fetchPointsNormalsColors(OutputArray, OutputArray, OutputArray) const
{
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
}
void TsdfVolume::reset()
{
CV_TRACE_FUNCTION();
#ifndef HAVE_OPENCL
//TODO: use setTo(Scalar(0, 0))
volume.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
{
TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
});
#else
if (useGPU)
gpu_volume.setTo(Scalar(0, 0));
else
//TODO: use setTo(Scalar(0, 0))
cpu_volume.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
{
TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
});
#endif
}
int TsdfVolume::getVisibleBlocks() const { return 1; }
size_t TsdfVolume::getTotalVolumeUnits() const { return 1; }
// HASH_TSDF
HashTsdfVolume::HashTsdfVolume(const VolumeSettings& _settings) :
Volume::Impl(_settings)
{
Vec3i resolution;
settings.getVolumeResolution(resolution);
const Point3i volResolution = Point3i(resolution);
volumeUnitDegree = calcVolumeUnitDegree(volResolution);
#ifndef HAVE_OPENCL
volUnitsData = cv::Mat(VOLUMES_SIZE, resolution[0] * resolution[1] * resolution[2], rawType<TsdfVoxel>());
reset();
#else
if (useGPU)
{
reset();
}
else
{
cpu_volUnitsData = cv::Mat(VOLUMES_SIZE, resolution[0] * resolution[1] * resolution[2], rawType<TsdfVoxel>());
reset();
}
#endif
}
HashTsdfVolume::~HashTsdfVolume() {}
void HashTsdfVolume::integrate(const OdometryFrame& frame, InputArray _cameraPose)
{
CV_TRACE_FUNCTION();
#ifndef HAVE_OPENCL
Mat depth;
#else
UMat depth;
#endif
frame.getDepth(depth);
integrate(depth, _cameraPose);
}
void HashTsdfVolume::integrate(InputArray _depth, InputArray _cameraPose)
{
#ifndef HAVE_OPENCL
Mat depth = _depth.getMat();
#else
UMat depth = _depth.getUMat();
#endif
const Matx44f cameraPose = _cameraPose.getMat();
Matx33f intr;
settings.getCameraIntegrateIntrinsics(intr);
Intr intrinsics(intr);
Vec6f newParams((float)depth.rows, (float)depth.cols,
intrinsics.fx, intrinsics.fy,
intrinsics.cx, intrinsics.cy);
if (!(frameParams == newParams))
{
frameParams = newParams;
#ifndef HAVE_OPENCL
preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
#else
if (useGPU)
ocl_preCalculationPixNorm(depth.size(), intrinsics, gpu_pixNorms);
else
preCalculationPixNorm(depth.size(), intrinsics, cpu_pixNorms);
#endif
}
#ifndef HAVE_OPENCL
integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, depth, pixNorms, volUnitsData, volumeUnits);
lastFrameId++;
#else
if (useGPU)
{
ocl_integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, bufferSizeDegree, volumeUnitDegree, depth, gpu_pixNorms, lastVisibleIndices, volUnitsDataCopy, gpu_volUnitsData, hashTable, isActiveFlags);
}
else
{
integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, depth, cpu_pixNorms, cpu_volUnitsData, cpu_volumeUnits);
lastFrameId++;
}
#endif
}
void HashTsdfVolume::integrate(InputArray, InputArray, InputArray)
{
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
}
void HashTsdfVolume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const
{
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), outFrame);
}
void HashTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const
{
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors);
}
void HashTsdfVolume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const
{
#ifndef HAVE_OPENCL
Mat points, normals;
raycast(cameraPose, height, width, points, normals, noArray());
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
#else
if (useGPU)
{
UMat points, normals;
raycast(cameraPose, height, width, points, normals, noArray());
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
}
else
{
Mat points, normals;
raycast(cameraPose, height, width, points, normals, noArray());
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
}
#endif
}
void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const
{
if (_colors.needed())
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
const Matx44f cameraPose = _cameraPose.getMat();
#ifndef HAVE_OPENCL
raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals);
#else
if (useGPU)
ocl_raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, hashTable, gpu_volUnitsData, _points, _normals);
else
raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, cpu_volUnitsData, cpu_volumeUnits, _points, _normals);
#endif
}
void HashTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
{
#ifndef HAVE_OPENCL
fetchNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
#else
if (useGPU)
olc_fetchNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals);
else
fetchNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals);
#endif
}
void HashTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
{
#ifndef HAVE_OPENCL
fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
#else
if (useGPU)
ocl_fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals);
else
fetchPointsNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals);
#endif
}
void HashTsdfVolume::fetchPointsNormalsColors(OutputArray, OutputArray, OutputArray) const
{
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
};
void HashTsdfVolume::reset()
{
CV_TRACE_FUNCTION();
lastVolIndex = 0;
lastFrameId = 0;
#ifndef HAVE_OPENCL
volUnitsData.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
{
TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
});
volumeUnits = VolumeUnitIndexes();
#else
if (useGPU)
{
Vec3i resolution;
settings.getVolumeResolution(resolution);
bufferSizeDegree = 15;
int buff_lvl = (int)(1 << bufferSizeDegree);
int volCubed = resolution[0] * resolution[1] * resolution[2];
volUnitsDataCopy = cv::Mat(buff_lvl, volCubed, rawType<TsdfVoxel>());
gpu_volUnitsData = cv::UMat(buff_lvl, volCubed, CV_8UC2);
lastVisibleIndices = cv::UMat(buff_lvl, 1, CV_32S);
isActiveFlags = cv::UMat(buff_lvl, 1, CV_8U);
hashTable = CustomHashSet();
frameParams = Vec6f();
gpu_pixNorms = UMat();
}
else
{
cpu_volUnitsData.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
{
TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
});
cpu_volumeUnits = VolumeUnitIndexes();
}
#endif
}
int HashTsdfVolume::getVisibleBlocks() const { return 1; }
size_t HashTsdfVolume::getTotalVolumeUnits() const { return 1; }
// COLOR_TSDF
ColorTsdfVolume::ColorTsdfVolume(const VolumeSettings& _settings) :
Volume::Impl(_settings)
{
Vec3i volResolution;
settings.getVolumeResolution(volResolution);
volume = Mat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<RGBTsdfVoxel>());
reset();
}
ColorTsdfVolume::~ColorTsdfVolume() {}
void ColorTsdfVolume::integrate(const OdometryFrame& frame, InputArray cameraPose)
{
CV_TRACE_FUNCTION();
Mat depth;
frame.getDepth(depth);
Mat rgb;
frame.getImage(rgb);
integrate(depth, rgb, cameraPose);
}
void ColorTsdfVolume::integrate(InputArray, InputArray)
{
CV_Error(cv::Error::StsBadFunc, "Color data should be passed for this volume type");
}
void ColorTsdfVolume::integrate(InputArray _depth, InputArray _image, InputArray _cameraPose)
{
Mat depth = _depth.getMat();
Colors image = _image.getMat();
const Matx44f cameraPose = _cameraPose.getMat();
Matx33f intr;
settings.getCameraIntegrateIntrinsics(intr);
Intr intrinsics(intr);
Vec6f newParams((float)depth.rows, (float)depth.cols,
intrinsics.fx, intrinsics.fy,
intrinsics.cx, intrinsics.cy);
if (!(frameParams == newParams))
{
frameParams = newParams;
preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
}
integrateColorTsdfVolumeUnit(settings, cameraPose, depth, image, pixNorms, volume);
}
void ColorTsdfVolume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const
{
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), outFrame);
}
void ColorTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const
{
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors);
}
void ColorTsdfVolume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const
{
Mat points, normals, colors;
raycast(cameraPose, height, width, points, normals, colors);
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_IMAGE);
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
outFrame.setPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0);
}
void ColorTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const
{
const Matx44f cameraPose = _cameraPose.getMat();
raycastColorTsdfVolumeUnit(settings, cameraPose, height, width, volume, _points, _normals, _colors);
}
void ColorTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
{
fetchNormalsFromColorTsdfVolumeUnit(settings, volume, points, normals);
}
void ColorTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
{
fetchPointsNormalsFromColorTsdfVolumeUnit(settings, volume, points, normals);
}
void ColorTsdfVolume::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const
{
fetchPointsNormalsColorsFromColorTsdfVolumeUnit(settings, volume, points, normals, colors);
}
void ColorTsdfVolume::reset()
{
CV_TRACE_FUNCTION();
volume.forEach<VecRGBTsdfVoxel>([](VecRGBTsdfVoxel& vv, const int* /* position */)
{
RGBTsdfVoxel& v = reinterpret_cast<RGBTsdfVoxel&>(vv);
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
});
}
int ColorTsdfVolume::getVisibleBlocks() const { return 1; }
size_t ColorTsdfVolume::getTotalVolumeUnits() const { return 1; }
}

220
modules/3d/src/rgbd/volume_impl.hpp Normal file
View File

@ -0,0 +1,220 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#ifndef OPENCV_3D_VOLUME_IMPL_HPP
#define OPENCV_3D_VOLUME_IMPL_HPP
#include <iostream>
#include "../precomp.hpp"
#include "hash_tsdf_functions.hpp"
namespace cv
{
class Volume::Impl
{
private:
// TODO: make debug function, which show histogram of volume points values
// make this function run with debug lvl == 10
public:
Impl(const VolumeSettings& settings);
virtual ~Impl() {};
virtual void integrate(const OdometryFrame& frame, InputArray pose) = 0;
virtual void integrate(InputArray depth, InputArray pose) = 0;
virtual void integrate(InputArray depth, InputArray image, InputArray pose) = 0;
virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const = 0;
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const = 0;
virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const = 0;
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const = 0;
virtual void fetchNormals(InputArray points, OutputArray normals) const = 0;
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0;
virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const = 0;
virtual void reset() = 0;
virtual int getVisibleBlocks() const = 0;
virtual size_t getTotalVolumeUnits() const = 0;
public:
const VolumeSettings& settings;
#ifdef HAVE_OPENCL
const bool useGPU;
#endif
};
class TsdfVolume : public Volume::Impl
{
public:
TsdfVolume(const VolumeSettings& settings);
~TsdfVolume();
virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
virtual void integrate(InputArray depth, InputArray pose) override;
virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const override;
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const override;
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void fetchNormals(InputArray points, OutputArray normals) const override;
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void reset() override;
virtual int getVisibleBlocks() const override;
virtual size_t getTotalVolumeUnits() const override;
public:
Vec6f frameParams;
#ifndef HAVE_OPENCL
Mat pixNorms;
// See zFirstMemOrder arg of parent class constructor
// for the array layout info
// Consist of Voxel elements
Mat volume;
#else
//temporary solution
Mat cpu_pixNorms;
Mat cpu_volume;
UMat gpu_pixNorms;
UMat gpu_volume;
#endif
};
typedef std::unordered_set<cv::Vec3i, tsdf_hash> VolumeUnitIndexSet;
typedef std::unordered_map<cv::Vec3i, VolumeUnit, tsdf_hash> VolumeUnitIndexes;
class HashTsdfVolume : public Volume::Impl
{
public:
HashTsdfVolume(const VolumeSettings& settings);
~HashTsdfVolume();
virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
virtual void integrate(InputArray depth, InputArray pose) override;
virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const override;
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const override;
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void fetchNormals(InputArray points, OutputArray normals) const override;
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void reset() override;
virtual int getVisibleBlocks() const override;
virtual size_t getTotalVolumeUnits() const override;
public:
int lastVolIndex;
int lastFrameId;
Vec6f frameParams;
int volumeUnitDegree;
#ifndef HAVE_OPENCL
Mat volUnitsData;
Mat pixNorms;
VolumeUnitIndexes volumeUnits;
#else
VolumeUnitIndexes cpu_volumeUnits;
Mat cpu_volUnitsData;
Mat cpu_pixNorms;
UMat gpu_volUnitsData;
UMat gpu_pixNorms;
int bufferSizeDegree;
// per-volume-unit data
UMat lastVisibleIndices;
UMat isActiveFlags;
//TODO: remove it when there's no CPU parts
Mat volUnitsDataCopy;
//TODO: move indexes.volumes to GPU
CustomHashSet hashTable;
#endif
};
class ColorTsdfVolume : public Volume::Impl
{
public:
ColorTsdfVolume(const VolumeSettings& settings);
~ColorTsdfVolume();
virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
virtual void integrate(InputArray depth, InputArray pose) override;
virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const override;
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const override;
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void fetchNormals(InputArray points, OutputArray normals) const override;
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void reset() override;
virtual int getVisibleBlocks() const override;
virtual size_t getTotalVolumeUnits() const override;
private:
Vec4i volStrides;
Vec6f frameParams;
Mat pixNorms;
// See zFirstMemOrder arg of parent class constructor
// for the array layout info
// Consist of Voxel elements
Mat volume;
};
Volume::Volume()
{
VolumeSettings settings;
this->impl = makePtr<TsdfVolume>(settings);
}
Volume::Volume(VolumeType vtype, const VolumeSettings& settings)
{
switch (vtype)
{
case VolumeType::TSDF:
this->impl = makePtr<TsdfVolume>(settings);
break;
case VolumeType::HashTSDF:
this->impl = makePtr<HashTsdfVolume>(settings);
break;
case VolumeType::ColorTSDF:
this->impl = makePtr<ColorTsdfVolume>(settings);
break;
default:
CV_Error(Error::StsInternal, "Incorrect OdometryType, you are able to use only { ICP, RGB, RGBD }");
break;
}
}
Volume::~Volume() {}
void Volume::integrate(const OdometryFrame& frame, InputArray pose) { this->impl->integrate(frame, pose); }
void Volume::integrate(InputArray depth, InputArray pose) { this->impl->integrate(depth, pose); }
void Volume::integrate(InputArray depth, InputArray image, InputArray pose) { this->impl->integrate(depth, image, pose); }
void Volume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const { this->impl->raycast(cameraPose, outFrame); }
void Volume::raycast(InputArray cameraPose, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, _points, _normals, _colors); }
void Volume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const { this->impl->raycast(cameraPose, height, width, outFrame); }
void Volume::raycast(InputArray cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, height, width, _points, _normals, _colors); }
void Volume::fetchNormals(InputArray points, OutputArray normals) const { this->impl->fetchNormals(points, normals); }
void Volume::fetchPointsNormals(OutputArray points, OutputArray normals) const { this->impl->fetchPointsNormals(points, normals); }
void Volume::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const { this->impl->fetchPointsNormalsColors(points, normals, colors); };
void Volume::reset() { this->impl->reset(); }
int Volume::getVisibleBlocks() const { return this->impl->getVisibleBlocks(); }
size_t Volume::getTotalVolumeUnits() const { return this->impl->getTotalVolumeUnits(); }
}
#endif // !OPENCV_3D_VOLUME_IMPL_HPP

532
modules/3d/src/rgbd/volume_settings_impl.cpp Normal file
View File

@ -0,0 +1,532 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#include "../precomp.hpp"
namespace cv
{
Vec4i calcVolumeDimensions(Point3i volumeResolution, bool ZFirstMemOrder);
class VolumeSettings::Impl
{
public:
Impl() {};
virtual ~Impl() {};
virtual void setIntegrateWidth(int val) = 0;
virtual int getIntegrateWidth() const = 0;
virtual void setIntegrateHeight(int val) = 0;
virtual int getIntegrateHeight() const = 0;
virtual void setRaycastWidth(int val) = 0;
virtual int getRaycastWidth() const = 0;
virtual void setRaycastHeight(int val) = 0;
virtual int getRaycastHeight() const = 0;
virtual void setDepthFactor(float val) = 0;
virtual float getDepthFactor() const = 0;
virtual void setVoxelSize(float val) = 0;
virtual float getVoxelSize() const = 0;
virtual void setTsdfTruncateDistance(float val) = 0;
virtual float getTsdfTruncateDistance() const = 0;
virtual void setMaxDepth(float val) = 0;
virtual float getMaxDepth() const = 0;
virtual void setMaxWeight(int val) = 0;
virtual int getMaxWeight() const = 0;
virtual void setRaycastStepFactor(float val) = 0;
virtual float getRaycastStepFactor() const = 0;
virtual void setVolumePose(InputArray val) = 0;
virtual void getVolumePose(OutputArray val) const = 0;
virtual void setVolumeResolution(InputArray val) = 0;
virtual void getVolumeResolution(OutputArray val) const = 0;
virtual void getVolumeDimensions(OutputArray val) const = 0;
virtual void setCameraIntegrateIntrinsics(InputArray val) = 0;
virtual void getCameraIntegrateIntrinsics(OutputArray val) const = 0;
virtual void setCameraRaycastIntrinsics(InputArray val) = 0;
virtual void getCameraRaycastIntrinsics(OutputArray val) const = 0;
};
class VolumeSettingsImpl : public VolumeSettings::Impl
{
public:
VolumeSettingsImpl();
VolumeSettingsImpl(VolumeType volumeType);
~VolumeSettingsImpl();
virtual void setIntegrateWidth(int val) override;
virtual int getIntegrateWidth() const override;
virtual void setIntegrateHeight(int val) override;
virtual int getIntegrateHeight() const override;
virtual void setRaycastWidth(int val) override;
virtual int getRaycastWidth() const override;
virtual void setRaycastHeight(int val) override;
virtual int getRaycastHeight() const override;
virtual void setDepthFactor(float val) override;
virtual float getDepthFactor() const override;
virtual void setVoxelSize(float val) override;
virtual float getVoxelSize() const override;
virtual void setTsdfTruncateDistance(float val) override;
virtual float getTsdfTruncateDistance() const override;
virtual void setMaxDepth(float val) override;
virtual float getMaxDepth() const override;
virtual void setMaxWeight(int val) override;
virtual int getMaxWeight() const override;
virtual void setRaycastStepFactor(float val) override;
virtual float getRaycastStepFactor() const override;
virtual void setVolumePose(InputArray val) override;
virtual void getVolumePose(OutputArray val) const override;
virtual void setVolumeResolution(InputArray val) override;
virtual void getVolumeResolution(OutputArray val) const override;
virtual void getVolumeDimensions(OutputArray val) const override;
virtual void setCameraIntegrateIntrinsics(InputArray val) override;
virtual void getCameraIntegrateIntrinsics(OutputArray val) const override;
virtual void setCameraRaycastIntrinsics(InputArray val) override;
virtual void getCameraRaycastIntrinsics(OutputArray val) const override;
private:
VolumeType volumeType;
int integrateWidth;
int integrateHeight;
int raycastWidth;
int raycastHeight;
float depthFactor;
float voxelSize;
float tsdfTruncateDistance;
float maxDepth;
int maxWeight;
float raycastStepFactor;
bool zFirstMemOrder;
Matx44f volumePose;
Point3i volumeResolution;
Vec4i volumeDimensions;
Matx33f cameraIntegrateIntrinsics;
Matx33f cameraRaycastIntrinsics;
public:
// duplicate classes for all volumes
class DefaultTsdfSets {
public:
static const int integrateWidth = 640;
static const int integrateHeight = 480;
float ifx = 525.f; // focus point x axis
float ify = 525.f; // focus point y axis
float icx = float(integrateWidth) / 2.f - 0.5f; // central point x axis
float icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis
const Matx33f cameraIntegrateIntrinsics = Matx33f(ifx, 0, icx, 0, ify, icy, 0, 0, 1); // camera settings
static const int raycastWidth = 640;
static const int raycastHeight = 480;
float rfx = 525.f; // focus point x axis
float rfy = 525.f; // focus point y axis
float rcx = float(raycastWidth) / 2.f - 0.5f; // central point x axis
float rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis
const Matx33f cameraRaycastIntrinsics = Matx33f(rfx, 0, rcx, 0, rfy, rcy, 0, 0, 1); // camera settings
static constexpr float depthFactor = 5000.f; // 5000 for the 16-bit PNG files, 1 for the 32-bit float images in the ROS bag files
static constexpr float volumeSize = 3.f; // meters
static constexpr float voxelSize = volumeSize / 128.f; //meters
static constexpr float tsdfTruncateDistance = 2 * voxelSize;
static constexpr float maxDepth = 0.f;
static const int maxWeight = 64; // number of frames
static constexpr float raycastStepFactor = 0.75f;
static const bool zFirstMemOrder = true; // order of voxels in volume
const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
const Matx44f volumePoseMatrix = volumePose.matrix;
// Unlike original code, this should work with any volume size
// Not only when (x,y,z % 32) == 0
const Point3i volumeResolution = Vec3i::all(128); //number of voxels
};
class DefaultHashTsdfSets {
public:
static const int integrateWidth = 640;
static const int integrateHeight = 480;
float ifx = 525.f; // focus point x axis
float ify = 525.f; // focus point y axis
float icx = float(integrateWidth) / 2.f - 0.5f; // central point x axis
float icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis
const Matx33f cameraIntegrateIntrinsics = Matx33f(ifx, 0, icx, 0, ify, icy, 0, 0, 1); // camera settings
static const int raycastWidth = 640;
static const int raycastHeight = 480;
float rfx = 525.f; // focus point x axis
float rfy = 525.f; // focus point y axis
float rcx = float(raycastWidth) / 2.f - 0.5f; // central point x axis
float rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis
const Matx33f cameraRaycastIntrinsics = Matx33f(rfx, 0, rcx, 0, rfy, rcy, 0, 0, 1); // camera settings
static constexpr float depthFactor = 5000.f; // 5000 for the 16-bit PNG files, 1 for the 32-bit float images in the ROS bag files
static constexpr float volumeSize = 3.f; // meters
static constexpr float voxelSize = volumeSize / 512.f; //meters
static constexpr float tsdfTruncateDistance = 7 * voxelSize;
static constexpr float maxDepth = 4.f;
static const int maxWeight = 64; // number of frames
static constexpr float raycastStepFactor = 0.25f;
static const bool zFirstMemOrder = true; // order of voxels in volume
const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
const Matx44f volumePoseMatrix = volumePose.matrix;
// Unlike original code, this should work with any volume size
// Not only when (x,y,z % 32) == 0
const Point3i volumeResolution = Vec3i::all(16); //number of voxels
};
class DefaultColorTsdfSets {
public:
static const int integrateWidth = 640;
static const int integrateHeight = 480;
float ifx = 525.f; // focus point x axis
float ify = 525.f; // focus point y axis
float icx = float(integrateWidth) / 2.f - 0.5f; // central point x axis
float icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis
float rgb_ifx = 525.f; // focus point x axis
float rgb_ify = 525.f; // focus point y axis
float rgb_icx = float(integrateWidth) / 2.f - 0.5f; // central point x axis
float rgb_icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis
const Matx33f cameraIntegrateIntrinsics = Matx33f(ifx, 0, icx, 0, ify, icy, 0, 0, 1); // camera settings
static const int raycastWidth = 640;
static const int raycastHeight = 480;
float rfx = 525.f; // focus point x axis
float rfy = 525.f; // focus point y axis
float rcx = float(raycastWidth) / 2.f - 0.5f; // central point x axis
float rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis
float rgb_rfx = 525.f; // focus point x axis
float rgb_rfy = 525.f; // focus point y axis
float rgb_rcx = float(raycastWidth) / 2.f - 0.5f; // central point x axis
float rgb_rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis
const Matx33f cameraRaycastIntrinsics = Matx33f(rfx, 0, rcx, 0, rfy, rcy, 0, 0, 1); // camera settings
static constexpr float depthFactor = 5000.f; // 5000 for the 16-bit PNG files, 1 for the 32-bit float images in the ROS bag files
static constexpr float volumeSize = 3.f; // meters
static constexpr float voxelSize = volumeSize / 128.f; //meters
static constexpr float tsdfTruncateDistance = 2 * voxelSize;
static constexpr float maxDepth = 0.f;
static const int maxWeight = 64; // number of frames
static constexpr float raycastStepFactor = 0.75f;
static const bool zFirstMemOrder = true; // order of voxels in volume
const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
const Matx44f volumePoseMatrix = volumePose.matrix;
// Unlike original code, this should work with any volume size
// Not only when (x,y,z % 32) == 0
const Point3i volumeResolution = Vec3i::all(128); //number of voxels
};
};
VolumeSettings::VolumeSettings()
{
this->impl = makePtr<VolumeSettingsImpl>();
}
VolumeSettings::VolumeSettings(VolumeType volumeType)
{
this->impl = makePtr<VolumeSettingsImpl>(volumeType);
}
VolumeSettings::~VolumeSettings() {}
void VolumeSettings::setIntegrateWidth(int val) { this->impl->setIntegrateWidth(val); };
int VolumeSettings::getIntegrateWidth() const { return this->impl->getIntegrateWidth(); };
void VolumeSettings::setIntegrateHeight(int val) { this->impl->setIntegrateHeight(val); };
int VolumeSettings::getIntegrateHeight() const { return this->impl->getIntegrateHeight(); };
void VolumeSettings::setRaycastWidth(int val) { this->impl->setRaycastWidth(val); };
int VolumeSettings::getRaycastWidth() const { return this->impl->getRaycastWidth(); };
void VolumeSettings::setRaycastHeight(int val) { this->impl->setRaycastHeight(val); };
int VolumeSettings::getRaycastHeight() const { return this->impl->getRaycastHeight(); };
void VolumeSettings::setVoxelSize(float val) { this->impl->setVoxelSize(val); };
float VolumeSettings::getVoxelSize() const { return this->impl->getVoxelSize(); };
void VolumeSettings::setRaycastStepFactor(float val) { this->impl->setRaycastStepFactor(val); };
float VolumeSettings::getRaycastStepFactor() const { return this->impl->getRaycastStepFactor(); };
void VolumeSettings::setTsdfTruncateDistance(float val) { this->impl->setTsdfTruncateDistance(val); };
float VolumeSettings::getTsdfTruncateDistance() const { return this->impl->getTsdfTruncateDistance(); };
void VolumeSettings::setMaxDepth(float val) { this->impl->setMaxDepth(val); };
float VolumeSettings::getMaxDepth() const { return this->impl->getMaxDepth(); };
void VolumeSettings::setDepthFactor(float val) { this->impl->setDepthFactor(val); };
float VolumeSettings::getDepthFactor() const { return this->impl->getDepthFactor(); };
void VolumeSettings::setMaxWeight(int val) { this->impl->setMaxWeight(val); };
int VolumeSettings::getMaxWeight() const { return this->impl->getMaxWeight(); };
void VolumeSettings::setVolumePose(InputArray val) { this->impl->setVolumePose(val); };
void VolumeSettings::getVolumePose(OutputArray val) const { this->impl->getVolumePose(val); };
void VolumeSettings::setVolumeResolution(InputArray val) { this->impl->setVolumeResolution(val); };
void VolumeSettings::getVolumeResolution(OutputArray val) const { this->impl->getVolumeResolution(val); };
void VolumeSettings::getVolumeDimensions(OutputArray val) const { this->impl->getVolumeDimensions(val); };
void VolumeSettings::setCameraIntegrateIntrinsics(InputArray val) { this->impl->setCameraIntegrateIntrinsics(val); };
void VolumeSettings::getCameraIntegrateIntrinsics(OutputArray val) const { this->impl->getCameraIntegrateIntrinsics(val); };
void VolumeSettings::setCameraRaycastIntrinsics(InputArray val) { this->impl->setCameraRaycastIntrinsics(val); };
void VolumeSettings::getCameraRaycastIntrinsics(OutputArray val) const { this->impl->getCameraRaycastIntrinsics(val); };
VolumeSettingsImpl::VolumeSettingsImpl()
: VolumeSettingsImpl(VolumeType::TSDF)
{
}
VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType)
{
volumeType = _volumeType;
if (volumeType == VolumeType::TSDF)
{
DefaultTsdfSets ds = DefaultTsdfSets();
this->integrateWidth = ds.integrateWidth;
this->integrateHeight = ds.integrateHeight;
this->raycastWidth = ds.raycastWidth;
this->raycastHeight = ds.raycastHeight;
this->depthFactor = ds.depthFactor;
this->voxelSize = ds.voxelSize;
this->tsdfTruncateDistance = ds.tsdfTruncateDistance;
this->maxDepth = ds.maxDepth;
this->maxWeight = ds.maxWeight;
this->raycastStepFactor = ds.raycastStepFactor;
this->zFirstMemOrder = ds.zFirstMemOrder;
this->volumePose = ds.volumePoseMatrix;
this->volumeResolution = ds.volumeResolution;
this->volumeDimensions = calcVolumeDimensions(ds.volumeResolution, ds.zFirstMemOrder);
this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
}
else if (volumeType == VolumeType::HashTSDF)
{
DefaultHashTsdfSets ds = DefaultHashTsdfSets();
this->integrateWidth = ds.integrateWidth;
this->integrateHeight = ds.integrateHeight;
this->raycastWidth = ds.raycastWidth;
this->raycastHeight = ds.raycastHeight;
this->depthFactor = ds.depthFactor;
this->voxelSize = ds.voxelSize;
this->tsdfTruncateDistance = ds.tsdfTruncateDistance;
this->maxDepth = ds.maxDepth;
this->maxWeight = ds.maxWeight;
this->raycastStepFactor = ds.raycastStepFactor;
this->zFirstMemOrder = ds.zFirstMemOrder;
this->volumePose = ds.volumePoseMatrix;
this->volumeResolution = ds.volumeResolution;
this->volumeDimensions = calcVolumeDimensions(ds.volumeResolution, ds.zFirstMemOrder);
this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
}
else if (volumeType == VolumeType::ColorTSDF)
{
DefaultColorTsdfSets ds = DefaultColorTsdfSets();
this->integrateWidth = ds.integrateWidth;
this->integrateHeight = ds.integrateHeight;
this->raycastWidth = ds.raycastWidth;
this->raycastHeight = ds.raycastHeight;
this->depthFactor = ds.depthFactor;
this->voxelSize = ds.voxelSize;
this->tsdfTruncateDistance = ds.tsdfTruncateDistance;
this->maxDepth = ds.maxDepth;
this->maxWeight = ds.maxWeight;
this->raycastStepFactor = ds.raycastStepFactor;
this->zFirstMemOrder = ds.zFirstMemOrder;
this->volumePose = ds.volumePoseMatrix;
this->volumeResolution = ds.volumeResolution;
this->volumeDimensions = calcVolumeDimensions(ds.volumeResolution, ds.zFirstMemOrder);
this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
}
}
VolumeSettingsImpl::~VolumeSettingsImpl() {}
void VolumeSettingsImpl::setIntegrateWidth(int val)
{
this->integrateWidth = val;
}
int VolumeSettingsImpl::getIntegrateWidth() const
{
return this->integrateWidth;
}
void VolumeSettingsImpl::setIntegrateHeight(int val)
{
this->integrateHeight = val;
}
int VolumeSettingsImpl::getIntegrateHeight() const
{
return this->integrateHeight;
}
void VolumeSettingsImpl::setRaycastWidth(int val)
{
this->raycastWidth = val;
}
int VolumeSettingsImpl::getRaycastWidth() const
{
return this->raycastWidth;
}
void VolumeSettingsImpl::setRaycastHeight(int val)
{
this->raycastHeight = val;
}
int VolumeSettingsImpl::getRaycastHeight() const
{
return this->raycastHeight;
}
void VolumeSettingsImpl::setDepthFactor(float val)
{
this->depthFactor = val;
}
float VolumeSettingsImpl::getDepthFactor() const
{
return this->depthFactor;
}
void VolumeSettingsImpl::setVoxelSize(float val)
{
this->voxelSize = val;
}
float VolumeSettingsImpl::getVoxelSize() const
{
return this->voxelSize;
}
void VolumeSettingsImpl::setTsdfTruncateDistance(float val)
{
this->tsdfTruncateDistance = val;
}
float VolumeSettingsImpl::getTsdfTruncateDistance() const
{
return this->tsdfTruncateDistance;
}
void VolumeSettingsImpl::setMaxDepth(float val)
{
this->maxDepth = val;
}
float VolumeSettingsImpl::getMaxDepth() const
{
return this->maxDepth;
}
void VolumeSettingsImpl::setMaxWeight(int val)
{
this->maxWeight = val;
}
int VolumeSettingsImpl::getMaxWeight() const
{
return this->maxWeight;
}
void VolumeSettingsImpl::setRaycastStepFactor(float val)
{
this->raycastStepFactor = val;
}
float VolumeSettingsImpl::getRaycastStepFactor() const
{
return this->raycastStepFactor;
}
void VolumeSettingsImpl::setVolumePose(InputArray val)
{
if (!val.empty())
{
val.copyTo(this->volumePose);
}
}
void VolumeSettingsImpl::getVolumePose(OutputArray val) const
{
Mat(this->volumePose).copyTo(val);
}
void VolumeSettingsImpl::setVolumeResolution(InputArray val)
{
if (!val.empty())
{
this->volumeResolution = Point3i(val.getMat());
this->volumeDimensions = calcVolumeDimensions(this->volumeResolution, this->zFirstMemOrder);
}
}
void VolumeSettingsImpl::getVolumeResolution(OutputArray val) const
{
Mat(this->volumeResolution).copyTo(val);
}
void VolumeSettingsImpl::getVolumeDimensions(OutputArray val) const
{
Mat(this->volumeDimensions).copyTo(val);
}
void VolumeSettingsImpl::setCameraIntegrateIntrinsics(InputArray val)
{
if (!val.empty())
{
this->cameraIntegrateIntrinsics = Matx33f(val.getMat());
}
}
void VolumeSettingsImpl::getCameraIntegrateIntrinsics(OutputArray val) const
{
Mat(this->cameraIntegrateIntrinsics).copyTo(val);
}
void VolumeSettingsImpl::setCameraRaycastIntrinsics(InputArray val)
{
if (!val.empty())
{
this->cameraRaycastIntrinsics = Matx33f(val.getMat());
}
}
void VolumeSettingsImpl::getCameraRaycastIntrinsics(OutputArray val) const
{
Mat(this->cameraRaycastIntrinsics).copyTo(val);
}
Vec4i calcVolumeDimensions(Point3i volumeResolution, bool ZFirstMemOrder)
{
// (xRes*yRes*zRes) array
// Depending on zFirstMemOrder arg:
// &elem(x, y, z) = data + x*zRes*yRes + y*zRes + z;
// &elem(x, y, z) = data + x + y*xRes + z*xRes*yRes;
int xdim, ydim, zdim;
if (ZFirstMemOrder)
{
xdim = volumeResolution.z * volumeResolution.y;
ydim = volumeResolution.z;
zdim = 1;
}
else
{
xdim = 1;
ydim = volumeResolution.x;
zdim = volumeResolution.x * volumeResolution.y;
}
return Vec4i(xdim, ydim, zdim);
}
}

511
modules/3d/test/ocl/test_tsdf.cpp
View File

@ -276,83 +276,6 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im
}
// ----------------------------
static const bool display = false;
static const bool parallelCheck = false;
class Settings
{
public:
float depthFactor;
Matx33f intr;
Size frameSize;
Vec3f lightPose;
Ptr<Volume> volume;
Ptr<Scene> scene;
std::vector<Affine3f> poses;
Settings(bool useHashTSDF, bool onlySemisphere)
{
frameSize = Size(640, 480);
float fx, fy, cx, cy;
fx = fy = 525.f;
cx = frameSize.width / 2 - 0.5f;
cy = frameSize.height / 2 - 0.5f;
intr = Matx33f(fx, 0, cx,
0, fy, cy,
0, 0, 1);
// 5000 for the 16-bit PNG files
// 1 for the 32-bit float images in the ROS bag files
depthFactor = 5000;
Vec3i volumeDims = Vec3i::all(512); //number of voxels
float volSize = 3.f;
float voxelSize = volSize / 512.f; //meters
// default pose of volume cube
Affine3f volumePose = Affine3f().translate(Vec3f(-volSize / 2.f, -volSize / 2.f, 0.5f));
float tsdf_trunc_dist = 7 * voxelSize; // about 0.04f in meters
int tsdf_max_weight = 64; //frames
float raycast_step_factor = 0.25f; //in voxel sizes
// gradient delta factor is fixed at 1.0f and is not used
//p.gradient_delta_factor = 0.5f; //in voxel sizes
//p.lightPose = p.volume_pose.translation()/4; //meters
lightPose = Vec3f::all(0.f); //meters
// depth truncation is not used by default but can be useful in some scenes
float truncateThreshold = 0.f; //meters
VolumeParams::VolumeKind volumeKind = VolumeParams::VolumeKind::TSDF;
if (useHashTSDF)
{
volumeKind = VolumeParams::VolumeKind::HASHTSDF;
truncateThreshold = 4.f;
}
else
{
volSize = 3.f;
volumeDims = Vec3i::all(128); //number of voxels
voxelSize = volSize / 128.f;
tsdf_trunc_dist = 2 * voxelSize; // 0.04f in meters
raycast_step_factor = 0.75f; //in voxel sizes
}
volume = makeVolume(volumeKind, voxelSize, volumePose.matrix,
raycast_step_factor, tsdf_trunc_dist, tsdf_max_weight,
truncateThreshold, volumeDims[0], volumeDims[1], volumeDims[2]);
scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
poses = scene->getPoses();
}
};
void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose)
{
Mat image;
@ -361,6 +284,7 @@ void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f l
renderPointsNormals(points, normals, image, lightPose);
imshow("render", image);
waitKey(2000);
destroyAllWindows();
}
void normalsCheck(Mat normals)
@ -400,123 +324,412 @@ int counterOfValid(Mat points)
return count;
}
void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals)
enum class VolumeTestFunction
{
Settings settings(isHashTSDF, false);
RAYCAST = 0,
FETCH_NORMALS = 1,
FETCH_POINTS_NORMALS = 2
};
Mat depth = settings.scene->depth(settings.poses[0]);
enum class VolumeTestSrcType
{
MAT = 0,
ODOMETRY_FRAME = 1
};
void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction testFunction, VolumeTestSrcType testSrcType)
{
VolumeSettings vs(volumeType);
Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr;
vs.getCameraIntegrateIntrinsics(intr);
bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]);
UMat udepth;
depth.copyTo(udepth);
UMat upoints, unormals, utmpnormals;
UMat unewPoints, unewNormals;
Mat points, normals;
Mat points, normals;
AccessFlag af = ACCESS_READ;
settings.volume->integrate(udepth, settings.depthFactor, settings.poses[0].matrix, settings.intr);
OdometryFrame odf(OdometryFrameStoreType::UMAT);
odf.setDepth(udepth);
if (isRaycast)
if (testSrcType == VolumeTestSrcType::MAT)
volume.integrate(depth, poses[0].matrix);
else
volume.integrate(odf, poses[0].matrix);
if (testFunction == VolumeTestFunction::RAYCAST)
{
settings.volume->raycast(settings.poses[0].matrix, settings.intr, settings.frameSize, upoints, unormals);
if (testSrcType == VolumeTestSrcType::MAT)
{
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals);
}
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
{
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, odf);
odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0);
odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0);
}
}
if (isFetchPointsNormals)
else if (testFunction == VolumeTestFunction::FETCH_NORMALS)
{
settings.volume->fetchPointsNormals(upoints, unormals);
if (testSrcType == VolumeTestSrcType::MAT)
{
// takes only point from raycast for checking fetched normals on the display
volume.raycast(poses[0].matrix, frameSize.height,frameSize.width, upoints, utmpnormals);
//volume.fetchPointsNormals(upoints, utmpnormals);
volume.fetchNormals(upoints, unormals);
}
}
if (isFetchNormals)
else if (testFunction == VolumeTestFunction::FETCH_POINTS_NORMALS)
{
settings.volume->fetchPointsNormals(upoints, utmpnormals);
settings.volume->fetchNormals(upoints, unormals);
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
{
volume.fetchPointsNormals(upoints, unormals);
}
}
normals = unormals.getMat(af);
points = upoints.getMat(af);
points = upoints.getMat(af);
auto normalCheck = [](Vec4f& vector, const int*)
{
if (!cvIsNaN(vector[0]))
{
float length = vector[0] * vector[0] +
vector[1] * vector[1] +
vector[2] * vector[2];
ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1";
}
};
if (testFunction == VolumeTestFunction::RAYCAST && cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, lightPose);
if (parallelCheck)
normals.forEach<Vec4f>(normalCheck);
else
normalsCheck(normals);
if (isRaycast && display)
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
if (isRaycast)
{
settings.volume->raycast(settings.poses[17].matrix, settings.intr, settings.frameSize, unewPoints, unewNormals);
normals = unewNormals.getMat(af);
points = unewPoints.getMat(af);
normalsCheck(normals);
if (parallelCheck)
normals.forEach<Vec4f>(normalCheck);
else
normalsCheck(normals);
if (display)
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
}
normalsCheck(normals);
}
void valid_points_test(bool isHashTSDF)
void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction testFunction, VolumeTestSrcType testSrcType)
{
Settings settings(isHashTSDF, true);
VolumeSettings vs(volumeType);
Volume volume(volumeType, vs);
Mat depth = settings.scene->depth(settings.poses[0]);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr;
vs.getCameraIntegrateIntrinsics(intr);
bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]);
UMat udepth;
depth.copyTo(udepth);
UMat upoints, unormals, unewPoints, unewNormals;
UMat upoints, unormals, utmpnormals;
Mat points, normals;
AccessFlag af = ACCESS_READ;
OdometryFrame odf(OdometryFrameStoreType::UMAT);
odf.setDepth(udepth);
if (testSrcType == VolumeTestSrcType::MAT)
volume.integrate(depth, poses[0].matrix);
else
volume.integrate(odf, poses[0].matrix);
if (testFunction == VolumeTestFunction::RAYCAST)
{
if (testSrcType == VolumeTestSrcType::MAT)
{
volume.raycast(poses[0].matrix, upoints, unormals);
}
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
{
volume.raycast(poses[0].matrix, odf);
odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0);
odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0);
}
}
else if (testFunction == VolumeTestFunction::FETCH_NORMALS)
{
if (testSrcType == VolumeTestSrcType::MAT)
{
// takes only point from raycast for checking fetched normals on the display
volume.raycast(poses[0].matrix, upoints, utmpnormals);
//volume.fetchPointsNormals(upoints, utmpnormals);
volume.fetchNormals(upoints, unormals);
}
}
else if (testFunction == VolumeTestFunction::FETCH_POINTS_NORMALS)
{
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
{
volume.fetchPointsNormals(upoints, unormals);
}
}
normals = unormals.getMat(af);
points = upoints.getMat(af);
if (testFunction == VolumeTestFunction::RAYCAST && cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, lightPose);
normalsCheck(normals);
}
void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType testSrcType)
{
VolumeSettings vs(volumeType);
Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr;
vs.getCameraIntegrateIntrinsics(intr);
bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]);
UMat udepth;
depth.copyTo(udepth);
UMat upoints, unormals;
UMat upoints1, unormals1;
Mat points, normals;
AccessFlag af = ACCESS_READ;
Mat points, normals;
int anfas, profile;
settings.volume->integrate(udepth, settings.depthFactor, settings.poses[0].matrix, settings.intr);
settings.volume->raycast(settings.poses[0].matrix, settings.intr, settings.frameSize, upoints, unormals);
OdometryFrame odf(OdometryFrameStoreType::UMAT);
odf.setDepth(udepth);
if (testSrcType == VolumeTestSrcType::MAT)
volume.integrate(depth, poses[0].matrix);
else
volume.integrate(odf, poses[0].matrix);
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
{
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals);
}
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
{
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, odf);
odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0);
odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0);
}
normals = unormals.getMat(af);
points = upoints.getMat(af);
patchNaNs(points);
anfas = counterOfValid(points);
ASSERT_NE(anfas, 0) << "There is no points in anfas";
if (cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, lightPose);
if (display)
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
{
volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, upoints1, unormals1);
}
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
{
volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, odf);
odf.getPyramidAt(upoints1, OdometryFramePyramidType::PYR_CLOUD, 0);
odf.getPyramidAt(unormals1, OdometryFramePyramidType::PYR_NORM, 0);
}
settings.volume->raycast(settings.poses[17].matrix, settings.intr, settings.frameSize, unewPoints, unewNormals);
normals = unewNormals.getMat(af);
points = unewPoints.getMat(af);
normals = unormals1.getMat(af);
points = upoints1.getMat(af);
patchNaNs(points);
profile = counterOfValid(points);
ASSERT_NE(profile, 0) << "There is no points in profile";
if (cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, lightPose);
// TODO: why profile == 2*anfas ?
float percentValidity = float(anfas) / float(profile);
ASSERT_NE(profile, 0) << "There is no points in profile";
ASSERT_NE(anfas, 0) << "There is no points in anfas";
ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")";
if (display)
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
}
TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); }
TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); }
TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); }
TEST(TSDF_GPU, valid_points) { valid_points_test(false); }
void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType testSrcType)
{
VolumeSettings vs(volumeType);
Volume volume(volumeType, vs);
TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); }
TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); }
TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); }
TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); }
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr;
vs.getCameraIntegrateIntrinsics(intr);
bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]);
UMat udepth;
depth.copyTo(udepth);
UMat upoints, unormals;
UMat upoints1, unormals1;
Mat points, normals;
AccessFlag af = ACCESS_READ;
int anfas, profile;
OdometryFrame odf(OdometryFrameStoreType::UMAT);
odf.setDepth(udepth);
if (testSrcType == VolumeTestSrcType::MAT)
volume.integrate(depth, poses[0].matrix);
else
volume.integrate(odf, poses[0].matrix);
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
{
volume.raycast(poses[0].matrix, upoints, unormals);
}
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
{
volume.raycast(poses[0].matrix, odf);
odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0);
odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0);
}
normals = unormals.getMat(af);
points = upoints.getMat(af);
patchNaNs(points);
anfas = counterOfValid(points);
if (cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, lightPose);
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
{
volume.raycast(poses[17].matrix, upoints1, unormals1);
}
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
{
volume.raycast(poses[17].matrix, odf);
odf.getPyramidAt(upoints1, OdometryFramePyramidType::PYR_CLOUD, 0);
odf.getPyramidAt(unormals1, OdometryFramePyramidType::PYR_NORM, 0);
}
normals = unormals1.getMat(af);
points = upoints1.getMat(af);
patchNaNs(points);
profile = counterOfValid(points);
if (cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, lightPose);
// TODO: why profile == 2*anfas ?
float percentValidity = float(anfas) / float(profile);
ASSERT_NE(profile, 0) << "There is no points in profile";
ASSERT_NE(anfas, 0) << "There is no points in anfas";
ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")";
}
TEST(TSDF_GPU, raycast_custom_framesize_normals_mat)
{
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
TEST(TSDF_GPU, raycast_custom_framesize_normals_frame)
{
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(TSDF_GPU, raycast_common_framesize_normals_mat)
{
normal_test_common_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
TEST(TSDF_GPU, raycast_common_framesize_normals_frame)
{
normal_test_common_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(TSDF_GPU, fetch_points_normals)
{
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
}
TEST(TSDF_GPU, fetch_normals)
{
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
}
TEST(TSDF_GPU, valid_points_custom_framesize_mat)
{
valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT);
}
TEST(TSDF_GPU, valid_points_custom_framesize_frame)
{
valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(TSDF_GPU, valid_points_common_framesize_mat)
{
valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT);
}
TEST(TSDF_GPU, valid_points_common_framesize_frame)
{
valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF_GPU, raycast_custom_framesize_normals_mat)
{
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_GPU, raycast_custom_framesize_normals_frame)
{
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF_GPU, raycast_common_framesize_normals_mat)
{
normal_test_common_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_GPU, raycast_common_framesize_normals_frame)
{
normal_test_common_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF_GPU, fetch_points_normals)
{
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_GPU, fetch_normals)
{
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_GPU, valid_points_custom_framesize_mat)
{
valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_GPU, valid_points_custom_framesize_frame)
{
valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF_GPU, valid_points_common_framesize_mat)
{
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_GPU, valid_points_common_framesize_frame)
{
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
}
} // namespace

961
modules/3d/test/test_tsdf.cpp
View File

File diff suppressed because it is too large Load Diff