diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 3c5d2d5d43d..35838352d43 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -216,7 +216,8 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma if (volumeUnit.isActive) { //! The volume unit should already be added into the Volume from the allocator - integrateVolumeUnit(truncDist, voxelSize, maxWeight, volumeUnit.pose, volumeUnitResolution, volStrides, depth, + integrateVolumeUnit(truncDist, voxelSize, maxWeight, volumeUnit.pose, + Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, depthFactor, cameraPose, intrinsics, pixNorms, volUnitsData.row(volumeUnit.index)); //! Ensure all active volumeUnits are set to inactive for next integration diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 6e05bebb1d1..1e8704170f4 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -133,7 +133,7 @@ void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44 pixNorms = preCalculationPixNorm(depth, intrinsics); } - integrateVolumeUnit(truncDist, voxelSize, maxWeight, (this->pose).matrix, volResolution.x, volStrides, depth, + integrateVolumeUnit(truncDist, voxelSize, maxWeight, (this->pose).matrix, volResolution, volStrides, depth, depthFactor, cameraPose, intrinsics, pixNorms, volume); } diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 3eb27742c1e..ff40dce248d 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -111,7 +111,7 @@ depthType bilinearDepth(const Depth& m, cv::Point2f pt) void integrateVolumeUnit( float truncDist, float voxelSize, int maxWeight, - cv::Matx44f _pose, int volResolution, Vec4i volStrides, + cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume) { @@ -122,7 +122,7 @@ void integrateVolumeUnit( cv::Affine3f vpose(_pose); Depth depth = _depth.getMat(); - Range integrateRange(0, volResolution); + Range integrateRange(0, volResolution.x); Mat volume = _volume.getMat(); Mat pixNorms = _pixNorms.getMat(); @@ -147,7 +147,7 @@ void integrateVolumeUnit( for (int x = range.start; x < range.end; x++) { TsdfVoxel* volDataX = volDataStart + x * volStrides[0]; - for (int y = 0; y < volResolution; y++) + for (int y = 0; y < volResolution.y; y++) { TsdfVoxel* volDataY = volDataX + y * volStrides[1]; // optimization of camSpace transformation (vector addition instead of matmul at each z) @@ -161,7 +161,7 @@ void integrateVolumeUnit( if (zStepPt.z > 0) { startZ = baseZ; - endZ = volResolution; + endZ = volResolution.z; } else { @@ -174,7 +174,7 @@ void integrateVolumeUnit( if (basePt.z > 0) { startZ = 0; - endZ = volResolution; + endZ = volResolution.z; } else { @@ -183,7 +183,7 @@ void integrateVolumeUnit( } } startZ = max(0, startZ); - endZ = min(int(volResolution), endZ); + endZ = min(int(volResolution.z), endZ); for (int z = startZ; z < endZ; z++) { // optimization of the following: @@ -281,7 +281,7 @@ void integrateVolumeUnit( for (int x = range.start; x < range.end; x++) { TsdfVoxel* volDataX = volDataStart + x * volStrides[0]; - for (int y = 0; y < volResolution; y++) + for (int y = 0; y < volResolution.y; y++) { TsdfVoxel* volDataY = volDataX + y * volStrides[1]; // optimization of camSpace transformation (vector addition instead of matmul at each z) @@ -299,7 +299,7 @@ void integrateVolumeUnit( if (zStep.z > 0) { startZ = baseZ; - endZ = volResolution; + endZ = volResolution.z; } else { @@ -312,7 +312,7 @@ void integrateVolumeUnit( if (basePt.z > 0) { startZ = 0; - endZ = volResolution; + endZ = volResolution.z; } else { @@ -321,7 +321,7 @@ void integrateVolumeUnit( } } startZ = max(0, startZ); - endZ = min(int(volResolution), endZ); + endZ = min(int(volResolution.z), endZ); for (int z = startZ; z < endZ; z++) { diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 28f776d752f..6d86595118f 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -39,7 +39,7 @@ depthType bilinearDepth(const Depth& m, cv::Point2f pt); void integrateVolumeUnit( float truncDist, float voxelSize, int maxWeight, - cv::Matx44f _pose, int volResolution, Vec4i volStrides, + cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume);