Merge pull request #22741 from savuor:hashtsdf_volposerot_fix

There's a bug which appears when volume pose contains non-trivial rotation.
It results in wrong depth integration which can be observed during raycasting 
or points/normals export.

- This PR fixes the bug for both CPU and OpenCL
- There is a reproducer for the bug
- The copy behavior of VolumeSettings fixed (now copy constructor creates a deep copy)
- Minor changes (e.g. unused vars removed)

modules/3d/include/opencv2/3d/volume_settings.hpp

@@ -31,6 +31,8 @@ public:
     * @param volumeType volume type.
     */
     VolumeSettings(VolumeType volumeType);
+
+    VolumeSettings(const VolumeSettings& vs);
     ~VolumeSettings();
 
     /** @brief Sets the width of the image for integration.

modules/3d/src/opencl/hash_tsdf.cl

@@ -238,9 +238,8 @@ __kernel void integrateAllVolumeUnits(
                         __global const uchar* isActiveFlagsPtr,
                         int isActiveFlagsStep, int isActiveFlagsOffset,
                         int isActiveFlagsRows, int isActiveFlagsCols,
-                        // cam matrices:
+                        // cam matrix:
                         const float16 vol2cam,
-                        const float16 camInv,
                         // scalars:
                         const float voxelSize,
                         const int volUnitResolution,
@@ -272,15 +271,16 @@ __kernel void integrateAllVolumeUnits(
                                                 (allVolumePtr + table_offset + row * volCubed);
 
         // volUnit2cam = world2cam * volUnit2world =
-        // camPoseInv * volUnitPose = camPoseInv * (volPose + idx * volUnitSize) =
+        // camPoseInv * volUnitPose = camPoseInv * (volPose + volPoseRot*(idx * volUnitSize)) =
-        // camPoseInv * (volPose + idx * volUnitResolution * voxelSize) =
+        // camPoseInv * (volPose + volPoseRot*(idx * volUnitResolution * voxelSize)) =
-        // camPoseInv * (volPose + mulIdx) = camPoseInv * volPose + camPoseInv * mulIdx =
+        // camPoseInv * (volPose + volPoseRot*mulIdx) = camPoseInv * volPose + camPoseInv * volPoseRot * mulIdx =
-        // vol2cam + camPoseInv * mulIdx
+        // vol2cam + camPoseInv * volPoseRot * mulIdx
         float3 mulIdx = convert_float3(idx * volUnitResolution) * voxelSize;
         float16 volUnit2cam = vol2cam;
-        volUnit2cam.s37b += (float3)(dot(mulIdx, camInv.s012),
+
-                                     dot(mulIdx, camInv.s456),
+        volUnit2cam.s37b += (float3)(dot(mulIdx, vol2cam.s012),
-                                     dot(mulIdx, camInv.s89a));
+                                     dot(mulIdx, vol2cam.s456),
+                                     dot(mulIdx, vol2cam.s89a));
 
         integrateVolumeUnit(
             i, j,

modules/3d/src/rgbd/hash_tsdf_functions.cpp

@@ -179,7 +179,7 @@ void integrateHashTsdfVolumeUnit(
     {
         VolumeUnit& vu = volumeUnits.emplace(idx, VolumeUnit()).first->second;
 
-        Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix;
+        Matx44f subvolumePose = pose.translate(pose.rotation() * volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix;
 
         vu.pose = subvolumePose;
         vu.index = lastVolIndex; lastVolIndex++;
@@ -204,7 +204,6 @@ void integrateHashTsdfVolumeUnit(
         totalVolUnits.push_back(keyvalue.first);
     }
 
-
     //! Mark volumes in the camera frustum as active
     Range inFrustumRange(0, (int)volumeUnits.size());
     parallel_for_(inFrustumRange, [&](const Range& range) {
@@ -510,7 +509,6 @@ void ocl_integrateHashTsdfVolumeUnit(
     settings.getVolumePose(_pose);
     const Affine3f pose = Affine3f(_pose);
     Matx44f vol2camMatrix = (Affine3f(cameraPose).inv() * pose).matrix;
-    Matx44f camInvMatrix = Affine3f(cameraPose).inv().matrix;
 
     // Save length to fill new data in ranges
     int sizeBefore = hashTable.last;
@@ -580,7 +578,6 @@ void ocl_integrateHashTsdfVolumeUnit(
         ocl::KernelArg::ReadOnly(pixNorms),
         ocl::KernelArg::ReadOnly(isActiveFlags),
         vol2camMatrix,
-        camInvMatrix,
         voxelSize,
         volumeUnitResolution,
         volStrides.val,
@@ -1174,8 +1171,8 @@ void ocl_raycastHashTsdfVolumeUnit(
     const float volumeUnitSize = voxelSize * volResolution.x;
 
     Vec4f boxMin, boxMax(volumeUnitSize - voxelSize,
-        volumeUnitSize - voxelSize,
+                         volumeUnitSize - voxelSize,
-        volumeUnitSize - voxelSize);
+                         volumeUnitSize - voxelSize);
 
     Matx44f _pose;
     settings.getVolumePose(_pose);
@@ -1186,9 +1183,6 @@ void ocl_raycastHashTsdfVolumeUnit(
     Matx44f cam2volRotGPU = cam2vol.matrix;
     Matx44f vol2camRotGPU = vol2cam.matrix;
 
-    UMat volPoseGpu = Mat(pose.matrix).getUMat(ACCESS_READ);
-    UMat invPoseGpu = Mat(pose.inv().matrix).getUMat(ACCESS_READ);
-
     UMat hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ);
     UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ);
 

modules/3d/src/rgbd/volume_settings_impl.cpp

@@ -233,6 +233,11 @@ VolumeSettings::VolumeSettings(VolumeType volumeType)
     this->impl = makePtr<VolumeSettingsImpl>(volumeType);
 }
 
+VolumeSettings::VolumeSettings(const VolumeSettings& vs)
+{
+    this->impl = makePtr<VolumeSettingsImpl>(*vs.impl.dynamicCast<VolumeSettingsImpl>());
+}
+
 VolumeSettings::~VolumeSettings() {}
 
 void  VolumeSettings::setIntegrateWidth(int val) { this->impl->setIntegrateWidth(val); };

modules/3d/test/test_tsdf.cpp

@@ -755,6 +755,162 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType
     ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")";
 }
 
+static Mat nanMask(Mat img)
+{
+    int depth = img.depth();
+    Mat mask(img.size(), CV_8U);
+    for (int y = 0; y < img.rows; y++)
+    {
+        uchar *maskRow = mask.ptr<uchar>(y);
+        if (depth == CV_32F)
+        {
+            Vec4f *imgrow = img.ptr<Vec4f>(y);
+            for (int x = 0; x < img.cols; x++)
+            {
+                maskRow[x] = (imgrow[x] == imgrow[x]) * 255;
+            }
+        }
+        else if (depth == CV_64F)
+        {
+            Vec4d *imgrow = img.ptr<Vec4d>(y);
+            for (int x = 0; x < img.cols; x++)
+            {
+                maskRow[x] = (imgrow[x] == imgrow[x]) * 255;
+            }
+        }
+    }
+    return mask;
+}
+
+template <typename VT>
+static Mat_<typename VT::value_type> normalsErrorT(Mat_<VT> srcNormals, Mat_<VT> dstNormals)
+{
+    typedef typename VT::value_type Val;
+    Mat out(srcNormals.size(), cv::traits::Depth<Val>::value, Scalar(0));
+    for (int y = 0; y < srcNormals.rows; y++)
+    {
+
+        VT *srcrow = srcNormals[y];
+        VT *dstrow = dstNormals[y];
+        Val *outrow = out.ptr<Val>(y);
+        for (int x = 0; x < srcNormals.cols; x++)
+        {
+            VT sn = srcrow[x];
+            VT dn = dstrow[x];
+
+            Val dot = sn.dot(dn);
+            Val v(0.0);
+            // Just for rounding errors
+            if (std::abs(dot) < 1)
+                v = std::min(std::acos(dot), std::acos(-dot));
+
+            outrow[x] = v;
+        }
+    }
+    return out;
+}
+
+static Mat normalsError(Mat srcNormals, Mat dstNormals)
+{
+    int depth = srcNormals.depth();
+    int channels = srcNormals.channels();
+
+    if (depth == CV_32F)
+    {
+        if (channels == 3)
+        {
+            return normalsErrorT<Vec3f>(srcNormals, dstNormals);
+        }
+        else if (channels == 4)
+        {
+            return normalsErrorT<Vec4f>(srcNormals, dstNormals);
+        }
+    }
+    else if (depth == CV_64F)
+    {
+        if (channels == 3)
+        {
+            return normalsErrorT<Vec3d>(srcNormals, dstNormals);
+        }
+        else if (channels == 4)
+        {
+            return normalsErrorT<Vec4d>(srcNormals, dstNormals);
+        }
+    }
+    else
+    {
+        CV_Error(Error::StsInternal, "This type is unsupported");
+    }
+    return Mat();
+}
+
+void regressionVolPoseRot()
+{
+    // Make 2 volumes which differ only in their pose (especially rotation)
+    VolumeSettings vs(VolumeType::HashTSDF);
+    Volume volume0(VolumeType::HashTSDF, vs);
+
+    VolumeSettings vsRot(vs);
+    Matx44f pose;
+    vsRot.getVolumePose(pose);
+    pose = Affine3f(Vec3f(1, 1, 1), Vec3f()).matrix;
+    vsRot.setVolumePose(pose);
+    Volume volumeRot(VolumeType::HashTSDF, vsRot);
+
+    Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
+    Matx33f intr;
+    vs.getCameraIntegrateIntrinsics(intr);
+    bool onlySemisphere = false;
+    float depthFactor = vs.getDepthFactor();
+    Vec3f lightPose = Vec3f::all(0.f);
+    Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
+    std::vector<Affine3f> poses = scene->getPoses();
+
+    Mat depth = scene->depth(poses[0]);
+    UMat udepth;
+    depth.copyTo(udepth);
+
+    volume0.integrate(udepth, poses[0].matrix);
+    volumeRot.integrate(udepth, poses[0].matrix);
+
+    UMat upts, unrm, uptsRot, unrmRot;
+
+    volume0.raycast(poses[0].matrix, upts, unrm);
+    volumeRot.raycast(poses[0].matrix, uptsRot, unrmRot);
+
+    Mat mpts = upts.getMat(ACCESS_READ), mnrm = unrm.getMat(ACCESS_READ);
+    Mat mptsRot = uptsRot.getMat(ACCESS_READ), mnrmRot = unrmRot.getMat(ACCESS_READ);
+
+    if (cvtest::debugLevel > 0)
+    {
+        displayImage(depth, mpts, mnrm, depthFactor, lightPose);
+        displayImage(depth, mptsRot, mnrmRot, depthFactor, lightPose);
+    }
+
+    std::vector<Mat> ptsCh(3), ptsRotCh(3);
+    split(mpts, ptsCh);
+    split(uptsRot, ptsRotCh);
+    Mat maskPts0 = ptsCh[2] > 0;
+    Mat maskPtsRot = ptsRotCh[2] > 0;
+    Mat maskNrm0 = nanMask(mnrm);
+    Mat maskNrmRot = nanMask(mnrmRot);
+    Mat maskPtsDiff, maskNrmDiff;
+    cv::bitwise_xor(maskPts0, maskPtsRot, maskPtsDiff);
+    cv::bitwise_xor(maskNrm0, maskNrmRot, maskNrmDiff);
+    double ptsDiffNorm = cv::sum(maskPtsDiff)[0]/255.0;
+    double nrmDiffNorm = cv::sum(maskNrmDiff)[0]/255.0;
+
+    EXPECT_LE(ptsDiffNorm, 786);
+    EXPECT_LE(nrmDiffNorm, 786);
+
+    double normPts = cv::norm(mpts, mptsRot, NORM_INF, (maskPts0 & maskPtsRot));
+    Mat absdot = normalsError(mnrm, mnrmRot);
+    double normNrm = cv::norm(absdot, NORM_L2, (maskNrm0 & maskNrmRot));
+
+    EXPECT_LE(normPts, 2.0);
+    EXPECT_LE(normNrm, 73.08);
+}
+
 
 #ifndef HAVE_OPENCL
 TEST(TSDF, raycast_custom_framesize_normals_mat)
@@ -857,6 +1013,11 @@ TEST(HashTSDF, valid_points_common_framesize_frame)
     valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
 }
 
+TEST(HashTSDF, reproduce_volPoseRot)
+{
+    regressionVolPoseRot();
+}
+
 TEST(ColorTSDF, raycast_custom_framesize_normals_mat)
 {
     normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
@@ -1048,6 +1209,13 @@ TEST(HashTSDF_CPU, valid_points_common_framesize_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);
@@ -1118,6 +1286,14 @@ TEST(ColorTSDF_CPU, valid_points_common_framesize_fetch)
     cv::ocl::setUseOpenCL(true);
 }
 
+
+// OpenCL tests
+TEST(HashTSDF_GPU, reproduce_volPoseRot)
+{
+    regressionVolPoseRot();
+}
+
+
 #endif
 }
 }  // namespace