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

Merge pull request #22845 from savuor:volume_interface_etc

Browse Source 
Corresponding contrib PR: #3382@contrib

Changes

- Volume::raycast(): camera intrinsics can be explicitly passed to the function. If not, the ones from current volume settings are used
- getVolumeDimensions() renamed to getVolumeStrides() because they are strides actually
- TSDF tests: OpenCLStatusRevert and parametrized fixture
- ColorTSDF::integrate(): extra RGB projector is redundant, removed
- Minor changes
This commit is contained in:
Rostislav Vasilikhin 2022-11-24 15:16:18 +01:00 committed by GitHub
parent 2407ab4e61
commit a423b07149
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
15 changed files with 616 additions and 557 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -52,7 +52,7 @@ public:
virtual ~Submap() = default; virtual ~Submap() = default;
virtual void integrate(InputArray _depth, const int currframeId); virtual void integrate(InputArray _depth, const int currframeId);
virtual void raycast(const cv::Affine3f& cameraPose, cv::Size frameSize, virtual void raycast(const cv::Affine3f& cameraPose, cv::Size frameSize, cv::Matx33f K,
OutputArray points = noArray(), OutputArray normals = noArray()); OutputArray points = noArray(), OutputArray normals = noArray());
virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); }; virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); };
@ -112,14 +112,14 @@ void Submap<MatType>::integrate(InputArray _depth, const int currFrameId)
} }
template<typename MatType> template<typename MatType>
void Submap<MatType>::raycast(const cv::Affine3f& _cameraPose, cv::Size frameSize, void Submap<MatType>::raycast(const cv::Affine3f& _cameraPose, cv::Size frameSize, cv::Matx33f K,
OutputArray points, OutputArray normals) OutputArray points, OutputArray normals)
{ {
if (!points.needed() && !normals.needed()) if (!points.needed() && !normals.needed())
{ {
MatType pts, nrm; MatType pts, nrm;
//TODO: get depth instead of pts from raycast //TODO: get depth instead of pts from raycast
volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.height, pts, nrm); volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.width, K, pts, nrm);
std::vector<MatType> pch(3); std::vector<MatType> pch(3);
split(pts, pch); split(pts, pch);
@ -130,7 +130,7 @@ void Submap<MatType>::raycast(const cv::Affine3f& _cameraPose, cv::Size frameSiz
} }
else else
{ {
volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.height, points, normals); volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.width, K, points, normals);
} }
} }

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

@ -22,7 +22,7 @@ public:
* @param vtype the volume type [TSDF, HashTSDF, ColorTSDF]. * @param vtype the volume type [TSDF, HashTSDF, ColorTSDF].
* @param settings the custom settings for volume. * @param settings the custom settings for volume.
*/ */
Volume(VolumeType vtype, const VolumeSettings& settings); Volume(VolumeType vtype, VolumeSettings settings);
virtual ~Volume(); virtual ~Volume();
/** @brief Integrates the input data to the volume. /** @brief Integrates the input data to the volume.
@ -73,13 +73,14 @@ public:
Rendered image size and camera intrinsics are taken from volume settings structure. Rendered image size and camera intrinsics are taken from volume settings structure.
* @param cameraPose the pose of camera in global coordinates. * @param cameraPose the pose of camera in global coordinates.
* @param height the height of result image. * @param height the height of result image
* @param width the width of result image. * @param width the width of result image
* @param K camera raycast intrinsics
* @param points image to store rendered points. * @param points image to store rendered points.
* @param normals image to store rendered normals corresponding to points. * @param normals image to store rendered normals corresponding to points.
* @param colors image to store rendered colors corresponding to points (only for ColorTSDF). * @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; void raycast(InputArray cameraPose, int height, int width, InputArray K, OutputArray points, OutputArray normals, OutputArray colors = noArray()) const;
/** @brief Extract the all data from volume. /** @brief Extract the all data from volume.
* @param points the input exist point. * @param points the input exist point.

16
modules/3d/include/opencv2/3d/volume_settings.hpp
View File

@ -54,21 +54,21 @@ public:
int getIntegrateHeight() const; int getIntegrateHeight() const;
/** @brief Sets the width of the raycasted image. /** @brief Sets the width of the raycasted image, used when user does not provide it at raycast() call.
* @param val input value. * @param val input value.
*/ */
void setRaycastWidth(int val); void setRaycastWidth(int val);
/** @brief Returns the width of the raycasted image. /** @brief Returns the width of the raycasted image, used when user does not provide it at raycast() call.
*/ */
int getRaycastWidth() const; int getRaycastWidth() const;
/** @brief Sets the height of the raycasted image. /** @brief Sets the height of the raycasted image, used when user does not provide it at raycast() call.
* @param val input value. * @param val input value.
*/ */
void setRaycastHeight(int val); void setRaycastHeight(int val);
/** @brief Returns the height of the raycasted image. /** @brief Returns the height of the raycasted image, used when user does not provide it at raycast() call.
*/ */
int getRaycastHeight() const; int getRaycastHeight() const;
@ -159,10 +159,10 @@ public:
void getVolumeResolution(OutputArray val) const; void getVolumeResolution(OutputArray val) const;
/** @brief Returns 3 integers representing strides by x, y and z dimension. /** @brief Returns 3 integers representing strides by x, y and z dimension.
Can be used to iterate over volume unit raw data. Can be used to iterate over raw volume unit data.
* @param val output value. * @param val output value.
*/ */
void getVolumeDimensions(OutputArray val) const; void getVolumeStrides(OutputArray val) const;
/** @brief Sets intrinsics of camera for integrations. /** @brief Sets intrinsics of camera for integrations.
* Format of input: * Format of input:
@ -184,7 +184,7 @@ public:
*/ */
void getCameraIntegrateIntrinsics(OutputArray val) const; void getCameraIntegrateIntrinsics(OutputArray val) const;
/** @brief Sets intrinsics of camera for raycast image. /** @brief Sets camera intrinsics for raycast image which, used when user does not provide them at raycast() call.
* Format of input: * Format of input:
* [ fx 0 cx ] * [ fx 0 cx ]
* [ 0 fy cy ] * [ 0 fy cy ]
@ -194,7 +194,7 @@ public:
*/ */
void setCameraRaycastIntrinsics(InputArray val); void setCameraRaycastIntrinsics(InputArray val);
/** @brief Returns intrinsics of camera for raycast image. /** @brief Returns camera intrinsics for raycast image, used when user does not provide them at raycast() call.
* Format of output: * Format of output:
* [ fx 0 cx ] * [ fx 0 cx ]
* [ 0 fy cy ] * [ 0 fy cy ]

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

@ -428,11 +428,12 @@ PERF_TEST(Perf_TSDF, integrate_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -460,11 +461,12 @@ PERF_TEST(Perf_TSDF, integrate_frame)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -492,12 +494,13 @@ PERF_TEST(Perf_TSDF, raycast_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -509,7 +512,7 @@ PERF_TEST(Perf_TSDF, raycast_mat)
volume.integrate(depth, pose); volume.integrate(depth, pose);
startTimer(); startTimer();
volume.raycast(pose, frameSize.height, frameSize.width, points, normals); volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals);
stopTimer(); stopTimer();
if (display) if (display)
@ -531,11 +534,12 @@ PERF_TEST(Perf_TSDF_CPU, integrate_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
@ -564,11 +568,12 @@ PERF_TEST(Perf_TSDF_CPU, integrate_frame)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -597,12 +602,13 @@ PERF_TEST(Perf_TSDF_CPU, raycast_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -614,7 +620,7 @@ PERF_TEST(Perf_TSDF_CPU, raycast_mat)
volume.integrate(depth, pose); volume.integrate(depth, pose);
startTimer(); startTimer();
volume.raycast(pose, frameSize.height, frameSize.width, points, normals); volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals);
stopTimer(); stopTimer();
if (display) if (display)
@ -640,11 +646,12 @@ PERF_TEST(Perf_HashTSDF, integrate_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -672,11 +679,12 @@ PERF_TEST(Perf_HashTSDF, integrate_frame)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -705,12 +713,13 @@ PERF_TEST(Perf_HashTSDF, raycast_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -722,7 +731,7 @@ PERF_TEST(Perf_HashTSDF, raycast_mat)
volume.integrate(depth, pose); volume.integrate(depth, pose);
startTimer(); startTimer();
volume.raycast(pose, frameSize.height, frameSize.width, points, normals); volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals);
stopTimer(); stopTimer();
if (display) if (display)
@ -743,11 +752,12 @@ PERF_TEST(Perf_HashTSDF_CPU, integrate_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
@ -776,11 +786,12 @@ PERF_TEST(Perf_HashTSDF_CPU, integrate_frame)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -809,12 +820,13 @@ PERF_TEST(Perf_HashTSDF_CPU, raycast_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -826,7 +838,7 @@ PERF_TEST(Perf_HashTSDF_CPU, raycast_mat)
volume.integrate(depth, pose); volume.integrate(depth, pose);
startTimer(); startTimer();
volume.raycast(pose, frameSize.height, frameSize.width, points, normals); volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals);
stopTimer(); stopTimer();
if (display) if (display)
@ -851,11 +863,12 @@ PERF_TEST(Perf_ColorTSDF, integrate_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -884,11 +897,12 @@ PERF_TEST(Perf_ColorTSDF, integrate_frame)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -918,12 +932,13 @@ PERF_TEST(Perf_ColorTSDF, raycast_mat)
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
for (size_t i = 0; i < poses.size(); i++) for (size_t i = 0; i < poses.size(); i++)
@ -937,7 +952,7 @@ PERF_TEST(Perf_ColorTSDF, raycast_mat)
volume.integrate(depth, rgb, pose); volume.integrate(depth, rgb, pose);
startTimer(); startTimer();
volume.raycast(pose, frameSize.height, frameSize.width, points, normals, colors); volume.raycast(pose, frameSize.height, frameSize.width, intrRaycast, points, normals, colors);
stopTimer(); stopTimer();
if (display) if (display)

98
modules/3d/src/rgbd/color_tsdf_functions.cpp
View File

@ -13,7 +13,7 @@ namespace cv {
void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose,
InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume) InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume)
{ {
Matx44f volumePose; Matx44f volumePose;
settings.getVolumePose(volumePose); settings.getVolumePose(volumePose);
@ -21,11 +21,10 @@ void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f&
} }
void integrateColorTsdfVolumeUnit( void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose,
const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose, InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume)
InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume)
{ {
//std::cout << "integrateColorTsdfVolumeUnit()" << std::endl; CV_TRACE_FUNCTION();
Depth depth = _depth.getMat(); Depth depth = _depth.getMat();
Colors color = _rgb.getMat(); Colors color = _rgb.getMat();
@ -35,7 +34,7 @@ void integrateColorTsdfVolumeUnit(
RGBTsdfVoxel* volDataStart = volume.ptr<RGBTsdfVoxel>(); RGBTsdfVoxel* volDataStart = volume.ptr<RGBTsdfVoxel>();
Vec4i volStrides; Vec4i volStrides;
settings.getVolumeDimensions(volStrides); settings.getVolumeStrides(volStrides);
Vec3i resolution; Vec3i resolution;
settings.getVolumeResolution(resolution); settings.getVolumeResolution(resolution);
@ -49,10 +48,6 @@ void integrateColorTsdfVolumeUnit(
settings.getCameraIntegrateIntrinsics(intr); settings.getCameraIntegrateIntrinsics(intr);
const Intr::Projector projDepth = Intr(intr).makeProjector(); const Intr::Projector projDepth = Intr(intr).makeProjector();
Matx33f rgb_intr;
settings.getCameraIntegrateIntrinsics(rgb_intr);
const Intr::Projector projColor = Intr(rgb_intr);
const float dfac(1.f / settings.getDepthFactor()); const float dfac(1.f / settings.getDepthFactor());
const float truncDist = settings.getTsdfTruncateDistance(); const float truncDist = settings.getTsdfTruncateDistance();
const float truncDistInv = 1.f / truncDist; const float truncDistInv = 1.f / truncDist;
@ -70,7 +65,6 @@ void integrateColorTsdfVolumeUnit(
v_float32x4 zStep(zStepPt.x, zStepPt.y, zStepPt.z, 0); v_float32x4 zStep(zStepPt.x, zStepPt.y, zStepPt.z, 0);
v_float32x4 vfxy(projDepth.fx, projDepth.fy, 0.f, 0.f), vcxy(projDepth.cx, projDepth.cy, 0.f, 0.f); v_float32x4 vfxy(projDepth.fx, projDepth.fy, 0.f, 0.f), vcxy(projDepth.cx, projDepth.cy, 0.f, 0.f);
v_float32x4 rgb_vfxy(projColor.fx, projColor.fy, 0.f, 0.f), rgb_vcxy(projColor.cx, projColor.cy, 0.f, 0.f);
const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols - 1, depth.rows - 1, 0, 0)); const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols - 1, depth.rows - 1, 0, 0));
for (int x = range.start; x < range.end; x++) for (int x = range.start; x < range.end; x++)
@ -128,7 +122,7 @@ void integrateColorTsdfVolumeUnit(
v_float32x4 projected = v_muladd(camPixVec, vfxy, vcxy); v_float32x4 projected = v_muladd(camPixVec, vfxy, vcxy);
// leave only first 2 lanes // leave only first 2 lanes
projected = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & projected = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) &
v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0));
depthType v; depthType v;
// bilinearly interpolate depth at projected // bilinearly interpolate depth at projected
@ -136,7 +130,7 @@ void integrateColorTsdfVolumeUnit(
const v_float32x4& pt = projected; const v_float32x4& pt = projected;
// check coords >= 0 and < imgSize // check coords >= 0 and < imgSize
v_uint32x4 limits = v_reinterpret_as_u32(pt < v_setzero_f32()) | v_uint32x4 limits = v_reinterpret_as_u32(pt < v_setzero_f32()) |
v_reinterpret_as_u32(pt >= upLimits); v_reinterpret_as_u32(pt >= upLimits);
limits = limits | v_rotate_right<1>(limits); limits = limits | v_rotate_right<1>(limits);
if (limits.get0()) if (limits.get0())
continue; continue;
@ -177,23 +171,15 @@ void integrateColorTsdfVolumeUnit(
continue; continue;
} }
v_float32x4 projectedRGB = v_muladd(camPixVec, rgb_vfxy, rgb_vcxy);
// leave only first 2 lanes
projectedRGB = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) &
v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0));
// norm(camPixVec) produces double which is too slow // norm(camPixVec) produces double which is too slow
int _u = (int)projected.get0(); int _u = (int)projected.get0();
int _v = (int)v_rotate_right<1>(projected).get0(); int _v = (int)v_rotate_right<1>(projected).get0();
int rgb_u = (int)projectedRGB.get0();
int rgb_v = (int)v_rotate_right<1>(projectedRGB).get0();
if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows && if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows))
rgb_v >= 0 && rgb_v < color.rows && rgb_u >= 0 && rgb_u < color.cols))
continue; continue;
float pixNorm = pixNorms.at<float>(_v, _u); float pixNorm = pixNorms.at<float>(_v, _u);
// TODO: Add support of 3point and 4 point representation // TODO: Add support of 3point and 4 point representation
Vec3f colorRGB = color.at<Vec3f>(rgb_v, rgb_u); Vec3f colorRGB = color.at<Vec3f>(_v, _u);
//float pixNorm = sqrt(v_reduce_sum(camPixVec*camPixVec)); //float pixNorm = sqrt(v_reduce_sum(camPixVec*camPixVec));
// difference between distances of point and of surface to camera // difference between distances of point and of surface to camera
float sdf = pixNorm * (v * dfac - zCamSpace); float sdf = pixNorm * (v * dfac - zCamSpace);
@ -283,7 +269,6 @@ void integrateColorTsdfVolumeUnit(
Point3f camPixVec; Point3f camPixVec;
Point2f projected = projDepth(camSpacePt, camPixVec); Point2f projected = projDepth(camSpacePt, camPixVec);
Point2f projectedRGB = projColor(camSpacePt, camPixVec);
depthType v = bilinearDepth(depth, projected); depthType v = bilinearDepth(depth, projected);
if (v == 0) { if (v == 0) {
@ -292,17 +277,12 @@ void integrateColorTsdfVolumeUnit(
int _u = projected.x; int _u = projected.x;
int _v = projected.y; int _v = projected.y;
int rgb_u = (int)projectedRGB.x; if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows))
int rgb_v = (int)projectedRGB.y;
if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows
&& rgb_v >= 0 && rgb_v < color.rows && rgb_u >= 0 && rgb_u < color.cols
))
continue; continue;
float pixNorm = pixNorms.at<float>(_v, _u); float pixNorm = pixNorms.at<float>(_v, _u);
// TODO: Add support of 3point and 4 point representation // TODO: Add support of 3point and 4 point representation
Vec3f colorRGB = color.at<Vec3f>(rgb_v, rgb_u); Vec3f colorRGB = color.at<Vec3f>(_v, _u);
// difference between distances of point and of surface to camera // difference between distances of point and of surface to camera
float sdf = pixNorm * (v * dfac - camSpacePt.z); float sdf = pixNorm * (v * dfac - camSpacePt.z);
@ -337,10 +317,6 @@ void integrateColorTsdfVolumeUnit(
}; };
#endif #endif
parallel_for_(integrateRange, IntegrateInvoker); parallel_for_(integrateRange, IntegrateInvoker);
//IntegrateInvoker(integrateRange);
//std::cout << "integrateColorTsdfVolumeUnit() end" << std::endl;
} }
@ -349,8 +325,8 @@ void integrateColorTsdfVolumeUnit(
// all coordinate checks should be done in inclosing cycle // all coordinate checks should be done in inclosing cycle
inline float interpolateColorVoxel(const Mat& volume, inline float interpolateColorVoxel(const Mat& volume,
const Vec4i& volDims, const Vec8i& neighbourCoords, const Vec4i& volDims, const Vec8i& neighbourCoords,
const v_float32x4& p) const v_float32x4& p)
{ {
// tx, ty, tz = floor(p) // tx, ty, tz = floor(p)
v_int32x4 ip = v_floor(p); v_int32x4 ip = v_floor(p);
@ -392,8 +368,8 @@ inline float interpolateColorVoxel(const Mat& volume,
} }
inline float interpolateColorVoxel(const Mat& volume, inline float interpolateColorVoxel(const Mat& volume,
const Vec4i& volDims, const Vec8i& neighbourCoords, const Vec4i& volDims, const Vec8i& neighbourCoords,
const Point3f& _p) const Point3f& _p)
{ {
v_float32x4 p(_p.x, _p.y, _p.z, 0); v_float32x4 p(_p.x, _p.y, _p.z, 0);
return interpolateColorVoxel(volume, volDims, neighbourCoords, p); return interpolateColorVoxel(volume, volDims, neighbourCoords, p);
@ -402,8 +378,8 @@ inline float interpolateColorVoxel(const Mat& volume,
#else #else
inline float interpolateColorVoxel(const Mat& volume, inline float interpolateColorVoxel(const Mat& volume,
const Vec4i& volDims, const Vec8i& neighbourCoords, const Vec4i& volDims, const Vec8i& neighbourCoords,
const Point3f& p) const Point3f& p)
{ {
int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
@ -440,8 +416,8 @@ inline float interpolateColorVoxel(const Mat& volume,
//gradientDeltaFactor is fixed at 1.0 of voxel size //gradientDeltaFactor is fixed at 1.0 of voxel size
inline v_float32x4 getNormalColorVoxel(const Mat& volume, inline v_float32x4 getNormalColorVoxel(const Mat& volume,
const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution,
const v_float32x4& p) const v_float32x4& p)
{ {
if (v_check_any(p < v_float32x4(1.f, 1.f, 1.f, 0.f)) || if (v_check_any(p < v_float32x4(1.f, 1.f, 1.f, 0.f)) ||
v_check_any(p >= v_float32x4((float)(volResolution.x - 2), v_check_any(p >= v_float32x4((float)(volResolution.x - 2),
@ -501,8 +477,8 @@ inline v_float32x4 getNormalColorVoxel(const Mat& volume,
} }
inline Point3f getNormalColorVoxel(const Mat& volume, inline Point3f getNormalColorVoxel(const Mat& volume,
const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution,
const Point3f& _p) const Point3f& _p)
{ {
v_float32x4 p(_p.x, _p.y, _p.z, 0.f); v_float32x4 p(_p.x, _p.y, _p.z, 0.f);
v_float32x4 result = getNormalColorVoxel(volume, volDims, neighbourCoords, volResolution, p); v_float32x4 result = getNormalColorVoxel(volume, volDims, neighbourCoords, volResolution, p);
@ -512,8 +488,8 @@ inline Point3f getNormalColorVoxel(const Mat& volume,
} }
#else #else
inline Point3f getNormalColorVoxel(const Mat& volume, inline Point3f getNormalColorVoxel(const Mat& volume,
const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution,
const Point3f& p) const Point3f& p)
{ {
int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
const RGBTsdfVoxel* volData = volume.ptr<RGBTsdfVoxel>(); const RGBTsdfVoxel* volData = volume.ptr<RGBTsdfVoxel>();
@ -712,16 +688,17 @@ inline Point3f getColorVoxel(const Mat& volume,
#endif #endif
void raycastColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &cameraPose,
int height, int width, InputArray intr,
void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors)
InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors)
{ {
//std::cout << "raycastColorTsdfVolumeUnit()" << std::endl; CV_TRACE_FUNCTION();
Size frameSize(width, height); Size frameSize(width, height);
CV_Assert(frameSize.area() > 0); CV_Assert(frameSize.area() > 0);
Matx33f mintr(intr.getMat());
_points.create(frameSize, POINT_TYPE); _points.create(frameSize, POINT_TYPE);
_normals.create(frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE);
_colors.create(frameSize, COLOR_TYPE); _colors.create(frameSize, COLOR_TYPE);
@ -731,7 +708,7 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& c
Colors colors = _colors.getMat(); Colors colors = _colors.getMat();
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const Vec8i neighbourCoords = Vec8i( const Vec8i neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 0, 1)),
@ -748,9 +725,7 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& c
const Point3i volResolution = Point3i(resolution); const Point3i volResolution = Point3i(resolution);
const Point3f volSize = Point3f(volResolution) * settings.getVoxelSize(); const Point3f volSize = Point3f(volResolution) * settings.getVoxelSize();
Matx33f intr; const Intr::Reprojector reprojDepth = Intr(mintr).makeReprojector();
settings.getCameraRaycastIntrinsics(intr);
const Intr::Reprojector reprojDepth = Intr(intr).makeReprojector();
Matx44f _pose; Matx44f _pose;
settings.getVolumePose(_pose); settings.getVolumePose(_pose);
@ -1027,17 +1002,12 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& c
#endif #endif
parallel_for_(raycastRange, RaycastInvoker); parallel_for_(raycastRange, RaycastInvoker);
//RaycastInvoker(raycastRange);
//std::cout << "raycastColorTsdfVolumeUnit() end" << std::endl;
} }
void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
InputArray _points, OutputArray _normals) InputArray _points, OutputArray _normals)
{ {
//std::cout << "fetchNormalsFromColorTsdfVolumeUnit" << std::endl;
CV_TRACE_FUNCTION(); CV_TRACE_FUNCTION();
CV_Assert(!_points.empty()); CV_Assert(!_points.empty());
if (!_normals.needed()) if (!_normals.needed())
@ -1058,7 +1028,7 @@ void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputAr
float voxelSizeInv = 1.f / settings.getVoxelSize(); float voxelSizeInv = 1.f / settings.getVoxelSize();
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const Vec8i neighbourCoords = Vec8i( const Vec8i neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 0, 1)),
@ -1171,7 +1141,7 @@ void fetchPointsNormalsColorsFromColorTsdfVolumeUnit(const VolumeSettings& setti
float voxelSizeInv = 1.f / settings.getVoxelSize(); float voxelSizeInv = 1.f / settings.getVoxelSize();
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const Vec8i neighbourCoords = Vec8i( const Vec8i neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 0, 1)),

33
modules/3d/src/rgbd/color_tsdf_functions.hpp
View File

@ -17,26 +17,19 @@
namespace cv namespace cv
{ {
void integrateColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &cameraPose,
void integrateColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume);
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 integrateColorTsdfVolumeUnit( void raycastColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &cameraPose,
const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose, int height, int width, InputArray intr,
InputArray _depth, InputArray _rgb, InputArray _pixNorms, InputArray _volume); InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors);
void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings &settings, InputArray _volume,
void raycastColorTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray _points, OutputArray _normals);
InputArray _volume, OutputArray _points, OutputArray _normals, OutputArray _colors); void fetchPointsNormalsFromColorTsdfVolumeUnit(const VolumeSettings &settings, InputArray _volume,
OutputArray _points, OutputArray _normals);
void fetchNormalsFromColorTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, void fetchPointsNormalsColorsFromColorTsdfVolumeUnit(const VolumeSettings &settings, InputArray _volume,
InputArray _points, OutputArray _normals); OutputArray _points, OutputArray _normals, OutputArray _colors);
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 } // namespace cv

36
modules/3d/src/rgbd/hash_tsdf_functions.cpp
View File

@ -497,7 +497,7 @@ void ocl_integrateHashTsdfVolumeUnit(
const Intr intrinsics(intr); const Intr intrinsics(intr);
Vec4i volStrides; Vec4i volStrides;
settings.getVolumeDimensions(volStrides); settings.getVolumeStrides(volStrides);
Vec3i resolution; Vec3i resolution;
settings.getVolumeResolution(resolution); settings.getVolumeResolution(resolution);
@ -988,15 +988,15 @@ Point3f ocl_getNormalVoxel(
#endif #endif
void raycastHashTsdfVolumeUnit( void raycastHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree, const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree,
InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals) InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals)
{ {
//std::cout << "raycastHashTsdfVolumeUnit()" << std::endl;
CV_TRACE_FUNCTION(); CV_TRACE_FUNCTION();
Size frameSize(width, height); Size frameSize(width, height);
CV_Assert(frameSize.area() > 0); CV_Assert(frameSize.area() > 0);
Matx33f mintr(intr.getMat());
Mat volUnitsData = _volUnitsData.getMat(); Mat volUnitsData = _volUnitsData.getMat();
_points.create(frameSize, POINT_TYPE); _points.create(frameSize, POINT_TYPE);
@ -1016,7 +1016,7 @@ void raycastHashTsdfVolumeUnit(
const float voxelSizeInv = 1.f / voxelSize; const float voxelSizeInv = 1.f / voxelSize;
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
Vec3i resolution; Vec3i resolution;
settings.getVolumeResolution(resolution); settings.getVolumeResolution(resolution);
const Point3i volResolution = Point3i(resolution); const Point3i volResolution = Point3i(resolution);
@ -1028,9 +1028,7 @@ void raycastHashTsdfVolumeUnit(
const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose));
const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
Matx33f intr; const Intr intrinsics(mintr);
settings.getCameraRaycastIntrinsics(intr);
const Intr intrinsics(intr);
const Intr::Reprojector reproj(intrinsics.makeReprojector()); const Intr::Reprojector reproj(intrinsics.makeReprojector());
const int nstripes = -1; const int nstripes = -1;
@ -1094,7 +1092,6 @@ void raycastHashTsdfVolumeUnit(
stepSize = tstep; stepSize = tstep;
} }
//std::cout << prevTsdf << " " << currTsdf << " " << currWeight << std::endl;
//! Surface crossing //! Surface crossing
if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0)
{ {
@ -1124,20 +1121,20 @@ void raycastHashTsdfVolumeUnit(
}; };
parallel_for_(Range(0, points.rows), _HashRaycastInvoker, nstripes); parallel_for_(Range(0, points.rows), _HashRaycastInvoker, nstripes);
//std::cout << "raycastHashTsdfVolumeUnit() end" << std::endl;
} }
#ifdef HAVE_OPENCL #ifdef HAVE_OPENCL
void ocl_raycastHashTsdfVolumeUnit( void ocl_raycastHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree, const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree,
const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals) const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals)
{ {
CV_TRACE_FUNCTION(); CV_TRACE_FUNCTION();
Size frameSize(width, height); Size frameSize(width, height);
CV_Assert(frameSize.area() > 0); CV_Assert(frameSize.area() > 0);
Matx33f mintr(intr.getMat());
UMat volUnitsData = _volUnitsData.getUMat(); UMat volUnitsData = _volUnitsData.getUMat();
String errorStr; String errorStr;
@ -1156,9 +1153,7 @@ void ocl_raycastHashTsdfVolumeUnit(
UMat points = _points.getUMat(); UMat points = _points.getUMat();
UMat normals = _normals.getUMat(); UMat normals = _normals.getUMat();
Matx33f intr; Intr intrinsics(mintr);
settings.getCameraRaycastIntrinsics(intr);
Intr intrinsics(intr);
Intr::Reprojector r = intrinsics.makeReprojector(); Intr::Reprojector r = intrinsics.makeReprojector();
Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy);
@ -1170,7 +1165,7 @@ void ocl_raycastHashTsdfVolumeUnit(
const float voxelSizeInv = 1.f / voxelSize; const float voxelSizeInv = 1.f / voxelSize;
const Vec4i volStrides; const Vec4i volStrides;
settings.getVolumeDimensions(volStrides); settings.getVolumeStrides(volStrides);
Vec3i resolution; Vec3i resolution;
settings.getVolumeResolution(resolution); settings.getVolumeResolution(resolution);
const Point3i volResolution = Point3i(resolution); const Point3i volResolution = Point3i(resolution);
@ -1223,6 +1218,7 @@ void ocl_raycastHashTsdfVolumeUnit(
} }
#endif #endif
void fetchNormalsFromHashTsdfVolumeUnit( void fetchNormalsFromHashTsdfVolumeUnit(
const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
const int volumeUnitDegree, InputArray _points, OutputArray _normals) const int volumeUnitDegree, InputArray _points, OutputArray _normals)
@ -1243,7 +1239,7 @@ void fetchNormalsFromHashTsdfVolumeUnit(
const float voxelSizeInv = 1.f / voxelSize; const float voxelSizeInv = 1.f / voxelSize;
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
Matx44f _pose; Matx44f _pose;
settings.getVolumePose(_pose); settings.getVolumePose(_pose);
@ -1288,7 +1284,7 @@ void olc_fetchNormalsFromHashTsdfVolumeUnit(
const float voxelSizeInv = 1.f / voxelSize; const float voxelSizeInv = 1.f / voxelSize;
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
Matx44f _pose; Matx44f _pose;
settings.getVolumePose(_pose); settings.getVolumePose(_pose);
@ -1334,7 +1330,7 @@ void fetchPointsNormalsFromHashTsdfVolumeUnit(
const float volumeUnitSize = voxelSize * resolution[0]; const float volumeUnitSize = voxelSize * resolution[0];
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
std::vector<Vec3i> totalVolUnits; std::vector<Vec3i> totalVolUnits;
for (const auto& keyvalue : volumeUnits) for (const auto& keyvalue : volumeUnits)
@ -1461,7 +1457,7 @@ void ocl_fetchPointsNormalsFromHashTsdfVolumeUnit(
const float volumeUnitSize = voxelSize * resolution[0]; const float volumeUnitSize = voxelSize * resolution[0];
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
Range _fetchRange(0, hashTable.last); Range _fetchRange(0, hashTable.last);

4
modules/3d/src/rgbd/hash_tsdf_functions.hpp
View File

@ -290,7 +290,7 @@ void integrateHashTsdfVolumeUnit(
InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits); InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits);
void raycastHashTsdfVolumeUnit( void raycastHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree, const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree,
InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals); InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals);
void fetchNormalsFromHashTsdfVolumeUnit( void fetchNormalsFromHashTsdfVolumeUnit(
@ -307,7 +307,7 @@ void ocl_integrateHashTsdfVolumeUnit(
InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputArray _volUnitsDataCopy, InputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags); InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputArray _volUnitsDataCopy, InputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags);
void ocl_raycastHashTsdfVolumeUnit( void ocl_raycastHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const int volumeUnitDegree, const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree,
const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals); const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals);
void olc_fetchNormalsFromHashTsdfVolumeUnit( void olc_fetchNormalsFromHashTsdfVolumeUnit(

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

@ -13,7 +13,7 @@ namespace cv {
void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm) void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm)
{ {
//std::cout << "preCalculationPixNorm" << std::endl; CV_TRACE_FUNCTION();
Point2f fl(intrinsics.fx, intrinsics.fy); Point2f fl(intrinsics.fx, intrinsics.fy);
Point2f pp(intrinsics.cx, intrinsics.cy); Point2f pp(intrinsics.cx, intrinsics.cy);
@ -67,7 +67,7 @@ void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& volu
TsdfVoxel* volDataStart = volume.ptr<TsdfVoxel>(); TsdfVoxel* volDataStart = volume.ptr<TsdfVoxel>();
Vec4i volStrides; Vec4i volStrides;
settings.getVolumeDimensions(volStrides); settings.getVolumeStrides(volStrides);
Vec3i resolution; Vec3i resolution;
settings.getVolumeResolution(resolution); settings.getVolumeResolution(resolution);
@ -330,12 +330,9 @@ void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& volu
#ifdef HAVE_OPENCL #ifdef HAVE_OPENCL
void ocl_integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, void ocl_integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose,
InputArray _depth, InputArray _pixNorms, InputArray _volume) InputArray _depth, InputArray _pixNorms, InputArray _volume)
{ {
//std::cout << "ocl_integrateTsdfVolumeUnit" << std::endl;
CV_TRACE_FUNCTION(); CV_TRACE_FUNCTION();
//CV_UNUSED(frameId);
CV_Assert(!_depth.empty()); CV_Assert(!_depth.empty());
UMat depth = _depth.getUMat(); UMat depth = _depth.getUMat();
@ -369,7 +366,7 @@ void ocl_integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f&
Intr intrinsics(intr); Intr intrinsics(intr);
Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy);
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const float voxelSize = settings.getVoxelSize(); const float voxelSize = settings.getVoxelSize();
const float truncatedDistance = settings.getTsdfTruncateDistance(); const float truncatedDistance = settings.getTsdfTruncateDistance();
@ -617,13 +614,17 @@ inline Point3f getNormalVoxel( const Mat& volume,
} }
#endif #endif
void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose,
InputArray _volume, OutputArray _points, OutputArray _normals) int height, int width, InputArray intr,
InputArray _volume, OutputArray _points, OutputArray _normals)
{ {
//std::cout << "raycastVolumeUnit" << std::endl; CV_TRACE_FUNCTION();
const Size frameSize(width, height); const Size frameSize(width, height);
//CV_Assert(frameSize.area() > 0); CV_Assert(frameSize.area() > 0);
Matx33f mintr(intr.getMat());
_points.create(frameSize, POINT_TYPE); _points.create(frameSize, POINT_TYPE);
_normals.create(frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE);
@ -631,7 +632,7 @@ void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& camera
Normals normals = _normals.getMat(); Normals normals = _normals.getMat();
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const Vec8i neighbourCoords = Vec8i( const Vec8i neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 0, 1)),
@ -648,9 +649,6 @@ void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& camera
const Point3i volResolution = Point3i(resolution); const Point3i volResolution = Point3i(resolution);
const Point3f volSize = Point3f(volResolution) * settings.getVoxelSize(); const Point3f volSize = Point3f(volResolution) * settings.getVoxelSize();
Matx33f intr;
settings.getCameraRaycastIntrinsics(intr);
Matx44f _pose; Matx44f _pose;
settings.getVolumePose(_pose); settings.getVolumePose(_pose);
const Affine3f pose = Affine3f(_pose); const Affine3f pose = Affine3f(_pose);
@ -663,7 +661,7 @@ void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& camera
const Mat volume = _volume.getMat(); const Mat volume = _volume.getMat();
float voxelSize = settings.getVoxelSize(); float voxelSize = settings.getVoxelSize();
float voxelSizeInv = 1.0f / voxelSize; float voxelSizeInv = 1.0f / voxelSize;
const Intr::Reprojector reproj = Intr(intr).makeReprojector(); const Intr::Reprojector reproj = Intr(mintr).makeReprojector();
float tstep = settings.getTsdfTruncateDistance() * settings.getRaycastStepFactor(); float tstep = settings.getTsdfTruncateDistance() * settings.getRaycastStepFactor();
Range raycastRange = Range(0, points.rows); Range raycastRange = Range(0, points.rows);
@ -917,21 +915,21 @@ void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& camera
#endif #endif
parallel_for_(raycastRange, RaycastInvoker); parallel_for_(raycastRange, RaycastInvoker);
//RaycastInvoker(raycastRange);
} }
#ifdef HAVE_OPENCL #ifdef HAVE_OPENCL
void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose,
InputArray _volume, OutputArray _points, OutputArray _normals) int height, int width, InputArray intr,
InputArray _volume, OutputArray _points, OutputArray _normals)
{ {
//std::cout << "ocl_raycastVolumeUnit" << std::endl;
CV_TRACE_FUNCTION(); CV_TRACE_FUNCTION();
const Size frameSize(width, height); const Size frameSize(width, height);
CV_Assert(frameSize.area() > 0); CV_Assert(frameSize.area() > 0);
Matx33f mintr(intr.getMat());
String errorStr; String errorStr;
String name = "raycast"; String name = "raycast";
ocl::ProgramSource source = ocl::_3d::tsdf_oclsrc; ocl::ProgramSource source = ocl::_3d::tsdf_oclsrc;
@ -949,7 +947,7 @@ void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& ca
UMat normals = _normals.getUMat(); UMat normals = _normals.getUMat();
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const Vec8i neighbourCoords = Vec8i( const Vec8i neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 0, 1)),
@ -975,9 +973,8 @@ void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& ca
Affine3f cam2vol = pose.inv() * Affine3f(cameraPose); Affine3f cam2vol = pose.inv() * Affine3f(cameraPose);
Mat(cam2vol.matrix).copyTo(cam2volGpu); Mat(cam2vol.matrix).copyTo(cam2volGpu);
Mat(vol2cam.matrix).copyTo(vol2camGpu); Mat(vol2cam.matrix).copyTo(vol2camGpu);
Matx33f intr;
settings.getCameraRaycastIntrinsics(intr); Intr intrinsics(mintr);
Intr intrinsics(intr);
Intr::Reprojector r = intrinsics.makeReprojector(); Intr::Reprojector r = intrinsics.makeReprojector();
const UMat volume = _volume.getUMat(); const UMat volume = _volume.getUMat();
@ -1023,8 +1020,6 @@ void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& ca
void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, InputArray _points, OutputArray _normals) void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, InputArray _points, OutputArray _normals)
{ {
//std::cout << "fetchNormalsFromTsdfVolumeUnit" << std::endl;
CV_TRACE_FUNCTION(); CV_TRACE_FUNCTION();
CV_Assert(!_points.empty()); CV_Assert(!_points.empty());
if (!_normals.needed()) if (!_normals.needed())
@ -1046,7 +1041,7 @@ void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _
float voxelSizeInv = 1.f / settings.getVoxelSize(); float voxelSizeInv = 1.f / settings.getVoxelSize();
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const Vec8i neighbourCoords = Vec8i( const Vec8i neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 0, 1)),
@ -1099,7 +1094,7 @@ void ocl_fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArr
float voxelSizeInv = 1.f / settings.getVoxelSize(); float voxelSizeInv = 1.f / settings.getVoxelSize();
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const Vec8i neighbourCoords = Vec8i( const Vec8i neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 0, 1)),
@ -1222,7 +1217,7 @@ void fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputA
float voxelSizeInv = 1.f / settings.getVoxelSize(); float voxelSizeInv = 1.f / settings.getVoxelSize();
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const Vec8i neighbourCoords = Vec8i( const Vec8i neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 0, 1)),
@ -1320,7 +1315,7 @@ void ocl_fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, In
float voxelSizeInv = 1.f / settings.getVoxelSize(); float voxelSizeInv = 1.f / settings.getVoxelSize();
const Vec4i volDims; const Vec4i volDims;
settings.getVolumeDimensions(volDims); settings.getVolumeStrides(volDims);
const Vec8i neighbourCoords = Vec8i( const Vec8i neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 0, 1)),

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

@ -175,7 +175,7 @@ void integrateTsdfVolumeUnit(
InputArray _depth, InputArray _pixNorms, InputArray _volume); InputArray _depth, InputArray _pixNorms, InputArray _volume);
void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr,
InputArray _volume, OutputArray _points, OutputArray _normals); InputArray _volume, OutputArray _points, OutputArray _normals);
void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume,
@ -191,7 +191,7 @@ void ocl_integrateTsdfVolumeUnit(
InputArray _depth, InputArray _pixNorms, InputArray _volume); InputArray _depth, InputArray _pixNorms, InputArray _volume);
void ocl_raycastTsdfVolumeUnit( void ocl_raycastTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr,
InputArray _volume, OutputArray _points, OutputArray _normals); InputArray _volume, OutputArray _points, OutputArray _normals);
void ocl_fetchNormalsFromTsdfVolumeUnit( void ocl_fetchNormalsFromTsdfVolumeUnit(

32
modules/3d/src/rgbd/volume_impl.cpp
View File

@ -99,11 +99,13 @@ void TsdfVolume::integrate(InputArray, InputArray, InputArray)
void TsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const void TsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const
{ {
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors); Matx33f intr;
settings.getCameraRaycastIntrinsics(intr);
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors);
} }
void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const
{ {
if (_colors.needed()) if (_colors.needed())
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors"); CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
@ -113,12 +115,12 @@ void TsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputAr
const Matx44f cameraPose = _cameraPose.getMat(); const Matx44f cameraPose = _cameraPose.getMat();
#ifndef HAVE_OPENCL #ifndef HAVE_OPENCL
raycastTsdfVolumeUnit(settings, cameraPose, height, width, volume, _points, _normals); raycastTsdfVolumeUnit(settings, cameraPose, height, width, intr, volume, _points, _normals);
#else #else
if (useGPU) if (useGPU)
ocl_raycastTsdfVolumeUnit(settings, cameraPose, height, width, gpu_volume, _points, _normals); ocl_raycastTsdfVolumeUnit(settings, cameraPose, height, width, intr, gpu_volume, _points, _normals);
else else
raycastTsdfVolumeUnit(settings, cameraPose, height, width, cpu_volume, _points, _normals); raycastTsdfVolumeUnit(settings, cameraPose, height, width, intr, cpu_volume, _points, _normals);
#endif #endif
} }
@ -293,11 +295,13 @@ void HashTsdfVolume::integrate(InputArray, InputArray, InputArray)
void HashTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const void HashTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const
{ {
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors); Matx33f intr;
settings.getCameraRaycastIntrinsics(intr);
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors);
} }
void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const
{ {
if (_colors.needed()) if (_colors.needed())
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors"); CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
@ -305,12 +309,12 @@ void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, Outp
const Matx44f cameraPose = _cameraPose.getMat(); const Matx44f cameraPose = _cameraPose.getMat();
#ifndef HAVE_OPENCL #ifndef HAVE_OPENCL
raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals); raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals);
#else #else
if (useGPU) if (useGPU)
ocl_raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, hashTable, gpu_volUnitsData, _points, _normals); ocl_raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, hashTable, gpu_volUnitsData, _points, _normals);
else else
raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, volumeUnitDegree, cpu_volUnitsData, cpu_volumeUnits, _points, _normals); raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, cpu_volUnitsData, cpu_volumeUnits, _points, _normals);
#endif #endif
} }
@ -525,13 +529,15 @@ void ColorTsdfVolume::integrate(InputArray _depth, InputArray _image, InputArray
void ColorTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const void ColorTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const
{ {
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), points, normals, colors); Matx33f intr;
settings.getCameraRaycastIntrinsics(intr);
raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors);
} }
void ColorTsdfVolume::raycast(InputArray _cameraPose, int height, int width, OutputArray _points, OutputArray _normals, OutputArray _colors) const void ColorTsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const
{ {
const Matx44f cameraPose = _cameraPose.getMat(); const Matx44f cameraPose = _cameraPose.getMat();
raycastColorTsdfVolumeUnit(settings, cameraPose, height, width, volume, _points, _normals, _colors); raycastColorTsdfVolumeUnit(settings, cameraPose, height, width, intr, volume, _points, _normals, _colors);
} }
void ColorTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const void ColorTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const

14
modules/3d/src/rgbd/volume_impl.hpp
View File

@ -27,7 +27,7 @@ public:
virtual void integrate(InputArray depth, InputArray image, InputArray pose) = 0; virtual void integrate(InputArray depth, InputArray image, InputArray pose) = 0;
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const = 0; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const = 0;
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const = 0; virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const = 0;
virtual void fetchNormals(InputArray points, OutputArray normals) const = 0; virtual void fetchNormals(InputArray points, OutputArray normals) const = 0;
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0;
@ -42,7 +42,7 @@ public:
virtual bool getEnableGrowth() const = 0; virtual bool getEnableGrowth() const = 0;
public: public:
const VolumeSettings& settings; VolumeSettings settings;
#ifdef HAVE_OPENCL #ifdef HAVE_OPENCL
const bool useGPU; const bool useGPU;
#endif #endif
@ -59,7 +59,7 @@ public:
virtual void integrate(InputArray depth, InputArray pose) override; virtual void integrate(InputArray depth, InputArray pose) override;
virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void fetchNormals(InputArray points, OutputArray normals) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override;
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
@ -113,7 +113,7 @@ public:
virtual void integrate(InputArray depth, InputArray pose) override; virtual void integrate(InputArray depth, InputArray pose) override;
virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void fetchNormals(InputArray points, OutputArray normals) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override;
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
@ -177,7 +177,7 @@ public:
virtual void integrate(InputArray depth, InputArray pose) override; virtual void integrate(InputArray depth, InputArray pose) override;
virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; virtual void integrate(InputArray depth, InputArray image, InputArray pose) override;
virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void raycast(InputArray cameraPose, int height, int width, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override;
virtual void fetchNormals(InputArray points, OutputArray normals) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override;
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
@ -216,7 +216,7 @@ Volume::Volume()
VolumeSettings settings; VolumeSettings settings;
this->impl = makePtr<TsdfVolume>(settings); this->impl = makePtr<TsdfVolume>(settings);
} }
Volume::Volume(VolumeType vtype, const VolumeSettings& settings) Volume::Volume(VolumeType vtype, VolumeSettings settings)
{ {
switch (vtype) switch (vtype)
{ {
@ -240,7 +240,7 @@ void Volume::integrate(const OdometryFrame& frame, InputArray pose) { this->impl
void Volume::integrate(InputArray depth, InputArray pose) { this->impl->integrate(depth, 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::integrate(InputArray depth, InputArray image, InputArray pose) { this->impl->integrate(depth, image, pose); }
void Volume::raycast(InputArray cameraPose, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, _points, _normals, _colors); } 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, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, height, width, _points, _normals, _colors); } void Volume::raycast(InputArray cameraPose, int height, int width, InputArray _intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const { this->impl->raycast(cameraPose, height, width, _intr, _points, _normals, _colors); }
void Volume::fetchNormals(InputArray points, OutputArray normals) const { this->impl->fetchNormals(points, normals); } 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::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::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const { this->impl->fetchPointsNormalsColors(points, normals, colors); };

65
modules/3d/src/rgbd/volume_settings_impl.cpp
View File

@ -8,7 +8,27 @@
namespace cv namespace cv
{ {
Vec4i calcVolumeDimensions(Point3i volumeResolution, bool ZFirstMemOrder); static Vec4i calcVolumeStrides(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);
}
class VolumeSettings::Impl class VolumeSettings::Impl
{ {
@ -41,7 +61,7 @@ public:
virtual void getVolumePose(OutputArray val) const = 0; virtual void getVolumePose(OutputArray val) const = 0;
virtual void setVolumeResolution(InputArray val) = 0; virtual void setVolumeResolution(InputArray val) = 0;
virtual void getVolumeResolution(OutputArray val) const = 0; virtual void getVolumeResolution(OutputArray val) const = 0;
virtual void getVolumeDimensions(OutputArray val) const = 0; virtual void getVolumeStrides(OutputArray val) const = 0;
virtual void setCameraIntegrateIntrinsics(InputArray val) = 0; virtual void setCameraIntegrateIntrinsics(InputArray val) = 0;
virtual void getCameraIntegrateIntrinsics(OutputArray val) const = 0; virtual void getCameraIntegrateIntrinsics(OutputArray val) const = 0;
virtual void setCameraRaycastIntrinsics(InputArray val) = 0; virtual void setCameraRaycastIntrinsics(InputArray val) = 0;
@ -80,7 +100,7 @@ public:
virtual void getVolumePose(OutputArray val) const override; virtual void getVolumePose(OutputArray val) const override;
virtual void setVolumeResolution(InputArray val) override; virtual void setVolumeResolution(InputArray val) override;
virtual void getVolumeResolution(OutputArray val) const override; virtual void getVolumeResolution(OutputArray val) const override;
virtual void getVolumeDimensions(OutputArray val) const override; virtual void getVolumeStrides(OutputArray val) const override;
virtual void setCameraIntegrateIntrinsics(InputArray val) override; virtual void setCameraIntegrateIntrinsics(InputArray val) override;
virtual void getCameraIntegrateIntrinsics(OutputArray val) const override; virtual void getCameraIntegrateIntrinsics(OutputArray val) const override;
virtual void setCameraRaycastIntrinsics(InputArray val) override; virtual void setCameraRaycastIntrinsics(InputArray val) override;
@ -103,7 +123,7 @@ private:
Matx44f volumePose; Matx44f volumePose;
Point3i volumeResolution; Point3i volumeResolution;
Vec4i volumeDimensions; Vec4i volumeStrides;
Matx33f cameraIntegrateIntrinsics; Matx33f cameraIntegrateIntrinsics;
Matx33f cameraRaycastIntrinsics; Matx33f cameraRaycastIntrinsics;
@ -265,7 +285,7 @@ void VolumeSettings::setVolumePose(InputArray val) { this->impl->setVolumePose(v
void VolumeSettings::getVolumePose(OutputArray val) const { this->impl->getVolumePose(val); }; void VolumeSettings::getVolumePose(OutputArray val) const { this->impl->getVolumePose(val); };
void VolumeSettings::setVolumeResolution(InputArray val) { this->impl->setVolumeResolution(val); }; void VolumeSettings::setVolumeResolution(InputArray val) { this->impl->setVolumeResolution(val); };
void VolumeSettings::getVolumeResolution(OutputArray val) const { this->impl->getVolumeResolution(val); }; void VolumeSettings::getVolumeResolution(OutputArray val) const { this->impl->getVolumeResolution(val); };
void VolumeSettings::getVolumeDimensions(OutputArray val) const { this->impl->getVolumeDimensions(val); }; void VolumeSettings::getVolumeStrides(OutputArray val) const { this->impl->getVolumeStrides(val); };
void VolumeSettings::setCameraIntegrateIntrinsics(InputArray val) { this->impl->setCameraIntegrateIntrinsics(val); }; void VolumeSettings::setCameraIntegrateIntrinsics(InputArray val) { this->impl->setCameraIntegrateIntrinsics(val); };
void VolumeSettings::getCameraIntegrateIntrinsics(OutputArray val) const { this->impl->getCameraIntegrateIntrinsics(val); }; void VolumeSettings::getCameraIntegrateIntrinsics(OutputArray val) const { this->impl->getCameraIntegrateIntrinsics(val); };
void VolumeSettings::setCameraRaycastIntrinsics(InputArray val) { this->impl->setCameraRaycastIntrinsics(val); }; void VolumeSettings::setCameraRaycastIntrinsics(InputArray val) { this->impl->setCameraRaycastIntrinsics(val); };
@ -298,7 +318,7 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType)
this->volumePose = ds.volumePoseMatrix; this->volumePose = ds.volumePoseMatrix;
this->volumeResolution = ds.volumeResolution; this->volumeResolution = ds.volumeResolution;
this->volumeDimensions = calcVolumeDimensions(ds.volumeResolution, ds.zFirstMemOrder); this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
} }
@ -320,7 +340,7 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType)
this->volumePose = ds.volumePoseMatrix; this->volumePose = ds.volumePoseMatrix;
this->volumeResolution = ds.volumeResolution; this->volumeResolution = ds.volumeResolution;
this->volumeDimensions = calcVolumeDimensions(ds.volumeResolution, ds.zFirstMemOrder); this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
} }
@ -342,7 +362,7 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType)
this->volumePose = ds.volumePoseMatrix; this->volumePose = ds.volumePoseMatrix;
this->volumeResolution = ds.volumeResolution; this->volumeResolution = ds.volumeResolution;
this->volumeDimensions = calcVolumeDimensions(ds.volumeResolution, ds.zFirstMemOrder); this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder);
this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics;
this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics;
} }
@ -470,7 +490,7 @@ void VolumeSettingsImpl::setVolumeResolution(InputArray val)
if (!val.empty()) if (!val.empty())
{ {
this->volumeResolution = Point3i(val.getMat()); this->volumeResolution = Point3i(val.getMat());
this->volumeDimensions = calcVolumeDimensions(this->volumeResolution, this->zFirstMemOrder); this->volumeStrides = calcVolumeStrides(this->volumeResolution, this->zFirstMemOrder);
} }
} }
@ -479,9 +499,9 @@ void VolumeSettingsImpl::getVolumeResolution(OutputArray val) const
Mat(this->volumeResolution).copyTo(val); Mat(this->volumeResolution).copyTo(val);
} }
void VolumeSettingsImpl::getVolumeDimensions(OutputArray val) const void VolumeSettingsImpl::getVolumeStrides(OutputArray val) const
{ {
Mat(this->volumeDimensions).copyTo(val); Mat(this->volumeStrides).copyTo(val);
} }
void VolumeSettingsImpl::setCameraIntegrateIntrinsics(InputArray val) void VolumeSettingsImpl::setCameraIntegrateIntrinsics(InputArray val)
@ -511,27 +531,4 @@ void VolumeSettingsImpl::getCameraRaycastIntrinsics(OutputArray val) const
Mat(this->cameraRaycastIntrinsics).copyTo(val); 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);
}
} }

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

@ -343,12 +343,13 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = true; bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);
@ -367,14 +368,14 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test
if (testFunction == VolumeTestFunction::RAYCAST) if (testFunction == VolumeTestFunction::RAYCAST)
{ {
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals); volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals);
} }
else if (testFunction == VolumeTestFunction::FETCH_NORMALS) else if (testFunction == VolumeTestFunction::FETCH_NORMALS)
{ {
if (testSrcType == VolumeTestSrcType::MAT) if (testSrcType == VolumeTestSrcType::MAT)
{ {
// takes only point from raycast for checking fetched normals on the display // takes only point from raycast for checking fetched normals on the display
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, utmpnormals); volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, utmpnormals);
// volume.fetchPointsNormals(upoints, utmpnormals); // volume.fetchPointsNormals(upoints, utmpnormals);
volume.fetchNormals(upoints, unormals); volume.fetchNormals(upoints, unormals);
} }
@ -402,12 +403,13 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = true; bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);
@ -461,12 +463,13 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = true; bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);
@ -485,7 +488,7 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType
else else
volume.integrate(odf, poses[0].matrix); volume.integrate(odf, poses[0].matrix);
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, upoints, unormals); volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals);
normals = unormals.getMat(af); normals = unormals.getMat(af);
points = upoints.getMat(af); points = upoints.getMat(af);
@ -495,7 +498,7 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType
if (cvtest::debugLevel > 0) if (cvtest::debugLevel > 0)
displayImage(depth, points, normals, depthFactor, lightPose); displayImage(depth, points, normals, depthFactor, lightPose);
volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, upoints1, unormals1); volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, upoints1, unormals1);
normals = unormals1.getMat(af); normals = unormals1.getMat(af);
points = upoints1.getMat(af); points = upoints1.getMat(af);
@ -519,12 +522,13 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = true; bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);

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

@ -477,26 +477,31 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
bool onlySemisphere = false; vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; //TODO: check both
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);
Mat rgb = scene->rgb(poses[0]); Mat rgb = scene->rgb(poses[0]);
Mat points, normals, tmpnormals, colors; //TODO: check this utmpnormals, it looks redundant
UMat upoints, unormals, utmpnormals, ucolors;
UMat udepth, urgb;
depth.copyTo(udepth);
rgb.copyTo(urgb);
OdometryFrame odf(rgb, depth); OdometryFrame odf(urgb, udepth);
if (testSrcType == VolumeTestSrcType::MAT) if (testSrcType == VolumeTestSrcType::MAT)
{ {
if (volumeType == VolumeType::ColorTSDF) if (volumeType == VolumeType::ColorTSDF)
volume.integrate(depth, rgb, poses[0].matrix); volume.integrate(udepth, urgb, poses[0].matrix);
else else
volume.integrate(depth, poses[0].matrix); volume.integrate(udepth, poses[0].matrix);
} }
else else
{ {
@ -506,26 +511,31 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test
if (testFunction == VolumeTestFunction::RAYCAST) if (testFunction == VolumeTestFunction::RAYCAST)
{ {
if (volumeType == VolumeType::ColorTSDF) if (volumeType == VolumeType::ColorTSDF)
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals, colors); volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals, ucolors);
else else
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals); volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals);
} }
else if (testFunction == VolumeTestFunction::FETCH_NORMALS) else if (testFunction == VolumeTestFunction::FETCH_NORMALS)
{ {
if (volumeType == VolumeType::ColorTSDF) if (volumeType == VolumeType::ColorTSDF)
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, tmpnormals, colors); volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, utmpnormals, ucolors);
else else
// hash_tsdf cpu don't works with raycast normals // hash_tsdf cpu doesn't work with raycast normals
//volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, tmpnormals); //volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, utmpnormals);
volume.fetchPointsNormals(points, tmpnormals); volume.fetchPointsNormals(upoints, utmpnormals);
volume.fetchNormals(points, normals); volume.fetchNormals(upoints, unormals);
} }
else if (testFunction == VolumeTestFunction::FETCH_POINTS_NORMALS) else if (testFunction == VolumeTestFunction::FETCH_POINTS_NORMALS)
{ {
volume.fetchPointsNormals(points, normals); volume.fetchPointsNormals(upoints, unormals);
} }
Mat points, normals, colors;
points = upoints.getMat(ACCESS_READ);
normals = unormals.getMat(ACCESS_READ);
colors = ucolors.getMat(ACCESS_READ);
if (testFunction == VolumeTestFunction::RAYCAST && cvtest::debugLevel > 0) if (testFunction == VolumeTestFunction::RAYCAST && cvtest::debugLevel > 0)
{ {
if (volumeType == VolumeType::ColorTSDF) if (volumeType == VolumeType::ColorTSDF)
@ -544,12 +554,13 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);
@ -611,12 +622,13 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = true; bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);
@ -639,9 +651,9 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType
} }
if (volumeType == VolumeType::ColorTSDF) if (volumeType == VolumeType::ColorTSDF)
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals, colors); volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, points, normals, colors);
else else
volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, points, normals); volume.raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, points, normals);
patchNaNs(points); patchNaNs(points);
anfas = counterOfValid(points); anfas = counterOfValid(points);
@ -658,9 +670,9 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType
normals.release(); normals.release();
if (volumeType == VolumeType::ColorTSDF) if (volumeType == VolumeType::ColorTSDF)
volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, points, normals, colors); volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, points, normals, colors);
else else
volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, points, normals); volume.raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, points, normals);
patchNaNs(points); patchNaNs(points);
profile = counterOfValid(points); profile = counterOfValid(points);
@ -688,12 +700,13 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType
Volume volume(volumeType, vs); Volume volume(volumeType, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = true; bool onlySemisphere = true;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);
@ -803,11 +816,12 @@ void boundingBoxGrowthTest(bool enableGrowth)
Volume volume(VolumeType::HashTSDF, vs); Volume volume(VolumeType::HashTSDF, vs);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);
@ -972,12 +986,13 @@ void regressionVolPoseRot()
Volume volumeRot(VolumeType::HashTSDF, vsRot); Volume volumeRot(VolumeType::HashTSDF, vsRot);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr; Matx33f intrIntegrate, intrRaycast;
vs.getCameraIntegrateIntrinsics(intr); vs.getCameraIntegrateIntrinsics(intrIntegrate);
vs.getCameraRaycastIntrinsics(intrRaycast);
bool onlySemisphere = false; bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor(); float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f); Vec3f lightPose = Vec3f::all(0.f);
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere); Ptr<Scene> scene = Scene::create(frameSize, intrIntegrate, depthFactor, onlySemisphere);
std::vector<Affine3f> poses = scene->getPoses(); std::vector<Affine3f> poses = scene->getPoses();
Mat depth = scene->depth(poses[0]); Mat depth = scene->depth(poses[0]);
@ -1203,250 +1218,330 @@ TEST_P(StaticVolumeBoundingBox, boundingBox)
INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF)); INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF));
#else #endif
TEST(TSDF_CPU, raycast_custom_framesize_normals_mat)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(TSDF_CPU, raycast_custom_framesize_normals_frame) enum PlatformType
{ {
cv::ocl::setUseOpenCL(false); CPU = 0, GPU = 1
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME); };
cv::ocl::setUseOpenCL(true); CV_ENUM(PlatformTypeEnum, PlatformType::CPU, PlatformType::GPU);
}
TEST(TSDF_CPU, raycast_common_framesize_normals_mat)
{
cv::ocl::setUseOpenCL(false);
normal_test_common_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(TSDF_CPU, raycast_common_framesize_normals_frame)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(TSDF_CPU, fetch_points_normals)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(TSDF_CPU, fetch_normals)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(TSDF_CPU, valid_points_custom_framesize_mat)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(TSDF_CPU, valid_points_custom_framesize_frame)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(TSDF_CPU, valid_points_common_framesize_mat)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(TSDF_CPU, valid_points_common_framesize_frame)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, raycast_custom_framesize_normals_mat)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, raycast_custom_framesize_normals_frame)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, raycast_common_framesize_normals_mat)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, raycast_common_framesize_normals_frame)
{
cv::ocl::setUseOpenCL(false);
normal_test_common_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, fetch_points_normals)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, fetch_normals)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, valid_points_custom_framesize_mat)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, valid_points_custom_framesize_frame)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, valid_points_common_framesize_mat)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, valid_points_common_framesize_frame)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, reproduce_volPoseRot)
{
cv::ocl::setUseOpenCL(false);
regressionVolPoseRot();
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, raycast_custom_framesize_normals_mat)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, raycast_custom_framesize_normals_frame)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, raycast_common_framesize_normals_mat)
{
cv::ocl::setUseOpenCL(false);
normal_test_common_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, raycast_common_framesize_normals_frame)
{
cv::ocl::setUseOpenCL(false);
normal_test_common_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, fetch_normals)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, fetch_points_normals)
{
cv::ocl::setUseOpenCL(false);
normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, valid_points_custom_framesize_mat)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, valid_points_custom_framesize_fetch)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, valid_points_common_framesize_mat)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::MAT);
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, valid_points_common_framesize_fetch)
{
cv::ocl::setUseOpenCL(false);
valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
cv::ocl::setUseOpenCL(true);
}
// used to store current OpenCL status (on/off) and revert it after test is done // used to store current OpenCL status (on/off) and revert it after test is done
// works even after exceptions thrown in test body // works even after exceptions thrown in test body
struct OpenCLStatusRevert struct OpenCLStatusRevert
{ {
OpenCLStatusRevert(bool oclStatus) : originalOpenCLStatus(oclStatus) { } #ifdef HAVE_OPENCL
OpenCLStatusRevert()
{
originalOpenCLStatus = cv::ocl::useOpenCL();
}
~OpenCLStatusRevert() ~OpenCLStatusRevert()
{ {
cv::ocl::setUseOpenCL(originalOpenCLStatus); cv::ocl::setUseOpenCL(originalOpenCLStatus);
} }
void off()
{
cv::ocl::setUseOpenCL(false);
}
bool originalOpenCLStatus; bool originalOpenCLStatus;
#else
void off() { }
#endif
}; };
class StaticVolumeBoundingBox : public ::testing::TestWithParam<VolumeType>
{ };
TEST_P(StaticVolumeBoundingBox, boundingBoxCPU) // CV_ENUM does not support enum class types, so let's implement the class explicitly
namespace
{ {
OpenCLStatusRevert revert(cv::ocl::useOpenCL()); struct VolumeTypeEnum
{
static const std::array<VolumeType, 3> vals;
static const std::array<std::string, 3> svals;
cv::ocl::setUseOpenCL(false); VolumeTypeEnum(VolumeType v = VolumeType::TSDF) : val(v) {}
operator VolumeType() const { return val; }
void PrintTo(std::ostream *os) const
{
int v = int(val);
if (v >= 0 && v < 3)
{
*os << svals[v];
}
else
{
*os << "UNKNOWN";
}
}
static ::testing::internal::ParamGenerator<VolumeTypeEnum> all()
{
return ::testing::Values(VolumeTypeEnum(vals[0]), VolumeTypeEnum(vals[1]), VolumeTypeEnum(vals[2]));
}
staticBoundingBoxTest(GetParam()); private:
VolumeType val;
};
const std::array<VolumeType, 3> VolumeTypeEnum::vals{VolumeType::TSDF, VolumeType::HashTSDF, VolumeType::ColorTSDF};
const std::array<std::string, 3> VolumeTypeEnum::svals{std::string("TSDF"), std::string("HashTSDF"), std::string("ColorTSDF")};
static inline void PrintTo(const VolumeTypeEnum &t, std::ostream *os) { t.PrintTo(os); }
} }
INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF)); typedef std::tuple<PlatformTypeEnum, VolumeTypeEnum> PlatformVolumeType;
struct VolumeTestFixture : public ::testing::TestWithParam<PlatformVolumeType>
{
protected:
void SetUp() override
{
auto p = GetParam();
gpu = std::get<0>(p);
volumeType = std::get<1>(p);
if (!gpu)
oclStatus.off();
}
bool gpu;
VolumeType volumeType;
OpenCLStatusRevert oclStatus;
};
TEST_P(VolumeTestFixture, raycast_custom_framesize_normals_mat)
{
normal_test_custom_framesize(volumeType, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
//TODO: uncomment it when ColorTSDF gets GPU version
INSTANTIATE_TEST_CASE_P(VolumeCPU, VolumeTestFixture, /*::testing::Combine(PlatformTypeEnum::all(), VolumeTypeEnum::all())*/
::testing::Values(PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF},
PlatformVolumeType {PlatformType::CPU, VolumeType::HashTSDF},
PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF},
PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF},
PlatformVolumeType {PlatformType::GPU, VolumeType::HashTSDF}));
#ifdef HAVE_OPENCL
TEST(TSDF_CPU, raycast_custom_framesize_normals_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(TSDF_CPU, raycast_common_framesize_normals_mat)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_common_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
TEST(TSDF_CPU, raycast_common_framesize_normals_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(TSDF_CPU, fetch_points_normals)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
}
TEST(TSDF_CPU, fetch_normals)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
}
TEST(TSDF_CPU, valid_points_custom_framesize_mat)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT);
}
TEST(TSDF_CPU, valid_points_custom_framesize_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_custom_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(TSDF_CPU, valid_points_common_framesize_mat)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::MAT);
}
TEST(TSDF_CPU, valid_points_common_framesize_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_common_framesize(VolumeType::TSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF_CPU, raycast_custom_framesize_normals_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF_CPU, raycast_common_framesize_normals_mat)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_CPU, raycast_common_framesize_normals_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_common_framesize(VolumeType::HashTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF_CPU, fetch_points_normals)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_CPU, fetch_normals)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::HashTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_CPU, valid_points_custom_framesize_mat)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_CPU, valid_points_custom_framesize_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_custom_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF_CPU, valid_points_common_framesize_mat)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::MAT);
}
TEST(HashTSDF_CPU, valid_points_common_framesize_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF_CPU, reproduce_volPoseRot)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
regressionVolPoseRot();
}
TEST(ColorTSDF_CPU, raycast_custom_framesize_normals_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(ColorTSDF_CPU, raycast_common_framesize_normals_mat)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_common_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
}
TEST(ColorTSDF_CPU, raycast_common_framesize_normals_frame)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_common_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(ColorTSDF_CPU, fetch_normals)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::FETCH_NORMALS, VolumeTestSrcType::MAT);
}
TEST(ColorTSDF_CPU, fetch_points_normals)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::FETCH_POINTS_NORMALS, VolumeTestSrcType::MAT);
}
TEST(ColorTSDF_CPU, valid_points_custom_framesize_mat)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::MAT);
}
TEST(ColorTSDF_CPU, valid_points_custom_framesize_fetch)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(ColorTSDF_CPU, valid_points_common_framesize_mat)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::MAT);
}
TEST(ColorTSDF_CPU, valid_points_common_framesize_fetch)
{
OpenCLStatusRevert oclStatus;
oclStatus.off();
valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
class StaticVolumeBoundingBox : public ::testing::TestWithParam<PlatformVolumeType>
{ };
TEST_P(StaticVolumeBoundingBox, staticBoundingBox)
{
auto p = GetParam();
bool gpu = bool(std::get<0>(p));
VolumeType volumeType = std::get<1>(p);
OpenCLStatusRevert oclStatus;
if (!gpu)
oclStatus.off();
staticBoundingBoxTest(volumeType);
}
//TODO: edit this list when ColorTSDF gets GPU support
INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(
PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF},
PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF},
PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF}));
// OpenCL tests // OpenCL tests
@ -1455,44 +1550,31 @@ TEST(HashTSDF_GPU, reproduce_volPoseRot)
regressionVolPoseRot(); regressionVolPoseRot();
} }
//TODO: use this when ColorTSDF gets GPU support
/*
class StaticVolumeBoundingBox : public ::testing::TestWithParam<VolumeType>
{ };
TEST_P(StaticVolumeBoundingBox, GPU) enum Growth
{ {
staticBoundingBoxTest(GetParam()); OFF = 0, ON = 1
} };
CV_ENUM(GrowthEnum, Growth::OFF, Growth::ON);
INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF)); class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam<std::tuple<PlatformTypeEnum, GrowthEnum>>
*/
// until that, we don't need parametrized test
TEST(TSDF, boundingBoxGPU)
{
staticBoundingBoxTest(VolumeType::TSDF);
}
class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam<std::tuple<bool, bool>>
{ }; { };
TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth) TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth)
{ {
auto p = GetParam(); auto p = GetParam();
bool gpu = std::get<0>(p); bool gpu = bool(std::get<0>(p));
bool enableGrowth = std::get<1>(p); bool enableGrowth = bool(std::get<1>(p));
OpenCLStatusRevert revert(cv::ocl::useOpenCL()); OpenCLStatusRevert oclStatus;
if (!gpu) if (!gpu)
cv::ocl::setUseOpenCL(false); oclStatus.off();
boundingBoxGrowthTest(enableGrowth); boundingBoxGrowthTest(enableGrowth);
} }
INSTANTIATE_TEST_CASE_P(TSDF, BoundingBoxEnableGrowthTest, ::testing::Combine(::testing::Bool(), ::testing::Bool())); INSTANTIATE_TEST_CASE_P(TSDF, BoundingBoxEnableGrowthTest, ::testing::Combine(PlatformTypeEnum::all(), GrowthEnum::all()));
#endif #endif
} }