mirror of
https://github.com/opencv/opencv.git
synced 2024-12-05 09:49:12 +08:00
Merge pull request #21189 from DumDereDum:new_volume
New Volume pipeline
This commit is contained in:
parent
4ba2b05df8
commit
9c87d8bf9c
@ -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;
|
||||
|
@ -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);
|
||||
|
@ -5,124 +5,135 @@
|
||||
#ifndef OPENCV_3D_VOLUME_HPP
|
||||
#define OPENCV_3D_VOLUME_HPP
|
||||
|
||||
#include "volume_settings.hpp"
|
||||
|
||||
#include "opencv2/core/affine.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
|
||||
class CV_EXPORTS_W Volume
|
||||
{
|
||||
public:
|
||||
Volume(float _voxelSize, Matx44f _pose, float _raycastStepFactor) :
|
||||
voxelSize(_voxelSize),
|
||||
voxelSizeInv(1.0f / voxelSize),
|
||||
pose(_pose),
|
||||
raycastStepFactor(_raycastStepFactor)
|
||||
{ }
|
||||
public:
|
||||
/** @brief Constructor of default volume - TSDF.
|
||||
*/
|
||||
Volume();
|
||||
/** @brief Constructor of custom volume.
|
||||
* @param vtype the volume type [TSDF, HashTSDF, ColorTSDF].
|
||||
* @param settings the custom settings for volume.
|
||||
*/
|
||||
Volume(VolumeType vtype, const VolumeSettings& settings);
|
||||
~Volume();
|
||||
|
||||
virtual ~Volume(){};
|
||||
/** @brief Integrates the input data to the volume.
|
||||
|
||||
CV_WRAP
|
||||
virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose,
|
||||
const Matx33f& intrinsics, const int frameId = 0) = 0;
|
||||
virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor,
|
||||
const Matx44f& cameraPose, const Matx33f& intrinsics,
|
||||
const Matx33f& rgb_intrinsics, const int frameId = 0) = 0;
|
||||
CV_WRAP
|
||||
virtual void raycast(const Matx44f& cameraPose, const Matx33f& intrinsics,
|
||||
const Size& frameSize, OutputArray points, OutputArray normals) const = 0;
|
||||
virtual void raycast(const Matx44f& cameraPose, const Matx33f& intrinsics, const Size& frameSize,
|
||||
OutputArray points, OutputArray normals, OutputArray colors) const = 0;
|
||||
CV_WRAP
|
||||
virtual void fetchNormals(InputArray points, OutputArray _normals) const = 0;
|
||||
CV_WRAP
|
||||
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0;
|
||||
CV_WRAP
|
||||
virtual void reset() = 0;
|
||||
Camera intrinsics are taken from volume settings structure.
|
||||
|
||||
// Works for hash-based volumes only, otherwise returns 1
|
||||
virtual int getVisibleBlocks(int /*currFrameId*/, int /*frameThreshold*/) const { return 1; }
|
||||
virtual size_t getTotalVolumeUnits() const { return 1; }
|
||||
* @param frame the object from which to take depth and image data.
|
||||
For color TSDF a depth data should be registered with color data, i.e. have the same intrinsics & camera pose.
|
||||
This can be done using function registerDepth() from 3d module.
|
||||
* @param pose the pose of camera in global coordinates.
|
||||
*/
|
||||
void integrate(const OdometryFrame& frame, InputArray pose);
|
||||
|
||||
public:
|
||||
const float voxelSize;
|
||||
const float voxelSizeInv;
|
||||
const Affine3f pose;
|
||||
const float raycastStepFactor;
|
||||
/** @brief Integrates the input data to the volume.
|
||||
|
||||
Camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param depth the depth image.
|
||||
* @param pose the pose of camera in global coordinates.
|
||||
*/
|
||||
void integrate(InputArray depth, InputArray pose);
|
||||
|
||||
/** @brief Integrates the input data to the volume.
|
||||
|
||||
Camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param depth the depth image.
|
||||
* @param image the color image (only for ColorTSDF).
|
||||
For color TSDF a depth data should be registered with color data, i.e. have the same intrinsics & camera pose.
|
||||
This can be done using function registerDepth() from 3d module.
|
||||
* @param pose the pose of camera in global coordinates.
|
||||
*/
|
||||
void integrate(InputArray depth, InputArray image, InputArray pose);
|
||||
|
||||
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
|
||||
|
||||
Rendered image size and camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param cameraPose the pose of camera in global coordinates.
|
||||
* @param outFrame the object where to store rendered points and normals.
|
||||
*/
|
||||
void raycast(InputArray cameraPose, OdometryFrame& outFrame) const;
|
||||
|
||||
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
|
||||
|
||||
Rendered image size and camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param cameraPose the pose of camera in global coordinates.
|
||||
* @param points image to store rendered points.
|
||||
* @param normals image to store rendered normals corresponding to points.
|
||||
* @param colors image to store rendered colors corresponding to points (only for ColorTSDF).
|
||||
*/
|
||||
void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const;
|
||||
|
||||
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
|
||||
|
||||
Rendered image size and camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param cameraPose the pose of camera in global coordinates.
|
||||
* @param height the height of result image.
|
||||
* @param width the width of result image.
|
||||
* @param outFrame the object where to store rendered points and normals.
|
||||
*/
|
||||
void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const;
|
||||
|
||||
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
|
||||
|
||||
Rendered image size and camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param cameraPose the pose of camera in global coordinates.
|
||||
* @param height the height of result image.
|
||||
* @param width the width of result image.
|
||||
* @param points image to store rendered points.
|
||||
* @param normals image to store rendered normals corresponding to points.
|
||||
* @param colors image to store rendered colors corresponding to points (only for ColorTSDF).
|
||||
*/
|
||||
void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const;
|
||||
|
||||
/** @brief Extract the all data from volume.
|
||||
* @param points the input exist point.
|
||||
* @param normals the storage of normals (corresponding to input points) in the image.
|
||||
*/
|
||||
void fetchNormals(InputArray points, OutputArray normals) const;
|
||||
/** @brief Extract the all data from volume.
|
||||
* @param points the storage of all points.
|
||||
* @param normals the storage of all normals, corresponding to points.
|
||||
*/
|
||||
void fetchPointsNormals(OutputArray points, OutputArray normals) const;
|
||||
/** @brief Extract the all data from volume.
|
||||
* @param points the storage of all points.
|
||||
* @param normals the storage of all normals, corresponding to points.
|
||||
* @param colors the storage of all colors, corresponding to points (only for ColorTSDF).
|
||||
*/
|
||||
void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const;
|
||||
|
||||
/** @brief clear all data in volume.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/** @brief return visible blocks in volume.
|
||||
*/
|
||||
int getVisibleBlocks() const;
|
||||
|
||||
/** @brief return number of vulmeunits in volume.
|
||||
*/
|
||||
size_t getTotalVolumeUnits() const;
|
||||
|
||||
class Impl;
|
||||
private:
|
||||
Ptr<Impl> impl;
|
||||
};
|
||||
|
||||
struct CV_EXPORTS_W VolumeParams
|
||||
{
|
||||
enum VolumeKind
|
||||
{
|
||||
TSDF = 0,
|
||||
HASHTSDF = 1,
|
||||
COLOREDTSDF = 2
|
||||
};
|
||||
|
||||
/** @brief Kind of Volume
|
||||
Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units)
|
||||
*/
|
||||
CV_PROP_RW int kind;
|
||||
|
||||
/** @brief Resolution of voxel space
|
||||
Number of voxels in each dimension.
|
||||
Applicable only for TSDF Volume.
|
||||
HashTSDF volume only supports equal resolution in all three dimensions
|
||||
*/
|
||||
CV_PROP_RW int resolutionX;
|
||||
CV_PROP_RW int resolutionY;
|
||||
CV_PROP_RW int resolutionZ;
|
||||
|
||||
/** @brief Resolution of volumeUnit in voxel space
|
||||
Number of voxels in each dimension for volumeUnit
|
||||
Applicable only for hashTSDF.
|
||||
*/
|
||||
CV_PROP_RW int unitResolution = {0};
|
||||
|
||||
/** @brief Initial pose of the volume in meters, should be 4x4 float or double matrix */
|
||||
CV_PROP_RW Mat pose;
|
||||
|
||||
/** @brief Length of voxels in meters */
|
||||
CV_PROP_RW float voxelSize;
|
||||
|
||||
/** @brief TSDF truncation distance
|
||||
Distances greater than value from surface will be truncated to 1.0
|
||||
*/
|
||||
CV_PROP_RW float tsdfTruncDist;
|
||||
|
||||
/** @brief Max number of frames to integrate per voxel
|
||||
Represents the max number of frames over which a running average
|
||||
of the TSDF is calculated for a voxel
|
||||
*/
|
||||
CV_PROP_RW int maxWeight;
|
||||
|
||||
/** @brief Threshold for depth truncation in meters
|
||||
Truncates the depth greater than threshold to 0
|
||||
*/
|
||||
CV_PROP_RW float depthTruncThreshold;
|
||||
|
||||
/** @brief Length of single raycast step
|
||||
Describes the percentage of voxel length that is skipped per march
|
||||
*/
|
||||
CV_PROP_RW float raycastStepFactor;
|
||||
|
||||
/** @brief Default set of parameters that provide higher quality reconstruction
|
||||
at the cost of slow performance.
|
||||
*/
|
||||
CV_WRAP static Ptr<VolumeParams> defaultParams(int _volumeType);
|
||||
|
||||
/** @brief Coarse set of parameters that provides relatively higher performance
|
||||
at the cost of reconstrution quality.
|
||||
*/
|
||||
CV_WRAP static Ptr<VolumeParams> coarseParams(int _volumeType);
|
||||
};
|
||||
|
||||
|
||||
CV_EXPORTS_W Ptr<Volume> makeVolume(const Ptr<VolumeParams>& _volumeParams);
|
||||
CV_EXPORTS_W Ptr<Volume> makeVolume(int _volumeType, float _voxelSize, Matx44f _pose,
|
||||
float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold,
|
||||
int _resolutionX, int _resolutionY, int _resolutionZ);
|
||||
|
||||
} // namespace cv
|
||||
|
||||
#endif // include guard
|
||||
|
213
modules/3d/include/opencv2/3d/volume_settings.hpp
Normal file
213
modules/3d/include/opencv2/3d/volume_settings.hpp
Normal 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
|
File diff suppressed because it is too large
Load Diff
1259
modules/3d/src/rgbd/color_tsdf_functions.cpp
Normal file
1259
modules/3d/src/rgbd/color_tsdf_functions.cpp
Normal file
File diff suppressed because it is too large
Load Diff
43
modules/3d/src/rgbd/color_tsdf_functions.hpp
Normal file
43
modules/3d/src/rgbd/color_tsdf_functions.hpp
Normal 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
|
File diff suppressed because it is too large
Load Diff
@ -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
|
File diff suppressed because it is too large
Load Diff
@ -1,47 +0,0 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html
|
||||
|
||||
#ifndef OPENCV_3D_HASH_TSDF_HPP
|
||||
#define OPENCV_3D_HASH_TSDF_HPP
|
||||
|
||||
#include "../precomp.hpp"
|
||||
#include "tsdf_functions.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
|
||||
class HashTSDFVolume : public Volume
|
||||
{
|
||||
public:
|
||||
// dimension in voxels, size in meters
|
||||
//! Use fixed volume cuboid
|
||||
HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist,
|
||||
int _maxWeight, float _truncateThreshold, int _volumeUnitRes,
|
||||
bool zFirstMemOrder = true);
|
||||
|
||||
virtual ~HashTSDFVolume() = default;
|
||||
|
||||
virtual int getVisibleBlocks(int currFrameId, int frameThreshold) const override = 0;
|
||||
virtual size_t getTotalVolumeUnits() const override = 0;
|
||||
|
||||
public:
|
||||
int maxWeight;
|
||||
float truncDist;
|
||||
float truncateThreshold;
|
||||
int volumeUnitResolution;
|
||||
int volumeUnitDegree;
|
||||
float volumeUnitSize;
|
||||
bool zFirstMemOrder;
|
||||
Vec4i volStrides;
|
||||
};
|
||||
|
||||
//template<typename T>
|
||||
Ptr<HashTSDFVolume> makeHashTSDFVolume(const VolumeParams& _volumeParams);
|
||||
//template<typename T>
|
||||
Ptr<HashTSDFVolume> makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist,
|
||||
int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16);
|
||||
|
||||
} // namespace cv
|
||||
|
||||
#endif // include guard
|
1534
modules/3d/src/rgbd/hash_tsdf_functions.cpp
Normal file
1534
modules/3d/src/rgbd/hash_tsdf_functions.cpp
Normal file
File diff suppressed because it is too large
Load Diff
325
modules/3d/src/rgbd/hash_tsdf_functions.hpp
Normal file
325
modules/3d/src/rgbd/hash_tsdf_functions.hpp
Normal 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
|
@ -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)
|
||||
{
|
||||
|
@ -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]);
|
||||
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -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
|
File diff suppressed because it is too large
Load Diff
@ -74,8 +74,10 @@ inline void colorFix(Point3f& c)
|
||||
if (c.z > 255) c.z = 255;
|
||||
}
|
||||
|
||||
cv::Mat preCalculationPixNorm(Size size, const Intr& intrinsics);
|
||||
cv::UMat preCalculationPixNormGPU(Size size, const Intr& intrinsics);
|
||||
void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm);
|
||||
#ifdef HAVE_OPENCL
|
||||
void ocl_preCalculationPixNorm(Size size, const Intr& intrinsics, UMat& pixNorm);
|
||||
#endif
|
||||
|
||||
inline depthType bilinearDepth(const Depth& m, cv::Point2f pt)
|
||||
{
|
||||
@ -151,256 +153,56 @@ inline depthType bilinearDepth(const Depth& m, cv::Point2f pt)
|
||||
}
|
||||
}
|
||||
|
||||
void integrateVolumeUnit(
|
||||
void _integrateVolumeUnit(
|
||||
float truncDist, float voxelSize, int maxWeight,
|
||||
cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides,
|
||||
InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose,
|
||||
const cv::Intr& intrinsics, InputArray _pixNorms, InputArray _volume);
|
||||
|
||||
void integrateRGBVolumeUnit(
|
||||
void _integrateRGBVolumeUnit(
|
||||
float truncDist, float voxelSize, int maxWeight,
|
||||
cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides,
|
||||
InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose,
|
||||
const cv::Intr& depth_intrinsics, const cv::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume);
|
||||
|
||||
|
||||
class CustomHashSet
|
||||
{
|
||||
public:
|
||||
static const int hashDivisor = 32768;
|
||||
static const int startCapacity = 2048;
|
||||
void integrateTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose,
|
||||
InputArray _depth, InputArray _pixNorms, InputArray _volume);
|
||||
|
||||
std::vector<int> hashes;
|
||||
// 0-3 for key, 4th for internal use
|
||||
// don't keep keep value
|
||||
std::vector<Vec4i> data;
|
||||
int capacity;
|
||||
int last;
|
||||
void integrateTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose,
|
||||
InputArray _depth, InputArray _pixNorms, InputArray _volume);
|
||||
|
||||
CustomHashSet()
|
||||
{
|
||||
hashes.resize(hashDivisor);
|
||||
for (int i = 0; i < hashDivisor; i++)
|
||||
hashes[i] = -1;
|
||||
capacity = startCapacity;
|
||||
|
||||
data.resize(capacity);
|
||||
for (int i = 0; i < capacity; i++)
|
||||
data[i] = { 0, 0, 0, -1 };
|
||||
void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width,
|
||||
InputArray _volume, OutputArray _points, OutputArray _normals);
|
||||
|
||||
last = 0;
|
||||
}
|
||||
void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
|
||||
InputArray _points, OutputArray _normals);
|
||||
|
||||
~CustomHashSet() { }
|
||||
void fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
|
||||
OutputArray points, OutputArray normals);
|
||||
|
||||
inline size_t calc_hash(Vec3i x) const
|
||||
{
|
||||
uint32_t seed = 0;
|
||||
constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
|
||||
}
|
||||
return seed;
|
||||
}
|
||||
|
||||
// should work on existing elements too
|
||||
// 0 - need resize
|
||||
// 1 - idx is inserted
|
||||
// 2 - idx already exists
|
||||
int insert(Vec3i idx)
|
||||
{
|
||||
if (last < capacity)
|
||||
{
|
||||
int hash = int(calc_hash(idx) % hashDivisor);
|
||||
int place = hashes[hash];
|
||||
if (place >= 0)
|
||||
{
|
||||
int oldPlace = place;
|
||||
while (place >= 0)
|
||||
{
|
||||
if (data[place][0] == idx[0] &&
|
||||
data[place][1] == idx[1] &&
|
||||
data[place][2] == idx[2])
|
||||
return 2;
|
||||
else
|
||||
{
|
||||
oldPlace = place;
|
||||
place = data[place][3];
|
||||
//std::cout << "place=" << place << std::endl;
|
||||
}
|
||||
}
|
||||
#ifdef HAVE_OPENCL
|
||||
void ocl_integrateTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose,
|
||||
InputArray _depth, InputArray _pixNorms, InputArray _volume);
|
||||
|
||||
// found, create here
|
||||
data[oldPlace][3] = last;
|
||||
}
|
||||
else
|
||||
{
|
||||
// insert at last
|
||||
hashes[hash] = last;
|
||||
}
|
||||
void ocl_raycastTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width,
|
||||
InputArray _volume, OutputArray _points, OutputArray _normals);
|
||||
|
||||
data[last][0] = idx[0];
|
||||
data[last][1] = idx[1];
|
||||
data[last][2] = idx[2];
|
||||
data[last][3] = -1;
|
||||
last++;
|
||||
void ocl_fetchNormalsFromTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, InputArray _volume,
|
||||
InputArray _points, OutputArray _normals);
|
||||
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
void ocl_fetchPointsNormalsFromTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, InputArray _volume,
|
||||
OutputArray _points, OutputArray _normals);
|
||||
#endif
|
||||
|
||||
int find(Vec3i idx) const
|
||||
{
|
||||
int hash = int(calc_hash(idx) % hashDivisor);
|
||||
int place = hashes[hash];
|
||||
// search a place
|
||||
while (place >= 0)
|
||||
{
|
||||
if (data[place][0] == idx[0] &&
|
||||
data[place][1] == idx[1] &&
|
||||
data[place][2] == idx[2])
|
||||
break;
|
||||
else
|
||||
{
|
||||
place = data[place][3];
|
||||
}
|
||||
}
|
||||
|
||||
return place;
|
||||
}
|
||||
};
|
||||
|
||||
// TODO: remove this structure as soon as HashTSDFGPU data is completely on GPU;
|
||||
// until then CustomHashTable can be replaced by this one if needed
|
||||
|
||||
const int NAN_ELEMENT = -2147483647;
|
||||
|
||||
struct Volume_NODE
|
||||
{
|
||||
Vec4i idx = Vec4i(NAN_ELEMENT);
|
||||
int32_t row = -1;
|
||||
int32_t nextVolumeRow = -1;
|
||||
int32_t dummy = 0;
|
||||
int32_t dummy2 = 0;
|
||||
};
|
||||
|
||||
const int _hash_divisor = 32768;
|
||||
const int _list_size = 4;
|
||||
|
||||
class VolumesTable
|
||||
{
|
||||
public:
|
||||
const int hash_divisor = _hash_divisor;
|
||||
const int list_size = _list_size;
|
||||
const int32_t free_row = -1;
|
||||
const int32_t free_isActive = 0;
|
||||
|
||||
const cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT);
|
||||
|
||||
int bufferNums;
|
||||
cv::Mat volumes;
|
||||
|
||||
VolumesTable() : bufferNums(1)
|
||||
{
|
||||
this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType<Volume_NODE>());
|
||||
for (int i = 0; i < volumes.size().height; i++)
|
||||
{
|
||||
Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
|
||||
v->idx = nan4;
|
||||
v->row = -1;
|
||||
v->nextVolumeRow = -1;
|
||||
}
|
||||
}
|
||||
const VolumesTable& operator=(const VolumesTable& vt)
|
||||
{
|
||||
this->volumes = vt.volumes;
|
||||
this->bufferNums = vt.bufferNums;
|
||||
return *this;
|
||||
}
|
||||
~VolumesTable() {};
|
||||
|
||||
bool insert(Vec3i idx, int row)
|
||||
{
|
||||
CV_Assert(row >= 0);
|
||||
|
||||
int bufferNum = 0;
|
||||
int hash = int(calc_hash(idx) % hash_divisor);
|
||||
int start = getPos(idx, bufferNum);
|
||||
int i = start;
|
||||
|
||||
while (i >= 0)
|
||||
{
|
||||
Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
|
||||
|
||||
if (v->idx[0] == NAN_ELEMENT)
|
||||
{
|
||||
Vec4i idx4(idx[0], idx[1], idx[2], 0);
|
||||
|
||||
bool extend = false;
|
||||
if (i != start && i % list_size == 0)
|
||||
{
|
||||
if (bufferNum >= bufferNums - 1)
|
||||
{
|
||||
extend = true;
|
||||
volumes.resize(hash_divisor * bufferNums);
|
||||
bufferNums++;
|
||||
}
|
||||
bufferNum++;
|
||||
v->nextVolumeRow = (bufferNum * hash_divisor + hash) * list_size;
|
||||
}
|
||||
else
|
||||
{
|
||||
v->nextVolumeRow = i + 1;
|
||||
}
|
||||
|
||||
v->idx = idx4;
|
||||
v->row = row;
|
||||
|
||||
return extend;
|
||||
}
|
||||
|
||||
i = v->nextVolumeRow;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
int findRow(Vec3i idx) const
|
||||
{
|
||||
int bufferNum = 0;
|
||||
int i = getPos(idx, bufferNum);
|
||||
|
||||
while (i >= 0)
|
||||
{
|
||||
const Volume_NODE* v = volumes.ptr<Volume_NODE>(i);
|
||||
|
||||
if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0))
|
||||
return v->row;
|
||||
else
|
||||
i = v->nextVolumeRow;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
inline int getPos(Vec3i idx, int bufferNum) const
|
||||
{
|
||||
int hash = int(calc_hash(idx) % hash_divisor);
|
||||
return (bufferNum * hash_divisor + hash) * list_size;
|
||||
}
|
||||
|
||||
inline size_t calc_hash(Vec3i x) const
|
||||
{
|
||||
uint32_t seed = 0;
|
||||
constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
|
||||
}
|
||||
return seed;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace cv
|
||||
|
||||
|
@ -1,120 +0,0 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html
|
||||
|
||||
#include "../precomp.hpp"
|
||||
#include "tsdf.hpp"
|
||||
#include "hash_tsdf.hpp"
|
||||
#include "colored_tsdf.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
|
||||
Ptr<VolumeParams> VolumeParams::defaultParams(int _volumeKind)
|
||||
{
|
||||
VolumeParams params;
|
||||
params.kind = _volumeKind;
|
||||
params.maxWeight = 64;
|
||||
params.raycastStepFactor = 0.25f;
|
||||
params.unitResolution = 0; // unitResolution not used for TSDF
|
||||
float volumeSize = 3.0f;
|
||||
Matx44f pose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)).matrix;
|
||||
params.pose = Mat(pose);
|
||||
|
||||
if(params.kind == VolumeKind::TSDF)
|
||||
{
|
||||
params.resolutionX = 512;
|
||||
params.resolutionY = 512;
|
||||
params.resolutionZ = 512;
|
||||
params.voxelSize = volumeSize / 512.f;
|
||||
params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF
|
||||
params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters
|
||||
return makePtr<VolumeParams>(params);
|
||||
}
|
||||
else if(params.kind == VolumeKind::HASHTSDF)
|
||||
{
|
||||
params.unitResolution = 16;
|
||||
params.voxelSize = volumeSize / 512.f;
|
||||
params.depthTruncThreshold = 4.f;
|
||||
params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters
|
||||
return makePtr<VolumeParams>(params);
|
||||
}
|
||||
else if (params.kind == VolumeKind::COLOREDTSDF)
|
||||
{
|
||||
params.resolutionX = 512;
|
||||
params.resolutionY = 512;
|
||||
params.resolutionZ = 512;
|
||||
params.voxelSize = volumeSize / 512.f;
|
||||
params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF
|
||||
params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters
|
||||
return makePtr<VolumeParams>(params);
|
||||
}
|
||||
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
|
||||
}
|
||||
|
||||
Ptr<VolumeParams> VolumeParams::coarseParams(int _volumeKind)
|
||||
{
|
||||
Ptr<VolumeParams> params = defaultParams(_volumeKind);
|
||||
|
||||
params->raycastStepFactor = 0.75f;
|
||||
float volumeSize = 3.0f;
|
||||
if(params->kind == VolumeKind::TSDF)
|
||||
{
|
||||
params->resolutionX = 128;
|
||||
params->resolutionY = 128;
|
||||
params->resolutionZ = 128;
|
||||
params->voxelSize = volumeSize / 128.f;
|
||||
params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters
|
||||
return params;
|
||||
}
|
||||
else if(params->kind == VolumeKind::HASHTSDF)
|
||||
{
|
||||
params->voxelSize = volumeSize / 128.f;
|
||||
params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters
|
||||
return params;
|
||||
}
|
||||
else if (params->kind == VolumeKind::COLOREDTSDF)
|
||||
{
|
||||
params->resolutionX = 128;
|
||||
params->resolutionY = 128;
|
||||
params->resolutionZ = 128;
|
||||
params->voxelSize = volumeSize / 128.f;
|
||||
params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters
|
||||
return params;
|
||||
}
|
||||
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
|
||||
}
|
||||
|
||||
Ptr<Volume> makeVolume(const Ptr<VolumeParams>& _volumeParams)
|
||||
{
|
||||
int kind = _volumeParams->kind;
|
||||
if(kind == VolumeParams::VolumeKind::TSDF)
|
||||
return makeTSDFVolume(*_volumeParams);
|
||||
else if(kind == VolumeParams::VolumeKind::HASHTSDF)
|
||||
return makeHashTSDFVolume(*_volumeParams);
|
||||
else if(kind == VolumeParams::VolumeKind::COLOREDTSDF)
|
||||
return makeColoredTSDFVolume(*_volumeParams);
|
||||
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
|
||||
}
|
||||
|
||||
Ptr<Volume> makeVolume(int _volumeKind, float _voxelSize, Matx44f _pose,
|
||||
float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold,
|
||||
int _resolutionX, int _resolutionY, int _resolutionZ)
|
||||
{
|
||||
Point3i _presolution(_resolutionX, _resolutionY, _resolutionZ);
|
||||
if (_volumeKind == VolumeParams::VolumeKind::TSDF)
|
||||
{
|
||||
return makeTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _presolution);
|
||||
}
|
||||
else if (_volumeKind == VolumeParams::VolumeKind::HASHTSDF)
|
||||
{
|
||||
return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold);
|
||||
}
|
||||
else if (_volumeKind == VolumeParams::VolumeKind::COLOREDTSDF)
|
||||
{
|
||||
return makeColoredTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _presolution);
|
||||
}
|
||||
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
|
||||
}
|
||||
|
||||
} // namespace cv
|
540
modules/3d/src/rgbd/volume_impl.cpp
Normal file
540
modules/3d/src/rgbd/volume_impl.cpp
Normal file
@ -0,0 +1,540 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html
|
||||
|
||||
|
||||
#include <iostream>
|
||||
#include "volume_impl.hpp"
|
||||
#include "tsdf_functions.hpp"
|
||||
#include "hash_tsdf_functions.hpp"
|
||||
#include "color_tsdf_functions.hpp"
|
||||
#include "opencv2/imgproc.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
|
||||
Volume::Impl::Impl(const VolumeSettings& _settings) :
|
||||
settings(_settings)
|
||||
#ifdef HAVE_OPENCL
|
||||
, useGPU(ocl::useOpenCL())
|
||||
#endif
|
||||
{}
|
||||
|
||||
// TSDF
|
||||
|
||||
TsdfVolume::TsdfVolume(const VolumeSettings& _settings) :
|
||||
Volume::Impl(_settings)
|
||||
{
|
||||
Vec3i volResolution;
|
||||
settings.getVolumeResolution(volResolution);
|
||||
#ifndef HAVE_OPENCL
|
||||
volume = Mat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<TsdfVoxel>());
|
||||
#else
|
||||
if (useGPU)
|
||||
gpu_volume = UMat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<TsdfVoxel>());
|
||||
else
|
||||
cpu_volume = Mat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<TsdfVoxel>());
|
||||
#endif
|
||||
|
||||
reset();
|
||||
}
|
||||
TsdfVolume::~TsdfVolume() {}
|
||||
|
||||
void TsdfVolume::integrate(const OdometryFrame& frame, InputArray _cameraPose)
|
||||
{
|
||||
CV_TRACE_FUNCTION();
|
||||
#ifndef HAVE_OPENCL
|
||||
Mat depth;
|
||||
#else
|
||||
UMat depth;
|
||||
#endif
|
||||
frame.getDepth(depth);
|
||||
integrate(depth, _cameraPose);
|
||||
}
|
||||
|
||||
void TsdfVolume::integrate(InputArray _depth, InputArray _cameraPose)
|
||||
{
|
||||
CV_TRACE_FUNCTION();
|
||||
#ifndef HAVE_OPENCL
|
||||
Mat depth = _depth.getMat();
|
||||
#else
|
||||
UMat depth = _depth.getUMat();
|
||||
#endif
|
||||
CV_Assert(!depth.empty());
|
||||
|
||||
Matx33f intr;
|
||||
settings.getCameraIntegrateIntrinsics(intr);
|
||||
Intr intrinsics(intr);
|
||||
Vec6f newParams((float)depth.rows, (float)depth.cols,
|
||||
intrinsics.fx, intrinsics.fy,
|
||||
intrinsics.cx, intrinsics.cy);
|
||||
if (!(frameParams == newParams))
|
||||
{
|
||||
frameParams = newParams;
|
||||
#ifndef HAVE_OPENCL
|
||||
preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
|
||||
#else
|
||||
if (useGPU)
|
||||
ocl_preCalculationPixNorm(depth.size(), intrinsics, gpu_pixNorms);
|
||||
else
|
||||
preCalculationPixNorm(depth.size(), intrinsics, cpu_pixNorms);
|
||||
#endif
|
||||
}
|
||||
const Matx44f cameraPose = _cameraPose.getMat();
|
||||
|
||||
#ifndef HAVE_OPENCL
|
||||
integrateTsdfVolumeUnit(settings, cameraPose, depth, pixNorms, volume);
|
||||
#else
|
||||
if (useGPU)
|
||||
ocl_integrateTsdfVolumeUnit(settings, cameraPose, depth, gpu_pixNorms, gpu_volume);
|
||||
else
|
||||
integrateTsdfVolumeUnit(settings, cameraPose, depth, cpu_pixNorms, cpu_volume);
|
||||
#endif
|
||||
}
|
||||
void TsdfVolume::integrate(InputArray, InputArray, InputArray)
|
||||
{
|
||||
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
|
||||
}
|
||||
|
||||
void TsdfVolume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const
|
||||
{
|
||||
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), outFrame);
|
||||
}
|
||||
|
||||
void TsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const
|
||||
{
|
||||
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors);
|
||||
}
|
||||
|
||||
void TsdfVolume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const
|
||||
{
|
||||
#ifndef HAVE_OPENCL
|
||||
Mat points, normals;
|
||||
raycast(cameraPose, height, width, points, normals, noArray());
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
|
||||
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
#else
|
||||
if (useGPU)
|
||||
{
|
||||
UMat points, normals;
|
||||
raycast(cameraPose, height, width, points, normals, noArray());
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
|
||||
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
Mat points, normals;
|
||||
raycast(cameraPose, height, width, points, normals, noArray());
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
|
||||
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const
|
||||
{
|
||||
if (_colors.needed())
|
||||
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
|
||||
|
||||
CV_Assert(height > 0);
|
||||
CV_Assert(width > 0);
|
||||
|
||||
const Matx44f cameraPose = _cameraPose.getMat();
|
||||
#ifndef HAVE_OPENCL
|
||||
raycastTsdfVolumeUnit(settings, cameraPose, height, width, volume, _points, _normals);
|
||||
#else
|
||||
if (useGPU)
|
||||
ocl_raycastTsdfVolumeUnit(settings, cameraPose, height, width, gpu_volume, _points, _normals);
|
||||
else
|
||||
raycastTsdfVolumeUnit(settings, cameraPose, height, width, cpu_volume, _points, _normals);
|
||||
#endif
|
||||
}
|
||||
|
||||
void TsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
|
||||
{
|
||||
#ifndef HAVE_OPENCL
|
||||
fetchNormalsFromTsdfVolumeUnit(settings, volume, points, normals);
|
||||
#else
|
||||
if (useGPU)
|
||||
ocl_fetchNormalsFromTsdfVolumeUnit(settings, gpu_volume, points, normals);
|
||||
else
|
||||
fetchNormalsFromTsdfVolumeUnit(settings, cpu_volume, points, normals);
|
||||
#endif
|
||||
}
|
||||
|
||||
void TsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
|
||||
{
|
||||
#ifndef HAVE_OPENCL
|
||||
fetchPointsNormalsFromTsdfVolumeUnit(settings, volume, points, normals);
|
||||
#else
|
||||
if (useGPU)
|
||||
ocl_fetchPointsNormalsFromTsdfVolumeUnit(settings, gpu_volume, points, normals);
|
||||
else
|
||||
fetchPointsNormalsFromTsdfVolumeUnit(settings, cpu_volume, points, normals);
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void TsdfVolume::fetchPointsNormalsColors(OutputArray, OutputArray, OutputArray) const
|
||||
{
|
||||
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
|
||||
}
|
||||
|
||||
void TsdfVolume::reset()
|
||||
{
|
||||
CV_TRACE_FUNCTION();
|
||||
#ifndef HAVE_OPENCL
|
||||
//TODO: use setTo(Scalar(0, 0))
|
||||
volume.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
|
||||
{
|
||||
TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
|
||||
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
|
||||
});
|
||||
#else
|
||||
if (useGPU)
|
||||
gpu_volume.setTo(Scalar(0, 0));
|
||||
else
|
||||
//TODO: use setTo(Scalar(0, 0))
|
||||
cpu_volume.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
|
||||
{
|
||||
TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
|
||||
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
|
||||
});
|
||||
#endif
|
||||
}
|
||||
int TsdfVolume::getVisibleBlocks() const { return 1; }
|
||||
size_t TsdfVolume::getTotalVolumeUnits() const { return 1; }
|
||||
|
||||
|
||||
|
||||
// HASH_TSDF
|
||||
|
||||
HashTsdfVolume::HashTsdfVolume(const VolumeSettings& _settings) :
|
||||
Volume::Impl(_settings)
|
||||
{
|
||||
Vec3i resolution;
|
||||
settings.getVolumeResolution(resolution);
|
||||
const Point3i volResolution = Point3i(resolution);
|
||||
volumeUnitDegree = calcVolumeUnitDegree(volResolution);
|
||||
|
||||
#ifndef HAVE_OPENCL
|
||||
volUnitsData = cv::Mat(VOLUMES_SIZE, resolution[0] * resolution[1] * resolution[2], rawType<TsdfVoxel>());
|
||||
reset();
|
||||
#else
|
||||
if (useGPU)
|
||||
{
|
||||
reset();
|
||||
}
|
||||
else
|
||||
{
|
||||
cpu_volUnitsData = cv::Mat(VOLUMES_SIZE, resolution[0] * resolution[1] * resolution[2], rawType<TsdfVoxel>());
|
||||
reset();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
HashTsdfVolume::~HashTsdfVolume() {}
|
||||
|
||||
void HashTsdfVolume::integrate(const OdometryFrame& frame, InputArray _cameraPose)
|
||||
{
|
||||
CV_TRACE_FUNCTION();
|
||||
#ifndef HAVE_OPENCL
|
||||
Mat depth;
|
||||
#else
|
||||
UMat depth;
|
||||
#endif
|
||||
frame.getDepth(depth);
|
||||
integrate(depth, _cameraPose);
|
||||
}
|
||||
|
||||
void HashTsdfVolume::integrate(InputArray _depth, InputArray _cameraPose)
|
||||
{
|
||||
#ifndef HAVE_OPENCL
|
||||
Mat depth = _depth.getMat();
|
||||
#else
|
||||
UMat depth = _depth.getUMat();
|
||||
#endif
|
||||
const Matx44f cameraPose = _cameraPose.getMat();
|
||||
Matx33f intr;
|
||||
settings.getCameraIntegrateIntrinsics(intr);
|
||||
Intr intrinsics(intr);
|
||||
Vec6f newParams((float)depth.rows, (float)depth.cols,
|
||||
intrinsics.fx, intrinsics.fy,
|
||||
intrinsics.cx, intrinsics.cy);
|
||||
if (!(frameParams == newParams))
|
||||
{
|
||||
frameParams = newParams;
|
||||
#ifndef HAVE_OPENCL
|
||||
preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
|
||||
#else
|
||||
if (useGPU)
|
||||
ocl_preCalculationPixNorm(depth.size(), intrinsics, gpu_pixNorms);
|
||||
else
|
||||
preCalculationPixNorm(depth.size(), intrinsics, cpu_pixNorms);
|
||||
#endif
|
||||
}
|
||||
#ifndef HAVE_OPENCL
|
||||
integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, depth, pixNorms, volUnitsData, volumeUnits);
|
||||
lastFrameId++;
|
||||
#else
|
||||
if (useGPU)
|
||||
{
|
||||
ocl_integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, bufferSizeDegree, volumeUnitDegree, depth, gpu_pixNorms, lastVisibleIndices, volUnitsDataCopy, gpu_volUnitsData, hashTable, isActiveFlags);
|
||||
}
|
||||
else
|
||||
{
|
||||
integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, depth, cpu_pixNorms, cpu_volUnitsData, cpu_volumeUnits);
|
||||
lastFrameId++;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void HashTsdfVolume::integrate(InputArray, InputArray, InputArray)
|
||||
{
|
||||
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
|
||||
}
|
||||
|
||||
void HashTsdfVolume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const
|
||||
{
|
||||
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), outFrame);
|
||||
}
|
||||
|
||||
void HashTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const
|
||||
{
|
||||
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors);
|
||||
}
|
||||
|
||||
void HashTsdfVolume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const
|
||||
{
|
||||
#ifndef HAVE_OPENCL
|
||||
Mat points, normals;
|
||||
raycast(cameraPose, height, width, points, normals, noArray());
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
|
||||
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
#else
|
||||
if (useGPU)
|
||||
{
|
||||
UMat points, normals;
|
||||
raycast(cameraPose, height, width, points, normals, noArray());
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
|
||||
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
Mat points, normals;
|
||||
raycast(cameraPose, height, width, points, normals, noArray());
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
|
||||
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
|
||||
}
|
||||
#endif
|
||||
}
|
||||
void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const
|
||||
{
|
||||
if (_colors.needed())
|
||||
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
|
||||
|
||||
const Matx44f cameraPose = _cameraPose.getMat();
|
||||
|
||||
#ifndef HAVE_OPENCL
|
||||
raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals);
|
||||
#else
|
||||
if (useGPU)
|
||||
ocl_raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, hashTable, gpu_volUnitsData, _points, _normals);
|
||||
else
|
||||
raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, cpu_volUnitsData, cpu_volumeUnits, _points, _normals);
|
||||
#endif
|
||||
}
|
||||
|
||||
void HashTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
|
||||
{
|
||||
#ifndef HAVE_OPENCL
|
||||
fetchNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
|
||||
#else
|
||||
if (useGPU)
|
||||
olc_fetchNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals);
|
||||
else
|
||||
fetchNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals);
|
||||
|
||||
#endif
|
||||
}
|
||||
void HashTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
|
||||
{
|
||||
#ifndef HAVE_OPENCL
|
||||
fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals);
|
||||
#else
|
||||
if (useGPU)
|
||||
ocl_fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals);
|
||||
else
|
||||
fetchPointsNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals);
|
||||
#endif
|
||||
}
|
||||
|
||||
void HashTsdfVolume::fetchPointsNormalsColors(OutputArray, OutputArray, OutputArray) const
|
||||
{
|
||||
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
|
||||
};
|
||||
|
||||
void HashTsdfVolume::reset()
|
||||
{
|
||||
CV_TRACE_FUNCTION();
|
||||
lastVolIndex = 0;
|
||||
lastFrameId = 0;
|
||||
#ifndef HAVE_OPENCL
|
||||
volUnitsData.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
|
||||
{
|
||||
TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
|
||||
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
|
||||
});
|
||||
volumeUnits = VolumeUnitIndexes();
|
||||
#else
|
||||
if (useGPU)
|
||||
{
|
||||
Vec3i resolution;
|
||||
settings.getVolumeResolution(resolution);
|
||||
|
||||
bufferSizeDegree = 15;
|
||||
int buff_lvl = (int)(1 << bufferSizeDegree);
|
||||
int volCubed = resolution[0] * resolution[1] * resolution[2];
|
||||
|
||||
volUnitsDataCopy = cv::Mat(buff_lvl, volCubed, rawType<TsdfVoxel>());
|
||||
gpu_volUnitsData = cv::UMat(buff_lvl, volCubed, CV_8UC2);
|
||||
lastVisibleIndices = cv::UMat(buff_lvl, 1, CV_32S);
|
||||
isActiveFlags = cv::UMat(buff_lvl, 1, CV_8U);
|
||||
hashTable = CustomHashSet();
|
||||
frameParams = Vec6f();
|
||||
gpu_pixNorms = UMat();
|
||||
}
|
||||
else
|
||||
{
|
||||
cpu_volUnitsData.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
|
||||
{
|
||||
TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
|
||||
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
|
||||
});
|
||||
cpu_volumeUnits = VolumeUnitIndexes();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
int HashTsdfVolume::getVisibleBlocks() const { return 1; }
|
||||
size_t HashTsdfVolume::getTotalVolumeUnits() const { return 1; }
|
||||
|
||||
// COLOR_TSDF
|
||||
|
||||
ColorTsdfVolume::ColorTsdfVolume(const VolumeSettings& _settings) :
|
||||
Volume::Impl(_settings)
|
||||
{
|
||||
Vec3i volResolution;
|
||||
settings.getVolumeResolution(volResolution);
|
||||
volume = Mat(1, volResolution[0] * volResolution[1] * volResolution[2], rawType<RGBTsdfVoxel>());
|
||||
reset();
|
||||
}
|
||||
|
||||
ColorTsdfVolume::~ColorTsdfVolume() {}
|
||||
|
||||
void ColorTsdfVolume::integrate(const OdometryFrame& frame, InputArray cameraPose)
|
||||
{
|
||||
CV_TRACE_FUNCTION();
|
||||
Mat depth;
|
||||
frame.getDepth(depth);
|
||||
Mat rgb;
|
||||
frame.getImage(rgb);
|
||||
|
||||
integrate(depth, rgb, cameraPose);
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::integrate(InputArray, InputArray)
|
||||
{
|
||||
CV_Error(cv::Error::StsBadFunc, "Color data should be passed for this volume type");
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::integrate(InputArray _depth, InputArray _image, InputArray _cameraPose)
|
||||
{
|
||||
Mat depth = _depth.getMat();
|
||||
Colors image = _image.getMat();
|
||||
const Matx44f cameraPose = _cameraPose.getMat();
|
||||
Matx33f intr;
|
||||
settings.getCameraIntegrateIntrinsics(intr);
|
||||
Intr intrinsics(intr);
|
||||
Vec6f newParams((float)depth.rows, (float)depth.cols,
|
||||
intrinsics.fx, intrinsics.fy,
|
||||
intrinsics.cx, intrinsics.cy);
|
||||
if (!(frameParams == newParams))
|
||||
{
|
||||
frameParams = newParams;
|
||||
preCalculationPixNorm(depth.size(), intrinsics, pixNorms);
|
||||
}
|
||||
integrateColorTsdfVolumeUnit(settings, cameraPose, depth, image, pixNorms, volume);
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const
|
||||
{
|
||||
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), outFrame);
|
||||
}
|
||||
void ColorTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const
|
||||
{
|
||||
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors);
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const
|
||||
{
|
||||
Mat points, normals, colors;
|
||||
raycast(cameraPose, height, width, points, normals, colors);
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_NORM);
|
||||
outFrame.setPyramidLevel(1, OdometryFramePyramidType::PYR_IMAGE);
|
||||
outFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
outFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
outFrame.setPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0);
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const
|
||||
{
|
||||
const Matx44f cameraPose = _cameraPose.getMat();
|
||||
raycastColorTsdfVolumeUnit(settings, cameraPose, height, width, volume, _points, _normals, _colors);
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const
|
||||
{
|
||||
fetchNormalsFromColorTsdfVolumeUnit(settings, volume, points, normals);
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const
|
||||
{
|
||||
fetchPointsNormalsFromColorTsdfVolumeUnit(settings, volume, points, normals);
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const
|
||||
{
|
||||
fetchPointsNormalsColorsFromColorTsdfVolumeUnit(settings, volume, points, normals, colors);
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::reset()
|
||||
{
|
||||
CV_TRACE_FUNCTION();
|
||||
|
||||
volume.forEach<VecRGBTsdfVoxel>([](VecRGBTsdfVoxel& vv, const int* /* position */)
|
||||
{
|
||||
RGBTsdfVoxel& v = reinterpret_cast<RGBTsdfVoxel&>(vv);
|
||||
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
|
||||
});
|
||||
}
|
||||
|
||||
int ColorTsdfVolume::getVisibleBlocks() const { return 1; }
|
||||
size_t ColorTsdfVolume::getTotalVolumeUnits() const { return 1; }
|
||||
|
||||
}
|
220
modules/3d/src/rgbd/volume_impl.hpp
Normal file
220
modules/3d/src/rgbd/volume_impl.hpp
Normal file
@ -0,0 +1,220 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html
|
||||
|
||||
#ifndef OPENCV_3D_VOLUME_IMPL_HPP
|
||||
#define OPENCV_3D_VOLUME_IMPL_HPP
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "../precomp.hpp"
|
||||
#include "hash_tsdf_functions.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
|
||||
class Volume::Impl
|
||||
{
|
||||
private:
|
||||
// TODO: make debug function, which show histogram of volume points values
|
||||
// make this function run with debug lvl == 10
|
||||
public:
|
||||
Impl(const VolumeSettings& settings);
|
||||
virtual ~Impl() {};
|
||||
|
||||
virtual void integrate(const OdometryFrame& frame, InputArray pose) = 0;
|
||||
virtual void integrate(InputArray depth, InputArray pose) = 0;
|
||||
virtual void integrate(InputArray depth, InputArray image, InputArray pose) = 0;
|
||||
|
||||
virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const = 0;
|
||||
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const = 0;
|
||||
|
||||
virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const = 0;
|
||||
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const = 0;
|
||||
|
||||
virtual void fetchNormals(InputArray points, OutputArray normals) const = 0;
|
||||
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0;
|
||||
virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const = 0;
|
||||
|
||||
virtual void reset() = 0;
|
||||
virtual int getVisibleBlocks() const = 0;
|
||||
virtual size_t getTotalVolumeUnits() const = 0;
|
||||
|
||||
public:
|
||||
const VolumeSettings& settings;
|
||||
#ifdef HAVE_OPENCL
|
||||
const bool useGPU;
|
||||
#endif
|
||||
};
|
||||
|
||||
|
||||
class TsdfVolume : public Volume::Impl
|
||||
{
|
||||
public:
|
||||
TsdfVolume(const VolumeSettings& settings);
|
||||
~TsdfVolume();
|
||||
|
||||
virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
|
||||
virtual void integrate(InputArray depth, InputArray pose) override;
|
||||
virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
|
||||
virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const override;
|
||||
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
|
||||
virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const override;
|
||||
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override;
|
||||
|
||||
virtual void fetchNormals(InputArray points, OutputArray normals) const override;
|
||||
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
|
||||
virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
|
||||
|
||||
virtual void reset() override;
|
||||
virtual int getVisibleBlocks() const override;
|
||||
virtual size_t getTotalVolumeUnits() const override;
|
||||
|
||||
public:
|
||||
Vec6f frameParams;
|
||||
#ifndef HAVE_OPENCL
|
||||
Mat pixNorms;
|
||||
// See zFirstMemOrder arg of parent class constructor
|
||||
// for the array layout info
|
||||
// Consist of Voxel elements
|
||||
Mat volume;
|
||||
#else
|
||||
//temporary solution
|
||||
Mat cpu_pixNorms;
|
||||
Mat cpu_volume;
|
||||
UMat gpu_pixNorms;
|
||||
UMat gpu_volume;
|
||||
#endif
|
||||
};
|
||||
|
||||
|
||||
typedef std::unordered_set<cv::Vec3i, tsdf_hash> VolumeUnitIndexSet;
|
||||
typedef std::unordered_map<cv::Vec3i, VolumeUnit, tsdf_hash> VolumeUnitIndexes;
|
||||
|
||||
class HashTsdfVolume : public Volume::Impl
|
||||
{
|
||||
public:
|
||||
HashTsdfVolume(const VolumeSettings& settings);
|
||||
~HashTsdfVolume();
|
||||
|
||||
virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
|
||||
virtual void integrate(InputArray depth, InputArray pose) override;
|
||||
virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
|
||||
virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const override;
|
||||
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
|
||||
virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const override;
|
||||
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override;
|
||||
|
||||
virtual void fetchNormals(InputArray points, OutputArray normals) const override;
|
||||
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
|
||||
virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
|
||||
|
||||
virtual void reset() override;
|
||||
virtual int getVisibleBlocks() const override;
|
||||
virtual size_t getTotalVolumeUnits() const override;
|
||||
public:
|
||||
int lastVolIndex;
|
||||
int lastFrameId;
|
||||
Vec6f frameParams;
|
||||
int volumeUnitDegree;
|
||||
|
||||
#ifndef HAVE_OPENCL
|
||||
Mat volUnitsData;
|
||||
Mat pixNorms;
|
||||
VolumeUnitIndexes volumeUnits;
|
||||
#else
|
||||
VolumeUnitIndexes cpu_volumeUnits;
|
||||
|
||||
Mat cpu_volUnitsData;
|
||||
Mat cpu_pixNorms;
|
||||
UMat gpu_volUnitsData;
|
||||
UMat gpu_pixNorms;
|
||||
|
||||
int bufferSizeDegree;
|
||||
// per-volume-unit data
|
||||
UMat lastVisibleIndices;
|
||||
UMat isActiveFlags;
|
||||
//TODO: remove it when there's no CPU parts
|
||||
Mat volUnitsDataCopy;
|
||||
//TODO: move indexes.volumes to GPU
|
||||
CustomHashSet hashTable;
|
||||
#endif
|
||||
};
|
||||
|
||||
|
||||
class ColorTsdfVolume : public Volume::Impl
|
||||
{
|
||||
public:
|
||||
ColorTsdfVolume(const VolumeSettings& settings);
|
||||
~ColorTsdfVolume();
|
||||
|
||||
virtual void integrate(const OdometryFrame& frame, InputArray pose) override;
|
||||
virtual void integrate(InputArray depth, InputArray pose) override;
|
||||
virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
|
||||
virtual void raycast(InputArray cameraPose, OdometryFrame& outFrame) const override;
|
||||
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
|
||||
virtual void raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const override;
|
||||
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override;
|
||||
|
||||
virtual void fetchNormals(InputArray points, OutputArray normals) const override;
|
||||
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
|
||||
virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
|
||||
|
||||
virtual void reset() override;
|
||||
virtual int getVisibleBlocks() const override;
|
||||
virtual size_t getTotalVolumeUnits() const override;
|
||||
private:
|
||||
Vec4i volStrides;
|
||||
Vec6f frameParams;
|
||||
Mat pixNorms;
|
||||
// See zFirstMemOrder arg of parent class constructor
|
||||
// for the array layout info
|
||||
// Consist of Voxel elements
|
||||
Mat volume;
|
||||
};
|
||||
|
||||
|
||||
Volume::Volume()
|
||||
{
|
||||
VolumeSettings settings;
|
||||
this->impl = makePtr<TsdfVolume>(settings);
|
||||
}
|
||||
Volume::Volume(VolumeType vtype, const VolumeSettings& settings)
|
||||
{
|
||||
switch (vtype)
|
||||
{
|
||||
case VolumeType::TSDF:
|
||||
this->impl = makePtr<TsdfVolume>(settings);
|
||||
break;
|
||||
case VolumeType::HashTSDF:
|
||||
this->impl = makePtr<HashTsdfVolume>(settings);
|
||||
break;
|
||||
case VolumeType::ColorTSDF:
|
||||
this->impl = makePtr<ColorTsdfVolume>(settings);
|
||||
break;
|
||||
default:
|
||||
CV_Error(Error::StsInternal, "Incorrect OdometryType, you are able to use only { ICP, RGB, RGBD }");
|
||||
break;
|
||||
}
|
||||
}
|
||||
Volume::~Volume() {}
|
||||
|
||||
void Volume::integrate(const OdometryFrame& frame, InputArray pose) { this->impl->integrate(frame, pose); }
|
||||
void Volume::integrate(InputArray depth, InputArray pose) { this->impl->integrate(depth, pose); }
|
||||
void Volume::integrate(InputArray depth, InputArray image, InputArray pose) { this->impl->integrate(depth, image, pose); }
|
||||
void Volume::raycast(InputArray cameraPose, OdometryFrame& outFrame) const { this->impl->raycast(cameraPose, outFrame); }
|
||||
void Volume::raycast(InputArray cameraPose, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, _points, _normals, _colors); }
|
||||
void Volume::raycast(InputArray cameraPose, int height, int width, OdometryFrame& outFrame) const { this->impl->raycast(cameraPose, height, width, outFrame); }
|
||||
void Volume::raycast(InputArray cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, height, width, _points, _normals, _colors); }
|
||||
void Volume::fetchNormals(InputArray points, OutputArray normals) const { this->impl->fetchNormals(points, normals); }
|
||||
void Volume::fetchPointsNormals(OutputArray points, OutputArray normals) const { this->impl->fetchPointsNormals(points, normals); }
|
||||
void Volume::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const { this->impl->fetchPointsNormalsColors(points, normals, colors); };
|
||||
|
||||
void Volume::reset() { this->impl->reset(); }
|
||||
int Volume::getVisibleBlocks() const { return this->impl->getVisibleBlocks(); }
|
||||
size_t Volume::getTotalVolumeUnits() const { return this->impl->getTotalVolumeUnits(); }
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif // !OPENCV_3D_VOLUME_IMPL_HPP
|
532
modules/3d/src/rgbd/volume_settings_impl.cpp
Normal file
532
modules/3d/src/rgbd/volume_settings_impl.cpp
Normal 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);
|
||||
}
|
||||
|
||||
}
|
@ -276,83 +276,6 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im
|
||||
}
|
||||
// ----------------------------
|
||||
|
||||
static const bool display = false;
|
||||
static const bool parallelCheck = false;
|
||||
|
||||
class Settings
|
||||
{
|
||||
public:
|
||||
float depthFactor;
|
||||
Matx33f intr;
|
||||
Size frameSize;
|
||||
Vec3f lightPose;
|
||||
|
||||
Ptr<Volume> volume;
|
||||
Ptr<Scene> scene;
|
||||
std::vector<Affine3f> poses;
|
||||
|
||||
Settings(bool useHashTSDF, bool onlySemisphere)
|
||||
{
|
||||
frameSize = Size(640, 480);
|
||||
|
||||
float fx, fy, cx, cy;
|
||||
fx = fy = 525.f;
|
||||
cx = frameSize.width / 2 - 0.5f;
|
||||
cy = frameSize.height / 2 - 0.5f;
|
||||
intr = Matx33f(fx, 0, cx,
|
||||
0, fy, cy,
|
||||
0, 0, 1);
|
||||
|
||||
// 5000 for the 16-bit PNG files
|
||||
// 1 for the 32-bit float images in the ROS bag files
|
||||
depthFactor = 5000;
|
||||
|
||||
Vec3i volumeDims = Vec3i::all(512); //number of voxels
|
||||
|
||||
float volSize = 3.f;
|
||||
float voxelSize = volSize / 512.f; //meters
|
||||
|
||||
// default pose of volume cube
|
||||
Affine3f volumePose = Affine3f().translate(Vec3f(-volSize / 2.f, -volSize / 2.f, 0.5f));
|
||||
float tsdf_trunc_dist = 7 * voxelSize; // about 0.04f in meters
|
||||
int tsdf_max_weight = 64; //frames
|
||||
|
||||
float raycast_step_factor = 0.25f; //in voxel sizes
|
||||
// gradient delta factor is fixed at 1.0f and is not used
|
||||
//p.gradient_delta_factor = 0.5f; //in voxel sizes
|
||||
|
||||
//p.lightPose = p.volume_pose.translation()/4; //meters
|
||||
lightPose = Vec3f::all(0.f); //meters
|
||||
|
||||
// depth truncation is not used by default but can be useful in some scenes
|
||||
float truncateThreshold = 0.f; //meters
|
||||
|
||||
VolumeParams::VolumeKind volumeKind = VolumeParams::VolumeKind::TSDF;
|
||||
|
||||
if (useHashTSDF)
|
||||
{
|
||||
volumeKind = VolumeParams::VolumeKind::HASHTSDF;
|
||||
truncateThreshold = 4.f;
|
||||
}
|
||||
else
|
||||
{
|
||||
volSize = 3.f;
|
||||
volumeDims = Vec3i::all(128); //number of voxels
|
||||
voxelSize = volSize / 128.f;
|
||||
tsdf_trunc_dist = 2 * voxelSize; // 0.04f in meters
|
||||
|
||||
raycast_step_factor = 0.75f; //in voxel sizes
|
||||
}
|
||||
|
||||
volume = makeVolume(volumeKind, voxelSize, volumePose.matrix,
|
||||
raycast_step_factor, tsdf_trunc_dist, tsdf_max_weight,
|
||||
truncateThreshold, volumeDims[0], volumeDims[1], volumeDims[2]);
|
||||
|
||||
scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
|
||||
poses = scene->getPoses();
|
||||
}
|
||||
};
|
||||
|
||||
void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f lightPose)
|
||||
{
|
||||
Mat image;
|
||||
@ -361,6 +284,7 @@ void displayImage(Mat depth, Mat points, Mat normals, float depthFactor, Vec3f l
|
||||
renderPointsNormals(points, normals, image, lightPose);
|
||||
imshow("render", image);
|
||||
waitKey(2000);
|
||||
destroyAllWindows();
|
||||
}
|
||||
|
||||
void normalsCheck(Mat normals)
|
||||
@ -400,123 +324,412 @@ int counterOfValid(Mat points)
|
||||
return count;
|
||||
}
|
||||
|
||||
void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals)
|
||||
enum class VolumeTestFunction
|
||||
{
|
||||
Settings settings(isHashTSDF, false);
|
||||
RAYCAST = 0,
|
||||
FETCH_NORMALS = 1,
|
||||
FETCH_POINTS_NORMALS = 2
|
||||
};
|
||||
|
||||
Mat depth = settings.scene->depth(settings.poses[0]);
|
||||
enum class VolumeTestSrcType
|
||||
{
|
||||
MAT = 0,
|
||||
ODOMETRY_FRAME = 1
|
||||
};
|
||||
|
||||
void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction testFunction, VolumeTestSrcType testSrcType)
|
||||
{
|
||||
VolumeSettings vs(volumeType);
|
||||
Volume volume(volumeType, vs);
|
||||
|
||||
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
|
||||
Matx33f intr;
|
||||
vs.getCameraIntegrateIntrinsics(intr);
|
||||
bool onlySemisphere = true;
|
||||
float depthFactor = vs.getDepthFactor();
|
||||
Vec3f lightPose = Vec3f::all(0.f);
|
||||
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
|
||||
std::vector<Affine3f> poses = scene->getPoses();
|
||||
|
||||
Mat depth = scene->depth(poses[0]);
|
||||
UMat udepth;
|
||||
depth.copyTo(udepth);
|
||||
UMat upoints, unormals, utmpnormals;
|
||||
UMat unewPoints, unewNormals;
|
||||
Mat points, normals;
|
||||
Mat points, normals;
|
||||
AccessFlag af = ACCESS_READ;
|
||||
|
||||
settings.volume->integrate(udepth, settings.depthFactor, settings.poses[0].matrix, settings.intr);
|
||||
OdometryFrame odf(OdometryFrameStoreType::UMAT);
|
||||
odf.setDepth(udepth);
|
||||
|
||||
if (isRaycast)
|
||||
if (testSrcType == VolumeTestSrcType::MAT)
|
||||
volume.integrate(depth, poses[0].matrix);
|
||||
else
|
||||
volume.integrate(odf, poses[0].matrix);
|
||||
|
||||
if (testFunction == VolumeTestFunction::RAYCAST)
|
||||
{
|
||||
settings.volume->raycast(settings.poses[0].matrix, settings.intr, settings.frameSize, upoints, unormals);
|
||||
if (testSrcType == VolumeTestSrcType::MAT)
|
||||
{
|
||||
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals);
|
||||
}
|
||||
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
|
||||
{
|
||||
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, odf);
|
||||
odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
}
|
||||
}
|
||||
if (isFetchPointsNormals)
|
||||
else if (testFunction == VolumeTestFunction::FETCH_NORMALS)
|
||||
{
|
||||
settings.volume->fetchPointsNormals(upoints, unormals);
|
||||
if (testSrcType == VolumeTestSrcType::MAT)
|
||||
{
|
||||
// takes only point from raycast for checking fetched normals on the display
|
||||
volume.raycast(poses[0].matrix, frameSize.height,frameSize.width, upoints, utmpnormals);
|
||||
//volume.fetchPointsNormals(upoints, utmpnormals);
|
||||
volume.fetchNormals(upoints, unormals);
|
||||
}
|
||||
}
|
||||
if (isFetchNormals)
|
||||
else if (testFunction == VolumeTestFunction::FETCH_POINTS_NORMALS)
|
||||
{
|
||||
settings.volume->fetchPointsNormals(upoints, utmpnormals);
|
||||
settings.volume->fetchNormals(upoints, unormals);
|
||||
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
|
||||
{
|
||||
volume.fetchPointsNormals(upoints, unormals);
|
||||
}
|
||||
}
|
||||
|
||||
normals = unormals.getMat(af);
|
||||
points = upoints.getMat(af);
|
||||
points = upoints.getMat(af);
|
||||
|
||||
auto normalCheck = [](Vec4f& vector, const int*)
|
||||
{
|
||||
if (!cvIsNaN(vector[0]))
|
||||
{
|
||||
float length = vector[0] * vector[0] +
|
||||
vector[1] * vector[1] +
|
||||
vector[2] * vector[2];
|
||||
ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1";
|
||||
}
|
||||
};
|
||||
if (testFunction == VolumeTestFunction::RAYCAST && cvtest::debugLevel > 0)
|
||||
displayImage(depth, points, normals, depthFactor, lightPose);
|
||||
|
||||
if (parallelCheck)
|
||||
normals.forEach<Vec4f>(normalCheck);
|
||||
else
|
||||
normalsCheck(normals);
|
||||
|
||||
if (isRaycast && display)
|
||||
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
|
||||
|
||||
if (isRaycast)
|
||||
{
|
||||
settings.volume->raycast(settings.poses[17].matrix, settings.intr, settings.frameSize, unewPoints, unewNormals);
|
||||
normals = unewNormals.getMat(af);
|
||||
points = unewPoints.getMat(af);
|
||||
normalsCheck(normals);
|
||||
|
||||
if (parallelCheck)
|
||||
normals.forEach<Vec4f>(normalCheck);
|
||||
else
|
||||
normalsCheck(normals);
|
||||
|
||||
if (display)
|
||||
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
|
||||
}
|
||||
normalsCheck(normals);
|
||||
}
|
||||
|
||||
void valid_points_test(bool isHashTSDF)
|
||||
void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction testFunction, VolumeTestSrcType testSrcType)
|
||||
{
|
||||
Settings settings(isHashTSDF, true);
|
||||
VolumeSettings vs(volumeType);
|
||||
Volume volume(volumeType, vs);
|
||||
|
||||
Mat depth = settings.scene->depth(settings.poses[0]);
|
||||
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
|
||||
Matx33f intr;
|
||||
vs.getCameraIntegrateIntrinsics(intr);
|
||||
bool onlySemisphere = true;
|
||||
float depthFactor = vs.getDepthFactor();
|
||||
Vec3f lightPose = Vec3f::all(0.f);
|
||||
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
|
||||
std::vector<Affine3f> poses = scene->getPoses();
|
||||
|
||||
Mat depth = scene->depth(poses[0]);
|
||||
UMat udepth;
|
||||
depth.copyTo(udepth);
|
||||
UMat upoints, unormals, unewPoints, unewNormals;
|
||||
UMat upoints, unormals, utmpnormals;
|
||||
Mat points, normals;
|
||||
AccessFlag af = ACCESS_READ;
|
||||
|
||||
OdometryFrame odf(OdometryFrameStoreType::UMAT);
|
||||
odf.setDepth(udepth);
|
||||
|
||||
if (testSrcType == VolumeTestSrcType::MAT)
|
||||
volume.integrate(depth, poses[0].matrix);
|
||||
else
|
||||
volume.integrate(odf, poses[0].matrix);
|
||||
|
||||
if (testFunction == VolumeTestFunction::RAYCAST)
|
||||
{
|
||||
if (testSrcType == VolumeTestSrcType::MAT)
|
||||
{
|
||||
volume.raycast(poses[0].matrix, upoints, unormals);
|
||||
}
|
||||
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
|
||||
{
|
||||
volume.raycast(poses[0].matrix, odf);
|
||||
odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
}
|
||||
}
|
||||
else if (testFunction == VolumeTestFunction::FETCH_NORMALS)
|
||||
{
|
||||
if (testSrcType == VolumeTestSrcType::MAT)
|
||||
{
|
||||
// takes only point from raycast for checking fetched normals on the display
|
||||
volume.raycast(poses[0].matrix, upoints, utmpnormals);
|
||||
//volume.fetchPointsNormals(upoints, utmpnormals);
|
||||
volume.fetchNormals(upoints, unormals);
|
||||
}
|
||||
}
|
||||
else if (testFunction == VolumeTestFunction::FETCH_POINTS_NORMALS)
|
||||
{
|
||||
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
|
||||
{
|
||||
volume.fetchPointsNormals(upoints, unormals);
|
||||
}
|
||||
}
|
||||
|
||||
normals = unormals.getMat(af);
|
||||
points = upoints.getMat(af);
|
||||
|
||||
if (testFunction == VolumeTestFunction::RAYCAST && cvtest::debugLevel > 0)
|
||||
displayImage(depth, points, normals, depthFactor, lightPose);
|
||||
|
||||
normalsCheck(normals);
|
||||
}
|
||||
|
||||
void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType testSrcType)
|
||||
{
|
||||
VolumeSettings vs(volumeType);
|
||||
Volume volume(volumeType, vs);
|
||||
|
||||
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
|
||||
Matx33f intr;
|
||||
vs.getCameraIntegrateIntrinsics(intr);
|
||||
bool onlySemisphere = true;
|
||||
float depthFactor = vs.getDepthFactor();
|
||||
Vec3f lightPose = Vec3f::all(0.f);
|
||||
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
|
||||
std::vector<Affine3f> poses = scene->getPoses();
|
||||
|
||||
Mat depth = scene->depth(poses[0]);
|
||||
UMat udepth;
|
||||
depth.copyTo(udepth);
|
||||
UMat upoints, unormals;
|
||||
UMat upoints1, unormals1;
|
||||
Mat points, normals;
|
||||
AccessFlag af = ACCESS_READ;
|
||||
Mat points, normals;
|
||||
int anfas, profile;
|
||||
|
||||
settings.volume->integrate(udepth, settings.depthFactor, settings.poses[0].matrix, settings.intr);
|
||||
settings.volume->raycast(settings.poses[0].matrix, settings.intr, settings.frameSize, upoints, unormals);
|
||||
OdometryFrame odf(OdometryFrameStoreType::UMAT);
|
||||
odf.setDepth(udepth);
|
||||
|
||||
if (testSrcType == VolumeTestSrcType::MAT)
|
||||
volume.integrate(depth, poses[0].matrix);
|
||||
else
|
||||
volume.integrate(odf, poses[0].matrix);
|
||||
|
||||
|
||||
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
|
||||
{
|
||||
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals);
|
||||
}
|
||||
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
|
||||
{
|
||||
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, odf);
|
||||
odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
}
|
||||
|
||||
normals = unormals.getMat(af);
|
||||
points = upoints.getMat(af);
|
||||
patchNaNs(points);
|
||||
anfas = counterOfValid(points);
|
||||
|
||||
ASSERT_NE(anfas, 0) << "There is no points in anfas";
|
||||
if (cvtest::debugLevel > 0)
|
||||
displayImage(depth, points, normals, depthFactor, lightPose);
|
||||
|
||||
if (display)
|
||||
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
|
||||
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
|
||||
{
|
||||
volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, upoints1, unormals1);
|
||||
}
|
||||
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
|
||||
{
|
||||
volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, odf);
|
||||
odf.getPyramidAt(upoints1, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
odf.getPyramidAt(unormals1, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
}
|
||||
|
||||
settings.volume->raycast(settings.poses[17].matrix, settings.intr, settings.frameSize, unewPoints, unewNormals);
|
||||
normals = unewNormals.getMat(af);
|
||||
points = unewPoints.getMat(af);
|
||||
normals = unormals1.getMat(af);
|
||||
points = upoints1.getMat(af);
|
||||
patchNaNs(points);
|
||||
profile = counterOfValid(points);
|
||||
|
||||
ASSERT_NE(profile, 0) << "There is no points in profile";
|
||||
if (cvtest::debugLevel > 0)
|
||||
displayImage(depth, points, normals, depthFactor, lightPose);
|
||||
|
||||
// TODO: why profile == 2*anfas ?
|
||||
float percentValidity = float(anfas) / float(profile);
|
||||
|
||||
ASSERT_NE(profile, 0) << "There is no points in profile";
|
||||
ASSERT_NE(anfas, 0) << "There is no points in anfas";
|
||||
ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")";
|
||||
|
||||
if (display)
|
||||
displayImage(depth, points, normals, settings.depthFactor, settings.lightPose);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); }
|
||||
TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); }
|
||||
TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); }
|
||||
TEST(TSDF_GPU, valid_points) { valid_points_test(false); }
|
||||
void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType testSrcType)
|
||||
{
|
||||
VolumeSettings vs(volumeType);
|
||||
Volume volume(volumeType, vs);
|
||||
|
||||
TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); }
|
||||
TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); }
|
||||
TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); }
|
||||
TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); }
|
||||
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
|
||||
Matx33f intr;
|
||||
vs.getCameraIntegrateIntrinsics(intr);
|
||||
bool onlySemisphere = true;
|
||||
float depthFactor = vs.getDepthFactor();
|
||||
Vec3f lightPose = Vec3f::all(0.f);
|
||||
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
|
||||
std::vector<Affine3f> poses = scene->getPoses();
|
||||
|
||||
Mat depth = scene->depth(poses[0]);
|
||||
UMat udepth;
|
||||
depth.copyTo(udepth);
|
||||
UMat upoints, unormals;
|
||||
UMat upoints1, unormals1;
|
||||
Mat points, normals;
|
||||
AccessFlag af = ACCESS_READ;
|
||||
int anfas, profile;
|
||||
|
||||
OdometryFrame odf(OdometryFrameStoreType::UMAT);
|
||||
odf.setDepth(udepth);
|
||||
|
||||
if (testSrcType == VolumeTestSrcType::MAT)
|
||||
volume.integrate(depth, poses[0].matrix);
|
||||
else
|
||||
volume.integrate(odf, poses[0].matrix);
|
||||
|
||||
|
||||
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
|
||||
{
|
||||
volume.raycast(poses[0].matrix, upoints, unormals);
|
||||
}
|
||||
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
|
||||
{
|
||||
volume.raycast(poses[0].matrix, odf);
|
||||
odf.getPyramidAt(upoints, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
odf.getPyramidAt(unormals, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
}
|
||||
|
||||
normals = unormals.getMat(af);
|
||||
points = upoints.getMat(af);
|
||||
patchNaNs(points);
|
||||
anfas = counterOfValid(points);
|
||||
|
||||
if (cvtest::debugLevel > 0)
|
||||
displayImage(depth, points, normals, depthFactor, lightPose);
|
||||
|
||||
if (testSrcType == VolumeTestSrcType::MAT) // Odometry frame or Mats
|
||||
{
|
||||
volume.raycast(poses[17].matrix, upoints1, unormals1);
|
||||
}
|
||||
else if (testSrcType == VolumeTestSrcType::ODOMETRY_FRAME)
|
||||
{
|
||||
volume.raycast(poses[17].matrix, odf);
|
||||
odf.getPyramidAt(upoints1, OdometryFramePyramidType::PYR_CLOUD, 0);
|
||||
odf.getPyramidAt(unormals1, OdometryFramePyramidType::PYR_NORM, 0);
|
||||
}
|
||||
|
||||
normals = unormals1.getMat(af);
|
||||
points = upoints1.getMat(af);
|
||||
patchNaNs(points);
|
||||
profile = counterOfValid(points);
|
||||
|
||||
if (cvtest::debugLevel > 0)
|
||||
displayImage(depth, points, normals, depthFactor, lightPose);
|
||||
|
||||
// TODO: why profile == 2*anfas ?
|
||||
float percentValidity = float(anfas) / float(profile);
|
||||
|
||||
ASSERT_NE(profile, 0) << "There is no points in profile";
|
||||
ASSERT_NE(anfas, 0) << "There is no points in anfas";
|
||||
ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")";
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, raycast_custom_framesize_normals_mat)
|
||||
{
|
||||
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, raycast_custom_framesize_normals_frame)
|
||||
{
|
||||
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, raycast_common_framesize_normals_mat)
|
||||
{
|
||||
normal_test_common_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, raycast_common_framesize_normals_frame)
|
||||
{
|
||||
normal_test_common_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, fetch_points_normals)
|
||||
{
|
||||
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, fetch_normals)
|
||||
{
|
||||
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, valid_points_custom_framesize_mat)
|
||||
{
|
||||
valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, valid_points_custom_framesize_frame)
|
||||
{
|
||||
valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, valid_points_common_framesize_mat)
|
||||
{
|
||||
valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(TSDF_GPU, valid_points_common_framesize_frame)
|
||||
{
|
||||
valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, raycast_custom_framesize_normals_mat)
|
||||
{
|
||||
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, raycast_custom_framesize_normals_frame)
|
||||
{
|
||||
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, raycast_common_framesize_normals_mat)
|
||||
{
|
||||
normal_test_common_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, raycast_common_framesize_normals_frame)
|
||||
{
|
||||
normal_test_common_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, fetch_points_normals)
|
||||
{
|
||||
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, fetch_normals)
|
||||
{
|
||||
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, valid_points_custom_framesize_mat)
|
||||
{
|
||||
valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, valid_points_custom_framesize_frame)
|
||||
{
|
||||
valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, valid_points_common_framesize_mat)
|
||||
{
|
||||
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT);
|
||||
}
|
||||
|
||||
TEST(HashTSDF_GPU, valid_points_common_framesize_frame)
|
||||
{
|
||||
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
}
|
||||
} // namespace
|
||||
|
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user