diff --git a/modules/3d/include/opencv2/3d/volume_settings.hpp b/modules/3d/include/opencv2/3d/volume_settings.hpp index 893fa60bfb..f5477d2ee0 100644 --- a/modules/3d/include/opencv2/3d/volume_settings.hpp +++ b/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. diff --git a/modules/3d/src/opencl/hash_tsdf.cl b/modules/3d/src/opencl/hash_tsdf.cl index 3b5a906518..a4e0f79e05 100644 --- a/modules/3d/src/opencl/hash_tsdf.cl +++ b/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 * (volPose + idx * volUnitResolution * voxelSize) = - // camPoseInv * (volPose + mulIdx) = camPoseInv * volPose + camPoseInv * mulIdx = - // vol2cam + camPoseInv * mulIdx + // camPoseInv * volUnitPose = camPoseInv * (volPose + volPoseRot*(idx * volUnitSize)) = + // camPoseInv * (volPose + volPoseRot*(idx * volUnitResolution * voxelSize)) = + // camPoseInv * (volPose + volPoseRot*mulIdx) = camPoseInv * volPose + camPoseInv * volPoseRot * 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), - dot(mulIdx, camInv.s89a)); + + volUnit2cam.s37b += (float3)(dot(mulIdx, vol2cam.s012), + dot(mulIdx, vol2cam.s456), + dot(mulIdx, vol2cam.s89a)); integrateVolumeUnit( i, j, diff --git a/modules/3d/src/rgbd/hash_tsdf_functions.cpp b/modules/3d/src/rgbd/hash_tsdf_functions.cpp index ccef82ed37..45066ae9f6 100644 --- a/modules/3d/src/rgbd/hash_tsdf_functions.cpp +++ b/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); diff --git a/modules/3d/src/rgbd/volume_settings_impl.cpp b/modules/3d/src/rgbd/volume_settings_impl.cpp index b5bc1c8b89..fa60360d50 100644 --- a/modules/3d/src/rgbd/volume_settings_impl.cpp +++ b/modules/3d/src/rgbd/volume_settings_impl.cpp @@ -233,6 +233,11 @@ VolumeSettings::VolumeSettings(VolumeType volumeType) this->impl = makePtr(volumeType); } +VolumeSettings::VolumeSettings(const VolumeSettings& vs) +{ + this->impl = makePtr(*vs.impl.dynamicCast()); +} + VolumeSettings::~VolumeSettings() {} void VolumeSettings::setIntegrateWidth(int val) { this->impl->setIntegrateWidth(val); }; diff --git a/modules/3d/test/test_tsdf.cpp b/modules/3d/test/test_tsdf.cpp index 12df205ccc..73676dd4c0 100644 --- a/modules/3d/test/test_tsdf.cpp +++ b/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(y); + if (depth == CV_32F) + { + Vec4f *imgrow = img.ptr(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(y); + for (int x = 0; x < img.cols; x++) + { + maskRow[x] = (imgrow[x] == imgrow[x]) * 255; + } + } + } + return mask; +} + +template +static Mat_ normalsErrorT(Mat_ srcNormals, Mat_ dstNormals) +{ + typedef typename VT::value_type Val; + Mat out(srcNormals.size(), cv::traits::Depth::value, Scalar(0)); + for (int y = 0; y < srcNormals.rows; y++) + { + + VT *srcrow = srcNormals[y]; + VT *dstrow = dstNormals[y]; + Val *outrow = out.ptr(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(srcNormals, dstNormals); + } + else if (channels == 4) + { + return normalsErrorT(srcNormals, dstNormals); + } + } + else if (depth == CV_64F) + { + if (channels == 3) + { + return normalsErrorT(srcNormals, dstNormals); + } + else if (channels == 4) + { + return normalsErrorT(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::create(frameSize, intr, depthFactor, onlySemisphere); + std::vector 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 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