From 15f55426ef091ea7b9534bd0dc48cb405d33e459 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 22 Oct 2020 09:37:26 +0300 Subject: [PATCH 1/6] replace int with Point3i at integrateVolumeUnit signature --- modules/rgbd/src/hash_tsdf.cpp | 3 ++- modules/rgbd/src/hash_tsdf.hpp | 1 + modules/rgbd/src/tsdf.cpp | 2 +- modules/rgbd/src/tsdf_functions.cpp | 12 ++++++------ modules/rgbd/src/tsdf_functions.hpp | 2 +- 5 files changed, 11 insertions(+), 9 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 3c5d2d5d43d..8259c8adfd6 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -31,6 +31,7 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca maxWeight(_maxWeight), truncateThreshold(_truncateThreshold), volumeUnitResolution(_volumeUnitRes), + volumeUnitResolutions(_volumeUnitRes, _volumeUnitRes, _volumeUnitRes), volumeUnitSize(voxelSize* volumeUnitResolution), zFirstMemOrder(_zFirstMemOrder) { @@ -216,7 +217,7 @@ 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, volumeUnitResolutions, 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/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 31bf026785b..be1529164e7 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -31,6 +31,7 @@ class HashTSDFVolume : public Volume float truncDist; float truncateThreshold; int volumeUnitResolution; + Point3i volumeUnitResolutions; float volumeUnitSize; bool zFirstMemOrder; }; 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..9f0bf569189 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: 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); From 637a22ad74ddb03d61376a9286e6b2a20ed885a8 Mon Sep 17 00:00:00 2001 From: DumDereDum <46279571+DumDereDum@users.noreply.github.com> Date: Fri, 23 Oct 2020 16:18:06 +0300 Subject: [PATCH 2/6] Update hash_tsdf.hpp --- modules/rgbd/src/hash_tsdf.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index be1529164e7..31bf026785b 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -31,7 +31,6 @@ class HashTSDFVolume : public Volume float truncDist; float truncateThreshold; int volumeUnitResolution; - Point3i volumeUnitResolutions; float volumeUnitSize; bool zFirstMemOrder; }; From 8118d6389a15edb2262bb435077f9eb923e29937 Mon Sep 17 00:00:00 2001 From: DumDereDum <46279571+DumDereDum@users.noreply.github.com> Date: Fri, 23 Oct 2020 16:20:11 +0300 Subject: [PATCH 3/6] Update hash_tsdf.cpp --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 8259c8adfd6..6381d804fd1 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -31,7 +31,6 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca maxWeight(_maxWeight), truncateThreshold(_truncateThreshold), volumeUnitResolution(_volumeUnitRes), - volumeUnitResolutions(_volumeUnitRes, _volumeUnitRes, _volumeUnitRes), volumeUnitSize(voxelSize* volumeUnitResolution), zFirstMemOrder(_zFirstMemOrder) { @@ -217,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, volumeUnitResolutions, volStrides, depth, + integrateVolumeUnit(truncDist, voxelSize, maxWeight, volumeUnit.pose, + Point3i(volumeUnitResolutions,volumeUnitResolutions,volumeUnitResolutions), volStrides, depth, depthFactor, cameraPose, intrinsics, pixNorms, volUnitsData.row(volumeUnit.index)); //! Ensure all active volumeUnits are set to inactive for next integration From 39c671ed3e1a1d65c489249765e59cc5fbc3c4be Mon Sep 17 00:00:00 2001 From: DumDereDum <46279571+DumDereDum@users.noreply.github.com> Date: Fri, 23 Oct 2020 16:24:56 +0300 Subject: [PATCH 4/6] Update hash_tsdf.cpp --- modules/rgbd/src/hash_tsdf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 6381d804fd1..35838352d43 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -217,7 +217,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma { //! The volume unit should already be added into the Volume from the allocator integrateVolumeUnit(truncDist, voxelSize, maxWeight, volumeUnit.pose, - Point3i(volumeUnitResolutions,volumeUnitResolutions,volumeUnitResolutions), volStrides, depth, + 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 From 2debbbb52305d341cd3dd7255421b28b87f94860 Mon Sep 17 00:00:00 2001 From: Saratovtsev Date: Mon, 26 Oct 2020 16:40:09 +0300 Subject: [PATCH 5/6] error fix --- modules/rgbd/src/tsdf_functions.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 9f0bf569189..1a3b95c8aac 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -132,7 +132,7 @@ void integrateVolumeUnit( const float dfac(1.f / depthFactor); TsdfVoxel* volDataStart = volume.ptr();; -#if USE_INTRINSICS +#if !USE_INTRINSICS auto IntegrateInvoker = [&](const Range& range) { // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; @@ -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++) { From 2dabd2c9f7ab355b46c7cf229898dfce1eec127e Mon Sep 17 00:00:00 2001 From: Saratovtsev Date: Mon, 26 Oct 2020 16:40:30 +0300 Subject: [PATCH 6/6] minor fix --- modules/rgbd/src/tsdf_functions.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 1a3b95c8aac..ff40dce248d 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -132,7 +132,7 @@ void integrateVolumeUnit( const float dfac(1.f / depthFactor); TsdfVoxel* volDataStart = volume.ptr();; -#if !USE_INTRINSICS +#if USE_INTRINSICS auto IntegrateInvoker = [&](const Range& range) { // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt;