Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #21189 from DumDereDum:new_volume

Browse Source 
New Volume pipeline
pull/21651/head
Artem Saratovtsev 3 years ago committed by GitHub
parent 4ba2b05df8
commit 9c87d8bf9c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
25 changed files with 7931 additions and 5293 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 39
      modules/3d/include/opencv2/3d/detail/submap.hpp
  2. 1
      modules/3d/include/opencv2/3d/odometry_frame.hpp
  3. 191
      modules/3d/include/opencv2/3d/volume.hpp
  4. 213
      modules/3d/include/opencv2/3d/volume_settings.hpp
  5. 941
      modules/3d/perf/perf_tsdf.cpp
  6. 1259
      modules/3d/src/rgbd/color_tsdf_functions.cpp
  7. 43
      modules/3d/src/rgbd/color_tsdf_functions.hpp
  8. 1024
      modules/3d/src/rgbd/colored_tsdf.cpp
  9. 39
      modules/3d/src/rgbd/colored_tsdf.hpp
  10. 1795
      modules/3d/src/rgbd/hash_tsdf.cpp
  11. 47
      modules/3d/src/rgbd/hash_tsdf.hpp
  12. 1534
      modules/3d/src/rgbd/hash_tsdf_functions.cpp
  13. 325
      modules/3d/src/rgbd/hash_tsdf_functions.hpp
  14. 26
      modules/3d/src/rgbd/odometry_frame_impl.cpp
  15. 9
      modules/3d/src/rgbd/odometry_functions.cpp
  16. 1228
      modules/3d/src/rgbd/tsdf.cpp
  17. 41
      modules/3d/src/rgbd/tsdf.hpp
  18. 1355
      modules/3d/src/rgbd/tsdf_functions.cpp
  19. 262
      modules/3d/src/rgbd/tsdf_functions.hpp
  20. 120
      modules/3d/src/rgbd/volume.cpp
  21. 540
      modules/3d/src/rgbd/volume_impl.cpp
  22. 220
      modules/3d/src/rgbd/volume_impl.hpp
  23. 532
      modules/3d/src/rgbd/volume_settings_impl.cpp
  24. 485
      modules/3d/test/ocl/test_tsdf.cpp
  25. 907
      modules/3d/test/test_tsdf.cpp

39
modules/3d/include/opencv2/3d/detail/submap.hpp
Unescape 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
Unescape 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);

191
modules/3d/include/opencv2/3d/volume.hpp
Unescape 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)
{ }
virtual ~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;
// 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; }
public:
const float voxelSize;
const float voxelSizeInv;
const Affine3f pose;
const float 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();
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)
/** @brief Integrates the input data to the volume.
Camera intrinsics are taken from volume settings structure.
* @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);
/** @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.
*/
CV_PROP_RW int kind;
void integrate(InputArray depth, InputArray pose);
/** @brief Integrates the input data to the volume.
/** @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
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.
*/
CV_PROP_RW int resolutionX;
CV_PROP_RW int resolutionY;
CV_PROP_RW int resolutionZ;
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.
/** @brief Resolution of volumeUnit in voxel space
Number of voxels in each dimension for volumeUnit
Applicable only for hashTSDF.
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.
*/
CV_PROP_RW int unitResolution = {0};
void raycast(InputArray cameraPose, OdometryFrame& outFrame) const;
/** @brief Initial pose of the volume in meters, should be 4x4 float or double matrix */
CV_PROP_RW Mat pose;
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
/** @brief Length of voxels in meters */
CV_PROP_RW float voxelSize;
Rendered image size and camera intrinsics are taken from volume settings structure.
/** @brief TSDF truncation distance
Distances greater than value from surface will be truncated to 1.0
* @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).
*/
CV_PROP_RW float tsdfTruncDist;
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.
/** @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
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.
*/
CV_PROP_RW int maxWeight;
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.
/** @brief Threshold for depth truncation in meters
Truncates the depth greater than threshold to 0
* @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).
*/
CV_PROP_RW float depthTruncThreshold;
void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const;
/** @brief Length of single raycast step
Describes the percentage of voxel length that is skipped per march
/** @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.
*/
CV_PROP_RW float raycastStepFactor;
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 Default set of parameters that provide higher quality reconstruction
at the cost of slow performance.
/** @brief clear all data in volume.
*/
CV_WRAP static Ptr<VolumeParams> defaultParams(int _volumeType);
void reset();
/** @brief Coarse set of parameters that provides relatively higher performance
at the cost of reconstrution quality.
/** @brief return visible blocks in volume.
*/
CV_WRAP static Ptr<VolumeParams> coarseParams(int _volumeType);
};
int getVisibleBlocks() const;
/** @brief return number of vulmeunits in volume.
*/
size_t getTotalVolumeUnits() const;
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);
class Impl;
private:
Ptr<Impl> impl;
};
} // namespace cv
#endif // include guard

213
modules/3d/include/opencv2/3d/volume_settings.hpp
Unescape 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

941
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
View File

File diff suppressed because it is too large Load Diff

43
modules/3d/src/rgbd/color_tsdf_functions.hpp
Unescape 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
Unescape 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
Unescape 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
View File

File diff suppressed because it is too large Load Diff

325
modules/3d/src/rgbd/hash_tsdf_functions.hpp
Unescape 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
Unescape 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
Unescape 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
Unescape 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

1355
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
Unescape 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;
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;
void integrateTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose,
InputArray _depth, InputArray _pixNorms, InputArray _volume);
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);
void integrateTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose,
InputArray _depth, InputArray _pixNorms, InputArray _volume);
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;
}
void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width,
InputArray _volume, OutputArray _points, OutputArray _normals);
v->idx = idx4;
v->row = row;
void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
InputArray _points, OutputArray _normals);
return extend;
}
void fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
OutputArray points, OutputArray normals);
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);
#ifdef HAVE_OPENCL
void ocl_integrateTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose,
InputArray _depth, InputArray _pixNorms, InputArray _volume);
if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0))
return v->row;
else
i = v->nextVolumeRow;
}
void ocl_raycastTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width,
InputArray _volume, OutputArray _points, OutputArray _normals);
return -1;
}
void ocl_fetchNormalsFromTsdfVolumeUnit(
const VolumeSettings& settings, InputArray _volume,
InputArray _points, OutputArray _normals);
inline int getPos(Vec3i idx, int bufferNum) const
{
int hash = int(calc_hash(idx) % hash_divisor);
return (bufferNum * hash_divisor + hash) * list_size;
}
void ocl_fetchPointsNormalsFromTsdfVolumeUnit(
const VolumeSettings& settings, InputArray _volume,
OutputArray _points, OutputArray _normals);
#endif
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
Unescape 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
Unescape 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
Unescape 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
Unescape 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);
}
}

485
modules/3d/test/ocl/test_tsdf.cpp
Unescape 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;
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)
{
if (testSrcType == VolumeTestSrcType::MAT)
{
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals);
}
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
{
settings.volume->raycast(settings.poses[0].matrix, settings.intr, settings.frameSize, upoints, unormals);
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)
{
if (testSrcType == VolumeTestSrcType::MAT)
{
settings.volume->fetchPointsNormals(upoints, unormals);
// 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)
{
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
{
settings.volume->fetchPointsNormals(upoints, utmpnormals);
settings.volume->fetchNormals(upoints, unormals);
volume.fetchPointsNormals(upoints, unormals);
}
}
normals = unormals.getMat(af);
points = upoints.getMat(af);
auto normalCheck = [](Vec4f& vector, const int*)
if (testFunction == VolumeTestFunction::RAYCAST && cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, lightPose);
normalsCheck(normals);
}
void normal_test_common_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;
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 (!cvIsNaN(vector[0]))
if (testSrcType == VolumeTestSrcType::MAT)
{
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";
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);
}
}
};
if (parallelCheck)
normals.forEach<Vec4f>(normalCheck);
else
normalsCheck(normals);
normals = unormals.getMat(af);
points = upoints.getMat(af);
if (isRaycast && display)
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
if (testFunction == VolumeTestFunction::RAYCAST && cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, 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);
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;
int anfas, profile;
OdometryFrame odf(OdometryFrameStoreType::UMAT);
odf.setDepth(udepth);
if (testSrcType == VolumeTestSrcType::MAT)
volume.integrate(depth, poses[0].matrix);
else
normalsCheck(normals);
volume.integrate(odf, poses[0].matrix);
if (display)
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
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);
if (cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, 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);
}
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 << ")";
}
void valid_points_test(bool isHashTSDF)
void valid_points_test_common_framesize(VolumeType volumeType, 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;
AccessFlag af = ACCESS_READ;
UMat upoints, unormals;
UMat upoints1, unormals1;
Mat points, normals;
AccessFlag af = ACCESS_READ;
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, 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);
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, 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);
}
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 << ")";
}
TEST(TSDF_GPU, raycast_custom_framesize_normals_mat)
{
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
if (display)
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
TEST(TSDF_GPU, raycast_custom_framesize_normals_frame)
{
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
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); }
TEST(TSDF_GPU, raycast_common_framesize_normals_mat)
{
normal_test_common_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
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); }
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

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

File diff suppressed because it is too large Load Diff
Write Preview
Loading…
Cancel
Save