From 111e2c89b7af3214a991be71c37421a0a239f861 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 6 Oct 2020 16:21:43 +0300 Subject: [PATCH 01/53] create new variables --- modules/rgbd/src/hash_tsdf.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index bac5ea9a46a..dbe86ccbfa9 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -63,6 +63,9 @@ struct tsdf_hash typedef std::unordered_set VolumeUnitIndexSet; typedef std::unordered_map VolumeUnitMap; +typedef unsigned int volumeIndex; +typedef std::unordered_set VolumeUnitIndexes; + class HashTSDFVolumeCPU : public HashTSDFVolume { public: @@ -101,8 +104,11 @@ class HashTSDFVolumeCPU : public HashTSDFVolume cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const; public: + Vec6f frameParams; + Mat pixNorms; + cv::Mat volumeUnits; //! Hashtable of individual smaller volume units - VolumeUnitMap volumeUnits; + //VolumeUnitMap volumeUnits; }; cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, From 46fe5a3d07f24964a96cd214d79d33f8d9ff1c71 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 6 Oct 2020 16:41:17 +0300 Subject: [PATCH 02/53] rewrite reset() --- modules/rgbd/src/hash_tsdf.cpp | 3 ++- modules/rgbd/src/hash_tsdf.hpp | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b414500217f..1c3a571dc44 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -62,7 +62,8 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); - volumeUnits.clear(); + //volumeUnits.clear(); + volumeUnits = cv::Mat(); } void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index dbe86ccbfa9..e510e307da3 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -106,7 +106,9 @@ class HashTSDFVolumeCPU : public HashTSDFVolume public: Vec6f frameParams; Mat pixNorms; + VolumeUnitIndexes volumeUnitIndexes; cv::Mat volumeUnits; + //! Hashtable of individual smaller volume units //VolumeUnitMap volumeUnits; }; From 4128df1741f83695b86cc0ede359e4b49f957e77 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 7 Oct 2020 10:44:01 +0300 Subject: [PATCH 03/53] first valid version of new HasHTSDF --- modules/rgbd/src/hash_tsdf.cpp | 491 ++++++++++++++++++++++++++------- modules/rgbd/src/hash_tsdf.hpp | 36 ++- 2 files changed, 418 insertions(+), 109 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 1c3a571dc44..a036209c3b5 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -38,14 +38,15 @@ static inline float tsdfToFloat(TsdfType num) } HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, float _truncateThreshold, - int _volumeUnitRes, bool _zFirstMemOrder) + float _truncDist, int _maxWeight, float _truncateThreshold, + int _volumeUnitRes, bool _zFirstMemOrder) : Volume(_voxelSize, _pose, _raycastStepFactor), - maxWeight(_maxWeight), - truncateThreshold(_truncateThreshold), - volumeUnitResolution(_volumeUnitRes), - volumeUnitSize(voxelSize * volumeUnitResolution), - zFirstMemOrder(_zFirstMemOrder) + maxWeight(_maxWeight), + truncateThreshold(_truncateThreshold), + volumeUnitResolution(_volumeUnitRes), + volumeUnitSize(voxelSize* volumeUnitResolution), + zFirstMemOrder(_zFirstMemOrder), + volumeDims(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution) { truncDist = std::max(_truncDist, 4.0f * voxelSize); } @@ -56,6 +57,7 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float : HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { + HashS = 0; } // zero volume, leave rest params the same @@ -63,7 +65,242 @@ void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); //volumeUnits.clear(); - volumeUnits = cv::Mat(); + volumes = cv::Mat(); + HashS = 0; +} + +static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) +{ + int height = depth.rows; + int widht = depth.cols; + Point2f fl(intrinsics.fx, intrinsics.fy); + Point2f pp(intrinsics.cx, intrinsics.cy); + Mat pixNorm(height, widht, CV_32F); + std::vector x(widht); + std::vector y(height); + for (int i = 0; i < widht; i++) + x[i] = (i - pp.x) / fl.x; + for (int i = 0; i < height; i++) + y[i] = (i - pp.y) / fl.y; + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < widht; j++) + { + pixNorm.at(i, j) = sqrtf(x[j] * x[j] + y[i] * y[i] + 1.0f); + } + } + return pixNorm; +} + +static const bool fixMissingData = false; +static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) +{ + const depthType defaultValue = qnan; + if (pt.x < 0 || pt.x >= m.cols - 1 || + pt.y < 0 || pt.y >= m.rows - 1) + return defaultValue; + + int xi = cvFloor(pt.x), yi = cvFloor(pt.y); + + const depthType* row0 = m[yi + 0]; + const depthType* row1 = m[yi + 1]; + + depthType v00 = row0[xi + 0]; + depthType v01 = row0[xi + 1]; + depthType v10 = row1[xi + 0]; + depthType v11 = row1[xi + 1]; + + // assume correct depth is positive + bool b00 = v00 > 0; + bool b01 = v01 > 0; + bool b10 = v10 > 0; + bool b11 = v11 > 0; + + if (!fixMissingData) + { + if (!(b00 && b01 && b10 && b11)) + return defaultValue; + else + { + float tx = pt.x - xi, ty = pt.y - yi; + depthType v0 = v00 + tx * (v01 - v00); + depthType v1 = v10 + tx * (v11 - v10); + return v0 + ty * (v1 - v0); + } + } + else + { + int nz = b00 + b01 + b10 + b11; + if (nz == 0) + { + return defaultValue; + } + if (nz == 1) + { + if (b00) return v00; + if (b01) return v01; + if (b10) return v10; + if (b11) return v11; + } + if (nz == 2) + { + if (b00 && b10) v01 = v00, v11 = v10; + if (b01 && b11) v00 = v01, v10 = v11; + if (b00 && b01) v10 = v00, v11 = v01; + if (b10 && b11) v00 = v10, v01 = v11; + if (b00 && b11) v01 = v10 = (v00 + v11) * 0.5f; + if (b01 && b10) v00 = v11 = (v01 + v10) * 0.5f; + } + if (nz == 3) + { + if (!b00) v00 = v10 + v01 - v11; + if (!b01) v01 = v00 + v11 - v10; + if (!b10) v10 = v00 + v11 - v01; + if (!b11) v11 = v01 + v10 - v00; + } + + float tx = pt.x - xi, ty = pt.y - yi; + depthType v0 = v00 + tx * (v01 - v00); + depthType v1 = v10 + tx * (v11 - v10); + return v0 + ty * (v1 - v0); + } +} + +void HashTSDFVolumeCPU::_integrate( + float voxelSize, cv::Matx44f _pose, float raycastStepFactor, + float _truncDist, int _maxWeight, Point3i volResolution, Vec4i volDims, + InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, + const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + CV_Assert(!_depth.empty()); + float voxelSizeInv = 1.0f / voxelSize; + cv::Affine3f pose(_pose); + float truncDist = std::max(_truncDist, 2.1f * voxelSize); + WeightType maxWeight = (unsigned char)_maxWeight; + + Vec8i neighbourCoords = Vec8i( + volDims.dot(Vec4i(0, 0, 0)), + volDims.dot(Vec4i(0, 0, 1)), + volDims.dot(Vec4i(0, 1, 0)), + volDims.dot(Vec4i(0, 1, 1)), + volDims.dot(Vec4i(1, 0, 0)), + volDims.dot(Vec4i(1, 0, 1)), + volDims.dot(Vec4i(1, 1, 0)), + volDims.dot(Vec4i(1, 1, 1)) + ); + + Depth depth = _depth.getMat(); + Mat pixNorms = _pixNorms.getMat(); + + Range range(0, volResolution.x); + + Mat volume = _volume.getMat(); + const Intr& intr(intrinsics); + const Intr::Projector proj(intrinsics.makeProjector()); + const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + const float truncDistInv(1.f / truncDist); + const float dfac(1.f / depthFactor); + TsdfVoxel* volDataStart = volume.ptr();; + + auto _IntegrateInvoker = [&](const Range& range) + { + //std::cout << "3"; + for (int x = range.start; x < range.end; x++) + { + TsdfVoxel* volDataX = volDataStart + x * volDims[0]; + for (int y = 0; y < volResolution.y; y++) + { + TsdfVoxel* volDataY = volDataX + y * volDims[1]; + // optimization of camSpace transformation (vector addition instead of matmul at each z) + Point3f basePt = vol2cam * (Point3f(float(x), float(y), 0.0f) * voxelSize); + Point3f camSpacePt = basePt; + // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; + // zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize + Point3f zStep = Point3f(vol2cam.matrix(0, 2), + vol2cam.matrix(1, 2), + vol2cam.matrix(2, 2)) * voxelSize; + int startZ, endZ; + if (abs(zStep.z) > 1e-5) + { + int baseZ = int(-basePt.z / zStep.z); + if (zStep.z > 0) + { + startZ = baseZ; + endZ = volResolution.z; + } + else + { + startZ = 0; + endZ = baseZ; + } + } + else + { + if (basePt.z > 0) + { + startZ = 0; + endZ = volResolution.z; + } + else + { + // z loop shouldn't be performed + startZ = endZ = 0; + } + } + startZ = max(0, startZ); + endZ = min(volResolution.z, endZ); + + for (int z = startZ; z < endZ; z++) + { + // optimization of the following: + //Point3f volPt = Point3f(x, y, z)*volume.voxelSize; + //Point3f camSpacePt = vol2cam * volPt; + + camSpacePt += zStep; + if (camSpacePt.z <= 0) + continue; + + Point3f camPixVec; + Point2f projected = proj(camSpacePt, camPixVec); + + depthType v = bilinearDepth(depth, projected); + if (v == 0) { + continue; + } + + int _u = projected.x; + int _v = projected.y; + if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows)) + continue; + float pixNorm = pixNorms.at(_v, _u); + + // difference between distances of point and of surface to camera + float sdf = pixNorm * (v * dfac - camSpacePt.z); + // possible alternative is: + // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); + //std::cout << "sdf: " << sdf << std::endl; + if (sdf >= -truncDist) + { + TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv)); + + TsdfVoxel& voxel = volDataY[z * volDims[2]]; + WeightType& weight = voxel.weight; + TsdfType& value = voxel.tsdf; + //std::cout << value << std::endl; + // update TSDF + value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); + weight = min(int(weight + 1), int(maxWeight)); + } + } + } + } + }; + parallel_for_(range, _IntegrateInvoker); + } void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) @@ -128,15 +365,38 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); //! Perform the allocation - int res = volumeUnitResolution; - Point3i volumeDims(res, res, res); + //int res = volumeUnitResolution; + //Point3i volumeDims(res, res, res); for (auto idx : newIndices) { VolumeUnit& vu = volumeUnits[idx]; Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; - vu.pVolume = makePtr(voxelSize, subvolumePose, raycastStepFactor, truncDist, maxWeight, volumeDims); + + int xdim, ydim, zdim; + if (zFirstMemOrder) + { + xdim = volumeDims.z * volumeDims.y; + ydim = volumeDims.z; + zdim = 1; + } + else + { + xdim = 1; + ydim = volumeDims.x; + zdim = volumeDims.x * volumeDims.y; + } + vu.volDims = Vec4i(xdim, ydim, zdim); + + vu.pose = subvolumePose; + volumes.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); + vu.index = HashS; HashS++; + volumes.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) + { + TsdfVoxel& v = reinterpret_cast(vv); + v.tsdf = floatToTsdf(0.0f); v.weight = 0; + }); //! This volume unit will definitely be required for current integration - vu.isActive = true; + vu.isActive = true; } //! Get keys for all the allocated volume Units @@ -155,7 +415,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = range.start; i < range.end; ++i) { Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitMap::iterator it = volumeUnits.find(tsdf_idx); + VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx); if (it == volumeUnits.end()) return; @@ -174,13 +434,22 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma } } }); + if (!(frameParams[0] == depth.rows && frameParams[1] == depth.cols && + frameParams[2] == intrinsics.fx && frameParams[3] == intrinsics.fy && + frameParams[4] == intrinsics.cx && frameParams[5] == intrinsics.cy)) + { + frameParams[0] = (float)depth.rows; frameParams[1] = (float)depth.cols; + frameParams[2] = intrinsics.fx; frameParams[3] = intrinsics.fy; + frameParams[4] = intrinsics.cx; frameParams[5] = intrinsics.cy; + pixNorms = preCalculationPixNorm(depth, intrinsics); + } //! Integrate the correct volumeUnits parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) { for (int i = range.start; i < range.end; i++) { Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitMap::iterator it = volumeUnits.find(tsdf_idx); + VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx); if (it == volumeUnits.end()) return; @@ -188,7 +457,18 @@ 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 - volumeUnit.pVolume->integrate(depth, depthFactor, cameraPose, intrinsics); + //volumeUnit.pVolume->integrate(depth, depthFactor, cameraPose, intrinsics); + _integrate( + voxelSize, volumeUnit.pose, raycastStepFactor, + truncDist, maxWeight, volumeDims, volumeUnit.volDims, + depth, depthFactor, cameraPose, intrinsics, + pixNorms, volumes.row(volumeUnit.index)); + /* + float voxelSize, cv::Matx44f _pose, float raycastStepFactor, + float _truncDist, int _maxWeight, Point3i volResolution, Vec4i volDims, + InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, + const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume + */ //! Ensure all active volumeUnits are set to inactive for next integration volumeUnit.isActive = false; } @@ -219,13 +499,33 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const cvFloor(point.z * voxelSizeInv)); } +TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, + Point3i volResolution, Vec4i volDims) +{ + //! Out of bounds + if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || + (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || + (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) + { + TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.0f); + dummy.weight = 0; + return dummy; + } + Mat volume = _volume.getMat(); + const TsdfVoxel* volData = volume.ptr(); + int coordBase = + volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; + return volData[coordBase]; +} + inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), cvFloor(volumeIdx[2] / volumeUnitResolution)); - VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); + VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); if (it == volumeUnits.end()) { TsdfVoxel dummy; @@ -233,8 +533,6 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const dummy.weight = 0; return dummy; } - cv::Ptr volumeUnit = - std::dynamic_pointer_cast(it->second.pVolume); cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, @@ -242,13 +540,13 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return volumeUnit->at(volUnitLocalIdx); + return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const { cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); - VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); + VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); if (it == volumeUnits.end()) { TsdfVoxel dummy; @@ -256,14 +554,12 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const dummy.weight = 0; return dummy; } - cv::Ptr volumeUnit = - std::dynamic_pointer_cast(it->second.pVolume); cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return volumeUnit->at(volUnitLocalIdx); + return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) @@ -282,8 +578,8 @@ static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) } } -inline TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitMap::const_iterator it, - VolumeUnitMap::const_iterator vend, int unitRes) +inline TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, + VolumeUnitIndexes::const_iterator vend, cv::Mat vol, int unitRes) { if (it == vend) { @@ -292,13 +588,14 @@ inline TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, Vo dummy.weight = 0; return dummy; } - Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + //Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; // expanding at(), removing bounds check - const TsdfVoxel* volData = volumeUnit->volume.ptr(); - Vec4i volDims = volumeUnit->volDims; + //const TsdfVoxel* volData = volumeUnit->volume.ptr(); + const TsdfVoxel* volData = vol.row(it->second.index).ptr(); + Vec4i volDims = it->second.volDims; int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; return volData[coordBase]; } @@ -344,7 +641,7 @@ float HashTSDFVolumeCPU::interpolateVoxelPoint(const Point3f& point) const // A small hash table to reduce a number of find() calls bool queried[8]; - VolumeUnitMap::const_iterator iterMap[8]; + VolumeUnitIndexes::const_iterator iterMap[8]; for (int i = 0; i < 8; i++) { iterMap[i] = volumeUnits.end(); @@ -376,7 +673,7 @@ float HashTSDFVolumeCPU::interpolateVoxelPoint(const Point3f& point) const } //VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); - vx[i] = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf; + vx[i] = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf; } return interpolate(tx, ty, tz, vx); @@ -396,7 +693,7 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const // A small hash table to reduce a number of find() calls bool queried[8]; - VolumeUnitMap::const_iterator iterMap[8]; + VolumeUnitIndexes::const_iterator iterMap[8]; for (int i = 0; i < 8; i++) { iterMap[i] = volumeUnits.end(); @@ -439,7 +736,7 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const } //VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); - vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf); + vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf); } #if !USE_INTERPOLATION_IN_GETNORMAL @@ -580,7 +877,7 @@ struct HashRaycastInvoker : ParallelLoopBody Point3f currRayPos = orig + tcurr * rayDirV; cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - VolumeUnitMap::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); + VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); float currTsdf = prevTsdf; int currWeight = 0; @@ -590,14 +887,14 @@ struct HashRaycastInvoker : ParallelLoopBody //! Does the subvolume exist in hashtable if (it != volume.volumeUnits.end()) { - currVolumeUnit = - std::dynamic_pointer_cast(it->second.pVolume); + //currVolumeUnit = + // std::dynamic_pointer_cast(it->second.pVolume); cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); + TsdfVoxel currVoxel = _at(volUnitLocalIdx, volume.volumes.row(it->second.index), volume.volumeDims, it->second.volDims); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -659,88 +956,80 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: parallel_for_(Range(0, points.rows), ri, nstripes); } -struct HashFetchPointsNormalsInvoker : ParallelLoopBody +void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const { - HashFetchPointsNormalsInvoker(const HashTSDFVolumeCPU& _volume, - const std::vector& _totalVolUnits, - std::vector>& _pVecs, - std::vector>& _nVecs, bool _needNormals) - : ParallelLoopBody(), - volume(_volume), - totalVolUnits(_totalVolUnits), - pVecs(_pVecs), - nVecs(_nVecs), - needNormals(_needNormals) - { - } + CV_TRACE_FUNCTION(); - virtual void operator()(const Range& range) const override + if (_points.needed()) { - std::vector points, normals; - for (int i = range.start; i < range.end; i++) + std::vector> pVecs, nVecs; + + std::vector totalVolUnits; + for (const auto& keyvalue : volumeUnits) + { + totalVolUnits.push_back(keyvalue.first); + } + Range range(0, (int)totalVolUnits.size()); + const int nstripes = -1; + + const HashTSDFVolumeCPU& volume(*this); + //std::vector totalVolUnits; + //std::vector>& pVecs; + //std::vector>& nVecs; + const TsdfVoxel* volDataStart; + bool needNormals(_normals.needed()); + Mutex mutex; + + auto _HashFetchPointsNormalsInvoker = [&](const Range& range) { - cv::Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitMap::const_iterator it = volume.volumeUnits.find(tsdf_idx); - Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); - if (it != volume.volumeUnits.end()) + std::vector points, normals; + for (int i = range.start; i < range.end; i++) { - cv::Ptr volumeUnit = - std::dynamic_pointer_cast(it->second.pVolume); - std::vector localPoints; - std::vector localNormals; - for (int x = 0; x < volume.volumeUnitResolution; x++) - for (int y = 0; y < volume.volumeUnitResolution; y++) - for (int z = 0; z < volume.volumeUnitResolution; z++) - { - cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = volumeUnit->at(voxelIdx); + cv::Vec3i tsdf_idx = totalVolUnits[i]; - if (voxel.tsdf != -128 && voxel.weight != 0) + VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(tsdf_idx); + Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); + if (it != volume.volumeUnits.end()) + { + //cv::Ptr volumeUnit = + // std::dynamic_pointer_cast(it->second.pVolume); + //cv::Ptr<_NewVolume> volumeUnit = + // std::dynamic_pointer_cast<_NewVolume>(it->second.pVolume); + std::vector localPoints; + std::vector localNormals; + for (int x = 0; x < volume.volumeUnitResolution; x++) + for (int y = 0; y < volume.volumeUnitResolution; y++) + for (int z = 0; z < volume.volumeUnitResolution; z++) { - Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); - localPoints.push_back(toPtype(point)); - if (needNormals) + cv::Vec3i voxelIdx(x, y, z); + //TsdfVoxel voxel = volumeUnit->at(voxelIdx, it->second.volume); + TsdfVoxel voxel = _at(voxelIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); + + if (voxel.tsdf != -128 && voxel.weight != 0) { - Point3f normal = volume.getNormalVoxel(point); - localNormals.push_back(toPtype(normal)); + Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); + localPoints.push_back(toPtype(point)); + if (needNormals) + { + Point3f normal = volume.getNormalVoxel(point); + localNormals.push_back(toPtype(normal)); + } } } - } - AutoLock al(mutex); - pVecs.push_back(localPoints); - nVecs.push_back(localNormals); + AutoLock al(mutex); + pVecs.push_back(localPoints); + nVecs.push_back(localNormals); + } } - } - } + }; - const HashTSDFVolumeCPU& volume; - std::vector totalVolUnits; - std::vector>& pVecs; - std::vector>& nVecs; - const TsdfVoxel* volDataStart; - bool needNormals; - mutable Mutex mutex; -}; -void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const -{ - CV_TRACE_FUNCTION(); + //parallel_for_(range, fi, nstripes); + parallel_for_(range, _HashFetchPointsNormalsInvoker, nstripes); - if (_points.needed()) - { - std::vector> pVecs, nVecs; - std::vector totalVolUnits; - for (const auto& keyvalue : volumeUnits) - { - totalVolUnits.push_back(keyvalue.first); - } - HashFetchPointsNormalsInvoker fi(*this, totalVolUnits, pVecs, nVecs, _normals.needed()); - Range range(0, (int)totalVolUnits.size()); - const int nstripes = -1; - parallel_for_(range, fi, nstripes); std::vector points, normals; for (size_t i = 0; i < pVecs.size(); i++) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index e510e307da3..53a990233ba 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -33,8 +33,9 @@ class HashTSDFVolume : public Volume int volumeUnitResolution; float volumeUnitSize; bool zFirstMemOrder; + Point3i volumeDims; }; - +/* struct VolumeUnit { VolumeUnit() : pVolume(nullptr){}; @@ -44,7 +45,7 @@ struct VolumeUnit cv::Vec3i index; bool isActive; }; - +*/ //! Spatial hashing struct tsdf_hash { @@ -60,14 +61,32 @@ struct tsdf_hash } }; -typedef std::unordered_set VolumeUnitIndexSet; -typedef std::unordered_map VolumeUnitMap; + +//typedef std::unordered_map VolumeUnitMap; typedef unsigned int volumeIndex; -typedef std::unordered_set VolumeUnitIndexes; +struct VolumeUnit +{ + cv::Vec3i coord; + volumeIndex index; + cv::Matx44f pose; + Vec4i volDims; + bool isActive; +}; + +typedef std::unordered_set VolumeUnitIndexSet; +typedef std::unordered_map VolumeUnitIndexes; +//typedef std::unordered_set VolumeUnitIndexSet; +//typedef std::unordered_map VolumeUnitIndexes; class HashTSDFVolumeCPU : public HashTSDFVolume { +private: + void _integrate( + float voxelSize, cv::Matx44f _pose, float raycastStepFactor, + float _truncDist, int _maxWeight, Point3i volResolution, Vec4i volDims, + InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, + const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); public: // dimension in voxels, size in meters HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, @@ -106,9 +125,10 @@ class HashTSDFVolumeCPU : public HashTSDFVolume public: Vec6f frameParams; Mat pixNorms; - VolumeUnitIndexes volumeUnitIndexes; - cv::Mat volumeUnits; - + //VolumeUnitIndexes volumeUnitIndexes; + VolumeUnitIndexes volumeUnits; + cv::Mat volumes; + volumeIndex HashS; //! Hashtable of individual smaller volume units //VolumeUnitMap volumeUnits; }; From 3bebec6d4caa0dd0fa197b780ec7cd6d676c5da6 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 8 Oct 2020 10:33:07 +0300 Subject: [PATCH 04/53] some warning fixes --- modules/rgbd/src/hash_tsdf.cpp | 85 ++++++++-------------------------- modules/rgbd/src/hash_tsdf.hpp | 9 ++-- 2 files changed, 22 insertions(+), 72 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a036209c3b5..d487e30f457 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -64,7 +64,6 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); - //volumeUnits.clear(); volumes = cv::Mat(); HashS = 0; } @@ -167,48 +166,29 @@ static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) } } -void HashTSDFVolumeCPU::_integrate( - float voxelSize, cv::Matx44f _pose, float raycastStepFactor, - float _truncDist, int _maxWeight, Point3i volResolution, Vec4i volDims, +void HashTSDFVolumeCPU::_integrate( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume) + const cv::kinfu::Intr& intrinsics, InputArray _volume) { CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); CV_Assert(!_depth.empty()); - float voxelSizeInv = 1.0f / voxelSize; - cv::Affine3f pose(_pose); - float truncDist = std::max(_truncDist, 2.1f * voxelSize); - WeightType maxWeight = (unsigned char)_maxWeight; - - Vec8i neighbourCoords = Vec8i( - volDims.dot(Vec4i(0, 0, 0)), - volDims.dot(Vec4i(0, 0, 1)), - volDims.dot(Vec4i(0, 1, 0)), - volDims.dot(Vec4i(0, 1, 1)), - volDims.dot(Vec4i(1, 0, 0)), - volDims.dot(Vec4i(1, 0, 1)), - volDims.dot(Vec4i(1, 1, 0)), - volDims.dot(Vec4i(1, 1, 1)) - ); - + cv::Affine3f vpose(_pose); Depth depth = _depth.getMat(); - Mat pixNorms = _pixNorms.getMat(); - Range range(0, volResolution.x); + + Range integrateRange(0, volResolution.x); Mat volume = _volume.getMat(); - const Intr& intr(intrinsics); const Intr::Projector proj(intrinsics.makeProjector()); - const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * vpose); const float truncDistInv(1.f / truncDist); const float dfac(1.f / depthFactor); TsdfVoxel* volDataStart = volume.ptr();; - auto _IntegrateInvoker = [&](const Range& range) + auto IntegrateInvoker = [&](const Range& range) { - //std::cout << "3"; for (int x = range.start; x < range.end; x++) { TsdfVoxel* volDataX = volDataStart + x * volDims[0]; @@ -282,7 +262,6 @@ void HashTSDFVolumeCPU::_integrate( float sdf = pixNorm * (v * dfac - camSpacePt.z); // possible alternative is: // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); - //std::cout << "sdf: " << sdf << std::endl; if (sdf >= -truncDist) { TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv)); @@ -290,7 +269,7 @@ void HashTSDFVolumeCPU::_integrate( TsdfVoxel& voxel = volDataY[z * volDims[2]]; WeightType& weight = voxel.weight; TsdfType& value = voxel.tsdf; - //std::cout << value << std::endl; + // update TSDF value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); weight = min(int(weight + 1), int(maxWeight)); @@ -299,7 +278,7 @@ void HashTSDFVolumeCPU::_integrate( } } }; - parallel_for_(range, _IntegrateInvoker); + parallel_for_(integrateRange, IntegrateInvoker); } @@ -319,6 +298,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma VolumeUnitIndexSet newIndices; Mutex mutex; Range allocateRange(0, depth.rows); + auto AllocateVolumeUnitsInvoker = [&](const Range& range) { VolumeUnitIndexSet localAccessVolUnits; for (int y = range.start; y < range.end; y += depthStride) @@ -365,8 +345,6 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); //! Perform the allocation - //int res = volumeUnitResolution; - //Point3i volumeDims(res, res, res); for (auto idx : newIndices) { VolumeUnit& vu = volumeUnits[idx]; @@ -434,6 +412,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma } } }); + if (!(frameParams[0] == depth.rows && frameParams[1] == depth.cols && frameParams[2] == intrinsics.fx && frameParams[3] == intrinsics.fy && frameParams[4] == intrinsics.cx && frameParams[5] == intrinsics.cy)) @@ -444,6 +423,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma pixNorms = preCalculationPixNorm(depth, intrinsics); } + //! Integrate the correct volumeUnits parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) { for (int i = range.start; i < range.end; i++) @@ -457,18 +437,9 @@ 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 - //volumeUnit.pVolume->integrate(depth, depthFactor, cameraPose, intrinsics); - _integrate( - voxelSize, volumeUnit.pose, raycastStepFactor, - truncDist, maxWeight, volumeDims, volumeUnit.volDims, - depth, depthFactor, cameraPose, intrinsics, - pixNorms, volumes.row(volumeUnit.index)); - /* - float voxelSize, cv::Matx44f _pose, float raycastStepFactor, - float _truncDist, int _maxWeight, Point3i volResolution, Vec4i volDims, - InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume - */ + _integrate( volumeUnit.pose, volumeDims, volumeUnit.volDims, depth, + depthFactor, cameraPose, intrinsics, volumes.row(volumeUnit.index)); + //! Ensure all active volumeUnits are set to inactive for next integration volumeUnit.isActive = false; } @@ -499,7 +470,7 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const cvFloor(point.z * voxelSizeInv)); } -TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, +inline TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, Point3i volResolution, Vec4i volDims) { //! Out of bounds @@ -588,12 +559,9 @@ inline TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, Vo dummy.weight = 0; return dummy; } - //Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; // expanding at(), removing bounds check - //const TsdfVoxel* volData = volumeUnit->volume.ptr(); const TsdfVoxel* volData = vol.row(it->second.index).ptr(); Vec4i volDims = it->second.volDims; int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; @@ -671,7 +639,6 @@ float HashTSDFVolumeCPU::interpolateVoxelPoint(const Point3f& point) const iterMap[dictIdx] = it; queried[dictIdx] = true; } - //VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); vx[i] = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf; } @@ -734,7 +701,6 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const iterMap[dictIdx] = it; queried[dictIdx] = true; } - //VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf); } @@ -887,8 +853,6 @@ struct HashRaycastInvoker : ParallelLoopBody //! Does the subvolume exist in hashtable if (it != volume.volumeUnits.end()) { - //currVolumeUnit = - // std::dynamic_pointer_cast(it->second.pVolume); cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); @@ -969,18 +933,14 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { totalVolUnits.push_back(keyvalue.first); } - Range range(0, (int)totalVolUnits.size()); + Range fetchRange(0, (int)totalVolUnits.size()); const int nstripes = -1; const HashTSDFVolumeCPU& volume(*this); - //std::vector totalVolUnits; - //std::vector>& pVecs; - //std::vector>& nVecs; - const TsdfVoxel* volDataStart; bool needNormals(_normals.needed()); Mutex mutex; - auto _HashFetchPointsNormalsInvoker = [&](const Range& range) + auto HashFetchPointsNormalsInvoker = [&](const Range& range) { std::vector points, normals; @@ -992,10 +952,6 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); if (it != volume.volumeUnits.end()) { - //cv::Ptr volumeUnit = - // std::dynamic_pointer_cast(it->second.pVolume); - //cv::Ptr<_NewVolume> volumeUnit = - // std::dynamic_pointer_cast<_NewVolume>(it->second.pVolume); std::vector localPoints; std::vector localNormals; for (int x = 0; x < volume.volumeUnitResolution; x++) @@ -1003,7 +959,6 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - //TsdfVoxel voxel = volumeUnit->at(voxelIdx, it->second.volume); TsdfVoxel voxel = _at(voxelIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); if (voxel.tsdf != -128 && voxel.weight != 0) @@ -1025,9 +980,7 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor } }; - - //parallel_for_(range, fi, nstripes); - parallel_for_(range, _HashFetchPointsNormalsInvoker, nstripes); + parallel_for_(fetchRange, HashFetchPointsNormalsInvoker, nstripes); std::vector points, normals; diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 53a990233ba..8609a0fb7c7 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -82,11 +82,9 @@ typedef std::unordered_map VolumeUnitIndexes; class HashTSDFVolumeCPU : public HashTSDFVolume { private: - void _integrate( - float voxelSize, cv::Matx44f _pose, float raycastStepFactor, - float _truncDist, int _maxWeight, Point3i volResolution, Vec4i volDims, + void _integrate( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); + const cv::kinfu::Intr& intrinsics, InputArray _volume); public: // dimension in voxels, size in meters HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, @@ -129,8 +127,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume VolumeUnitIndexes volumeUnits; cv::Mat volumes; volumeIndex HashS; - //! Hashtable of individual smaller volume units - //VolumeUnitMap volumeUnits; + }; cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, From 4b507718874b0ea9cfe2483e260e9f7ac8aa81b5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 8 Oct 2020 12:35:25 +0300 Subject: [PATCH 05/53] create lambda raycast --- modules/rgbd/src/hash_tsdf.cpp | 95 ++++++++++++++-------------------- 1 file changed, 39 insertions(+), 56 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d487e30f457..d82bc0f3be1 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -789,26 +789,34 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const return nv < 0.0001f ? nan3 : normal / nv; } -struct HashRaycastInvoker : ParallelLoopBody +void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, + cv::Size frameSize, cv::OutputArray _points, + cv::OutputArray _normals) const { - HashRaycastInvoker(Points& _points, Normals& _normals, const Matx44f& cameraPose, - const Intr& intrinsics, const HashTSDFVolumeCPU& _volume) - : ParallelLoopBody(), - points(_points), - normals(_normals), - volume(_volume), - tstep(_volume.truncDist * _volume.raycastStepFactor), - cam2vol(volume.pose.inv() * Affine3f(cameraPose)), - vol2cam(Affine3f(cameraPose.inv()) * volume.pose), - reproj(intrinsics.makeReprojector()) - { - } + CV_TRACE_FUNCTION(); + CV_Assert(frameSize.area() > 0); + + _points.create(frameSize, POINT_TYPE); + _normals.create(frameSize, POINT_TYPE); + + Points points1 = _points.getMat(); + Normals normals1 = _normals.getMat(); + + Points& points(points1); + Normals& normals(normals1); + const HashTSDFVolumeCPU& volume(*this); + const float tstep(volume.truncDist * volume.raycastStepFactor); + const Affine3f cam2vol(volume.pose.inv() * Affine3f(cameraPose)); + const Affine3f vol2cam(Affine3f(cameraPose.inv()) * volume.pose); + const Intr::Reprojector reproj(intrinsics.makeReprojector()); + + const int nstripes = -1; - virtual void operator()(const Range& range) const override + auto _HashRaycastInvoker = [&](const Range& range) { const Point3f cam2volTrans = cam2vol.translation(); - const Matx33f cam2volRot = cam2vol.rotation(); - const Matx33f vol2camRot = vol2cam.rotation(); + const Matx33f cam2volRot = cam2vol.rotation(); + const Matx33f vol2camRot = vol2cam.rotation(); const float blockSize = volume.volumeUnitSize; @@ -827,27 +835,27 @@ struct HashRaycastInvoker : ParallelLoopBody Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); - float tmin = 0; - float tmax = volume.truncateThreshold; + float tmin = 0; + float tmax = volume.truncateThreshold; float tcurr = tmin; cv::Vec3i prevVolumeUnitIdx = cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), - std::numeric_limits::min()); + std::numeric_limits::min()); - float tprev = tcurr; + float tprev = tcurr; float prevTsdf = volume.truncDist; cv::Ptr currVolumeUnit; while (tcurr < tmax) { - Point3f currRayPos = orig + tcurr * rayDirV; + Point3f currRayPos = orig + tcurr * rayDirV; cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); float currTsdf = prevTsdf; - int currWeight = 0; - float stepSize = 0.5f * blockSize; + int currWeight = 0; + float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; //! Does the subvolume exist in hashtable @@ -859,9 +867,9 @@ struct HashRaycastInvoker : ParallelLoopBody //! TODO: Figure out voxel interpolation TsdfVoxel currVoxel = _at(volUnitLocalIdx, volume.volumes.row(it->second.index), volume.volumeDims, it->second.volDims); - currTsdf = tsdfToFloat(currVoxel.tsdf); - currWeight = currVoxel.weight; - stepSize = tstep; + currTsdf = tsdfToFloat(currVoxel.tsdf); + currWeight = currVoxel.weight; + stepSize = tstep; } //! Surface crossing if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) @@ -876,48 +884,23 @@ struct HashRaycastInvoker : ParallelLoopBody if (!isNaN(nv)) { normal = vol2camRot * nv; - point = vol2cam * pv; + point = vol2cam * pv; } } break; } prevVolumeUnitIdx = currVolumeUnitIdx; - prevTsdf = currTsdf; - tprev = tcurr; + prevTsdf = currTsdf; + tprev = tcurr; tcurr += stepSize; } ptsRow[x] = toPtype(point); nrmRow[x] = toPtype(normal); } } - } - - Points& points; - Normals& normals; - const HashTSDFVolumeCPU& volume; - const float tstep; - const Affine3f cam2vol; - const Affine3f vol2cam; - const Intr::Reprojector reproj; -}; - -void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray _points, - cv::OutputArray _normals) const -{ - CV_TRACE_FUNCTION(); - CV_Assert(frameSize.area() > 0); - - _points.create(frameSize, POINT_TYPE); - _normals.create(frameSize, POINT_TYPE); - - Points points = _points.getMat(); - Normals normals = _normals.getMat(); - - HashRaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); + }; - const int nstripes = -1; - parallel_for_(Range(0, points.rows), ri, nstripes); + parallel_for_(Range(0, points.rows), _HashRaycastInvoker, nstripes); } void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const From 5dcda08a620274dd2efd3aad5aabb2b709c8458d Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 8 Oct 2020 15:06:18 +0300 Subject: [PATCH 06/53] reduce time raycast --- modules/rgbd/src/hash_tsdf.cpp | 29 ++++++++++++++++++++++++++--- modules/rgbd/src/hash_tsdf.hpp | 2 ++ 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d82bc0f3be1..59b19ec592e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -550,7 +550,7 @@ static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) } inline TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, - VolumeUnitIndexes::const_iterator vend, cv::Mat vol, int unitRes) + VolumeUnitIndexes::const_iterator vend, cv::Mat vol, int unitRes) { if (it == vend) { @@ -568,6 +568,27 @@ inline TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, Vo return volData[coordBase]; } +TsdfVoxel HashTSDFVolumeCPU::_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, + VolumeUnitIndexes::const_iterator vend, int unitRes) const +{ + if (it == vend) + { + TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.f); + dummy.weight = 0; + return dummy; + } + Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; + + // expanding at(), removing bounds check + const TsdfVoxel* volData = volumes.row(it->second.index).ptr(); + Vec4i volDims = it->second.volDims; + int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; + return volData[coordBase]; +} + + + #if USE_INTRINSICS inline float interpolate(float tx, float ty, float tz, float vx[8]) { @@ -640,7 +661,8 @@ float HashTSDFVolumeCPU::interpolateVoxelPoint(const Point3f& point) const queried[dictIdx] = true; } - vx[i] = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf; + //vx[i] = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf; + vx[i] = _atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf; } return interpolate(tx, ty, tz, vx); @@ -702,7 +724,8 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const queried[dictIdx] = true; } - vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf); + //vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf); + vals[i] = tsdfToFloat(_atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf); } #if !USE_INTERPOLATION_IN_GETNORMAL diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 8609a0fb7c7..0ed5657db78 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -108,6 +108,8 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) virtual TsdfVoxel at(const cv::Point3f& point) const; + TsdfVoxel HashTSDFVolumeCPU::_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, + VolumeUnitIndexes::const_iterator vend, int unitRes) const; float interpolateVoxelPoint(const Point3f& point) const; inline float interpolateVoxel(const cv::Point3f& point) const; From a9acd1b2ef3d8a715b01a6bdeb64b50b2e3afced Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 8 Oct 2020 15:13:17 +0300 Subject: [PATCH 07/53] minor fix --- modules/rgbd/src/hash_tsdf.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 0ed5657db78..905035ef859 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -108,7 +108,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) virtual TsdfVoxel at(const cv::Point3f& point) const; - TsdfVoxel HashTSDFVolumeCPU::_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, + TsdfVoxel _atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, VolumeUnitIndexes::const_iterator vend, int unitRes) const; float interpolateVoxelPoint(const Point3f& point) const; From c329cdfafe763640224325180476ff3e99fb2fd0 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 08:57:01 +0300 Subject: [PATCH 08/53] minor fix volDims --- modules/rgbd/src/hash_tsdf.cpp | 31 ++++++++++++++++--------------- modules/rgbd/src/hash_tsdf.hpp | 1 + 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 59b19ec592e..ba639e898fd 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -49,6 +49,20 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca volumeDims(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution) { truncDist = std::max(_truncDist, 4.0f * voxelSize); + int xdim, ydim, zdim; + if (zFirstMemOrder) + { + xdim = volumeDims.z * volumeDims.y; + ydim = volumeDims.z; + zdim = 1; + } + else + { + xdim = 1; + ydim = volumeDims.x; + zdim = volumeDims.x * volumeDims.y; + } + volDims = Vec4i(xdim, ydim, zdim); } HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, @@ -349,21 +363,8 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma { VolumeUnit& vu = volumeUnits[idx]; Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; - - int xdim, ydim, zdim; - if (zFirstMemOrder) - { - xdim = volumeDims.z * volumeDims.y; - ydim = volumeDims.z; - zdim = 1; - } - else - { - xdim = 1; - ydim = volumeDims.x; - zdim = volumeDims.x * volumeDims.y; - } - vu.volDims = Vec4i(xdim, ydim, zdim); + + vu.volDims = volDims; vu.pose = subvolumePose; volumes.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 905035ef859..d20a731bdcd 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -34,6 +34,7 @@ class HashTSDFVolume : public Volume float volumeUnitSize; bool zFirstMemOrder; Point3i volumeDims; + Vec4i volDims; }; /* struct VolumeUnit From fcc2238a21939d7041c712803ceb68944235723e Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 10:00:44 +0300 Subject: [PATCH 09/53] changed _atVolumeUnit, reduce memory consumption --- modules/rgbd/src/hash_tsdf.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ba639e898fd..b955ca14b4a 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -369,6 +369,11 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma vu.pose = subvolumePose; volumes.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); vu.index = HashS; HashS++; + //volumes.ptr(vu.index)->forEach([](VecTsdfVoxel& vv, const int*) + // { + // TsdfVoxel& v = reinterpret_cast(vv); + // v.tsdf = floatToTsdf(0.0f); v.weight = 0; + // }); volumes.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) { TsdfVoxel& v = reinterpret_cast(vv); @@ -582,10 +587,10 @@ TsdfVoxel HashTSDFVolumeCPU::_atVolumeUnit(const Vec3i& point, const Vec3i& volu Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; // expanding at(), removing bounds check - const TsdfVoxel* volData = volumes.row(it->second.index).ptr(); + const TsdfVoxel* volData = volumes.ptr(); Vec4i volDims = it->second.volDims; int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; - return volData[coordBase]; + return volData[it->second.index, coordBase]; } From fe337974df243e8f5115cf7ffe9efe3b4a48de71 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 10:12:54 +0300 Subject: [PATCH 10/53] delete older inmplemetation of atVolumeUnit --- modules/rgbd/src/hash_tsdf.cpp | 27 +++------------------------ modules/rgbd/src/hash_tsdf.hpp | 2 +- 2 files changed, 4 insertions(+), 25 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b955ca14b4a..835d5a95a85 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -555,26 +555,7 @@ static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) } } -inline TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, - VolumeUnitIndexes::const_iterator vend, cv::Mat vol, int unitRes) -{ - if (it == vend) - { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.f); - dummy.weight = 0; - return dummy; - } - Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; - - // expanding at(), removing bounds check - const TsdfVoxel* volData = vol.row(it->second.index).ptr(); - Vec4i volDims = it->second.volDims; - int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; - return volData[coordBase]; -} - -TsdfVoxel HashTSDFVolumeCPU::_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, +TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, VolumeUnitIndexes::const_iterator vend, int unitRes) const { if (it == vend) @@ -667,8 +648,7 @@ float HashTSDFVolumeCPU::interpolateVoxelPoint(const Point3f& point) const queried[dictIdx] = true; } - //vx[i] = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf; - vx[i] = _atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf; + vx[i] = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf; } return interpolate(tx, ty, tz, vx); @@ -730,8 +710,7 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const queried[dictIdx] = true; } - //vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumes, volumeUnitResolution).tsdf); - vals[i] = tsdfToFloat(_atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf); + vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf); } #if !USE_INTERPOLATION_IN_GETNORMAL diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index d20a731bdcd..ba4085ef3da 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -109,7 +109,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) virtual TsdfVoxel at(const cv::Point3f& point) const; - TsdfVoxel _atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, + TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, VolumeUnitIndexes::const_iterator vend, int unitRes) const; float interpolateVoxelPoint(const Point3f& point) const; From 82303c044833f259517a4364ee9ebdea9e123edc Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 10:29:14 +0300 Subject: [PATCH 11/53] changes _at --- modules/rgbd/src/hash_tsdf.cpp | 30 +++++++++++++++++++++++++----- modules/rgbd/src/hash_tsdf.hpp | 3 +++ 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 835d5a95a85..a6cb8c237fd 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -475,7 +475,7 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } - +/* inline TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, Point3i volResolution, Vec4i volDims) { @@ -495,6 +495,26 @@ inline TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; return volData[coordBase]; } +*/ +inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex indx, + Point3i volResolution, Vec4i volDims) const +{ + //! Out of bounds + if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || + (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || + (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) + { + TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.0f); + dummy.weight = 0; + return dummy; + } + //Mat volume = _volume.getMat(); + const TsdfVoxel* volData = volumes.ptr(); + int coordBase = + volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; + return volData[indx, coordBase]; +} inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { @@ -517,7 +537,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); + return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); } inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const @@ -536,7 +556,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); + return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); } static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) @@ -874,7 +894,7 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = _at(volUnitLocalIdx, volume.volumes.row(it->second.index), volume.volumeDims, it->second.volDims); + TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volumeDims, it->second.volDims); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -950,7 +970,7 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = _at(voxelIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); + TsdfVoxel voxel = _at(voxelIdx, it->second.index, volumeDims, it->second.volDims); if (voxel.tsdf != -128 && voxel.weight != 0) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index ba4085ef3da..cb4f89c5376 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -109,6 +109,9 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) virtual TsdfVoxel at(const cv::Point3f& point) const; + virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, volumeIndex indx, + Point3i volResolution, Vec4i volDims) const; + TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, VolumeUnitIndexes::const_iterator vend, int unitRes) const; From 31a7593ad336cef6de471374703d6822ba92f14d Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 10:48:13 +0300 Subject: [PATCH 12/53] AAA, I want to cry! --- modules/rgbd/src/hash_tsdf.cpp | 30 +++++++++++++++++++++--------- modules/rgbd/src/hash_tsdf.hpp | 4 ++-- 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a6cb8c237fd..03012980552 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -475,7 +475,7 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } -/* +///* inline TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, Point3i volResolution, Vec4i volDims) { @@ -495,7 +495,8 @@ inline TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; return volData[coordBase]; } -*/ +//*/ +/* inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex indx, Point3i volResolution, Vec4i volDims) const { @@ -515,7 +516,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; return volData[indx, coordBase]; } - +*/ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), @@ -537,7 +538,8 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); + //return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); + return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const @@ -556,7 +558,8 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); + //return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); + return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) @@ -588,10 +591,17 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; // expanding at(), removing bounds check - const TsdfVoxel* volData = volumes.ptr(); + //const TsdfVoxel* volData = volumes.ptr(); + //Vec4i volDims = it->second.volDims; + //int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; + //return volData[it->second.index, coordBase]; + + const TsdfVoxel* volData = volumes.row(it->second.index).ptr(); Vec4i volDims = it->second.volDims; int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; - return volData[it->second.index, coordBase]; + return volData[coordBase]; + + } @@ -894,7 +904,8 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volumeDims, it->second.volDims); + //TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volumeDims, it->second.volDims); + TsdfVoxel currVoxel = _at(volUnitLocalIdx, volume.volumes.row(it->second.index), volume.volumeDims, it->second.volDims); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -970,7 +981,8 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = _at(voxelIdx, it->second.index, volumeDims, it->second.volDims); + //TsdfVoxel voxel = _at(voxelIdx, it->second.index, volumeDims, it->second.volDims); + TsdfVoxel voxel = _at(voxelIdx, volume.volumes.row(it->second.index), volumeDims, it->second.volDims); if (voxel.tsdf != -128 && voxel.weight != 0) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index cb4f89c5376..3bea9caec69 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -109,8 +109,8 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) virtual TsdfVoxel at(const cv::Point3f& point) const; - virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, volumeIndex indx, - Point3i volResolution, Vec4i volDims) const; + //virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, volumeIndex indx, + // Point3i volResolution, Vec4i volDims) const; TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, VolumeUnitIndexes::const_iterator vend, int unitRes) const; From 186e0725491b6d00716f6bca31a4fc11bbe626b5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 10:58:18 +0300 Subject: [PATCH 13/53] it works! --- modules/rgbd/src/hash_tsdf.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 03012980552..32f178e2ef9 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -591,17 +591,10 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; // expanding at(), removing bounds check - //const TsdfVoxel* volData = volumes.ptr(); - //Vec4i volDims = it->second.volDims; - //int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; - //return volData[it->second.index, coordBase]; - - const TsdfVoxel* volData = volumes.row(it->second.index).ptr(); + const TsdfVoxel* volData = volumes.ptr(it->second.index); Vec4i volDims = it->second.volDims; int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; return volData[coordBase]; - - } From 040c6a8843d2385917e35930cf875ecaabc94c64 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 11:03:22 +0300 Subject: [PATCH 14/53] it works twice o_o --- modules/rgbd/src/hash_tsdf.cpp | 28 ++++++++++++++-------------- modules/rgbd/src/hash_tsdf.hpp | 4 ++-- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 32f178e2ef9..f74b6d8db31 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -475,7 +475,7 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } -///* +/* inline TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, Point3i volResolution, Vec4i volDims) { @@ -495,8 +495,8 @@ inline TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; return volData[coordBase]; } -//*/ -/* +*/ + inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex indx, Point3i volResolution, Vec4i volDims) const { @@ -511,12 +511,12 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex return dummy; } //Mat volume = _volume.getMat(); - const TsdfVoxel* volData = volumes.ptr(); + const TsdfVoxel* volData = volumes.ptr(indx); int coordBase = volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; - return volData[indx, coordBase]; + return volData[coordBase]; } -*/ + inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), @@ -538,8 +538,8 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - //return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); - return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); + return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); + //return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const @@ -558,8 +558,8 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - //return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); - return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); + return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); + //return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) @@ -897,8 +897,8 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - //TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volumeDims, it->second.volDims); - TsdfVoxel currVoxel = _at(volUnitLocalIdx, volume.volumes.row(it->second.index), volume.volumeDims, it->second.volDims); + TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volumeDims, it->second.volDims); + //TsdfVoxel currVoxel = _at(volUnitLocalIdx, volume.volumes.row(it->second.index), volume.volumeDims, it->second.volDims); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -974,8 +974,8 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - //TsdfVoxel voxel = _at(voxelIdx, it->second.index, volumeDims, it->second.volDims); - TsdfVoxel voxel = _at(voxelIdx, volume.volumes.row(it->second.index), volumeDims, it->second.volDims); + TsdfVoxel voxel = _at(voxelIdx, it->second.index, volumeDims, it->second.volDims); + //TsdfVoxel voxel = _at(voxelIdx, volume.volumes.row(it->second.index), volumeDims, it->second.volDims); if (voxel.tsdf != -128 && voxel.weight != 0) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 3bea9caec69..cb4f89c5376 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -109,8 +109,8 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) virtual TsdfVoxel at(const cv::Point3f& point) const; - //virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, volumeIndex indx, - // Point3i volResolution, Vec4i volDims) const; + virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, volumeIndex indx, + Point3i volResolution, Vec4i volDims) const; TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, VolumeUnitIndexes::const_iterator vend, int unitRes) const; From 4bbf818dd42458cf2a317328e8de0cbcbcb6e949 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 11:23:25 +0300 Subject: [PATCH 15/53] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- modules/rgbd/src/hash_tsdf.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index f74b6d8db31..db472878238 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -180,7 +180,7 @@ static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) } } -void HashTSDFVolumeCPU::_integrate( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, +void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _volume) { @@ -443,7 +443,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 - _integrate( volumeUnit.pose, volumeDims, volumeUnit.volDims, depth, + integrateVolumeUnit(volumeUnit.pose, volumeDims, volumeUnit.volDims, depth, depthFactor, cameraPose, intrinsics, volumes.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 cb4f89c5376..ae94dd5c7e2 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -83,7 +83,7 @@ typedef std::unordered_map VolumeUnitIndexes; class HashTSDFVolumeCPU : public HashTSDFVolume { private: - void _integrate( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, + void integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _volume); public: From f4b52cc90556bc8ae111be2b93aa1bf5004d4625 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 11:49:13 +0300 Subject: [PATCH 16/53] new adding to volumes --- modules/rgbd/src/hash_tsdf.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index db472878238..c3542fcd6d8 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -17,7 +17,7 @@ #include "utils.hpp" #define USE_INTERPOLATION_IN_GETNORMAL 1 - +#define VOLUMES_SIZE 32 namespace cv { @@ -72,13 +72,14 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { HashS = 0; + volumes = cv::Mat(VOLUMES_SIZE, volumeDims.x * volumeDims.y * volumeDims.z, rawType()); } // zero volume, leave rest params the same void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); - volumes = cv::Mat(); + volumes = cv::Mat(VOLUMES_SIZE, volumeDims.x * volumeDims.y * volumeDims.z, rawType()); HashS = 0; } @@ -367,13 +368,18 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma vu.volDims = volDims; vu.pose = subvolumePose; - volumes.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); + //volumes.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); vu.index = HashS; HashS++; //volumes.ptr(vu.index)->forEach([](VecTsdfVoxel& vv, const int*) // { // TsdfVoxel& v = reinterpret_cast(vv); // v.tsdf = floatToTsdf(0.0f); v.weight = 0; // }); + if (HashS > volumes.size().height) + { + for (int i = 0; i < VOLUMES_SIZE; i++) + volumes.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); + } volumes.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) { TsdfVoxel& v = reinterpret_cast(vv); From 30a64a205b6f64ee08389c706a56abd5b5af785c Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 12:02:59 +0300 Subject: [PATCH 17/53] delete volDims at strust VolumeUnit --- modules/rgbd/src/hash_tsdf.cpp | 13 +++++-------- modules/rgbd/src/hash_tsdf.hpp | 1 - 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index c3542fcd6d8..506fef69f62 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -364,8 +364,6 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma { VolumeUnit& vu = volumeUnits[idx]; Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; - - vu.volDims = volDims; vu.pose = subvolumePose; //volumes.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); @@ -449,7 +447,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(volumeUnit.pose, volumeDims, volumeUnit.volDims, depth, + integrateVolumeUnit(volumeUnit.pose, volumeDims, volDims, depth, depthFactor, cameraPose, intrinsics, volumes.row(volumeUnit.index)); //! Ensure all active volumeUnits are set to inactive for next integration @@ -544,7 +542,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); + return _at(volUnitLocalIdx, it->second.index, volumeDims, volDims); //return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } @@ -564,7 +562,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volumeDims, it->second.volDims); + return _at(volUnitLocalIdx, it->second.index, volumeDims, volDims); //return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } @@ -598,7 +596,6 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum // expanding at(), removing bounds check const TsdfVoxel* volData = volumes.ptr(it->second.index); - Vec4i volDims = it->second.volDims; int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; return volData[coordBase]; } @@ -903,7 +900,7 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volumeDims, it->second.volDims); + TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volumeDims, volDims); //TsdfVoxel currVoxel = _at(volUnitLocalIdx, volume.volumes.row(it->second.index), volume.volumeDims, it->second.volDims); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; @@ -980,7 +977,7 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = _at(voxelIdx, it->second.index, volumeDims, it->second.volDims); + TsdfVoxel voxel = _at(voxelIdx, it->second.index, volumeDims, volDims); //TsdfVoxel voxel = _at(voxelIdx, volume.volumes.row(it->second.index), volumeDims, it->second.volDims); if (voxel.tsdf != -128 && voxel.weight != 0) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index ae94dd5c7e2..de16c5d8ba2 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -71,7 +71,6 @@ struct VolumeUnit cv::Vec3i coord; volumeIndex index; cv::Matx44f pose; - Vec4i volDims; bool isActive; }; From 864d755bb648ccac5a08b4e67e50498ecf919873 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 12:30:31 +0300 Subject: [PATCH 18/53] new names of vars --- modules/rgbd/src/hash_tsdf.cpp | 27 +++++++++++---------------- modules/rgbd/src/hash_tsdf.hpp | 4 ++-- 2 files changed, 13 insertions(+), 18 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 506fef69f62..942b8815420 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -71,16 +71,16 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float : HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { - HashS = 0; - volumes = cv::Mat(VOLUMES_SIZE, volumeDims.x * volumeDims.y * volumeDims.z, rawType()); + lastVolIndex = 0; + volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volumeDims.x * volumeDims.y * volumeDims.z, rawType()); } // zero volume, leave rest params the same void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); - volumes = cv::Mat(VOLUMES_SIZE, volumeDims.x * volumeDims.y * volumeDims.z, rawType()); - HashS = 0; + lastVolIndex = 0; + volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volumeDims.x * volumeDims.y * volumeDims.z, rawType()); } static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) @@ -366,19 +366,18 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; vu.pose = subvolumePose; - //volumes.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); - vu.index = HashS; HashS++; + vu.index = lastVolIndex; lastVolIndex++; //volumes.ptr(vu.index)->forEach([](VecTsdfVoxel& vv, const int*) // { // TsdfVoxel& v = reinterpret_cast(vv); // v.tsdf = floatToTsdf(0.0f); v.weight = 0; // }); - if (HashS > volumes.size().height) + if (lastVolIndex > volUnitsMatrix.size().height) { for (int i = 0; i < VOLUMES_SIZE; i++) - volumes.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); + volUnitsMatrix.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); } - volumes.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) + volUnitsMatrix.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) { TsdfVoxel& v = reinterpret_cast(vv); v.tsdf = floatToTsdf(0.0f); v.weight = 0; @@ -448,7 +447,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma { //! The volume unit should already be added into the Volume from the allocator integrateVolumeUnit(volumeUnit.pose, volumeDims, volDims, depth, - depthFactor, cameraPose, intrinsics, volumes.row(volumeUnit.index)); + depthFactor, cameraPose, intrinsics, volUnitsMatrix.row(volumeUnit.index)); //! Ensure all active volumeUnits are set to inactive for next integration volumeUnit.isActive = false; @@ -515,7 +514,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex return dummy; } //Mat volume = _volume.getMat(); - const TsdfVoxel* volData = volumes.ptr(indx); + const TsdfVoxel* volData = volUnitsMatrix.ptr(indx); int coordBase = volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; return volData[coordBase]; @@ -543,7 +542,6 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); return _at(volUnitLocalIdx, it->second.index, volumeDims, volDims); - //return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const @@ -563,7 +561,6 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); return _at(volUnitLocalIdx, it->second.index, volumeDims, volDims); - //return _at(volUnitLocalIdx, volumes.row(it->second.index), volumeDims, it->second.volDims); } static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) @@ -595,7 +592,7 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; // expanding at(), removing bounds check - const TsdfVoxel* volData = volumes.ptr(it->second.index); + const TsdfVoxel* volData = volUnitsMatrix.ptr(it->second.index); int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; return volData[coordBase]; } @@ -901,7 +898,6 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: //! TODO: Figure out voxel interpolation TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volumeDims, volDims); - //TsdfVoxel currVoxel = _at(volUnitLocalIdx, volume.volumes.row(it->second.index), volume.volumeDims, it->second.volDims); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -978,7 +974,6 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { cv::Vec3i voxelIdx(x, y, z); TsdfVoxel voxel = _at(voxelIdx, it->second.index, volumeDims, volDims); - //TsdfVoxel voxel = _at(voxelIdx, volume.volumes.row(it->second.index), volumeDims, it->second.volDims); if (voxel.tsdf != -128 && voxel.weight != 0) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index de16c5d8ba2..31b5db1e423 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -130,8 +130,8 @@ class HashTSDFVolumeCPU : public HashTSDFVolume Mat pixNorms; //VolumeUnitIndexes volumeUnitIndexes; VolumeUnitIndexes volumeUnits; - cv::Mat volumes; - volumeIndex HashS; + cv::Mat volUnitsMatrix; + volumeIndex lastVolIndex; }; cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Matx44f _pose, From eeb7e79be8421184c1e6457bd383ef28773edb84 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 9 Oct 2020 15:34:55 +0300 Subject: [PATCH 19/53] rename one var --- modules/rgbd/src/hash_tsdf.cpp | 26 +++++++++++++------------- modules/rgbd/src/hash_tsdf.hpp | 2 +- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 942b8815420..0bf876ac235 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -46,21 +46,21 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca volumeUnitResolution(_volumeUnitRes), volumeUnitSize(voxelSize* volumeUnitResolution), zFirstMemOrder(_zFirstMemOrder), - volumeDims(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution) + volStrides(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution) { truncDist = std::max(_truncDist, 4.0f * voxelSize); int xdim, ydim, zdim; if (zFirstMemOrder) { - xdim = volumeDims.z * volumeDims.y; - ydim = volumeDims.z; + xdim = volStrides.z * volStrides.y; + ydim = volStrides.z; zdim = 1; } else { xdim = 1; - ydim = volumeDims.x; - zdim = volumeDims.x * volumeDims.y; + ydim = volStrides.x; + zdim = volStrides.x * volStrides.y; } volDims = Vec4i(xdim, ydim, zdim); } @@ -72,7 +72,7 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { lastVolIndex = 0; - volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volumeDims.x * volumeDims.y * volumeDims.z, rawType()); + volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volStrides.x * volStrides.y * volStrides.z, rawType()); } // zero volume, leave rest params the same @@ -80,7 +80,7 @@ void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); lastVolIndex = 0; - volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volumeDims.x * volumeDims.y * volumeDims.z, rawType()); + volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volStrides.x * volStrides.y * volStrides.z, rawType()); } static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) @@ -375,7 +375,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma if (lastVolIndex > volUnitsMatrix.size().height) { for (int i = 0; i < VOLUMES_SIZE; i++) - volUnitsMatrix.push_back(Mat(1, volumeDims.x * volumeDims.y * volumeDims.z, rawType())); + volUnitsMatrix.push_back(Mat(1, volStrides.x * volStrides.y * volStrides.z, rawType())); } volUnitsMatrix.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) { @@ -446,7 +446,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(volumeUnit.pose, volumeDims, volDims, depth, + integrateVolumeUnit(volumeUnit.pose, volStrides, volDims, depth, depthFactor, cameraPose, intrinsics, volUnitsMatrix.row(volumeUnit.index)); //! Ensure all active volumeUnits are set to inactive for next integration @@ -541,7 +541,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volumeDims, volDims); + return _at(volUnitLocalIdx, it->second.index, volStrides, volDims); } inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const @@ -560,7 +560,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volumeDims, volDims); + return _at(volUnitLocalIdx, it->second.index, volStrides, volDims); } static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) @@ -897,7 +897,7 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volumeDims, volDims); + TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volStrides, volDims); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -973,7 +973,7 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = _at(voxelIdx, it->second.index, volumeDims, volDims); + TsdfVoxel voxel = _at(voxelIdx, it->second.index, volStrides, volDims); if (voxel.tsdf != -128 && voxel.weight != 0) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 31b5db1e423..5b5802b9371 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -33,7 +33,7 @@ class HashTSDFVolume : public Volume int volumeUnitResolution; float volumeUnitSize; bool zFirstMemOrder; - Point3i volumeDims; + Point3i volStrides; Vec4i volDims; }; /* From 0eeb2081567dcb83511673385b5fa509d2be7e53 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 12 Oct 2020 09:31:11 +0300 Subject: [PATCH 20/53] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 63 ++++++++++++---------------------- modules/rgbd/src/hash_tsdf.hpp | 4 +-- 2 files changed, 23 insertions(+), 44 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 0bf876ac235..7e474ac08bb 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -46,23 +46,23 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca volumeUnitResolution(_volumeUnitRes), volumeUnitSize(voxelSize* volumeUnitResolution), zFirstMemOrder(_zFirstMemOrder), - volStrides(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution) + volDims(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution) { truncDist = std::max(_truncDist, 4.0f * voxelSize); int xdim, ydim, zdim; if (zFirstMemOrder) { - xdim = volStrides.z * volStrides.y; - ydim = volStrides.z; + xdim = volDims.z * volDims.y; + ydim = volDims.z; zdim = 1; } else { xdim = 1; - ydim = volStrides.x; - zdim = volStrides.x * volStrides.y; + ydim = volDims.x; + zdim = volDims.x * volDims.y; } - volDims = Vec4i(xdim, ydim, zdim); + volStrides = Vec4i(xdim, ydim, zdim); } HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, @@ -72,7 +72,7 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { lastVolIndex = 0; - volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volStrides.x * volStrides.y * volStrides.z, rawType()); + volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); } // zero volume, leave rest params the same @@ -80,7 +80,7 @@ void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); lastVolIndex = 0; - volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volStrides.x * volStrides.y * volStrides.z, rawType()); + volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); } static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) @@ -181,7 +181,7 @@ static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) } } -void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, +void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides1, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _volume) { @@ -206,10 +206,10 @@ void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResol { for (int x = range.start; x < range.end; x++) { - TsdfVoxel* volDataX = volDataStart + x * volDims[0]; + TsdfVoxel* volDataX = volDataStart + x * volStrides1[0]; for (int y = 0; y < volResolution.y; y++) { - TsdfVoxel* volDataY = volDataX + y * volDims[1]; + TsdfVoxel* volDataY = volDataX + y * volStrides1[1]; // optimization of camSpace transformation (vector addition instead of matmul at each z) Point3f basePt = vol2cam * (Point3f(float(x), float(y), 0.0f) * voxelSize); Point3f camSpacePt = basePt; @@ -281,7 +281,7 @@ void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResol { TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv)); - TsdfVoxel& voxel = volDataY[z * volDims[2]]; + TsdfVoxel& voxel = volDataY[z * volStrides1[2]]; WeightType& weight = voxel.weight; TsdfType& value = voxel.tsdf; @@ -375,7 +375,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma if (lastVolIndex > volUnitsMatrix.size().height) { for (int i = 0; i < VOLUMES_SIZE; i++) - volUnitsMatrix.push_back(Mat(1, volStrides.x * volStrides.y * volStrides.z, rawType())); + volUnitsMatrix.push_back(Mat(1, volDims.x * volDims.y * volDims.z, rawType())); } volUnitsMatrix.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) { @@ -446,7 +446,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(volumeUnit.pose, volStrides, volDims, depth, + integrateVolumeUnit(volumeUnit.pose, volDims, volStrides, depth, depthFactor, cameraPose, intrinsics, volUnitsMatrix.row(volumeUnit.index)); //! Ensure all active volumeUnits are set to inactive for next integration @@ -478,30 +478,9 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } -/* -inline TsdfVoxel _at(const cv::Vec3i& volumeIdx, InputArray _volume, - Point3i volResolution, Vec4i volDims) -{ - //! Out of bounds - if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || - (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || - (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) - { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.0f); - dummy.weight = 0; - return dummy; - } - Mat volume = _volume.getMat(); - const TsdfVoxel* volData = volume.ptr(); - int coordBase = - volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; - return volData[coordBase]; -} -*/ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex indx, - Point3i volResolution, Vec4i volDims) const + Point3i volResolution, Vec4i volStrides1) const { //! Out of bounds if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || @@ -516,7 +495,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex //Mat volume = _volume.getMat(); const TsdfVoxel* volData = volUnitsMatrix.ptr(indx); int coordBase = - volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; + volumeIdx[0] * volStrides1[0] + volumeIdx[1] * volStrides1[1] + volumeIdx[2] * volStrides1[2]; return volData[coordBase]; } @@ -541,7 +520,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volStrides, volDims); + return _at(volUnitLocalIdx, it->second.index, volDims, volStrides); } inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const @@ -560,7 +539,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volStrides, volDims); + return _at(volUnitLocalIdx, it->second.index, volDims, volStrides); } static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) @@ -593,7 +572,7 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum // expanding at(), removing bounds check const TsdfVoxel* volData = volUnitsMatrix.ptr(it->second.index); - int coordBase = volUnitLocalIdx[0] * volDims[0] + volUnitLocalIdx[1] * volDims[1] + volUnitLocalIdx[2] * volDims[2]; + int coordBase = volUnitLocalIdx[0] * volStrides[0] + volUnitLocalIdx[1] * volStrides[1] + volUnitLocalIdx[2] * volStrides[2]; return volData[coordBase]; } @@ -897,7 +876,7 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volStrides, volDims); + TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volDims, volStrides); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -973,7 +952,7 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = _at(voxelIdx, it->second.index, volStrides, volDims); + TsdfVoxel voxel = _at(voxelIdx, it->second.index, volDims, volStrides); if (voxel.tsdf != -128 && voxel.weight != 0) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 5b5802b9371..e36b33693bc 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -33,8 +33,8 @@ class HashTSDFVolume : public Volume int volumeUnitResolution; float volumeUnitSize; bool zFirstMemOrder; - Point3i volStrides; - Vec4i volDims; + Point3i volDims; + Vec4i volStrides; }; /* struct VolumeUnit From 0f2d149f43a8ae2ced2fc407731bbf021ee4376a Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 12 Oct 2020 10:11:02 +0300 Subject: [PATCH 21/53] new resize volumes --- modules/rgbd/src/hash_tsdf.cpp | 14 ++++++-------- modules/rgbd/src/hash_tsdf.hpp | 13 ------------- 2 files changed, 6 insertions(+), 21 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 7e474ac08bb..c9e79bd7686 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -17,7 +17,7 @@ #include "utils.hpp" #define USE_INTERPOLATION_IN_GETNORMAL 1 -#define VOLUMES_SIZE 32 +#define VOLUMES_SIZE 512 namespace cv { @@ -299,6 +299,7 @@ void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResol void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { + std::cout << "integrate: " << std::endl; CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); @@ -367,15 +368,12 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma vu.pose = subvolumePose; vu.index = lastVolIndex; lastVolIndex++; - //volumes.ptr(vu.index)->forEach([](VecTsdfVoxel& vv, const int*) - // { - // TsdfVoxel& v = reinterpret_cast(vv); - // v.tsdf = floatToTsdf(0.0f); v.weight = 0; - // }); if (lastVolIndex > volUnitsMatrix.size().height) { - for (int i = 0; i < VOLUMES_SIZE; i++) - volUnitsMatrix.push_back(Mat(1, volDims.x * volDims.y * volDims.z, rawType())); + std::cout << " +" << std::endl; + volUnitsMatrix.resize(lastVolIndex - 1 + VOLUMES_SIZE); + //for (int i = 0; i < VOLUMES_SIZE; i++) + // volUnitsMatrix.push_back(Mat(1, volDims.x * volDims.y * volDims.z, rawType())); } volUnitsMatrix.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index e36b33693bc..16c16776909 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -36,17 +36,7 @@ class HashTSDFVolume : public Volume Point3i volDims; Vec4i volStrides; }; -/* -struct VolumeUnit -{ - VolumeUnit() : pVolume(nullptr){}; - ~VolumeUnit() = default; - cv::Ptr pVolume; - cv::Vec3i index; - bool isActive; -}; -*/ //! Spatial hashing struct tsdf_hash { @@ -62,9 +52,6 @@ struct tsdf_hash } }; - -//typedef std::unordered_map VolumeUnitMap; - typedef unsigned int volumeIndex; struct VolumeUnit { From 2ea082b9b664007ef173f231f45a8a283e323c21 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 12 Oct 2020 11:37:49 +0300 Subject: [PATCH 22/53] rename volUnitsMatrix --- modules/rgbd/src/hash_tsdf.cpp | 16 ++++++++-------- modules/rgbd/src/hash_tsdf.hpp | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index c9e79bd7686..59b1602ace6 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -72,7 +72,7 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { lastVolIndex = 0; - volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); + volUnitsData = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); } // zero volume, leave rest params the same @@ -80,7 +80,7 @@ void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); lastVolIndex = 0; - volUnitsMatrix = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); + volUnitsData = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); } static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) @@ -368,14 +368,14 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma vu.pose = subvolumePose; vu.index = lastVolIndex; lastVolIndex++; - if (lastVolIndex > volUnitsMatrix.size().height) + if (lastVolIndex > volUnitsData.size().height) { std::cout << " +" << std::endl; - volUnitsMatrix.resize(lastVolIndex - 1 + VOLUMES_SIZE); + volUnitsData.resize(lastVolIndex - 1 + VOLUMES_SIZE); //for (int i = 0; i < VOLUMES_SIZE; i++) // volUnitsMatrix.push_back(Mat(1, volDims.x * volDims.y * volDims.z, rawType())); } - volUnitsMatrix.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) + volUnitsData.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) { TsdfVoxel& v = reinterpret_cast(vv); v.tsdf = floatToTsdf(0.0f); v.weight = 0; @@ -445,7 +445,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma { //! The volume unit should already be added into the Volume from the allocator integrateVolumeUnit(volumeUnit.pose, volDims, volStrides, depth, - depthFactor, cameraPose, intrinsics, volUnitsMatrix.row(volumeUnit.index)); + depthFactor, cameraPose, intrinsics, volUnitsData.row(volumeUnit.index)); //! Ensure all active volumeUnits are set to inactive for next integration volumeUnit.isActive = false; @@ -491,7 +491,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex return dummy; } //Mat volume = _volume.getMat(); - const TsdfVoxel* volData = volUnitsMatrix.ptr(indx); + const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = volumeIdx[0] * volStrides1[0] + volumeIdx[1] * volStrides1[1] + volumeIdx[2] * volStrides1[2]; return volData[coordBase]; @@ -569,7 +569,7 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; // expanding at(), removing bounds check - const TsdfVoxel* volData = volUnitsMatrix.ptr(it->second.index); + const TsdfVoxel* volData = volUnitsData.ptr(it->second.index); int coordBase = volUnitLocalIdx[0] * volStrides[0] + volUnitLocalIdx[1] * volStrides[1] + volUnitLocalIdx[2] * volStrides[2]; return volData[coordBase]; } diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 16c16776909..69ec82cb8e9 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -117,7 +117,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume Mat pixNorms; //VolumeUnitIndexes volumeUnitIndexes; VolumeUnitIndexes volumeUnits; - cv::Mat volUnitsMatrix; + cv::Mat volUnitsData; volumeIndex lastVolIndex; }; From 8cce8b99b3bccd6b0dcc4480110a4f72e222bfd2 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 12 Oct 2020 12:41:26 +0300 Subject: [PATCH 23/53] minor fix in at function --- modules/rgbd/src/hash_tsdf.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 59b1602ace6..bc36ec5a3a5 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -17,7 +17,7 @@ #include "utils.hpp" #define USE_INTERPOLATION_IN_GETNORMAL 1 -#define VOLUMES_SIZE 512 +#define VOLUMES_SIZE 1024 namespace cv { @@ -299,7 +299,7 @@ void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResol void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { - std::cout << "integrate: " << std::endl; + //std::cout << "integrate: " << std::endl; CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); @@ -370,7 +370,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma vu.index = lastVolIndex; lastVolIndex++; if (lastVolIndex > volUnitsData.size().height) { - std::cout << " +" << std::endl; + //std::cout << " +" << std::endl; volUnitsData.resize(lastVolIndex - 1 + VOLUMES_SIZE); //for (int i = 0; i < VOLUMES_SIZE; i++) // volUnitsMatrix.push_back(Mat(1, volDims.x * volDims.y * volDims.z, rawType())); @@ -481,16 +481,18 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex Point3i volResolution, Vec4i volStrides1) const { //! Out of bounds + /* if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) { + std::cout << "ASSERT (_at)" << std::endl; TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); dummy.weight = 0; return dummy; } - //Mat volume = _volume.getMat(); + */ const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = volumeIdx[0] * volStrides1[0] + volumeIdx[1] * volStrides1[1] + volumeIdx[2] * volStrides1[2]; @@ -504,14 +506,16 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const cvFloor(volumeIdx[2] / volumeUnitResolution)); VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); + /* if (it == volumeUnits.end()) { + std::cout << "ASSERT (at Vec3i)" << std::endl; TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.f); dummy.weight = 0; return dummy; } - + */ cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, volumeUnitIdx[2] * volumeUnitResolution); @@ -525,14 +529,16 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const { cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); + /* if (it == volumeUnits.end()) { + std::cout << "ASSERT (at Point3f)" << std::endl; TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.f); dummy.weight = 0; return dummy; } - + */ cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = @@ -561,6 +567,7 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum { if (it == vend) { + //std::cout << "ASSERT (atVolumeUnit)" << std::endl; TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.f); dummy.weight = 0; From f2991f88c1ee800896e1a7ecc42a3b353db1a6b4 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 12 Oct 2020 16:38:25 +0300 Subject: [PATCH 24/53] add tsdf_functions.hpp --- modules/rgbd/src/hash_tsdf.cpp | 10 +- modules/rgbd/src/hash_tsdf.hpp | 2 +- modules/rgbd/src/tsdf.cpp | 14 +-- modules/rgbd/src/tsdf_functions.cpp | 133 +++++++++++++++++++++++ modules/rgbd/src/tsdf_functions.hpp | 161 ++++++++++++++++++++++++++++ 5 files changed, 309 insertions(+), 11 deletions(-) create mode 100644 modules/rgbd/src/tsdf_functions.cpp create mode 100644 modules/rgbd/src/tsdf_functions.hpp diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index bc36ec5a3a5..e94b897cde3 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -23,7 +23,7 @@ namespace cv { namespace kinfu { - +/* static inline TsdfType floatToTsdf(float num) { //CV_Assert(-1 < num <= 1); @@ -36,7 +36,7 @@ static inline float tsdfToFloat(TsdfType num) { return float(num) * (-1.f / 128.f); } - +*/ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) @@ -82,7 +82,7 @@ void HashTSDFVolumeCPU::reset() lastVolIndex = 0; volUnitsData = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); } - +///* static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) { int height = depth.rows; @@ -106,6 +106,8 @@ static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) } return pixNorm; } +//*/ +/* static const bool fixMissingData = false; static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) @@ -180,7 +182,7 @@ static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) return v0 + ty * (v1 - v0); } } - +*/ void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides1, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _volume) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 69ec82cb8e9..090b82eda81 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -9,7 +9,7 @@ #include #include -#include "tsdf.hpp" +#include "tsdf_functions.hpp" namespace cv { diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 947428065d1..7fa12fee583 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -5,13 +5,14 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory #include "precomp.hpp" -#include "tsdf.hpp" +//#include "tsdf.hpp" +#include "tsdf_functions.hpp" #include "opencl_kernels_rgbd.hpp" namespace cv { namespace kinfu { - +/* static inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) { v_float32x4 num128 = v_setall_f32(-1.f / 128.f); @@ -30,7 +31,7 @@ static inline float tsdfToFloat(TsdfType num) { return float(num) * (-1.f / 128.f); } - +*/ TSDFVolume::TSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution, bool zFirstMemOrder) : Volume(_voxelSize, _pose, _raycastStepFactor), @@ -118,6 +119,7 @@ TsdfVoxel TSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const } // SIMD version of that code is manually inlined +/* #if !USE_INTRINSICS static const bool fixMissingData = false; @@ -194,7 +196,7 @@ static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) } } #endif - +*/ struct IntegrateInvoker : ParallelLoopBody @@ -461,7 +463,7 @@ struct IntegrateInvoker : ParallelLoopBody TsdfVoxel* volDataStart; Mat pixNorms; }; - +///* static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) { int height = depth.rows; @@ -485,7 +487,7 @@ static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) } return pixNorm; } - +//*/ // use depth instead of distance (optimization) void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const Intr& intrinsics) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp new file mode 100644 index 00000000000..1c14c3a2562 --- /dev/null +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -0,0 +1,133 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory + +#include "tsdf_functions.hpp" + +namespace cv { + +namespace kinfu { +/* +inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) +{ + v_float32x4 num128 = v_setall_f32(-1.f / 128.f); + return v_cvt_f32(num) * num128; +} + +inline TsdfType floatToTsdf(float num) +{ + //CV_Assert(-1 < num <= 1); + int8_t res = int8_t(num * (-128.f)); + res = res ? res : (num < 0 ? 1 : -1); + return res; +} + +inline float tsdfToFloat(TsdfType num) +{ + return float(num) * (-1.f / 128.f); +} + +/* +cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) +{ + int height = depth.rows; + int widht = depth.cols; + Point2f fl(intrinsics.fx, intrinsics.fy); + Point2f pp(intrinsics.cx, intrinsics.cy); + Mat pixNorm(height, widht, CV_32F); + std::vector x(widht); + std::vector y(height); + for (int i = 0; i < widht; i++) + x[i] = (i - pp.x) / fl.x; + for (int i = 0; i < height; i++) + y[i] = (i - pp.y) / fl.y; + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < widht; j++) + { + pixNorm.at(i, j) = sqrtf(x[j] * x[j] + y[i] * y[i] + 1.0f); + } + } + return pixNorm; +} + +const bool fixMissingData = false; +inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) +{ + const depthType defaultValue = qnan; + if (pt.x < 0 || pt.x >= m.cols - 1 || + pt.y < 0 || pt.y >= m.rows - 1) + return defaultValue; + + int xi = cvFloor(pt.x), yi = cvFloor(pt.y); + + const depthType* row0 = m[yi + 0]; + const depthType* row1 = m[yi + 1]; + + depthType v00 = row0[xi + 0]; + depthType v01 = row0[xi + 1]; + depthType v10 = row1[xi + 0]; + depthType v11 = row1[xi + 1]; + + // assume correct depth is positive + bool b00 = v00 > 0; + bool b01 = v01 > 0; + bool b10 = v10 > 0; + bool b11 = v11 > 0; + + if (!fixMissingData) + { + if (!(b00 && b01 && b10 && b11)) + return defaultValue; + else + { + float tx = pt.x - xi, ty = pt.y - yi; + depthType v0 = v00 + tx * (v01 - v00); + depthType v1 = v10 + tx * (v11 - v10); + return v0 + ty * (v1 - v0); + } + } + else + { + int nz = b00 + b01 + b10 + b11; + if (nz == 0) + { + return defaultValue; + } + if (nz == 1) + { + if (b00) return v00; + if (b01) return v01; + if (b10) return v10; + if (b11) return v11; + } + if (nz == 2) + { + if (b00 && b10) v01 = v00, v11 = v10; + if (b01 && b11) v00 = v01, v10 = v11; + if (b00 && b01) v10 = v00, v11 = v01; + if (b10 && b11) v00 = v10, v01 = v11; + if (b00 && b11) v01 = v10 = (v00 + v11) * 0.5f; + if (b01 && b10) v00 = v11 = (v01 + v10) * 0.5f; + } + if (nz == 3) + { + if (!b00) v00 = v10 + v01 - v11; + if (!b01) v01 = v00 + v11 - v10; + if (!b10) v10 = v00 + v11 - v01; + if (!b11) v11 = v01 + v10 - v00; + } + + float tx = pt.x - xi, ty = pt.y - yi; + depthType v0 = v00 + tx * (v01 - v00); + depthType v1 = v10 + tx * (v11 - v10); + return v0 + ty * (v1 - v0); + } +} + +*/ +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp new file mode 100644 index 00000000000..d10c10151c7 --- /dev/null +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -0,0 +1,161 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory + +#ifndef __OPENCV_TSDF_FUNCTIONS_H__ +#define __OPENCV_TSDF_FUNCTIONS_H__ + +#include +#include + +#include "tsdf.hpp" +//#include "hash_tsdf.hpp" + +namespace cv +{ +namespace kinfu +{ +/* +typedef int8_t TsdfType; +typedef uchar WeightType; + +struct TsdfVoxel +{ + TsdfType tsdf; + WeightType weight; +}; + +typedef Vec VecTsdfVoxel; +*/ + +inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num); +inline TsdfType floatToTsdf(float num); +inline float tsdfToFloat(TsdfType num); + +cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics); +inline depthType bilinearDepth(const Depth& m, cv::Point2f pt); + + +inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) +{ + v_float32x4 num128 = v_setall_f32(-1.f / 128.f); + return v_cvt_f32(num) * num128; +} + +inline TsdfType floatToTsdf(float num) +{ + //CV_Assert(-1 < num <= 1); + int8_t res = int8_t(num * (-128.f)); + res = res ? res : (num < 0 ? 1 : -1); + return res; +} + +inline float tsdfToFloat(TsdfType num) +{ + return float(num) * (-1.f / 128.f); +} + +/* +cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) +{ + int height = depth.rows; + int widht = depth.cols; + Point2f fl(intrinsics.fx, intrinsics.fy); + Point2f pp(intrinsics.cx, intrinsics.cy); + Mat pixNorm(height, widht, CV_32F); + std::vector x(widht); + std::vector y(height); + for (int i = 0; i < widht; i++) + x[i] = (i - pp.x) / fl.x; + for (int i = 0; i < height; i++) + y[i] = (i - pp.y) / fl.y; + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < widht; j++) + { + pixNorm.at(i, j) = sqrtf(x[j] * x[j] + y[i] * y[i] + 1.0f); + } + } + return pixNorm; +} +*/ +const bool fixMissingData = false; +inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) +{ + const depthType defaultValue = qnan; + if (pt.x < 0 || pt.x >= m.cols - 1 || + pt.y < 0 || pt.y >= m.rows - 1) + return defaultValue; + + int xi = cvFloor(pt.x), yi = cvFloor(pt.y); + + const depthType* row0 = m[yi + 0]; + const depthType* row1 = m[yi + 1]; + + depthType v00 = row0[xi + 0]; + depthType v01 = row0[xi + 1]; + depthType v10 = row1[xi + 0]; + depthType v11 = row1[xi + 1]; + + // assume correct depth is positive + bool b00 = v00 > 0; + bool b01 = v01 > 0; + bool b10 = v10 > 0; + bool b11 = v11 > 0; + + if (!fixMissingData) + { + if (!(b00 && b01 && b10 && b11)) + return defaultValue; + else + { + float tx = pt.x - xi, ty = pt.y - yi; + depthType v0 = v00 + tx * (v01 - v00); + depthType v1 = v10 + tx * (v11 - v10); + return v0 + ty * (v1 - v0); + } + } + else + { + int nz = b00 + b01 + b10 + b11; + if (nz == 0) + { + return defaultValue; + } + if (nz == 1) + { + if (b00) return v00; + if (b01) return v01; + if (b10) return v10; + if (b11) return v11; + } + if (nz == 2) + { + if (b00 && b10) v01 = v00, v11 = v10; + if (b01 && b11) v00 = v01, v10 = v11; + if (b00 && b01) v10 = v00, v11 = v01; + if (b10 && b11) v00 = v10, v01 = v11; + if (b00 && b11) v01 = v10 = (v00 + v11) * 0.5f; + if (b01 && b10) v00 = v11 = (v01 + v10) * 0.5f; + } + if (nz == 3) + { + if (!b00) v00 = v10 + v01 - v11; + if (!b01) v01 = v00 + v11 - v10; + if (!b10) v10 = v00 + v11 - v01; + if (!b11) v11 = v01 + v10 - v00; + } + + float tx = pt.x - xi, ty = pt.y - yi; + depthType v0 = v00 + tx * (v01 - v00); + depthType v1 = v10 + tx * (v11 - v10); + return v0 + ty * (v1 - v0); + } +} + +} // namespace kinfu +} // namespace cv +#endif From 18057289f8c34028c73856e038020bfb022351b9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 08:30:42 +0300 Subject: [PATCH 25/53] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index e94b897cde3..96872c0eeae 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -183,7 +183,7 @@ static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) } } */ -void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides1, +void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _volume) { @@ -208,10 +208,10 @@ void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResol { for (int x = range.start; x < range.end; x++) { - TsdfVoxel* volDataX = volDataStart + x * volStrides1[0]; + TsdfVoxel* volDataX = volDataStart + x * volStrides[0]; for (int y = 0; y < volResolution.y; y++) { - TsdfVoxel* volDataY = volDataX + y * volStrides1[1]; + TsdfVoxel* volDataY = volDataX + y * volStrides[1]; // optimization of camSpace transformation (vector addition instead of matmul at each z) Point3f basePt = vol2cam * (Point3f(float(x), float(y), 0.0f) * voxelSize); Point3f camSpacePt = basePt; @@ -283,7 +283,7 @@ void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResol { TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv)); - TsdfVoxel& voxel = volDataY[z * volStrides1[2]]; + TsdfVoxel& voxel = volDataY[z * volStrides[2]]; WeightType& weight = voxel.weight; TsdfType& value = voxel.tsdf; @@ -480,7 +480,7 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const } inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex indx, - Point3i volResolution, Vec4i volStrides1) const + Point3i volResolution, Vec4i volStrides) const { //! Out of bounds /* @@ -497,7 +497,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex */ const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = - volumeIdx[0] * volStrides1[0] + volumeIdx[1] * volStrides1[1] + volumeIdx[2] * volStrides1[2]; + volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2]; return volData[coordBase]; } From 66f33e16b732fb3d135263efd3ae30837132d3d6 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 08:36:44 +0300 Subject: [PATCH 26/53] remove two args at _at function signature --- modules/rgbd/src/hash_tsdf.cpp | 12 ++++++------ modules/rgbd/src/hash_tsdf.hpp | 3 +-- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 96872c0eeae..0ff8bc80dd5 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -479,11 +479,11 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const cvFloor(point.z * voxelSizeInv)); } -inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex indx, - Point3i volResolution, Vec4i volStrides) const +inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex indx) const { //! Out of bounds /* + //volResolution = volDims if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) @@ -524,7 +524,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volDims, volStrides); + return _at(volUnitLocalIdx, it->second.index); } inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const @@ -545,7 +545,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index, volDims, volStrides); + return _at(volUnitLocalIdx, it->second.index); } static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) @@ -883,7 +883,7 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index, volume.volDims, volStrides); + TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -959,7 +959,7 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = _at(voxelIdx, it->second.index, volDims, volStrides); + TsdfVoxel voxel = _at(voxelIdx, it->second.index); if (voxel.tsdf != -128 && voxel.weight != 0) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 090b82eda81..68e0a87aec4 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -95,8 +95,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) virtual TsdfVoxel at(const cv::Point3f& point) const; - virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, volumeIndex indx, - Point3i volResolution, Vec4i volDims) const; + virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, volumeIndex indx) const; TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, VolumeUnitIndexes::const_iterator vend, int unitRes) const; From ebb76c6ad7dafadc1b3bc5c2b8a9a2bdf52dacf9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 08:52:18 +0300 Subject: [PATCH 27/53] solved the link problem with tsdf_functions --- modules/rgbd/src/hash_tsdf.cpp | 100 ---------------------- modules/rgbd/src/tsdf.cpp | 107 +---------------------- modules/rgbd/src/tsdf_functions.cpp | 14 +-- modules/rgbd/src/tsdf_functions.hpp | 127 +--------------------------- 4 files changed, 12 insertions(+), 336 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 0ff8bc80dd5..b56190c3e68 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -82,107 +82,7 @@ void HashTSDFVolumeCPU::reset() lastVolIndex = 0; volUnitsData = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); } -///* -static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) -{ - int height = depth.rows; - int widht = depth.cols; - Point2f fl(intrinsics.fx, intrinsics.fy); - Point2f pp(intrinsics.cx, intrinsics.cy); - Mat pixNorm(height, widht, CV_32F); - std::vector x(widht); - std::vector y(height); - for (int i = 0; i < widht; i++) - x[i] = (i - pp.x) / fl.x; - for (int i = 0; i < height; i++) - y[i] = (i - pp.y) / fl.y; - - for (int i = 0; i < height; i++) - { - for (int j = 0; j < widht; j++) - { - pixNorm.at(i, j) = sqrtf(x[j] * x[j] + y[i] * y[i] + 1.0f); - } - } - return pixNorm; -} -//*/ -/* - -static const bool fixMissingData = false; -static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) -{ - const depthType defaultValue = qnan; - if (pt.x < 0 || pt.x >= m.cols - 1 || - pt.y < 0 || pt.y >= m.rows - 1) - return defaultValue; - - int xi = cvFloor(pt.x), yi = cvFloor(pt.y); - - const depthType* row0 = m[yi + 0]; - const depthType* row1 = m[yi + 1]; - - depthType v00 = row0[xi + 0]; - depthType v01 = row0[xi + 1]; - depthType v10 = row1[xi + 0]; - depthType v11 = row1[xi + 1]; - // assume correct depth is positive - bool b00 = v00 > 0; - bool b01 = v01 > 0; - bool b10 = v10 > 0; - bool b11 = v11 > 0; - - if (!fixMissingData) - { - if (!(b00 && b01 && b10 && b11)) - return defaultValue; - else - { - float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx * (v01 - v00); - depthType v1 = v10 + tx * (v11 - v10); - return v0 + ty * (v1 - v0); - } - } - else - { - int nz = b00 + b01 + b10 + b11; - if (nz == 0) - { - return defaultValue; - } - if (nz == 1) - { - if (b00) return v00; - if (b01) return v01; - if (b10) return v10; - if (b11) return v11; - } - if (nz == 2) - { - if (b00 && b10) v01 = v00, v11 = v10; - if (b01 && b11) v00 = v01, v10 = v11; - if (b00 && b01) v10 = v00, v11 = v01; - if (b10 && b11) v00 = v10, v01 = v11; - if (b00 && b11) v01 = v10 = (v00 + v11) * 0.5f; - if (b01 && b10) v00 = v11 = (v01 + v10) * 0.5f; - } - if (nz == 3) - { - if (!b00) v00 = v10 + v01 - v11; - if (!b01) v01 = v00 + v11 - v10; - if (!b10) v10 = v00 + v11 - v01; - if (!b11) v11 = v01 + v10 - v00; - } - - float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx * (v01 - v00); - depthType v1 = v10 + tx * (v11 - v10); - return v0 + ty * (v1 - v0); - } -} -*/ void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _volume) diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 7fa12fee583..f99a67e4571 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -118,87 +118,6 @@ TsdfVoxel TSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const return volData[coordBase]; } -// SIMD version of that code is manually inlined -/* -#if !USE_INTRINSICS -static const bool fixMissingData = false; - -static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) -{ - const depthType defaultValue = qnan; - if(pt.x < 0 || pt.x >= m.cols-1 || - pt.y < 0 || pt.y >= m.rows-1) - return defaultValue; - - int xi = cvFloor(pt.x), yi = cvFloor(pt.y); - - const depthType* row0 = m[yi+0]; - const depthType* row1 = m[yi+1]; - - depthType v00 = row0[xi+0]; - depthType v01 = row0[xi+1]; - depthType v10 = row1[xi+0]; - depthType v11 = row1[xi+1]; - - // assume correct depth is positive - bool b00 = v00 > 0; - bool b01 = v01 > 0; - bool b10 = v10 > 0; - bool b11 = v11 > 0; - - if(!fixMissingData) - { - if(!(b00 && b01 && b10 && b11)) - return defaultValue; - else - { - float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx*(v01 - v00); - depthType v1 = v10 + tx*(v11 - v10); - return v0 + ty*(v1 - v0); - } - } - else - { - int nz = b00 + b01 + b10 + b11; - if(nz == 0) - { - return defaultValue; - } - if(nz == 1) - { - if(b00) return v00; - if(b01) return v01; - if(b10) return v10; - if(b11) return v11; - } - if(nz == 2) - { - if(b00 && b10) v01 = v00, v11 = v10; - if(b01 && b11) v00 = v01, v10 = v11; - if(b00 && b01) v10 = v00, v11 = v01; - if(b10 && b11) v00 = v10, v01 = v11; - if(b00 && b11) v01 = v10 = (v00 + v11)*0.5f; - if(b01 && b10) v00 = v11 = (v01 + v10)*0.5f; - } - if(nz == 3) - { - if(!b00) v00 = v10 + v01 - v11; - if(!b01) v01 = v00 + v11 - v10; - if(!b10) v10 = v00 + v11 - v01; - if(!b11) v11 = v01 + v10 - v00; - } - - float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx*(v01 - v00); - depthType v1 = v10 + tx*(v11 - v10); - return v0 + ty*(v1 - v0); - } -} -#endif -*/ - - struct IntegrateInvoker : ParallelLoopBody { IntegrateInvoker(TSDFVolumeCPU& _volume, const Depth& _depth, const Intr& intrinsics, @@ -463,31 +382,7 @@ struct IntegrateInvoker : ParallelLoopBody TsdfVoxel* volDataStart; Mat pixNorms; }; -///* -static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) -{ - int height = depth.rows; - int widht = depth.cols; - Point2f fl(intrinsics.fx, intrinsics.fy); - Point2f pp(intrinsics.cx, intrinsics.cy); - Mat pixNorm (height, widht, CV_32F); - std::vector x(widht); - std::vector y(height); - for (int i = 0; i < widht; i++) - x[i] = (i - pp.x) / fl.x; - for (int i = 0; i < height; i++) - y[i] = (i - pp.y) / fl.y; - - for (int i = 0; i < height; i++) - { - for (int j = 0; j < widht; j++) - { - pixNorm.at(i, j) = sqrtf(x[j] * x[j] + y[i] * y[i] + 1.0f); - } - } - return pixNorm; -} -//*/ + // use depth instead of distance (optimization) void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const Intr& intrinsics) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 1c14c3a2562..dd2602a732f 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -9,14 +9,14 @@ namespace cv { namespace kinfu { -/* -inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) + +v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) { v_float32x4 num128 = v_setall_f32(-1.f / 128.f); return v_cvt_f32(num) * num128; } -inline TsdfType floatToTsdf(float num) +TsdfType floatToTsdf(float num) { //CV_Assert(-1 < num <= 1); int8_t res = int8_t(num * (-128.f)); @@ -24,12 +24,12 @@ inline TsdfType floatToTsdf(float num) return res; } -inline float tsdfToFloat(TsdfType num) +float tsdfToFloat(TsdfType num) { return float(num) * (-1.f / 128.f); } -/* + cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) { int height = depth.rows; @@ -55,7 +55,7 @@ cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) } const bool fixMissingData = false; -inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) +depthType bilinearDepth(const Depth& m, cv::Point2f pt) { const depthType defaultValue = qnan; if (pt.x < 0 || pt.x >= m.cols - 1 || @@ -128,6 +128,6 @@ inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) } } -*/ + } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index d10c10151c7..d36330cc9f3 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -30,131 +30,12 @@ struct TsdfVoxel typedef Vec VecTsdfVoxel; */ -inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num); -inline TsdfType floatToTsdf(float num); -inline float tsdfToFloat(TsdfType num); +v_float32x4 tsdfToFloat_INTR(const v_int32x4& num); +TsdfType floatToTsdf(float num); +float tsdfToFloat(TsdfType num); cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics); -inline depthType bilinearDepth(const Depth& m, cv::Point2f pt); - - -inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) -{ - v_float32x4 num128 = v_setall_f32(-1.f / 128.f); - return v_cvt_f32(num) * num128; -} - -inline TsdfType floatToTsdf(float num) -{ - //CV_Assert(-1 < num <= 1); - int8_t res = int8_t(num * (-128.f)); - res = res ? res : (num < 0 ? 1 : -1); - return res; -} - -inline float tsdfToFloat(TsdfType num) -{ - return float(num) * (-1.f / 128.f); -} - -/* -cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) -{ - int height = depth.rows; - int widht = depth.cols; - Point2f fl(intrinsics.fx, intrinsics.fy); - Point2f pp(intrinsics.cx, intrinsics.cy); - Mat pixNorm(height, widht, CV_32F); - std::vector x(widht); - std::vector y(height); - for (int i = 0; i < widht; i++) - x[i] = (i - pp.x) / fl.x; - for (int i = 0; i < height; i++) - y[i] = (i - pp.y) / fl.y; - - for (int i = 0; i < height; i++) - { - for (int j = 0; j < widht; j++) - { - pixNorm.at(i, j) = sqrtf(x[j] * x[j] + y[i] * y[i] + 1.0f); - } - } - return pixNorm; -} -*/ -const bool fixMissingData = false; -inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) -{ - const depthType defaultValue = qnan; - if (pt.x < 0 || pt.x >= m.cols - 1 || - pt.y < 0 || pt.y >= m.rows - 1) - return defaultValue; - - int xi = cvFloor(pt.x), yi = cvFloor(pt.y); - - const depthType* row0 = m[yi + 0]; - const depthType* row1 = m[yi + 1]; - - depthType v00 = row0[xi + 0]; - depthType v01 = row0[xi + 1]; - depthType v10 = row1[xi + 0]; - depthType v11 = row1[xi + 1]; - - // assume correct depth is positive - bool b00 = v00 > 0; - bool b01 = v01 > 0; - bool b10 = v10 > 0; - bool b11 = v11 > 0; - - if (!fixMissingData) - { - if (!(b00 && b01 && b10 && b11)) - return defaultValue; - else - { - float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx * (v01 - v00); - depthType v1 = v10 + tx * (v11 - v10); - return v0 + ty * (v1 - v0); - } - } - else - { - int nz = b00 + b01 + b10 + b11; - if (nz == 0) - { - return defaultValue; - } - if (nz == 1) - { - if (b00) return v00; - if (b01) return v01; - if (b10) return v10; - if (b11) return v11; - } - if (nz == 2) - { - if (b00 && b10) v01 = v00, v11 = v10; - if (b01 && b11) v00 = v01, v10 = v11; - if (b00 && b01) v10 = v00, v11 = v01; - if (b10 && b11) v00 = v10, v01 = v11; - if (b00 && b11) v01 = v10 = (v00 + v11) * 0.5f; - if (b01 && b10) v00 = v11 = (v01 + v10) * 0.5f; - } - if (nz == 3) - { - if (!b00) v00 = v10 + v01 - v11; - if (!b01) v01 = v00 + v11 - v10; - if (!b10) v10 = v00 + v11 - v01; - if (!b11) v11 = v01 + v10 - v00; - } - - float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx * (v01 - v00); - depthType v1 = v10 + tx * (v11 - v10); - return v0 + ty * (v1 - v0); - } -} +depthType bilinearDepth(const Depth& m, cv::Point2f pt); } // namespace kinfu } // namespace cv From d3692ff555589e8501ddb75300913c2ed07a7ace Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 12:25:17 +0300 Subject: [PATCH 28/53] build fix --- modules/rgbd/src/tsdf_functions.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index d36330cc9f3..1f1f0fc0c5d 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -7,11 +7,7 @@ #ifndef __OPENCV_TSDF_FUNCTIONS_H__ #define __OPENCV_TSDF_FUNCTIONS_H__ -#include -#include - #include "tsdf.hpp" -//#include "hash_tsdf.hpp" namespace cv { From b43d7f105a8b52f9dcf7bc79b11d6bda3465628a Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 12:45:06 +0300 Subject: [PATCH 29/53] build fix 1 --- modules/rgbd/src/tsdf_functions.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 1f1f0fc0c5d..6ec4006033f 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -7,7 +7,11 @@ #ifndef __OPENCV_TSDF_FUNCTIONS_H__ #define __OPENCV_TSDF_FUNCTIONS_H__ +//#include +#include + #include "tsdf.hpp" +//#include "hash_tsdf.hpp" namespace cv { From 602861a4335dbad97ca7fcc2c8d08d9a9b7e4f42 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 12:53:42 +0300 Subject: [PATCH 30/53] build fix 2 --- modules/rgbd/src/tsdf_functions.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 6ec4006033f..640ba6146f8 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -8,7 +8,7 @@ #define __OPENCV_TSDF_FUNCTIONS_H__ //#include -#include +//#include #include "tsdf.hpp" //#include "hash_tsdf.hpp" From d6357bf3179b85e8f03a20573c77022c57daa892 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 13:00:15 +0300 Subject: [PATCH 31/53] build fix 3 --- modules/rgbd/src/tsdf_functions.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 640ba6146f8..569aefcda4e 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -7,7 +7,7 @@ #ifndef __OPENCV_TSDF_FUNCTIONS_H__ #define __OPENCV_TSDF_FUNCTIONS_H__ -//#include +#include //#include #include "tsdf.hpp" From f40aeda0a6a2df12a102acd8e5b8cdea992e25b3 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 13:08:07 +0300 Subject: [PATCH 32/53] build fix 4 --- modules/rgbd/src/tsdf_functions.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index dd2602a732f..c316cc3bccd 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -4,6 +4,7 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +#include "precomp.hpp" #include "tsdf_functions.hpp" namespace cv { From 0ae59c810690c3ef05e287fa91130fc6f6f1b197 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 13:42:28 +0300 Subject: [PATCH 33/53] replace integrateVolumeUnit to tsdf_functions and fix 2 warnings --- modules/rgbd/src/hash_tsdf.cpp | 122 +--------------------------- modules/rgbd/src/hash_tsdf.hpp | 6 +- modules/rgbd/src/tsdf_functions.cpp | 116 ++++++++++++++++++++++++++ modules/rgbd/src/tsdf_functions.hpp | 6 ++ 4 files changed, 128 insertions(+), 122 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b56190c3e68..1766d7b11fe 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -83,122 +83,6 @@ void HashTSDFVolumeCPU::reset() volUnitsData = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); } -void HashTSDFVolumeCPU::integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, - InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics, InputArray _volume) -{ - CV_TRACE_FUNCTION(); - - CV_Assert(_depth.type() == DEPTH_TYPE); - CV_Assert(!_depth.empty()); - cv::Affine3f vpose(_pose); - Depth depth = _depth.getMat(); - - - Range integrateRange(0, volResolution.x); - - Mat volume = _volume.getMat(); - const Intr::Projector proj(intrinsics.makeProjector()); - const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * vpose); - const float truncDistInv(1.f / truncDist); - const float dfac(1.f / depthFactor); - TsdfVoxel* volDataStart = volume.ptr();; - - auto IntegrateInvoker = [&](const Range& range) - { - for (int x = range.start; x < range.end; x++) - { - TsdfVoxel* volDataX = volDataStart + x * volStrides[0]; - 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) - Point3f basePt = vol2cam * (Point3f(float(x), float(y), 0.0f) * voxelSize); - Point3f camSpacePt = basePt; - // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; - // zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize - Point3f zStep = Point3f(vol2cam.matrix(0, 2), - vol2cam.matrix(1, 2), - vol2cam.matrix(2, 2)) * voxelSize; - int startZ, endZ; - if (abs(zStep.z) > 1e-5) - { - int baseZ = int(-basePt.z / zStep.z); - if (zStep.z > 0) - { - startZ = baseZ; - endZ = volResolution.z; - } - else - { - startZ = 0; - endZ = baseZ; - } - } - else - { - if (basePt.z > 0) - { - startZ = 0; - endZ = volResolution.z; - } - else - { - // z loop shouldn't be performed - startZ = endZ = 0; - } - } - startZ = max(0, startZ); - endZ = min(volResolution.z, endZ); - - for (int z = startZ; z < endZ; z++) - { - // optimization of the following: - //Point3f volPt = Point3f(x, y, z)*volume.voxelSize; - //Point3f camSpacePt = vol2cam * volPt; - - camSpacePt += zStep; - if (camSpacePt.z <= 0) - continue; - - Point3f camPixVec; - Point2f projected = proj(camSpacePt, camPixVec); - - depthType v = bilinearDepth(depth, projected); - if (v == 0) { - continue; - } - - int _u = projected.x; - int _v = projected.y; - if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows)) - continue; - float pixNorm = pixNorms.at(_v, _u); - - // difference between distances of point and of surface to camera - float sdf = pixNorm * (v * dfac - camSpacePt.z); - // possible alternative is: - // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); - if (sdf >= -truncDist) - { - TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv)); - - TsdfVoxel& voxel = volDataY[z * volStrides[2]]; - WeightType& weight = voxel.weight; - TsdfType& value = voxel.tsdf; - - // update TSDF - value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); - weight = min(int(weight + 1), int(maxWeight)); - } - } - } - } - }; - parallel_for_(integrateRange, IntegrateInvoker); - -} - void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { //std::cout << "integrate: " << std::endl; @@ -270,7 +154,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma vu.pose = subvolumePose; vu.index = lastVolIndex; lastVolIndex++; - if (lastVolIndex > volUnitsData.size().height) + if (lastVolIndex > volumeIndex(volUnitsData.size().height)) { //std::cout << " +" << std::endl; volUnitsData.resize(lastVolIndex - 1 + VOLUMES_SIZE); @@ -346,8 +230,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(volumeUnit.pose, volDims, volStrides, depth, - depthFactor, cameraPose, intrinsics, volUnitsData.row(volumeUnit.index)); + integrateVolumeUnit(truncDist, voxelSize, maxWeight, volumeUnit.pose, volDims, volStrides, depth, + depthFactor, cameraPose, intrinsics, pixNorms, volUnitsData.row(volumeUnit.index)); //! Ensure all active volumeUnits are set to inactive for next integration volumeUnit.isActive = false; diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 68e0a87aec4..c414aa77fae 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -69,9 +69,9 @@ typedef std::unordered_map VolumeUnitIndexes; class HashTSDFVolumeCPU : public HashTSDFVolume { private: - void integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, - InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics, InputArray _volume); + //void integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, + // InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, + // const cv::kinfu::Intr& intrinsics, InputArray _volume); public: // dimension in voxels, size in meters HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index c316cc3bccd..825b8a3ceca 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -129,6 +129,122 @@ depthType bilinearDepth(const Depth& m, cv::Point2f pt) } } +void integrateVolumeUnit( + float truncDist, float voxelSize, int maxWeight, + cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, + InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, + const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + CV_Assert(!_depth.empty()); + cv::Affine3f vpose(_pose); + Depth depth = _depth.getMat(); + + Range integrateRange(0, volResolution.x); + + Mat volume = _volume.getMat(); + Mat pixNorms = _pixNorms.getMat(); + const Intr::Projector proj(intrinsics.makeProjector()); + const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * vpose); + const float truncDistInv(1.f / truncDist); + const float dfac(1.f / depthFactor); + TsdfVoxel* volDataStart = volume.ptr();; + + auto IntegrateInvoker = [&](const Range& range) + { + for (int x = range.start; x < range.end; x++) + { + TsdfVoxel* volDataX = volDataStart + x * volStrides[0]; + 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) + Point3f basePt = vol2cam * (Point3f(float(x), float(y), 0.0f) * voxelSize); + Point3f camSpacePt = basePt; + // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; + // zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize + Point3f zStep = Point3f(vol2cam.matrix(0, 2), + vol2cam.matrix(1, 2), + vol2cam.matrix(2, 2)) * voxelSize; + int startZ, endZ; + if (abs(zStep.z) > 1e-5) + { + int baseZ = int(-basePt.z / zStep.z); + if (zStep.z > 0) + { + startZ = baseZ; + endZ = volResolution.z; + } + else + { + startZ = 0; + endZ = baseZ; + } + } + else + { + if (basePt.z > 0) + { + startZ = 0; + endZ = volResolution.z; + } + else + { + // z loop shouldn't be performed + startZ = endZ = 0; + } + } + startZ = max(0, startZ); + endZ = min(volResolution.z, endZ); + + for (int z = startZ; z < endZ; z++) + { + // optimization of the following: + //Point3f volPt = Point3f(x, y, z)*volume.voxelSize; + //Point3f camSpacePt = vol2cam * volPt; + + camSpacePt += zStep; + if (camSpacePt.z <= 0) + continue; + + Point3f camPixVec; + Point2f projected = proj(camSpacePt, camPixVec); + + depthType v = bilinearDepth(depth, projected); + if (v == 0) { + continue; + } + + int _u = projected.x; + int _v = projected.y; + if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows)) + continue; + float pixNorm = pixNorms.at(_v, _u); + + // difference between distances of point and of surface to camera + float sdf = pixNorm * (v * dfac - camSpacePt.z); + // possible alternative is: + // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); + if (sdf >= -truncDist) + { + TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv)); + + TsdfVoxel& voxel = volDataY[z * volStrides[2]]; + WeightType& weight = voxel.weight; + TsdfType& value = voxel.tsdf; + + // update TSDF + value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); + weight = min(int(weight + 1), int(maxWeight)); + } + } + } + } + }; + parallel_for_(integrateRange, IntegrateInvoker); +} } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 569aefcda4e..d62abf956f1 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -37,6 +37,12 @@ float tsdfToFloat(TsdfType num); cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics); depthType bilinearDepth(const Depth& m, cv::Point2f pt); +void integrateVolumeUnit( + float truncDist, float voxelSize, int maxWeight, + cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, + InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, + const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); + } // namespace kinfu } // namespace cv #endif From 8c5540e8727c767cb36f96cecc9b97a0cabfc7d6 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 13 Oct 2020 13:59:51 +0300 Subject: [PATCH 34/53] docs fix --- modules/rgbd/src/hash_tsdf.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 1766d7b11fe..cf06df03b15 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -151,7 +151,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma { VolumeUnit& vu = volumeUnits[idx]; Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; - + vu.pose = subvolumePose; vu.index = lastVolIndex; lastVolIndex++; if (lastVolIndex > volumeIndex(volUnitsData.size().height)) @@ -167,7 +167,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma v.tsdf = floatToTsdf(0.0f); v.weight = 0; }); //! This volume unit will definitely be required for current integration - vu.isActive = true; + vu.isActive = true; } //! Get keys for all the allocated volume Units @@ -216,7 +216,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma pixNorms = preCalculationPixNorm(depth, intrinsics); } - + //! Integrate the correct volumeUnits parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) { for (int i = range.start; i < range.end; i++) @@ -613,7 +613,7 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: const int nstripes = -1; - auto _HashRaycastInvoker = [&](const Range& range) + auto _HashRaycastInvoker = [&](const Range& range) { const Point3f cam2volTrans = cam2vol.translation(); const Matx33f cam2volRot = cam2vol.rotation(); From 3d76d0a7976075ee2ead5ddce2ace1318ba88db9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 14 Oct 2020 09:44:47 +0300 Subject: [PATCH 35/53] remove extra args at atVolumeUnit signature --- modules/rgbd/src/hash_tsdf.cpp | 11 +++++------ modules/rgbd/src/hash_tsdf.hpp | 3 +-- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index cf06df03b15..9ee74e3cdf2 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -348,10 +348,9 @@ static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) } } -TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, - VolumeUnitIndexes::const_iterator vend, int unitRes) const +TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const { - if (it == vend) + if (it == volumeUnits.end()) { //std::cout << "ASSERT (atVolumeUnit)" << std::endl; TsdfVoxel dummy; @@ -359,7 +358,7 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum dummy.weight = 0; return dummy; } - Vec3i volUnitLocalIdx = point - volumeUnitIdx * unitRes; + Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; // expanding at(), removing bounds check const TsdfVoxel* volData = volUnitsData.ptr(it->second.index); @@ -441,7 +440,7 @@ float HashTSDFVolumeCPU::interpolateVoxelPoint(const Point3f& point) const queried[dictIdx] = true; } - vx[i] = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf; + vx[i] = atVolumeUnit(pt, volumeUnitIdx, it).tsdf; } return interpolate(tx, ty, tz, vx); @@ -503,7 +502,7 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const queried[dictIdx] = true; } - vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it, volumeUnits.end(), volumeUnitResolution).tsdf); + vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it).tsdf); } #if !USE_INTERPOLATION_IN_GETNORMAL diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index c414aa77fae..4cea21a2222 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -97,8 +97,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume virtual TsdfVoxel at(const cv::Point3f& point) const; virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, volumeIndex indx) const; - TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, - VolumeUnitIndexes::const_iterator vend, int unitRes) const; + TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const; float interpolateVoxelPoint(const Point3f& point) const; inline float interpolateVoxel(const cv::Point3f& point) const; From a9bbd58f991141afb1b77e9ed5ca11c0beb23f30 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 14 Oct 2020 09:57:14 +0300 Subject: [PATCH 36/53] change frame params checking --- modules/rgbd/src/hash_tsdf.cpp | 14 ++++++-------- modules/rgbd/src/tsdf.cpp | 13 ++++++------- 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 9ee74e3cdf2..bea491bc31d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -205,15 +205,13 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma } } }); - - if (!(frameParams[0] == depth.rows && frameParams[1] == depth.cols && - frameParams[2] == intrinsics.fx && frameParams[3] == intrinsics.fy && - frameParams[4] == intrinsics.cx && frameParams[5] == intrinsics.cy)) + + Vec6f newParams((float)depth.rows, (float)depth.cols, + intrinsics.fx, intrinsics.fy, + intrinsics.cx, intrinsics.cy); + if ( !(frameParams==newParams) ) { - frameParams[0] = (float)depth.rows; frameParams[1] = (float)depth.cols; - frameParams[2] = intrinsics.fx; frameParams[3] = intrinsics.fy; - frameParams[4] = intrinsics.cx; frameParams[5] = intrinsics.cy; - + frameParams = newParams; pixNorms = preCalculationPixNorm(depth, intrinsics); } diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index f99a67e4571..081a1732e5d 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -392,14 +392,13 @@ void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const cv::Ma CV_Assert(_depth.type() == DEPTH_TYPE); CV_Assert(!_depth.empty()); Depth depth = _depth.getMat(); - if (!(frameParams[0] == depth.rows && frameParams[1] == depth.cols && - frameParams[2] == intrinsics.fx && frameParams[3] == intrinsics.fy && - frameParams[4] == intrinsics.cx && frameParams[5] == intrinsics.cy)) + + Vec6f newParams((float)depth.rows, (float)depth.cols, + intrinsics.fx, intrinsics.fy, + intrinsics.cx, intrinsics.cy); + if (!(frameParams == newParams)) { - frameParams[0] = (float)depth.rows; frameParams[1] = (float)depth.cols; - frameParams[2] = intrinsics.fx; frameParams[3] = intrinsics.fy; - frameParams[4] = intrinsics.cx; frameParams[5] = intrinsics.cy; - + frameParams = newParams; pixNorms = preCalculationPixNorm(depth, intrinsics); } From 0415828e16c0a4737b4d5504b39d88d48f2b9b2f Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 14 Oct 2020 10:00:55 +0300 Subject: [PATCH 37/53] move volStrides to CPU class --- modules/rgbd/src/hash_tsdf.cpp | 16 +++++++++------- modules/rgbd/src/hash_tsdf.hpp | 2 +- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index bea491bc31d..ba2b8fcae42 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -49,6 +49,15 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca volDims(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution) { truncDist = std::max(_truncDist, 4.0f * voxelSize); + +} + +HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, float _truncateThreshold, + int _volumeUnitRes, bool _zFirstMemOrder) + : HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) +{ int xdim, ydim, zdim; if (zFirstMemOrder) { @@ -63,14 +72,7 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca zdim = volDims.x * volDims.y; } volStrides = Vec4i(xdim, ydim, zdim); -} -HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, float _truncateThreshold, - int _volumeUnitRes, bool _zFirstMemOrder) - : HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, - _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) -{ lastVolIndex = 0; volUnitsData = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); } diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 4cea21a2222..c2df1c2898c 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -34,7 +34,6 @@ class HashTSDFVolume : public Volume float volumeUnitSize; bool zFirstMemOrder; Point3i volDims; - Vec4i volStrides; }; //! Spatial hashing @@ -111,6 +110,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const; public: + Vec4i volStrides; Vec6f frameParams; Mat pixNorms; //VolumeUnitIndexes volumeUnitIndexes; From c5c2032c1d9fed28b903926f8460ebeae6c58d39 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 14 Oct 2020 10:05:42 +0300 Subject: [PATCH 38/53] inline convertion functions in tsdf_functions --- modules/rgbd/src/tsdf_functions.cpp | 20 -------------------- modules/rgbd/src/tsdf_functions.hpp | 21 ++++++++++++++++++--- 2 files changed, 18 insertions(+), 23 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 825b8a3ceca..1aa20cc28e2 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -11,26 +11,6 @@ namespace cv { namespace kinfu { -v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) -{ - v_float32x4 num128 = v_setall_f32(-1.f / 128.f); - return v_cvt_f32(num) * num128; -} - -TsdfType floatToTsdf(float num) -{ - //CV_Assert(-1 < num <= 1); - int8_t res = int8_t(num * (-128.f)); - res = res ? res : (num < 0 ? 1 : -1); - return res; -} - -float tsdfToFloat(TsdfType num) -{ - return float(num) * (-1.f / 128.f); -} - - cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) { int height = depth.rows; diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index d62abf956f1..2302457e34e 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -30,9 +30,24 @@ struct TsdfVoxel typedef Vec VecTsdfVoxel; */ -v_float32x4 tsdfToFloat_INTR(const v_int32x4& num); -TsdfType floatToTsdf(float num); -float tsdfToFloat(TsdfType num); +inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) +{ + v_float32x4 num128 = v_setall_f32(-1.f / 128.f); + return v_cvt_f32(num) * num128; +} + +inline TsdfType floatToTsdf(float num) +{ + //CV_Assert(-1 < num <= 1); + int8_t res = int8_t(num * (-128.f)); + res = res ? res : (num < 0 ? 1 : -1); + return res; +} + +inline float tsdfToFloat(TsdfType num) +{ + return float(num) * (-1.f / 128.f); +} cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics); depthType bilinearDepth(const Depth& m, cv::Point2f pt); From 5f03c144dd4d6f6ccbf350e867b4e4410a1bfd7b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 14 Oct 2020 11:55:02 +0300 Subject: [PATCH 39/53] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 21 --------------------- modules/rgbd/src/hash_tsdf.hpp | 6 ------ modules/rgbd/src/tsdf.cpp | 19 ------------------- modules/rgbd/src/tsdf_functions.hpp | 15 --------------- 4 files changed, 61 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ba2b8fcae42..33e9fc4eb6f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -23,20 +23,7 @@ namespace cv { namespace kinfu { -/* -static inline TsdfType floatToTsdf(float num) -{ - //CV_Assert(-1 < num <= 1); - int8_t res = int8_t(num * (-128.f)); - res = res ? res : (num < 0 ? 1 : -1); - return res; -} -static inline float tsdfToFloat(TsdfType num) -{ - return float(num) * (-1.f / 128.f); -} -*/ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) @@ -87,7 +74,6 @@ void HashTSDFVolumeCPU::reset() void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { - //std::cout << "integrate: " << std::endl; CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); @@ -158,10 +144,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma vu.index = lastVolIndex; lastVolIndex++; if (lastVolIndex > volumeIndex(volUnitsData.size().height)) { - //std::cout << " +" << std::endl; volUnitsData.resize(lastVolIndex - 1 + VOLUMES_SIZE); - //for (int i = 0; i < VOLUMES_SIZE; i++) - // volUnitsMatrix.push_back(Mat(1, volDims.x * volDims.y * volDims.z, rawType())); } volUnitsData.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) { @@ -272,7 +255,6 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) { - std::cout << "ASSERT (_at)" << std::endl; TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); dummy.weight = 0; @@ -295,7 +277,6 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const /* if (it == volumeUnits.end()) { - std::cout << "ASSERT (at Vec3i)" << std::endl; TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.f); dummy.weight = 0; @@ -318,7 +299,6 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const /* if (it == volumeUnits.end()) { - std::cout << "ASSERT (at Point3f)" << std::endl; TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.f); dummy.weight = 0; @@ -352,7 +332,6 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum { if (it == volumeUnits.end()) { - //std::cout << "ASSERT (atVolumeUnit)" << std::endl; TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.f); dummy.weight = 0; diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index c2df1c2898c..93104612e7f 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -62,15 +62,10 @@ struct VolumeUnit typedef std::unordered_set VolumeUnitIndexSet; typedef std::unordered_map VolumeUnitIndexes; -//typedef std::unordered_set VolumeUnitIndexSet; -//typedef std::unordered_map VolumeUnitIndexes; class HashTSDFVolumeCPU : public HashTSDFVolume { private: - //void integrateVolumeUnit( cv::Matx44f _pose, Point3i volResolution, Vec4i volDims, - // InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - // const cv::kinfu::Intr& intrinsics, InputArray _volume); public: // dimension in voxels, size in meters HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, @@ -113,7 +108,6 @@ class HashTSDFVolumeCPU : public HashTSDFVolume Vec4i volStrides; Vec6f frameParams; Mat pixNorms; - //VolumeUnitIndexes volumeUnitIndexes; VolumeUnitIndexes volumeUnits; cv::Mat volUnitsData; volumeIndex lastVolIndex; diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 081a1732e5d..1fa26d0c360 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -12,26 +12,7 @@ namespace cv { namespace kinfu { -/* -static inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) -{ - v_float32x4 num128 = v_setall_f32(-1.f / 128.f); - return v_cvt_f32(num) * num128; -} -static inline TsdfType floatToTsdf(float num) -{ - //CV_Assert(-1 < num <= 1); - int8_t res = int8_t(num * (-128.f)); - res = res ? res : (num < 0 ? 1 : -1); - return res; -} - -static inline float tsdfToFloat(TsdfType num) -{ - return float(num) * (-1.f / 128.f); -} -*/ TSDFVolume::TSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution, bool zFirstMemOrder) : Volume(_voxelSize, _pose, _raycastStepFactor), diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 2302457e34e..6d86595118f 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -8,27 +8,12 @@ #define __OPENCV_TSDF_FUNCTIONS_H__ #include -//#include - #include "tsdf.hpp" -//#include "hash_tsdf.hpp" namespace cv { namespace kinfu { -/* -typedef int8_t TsdfType; -typedef uchar WeightType; - -struct TsdfVoxel -{ - TsdfType tsdf; - WeightType weight; -}; - -typedef Vec VecTsdfVoxel; -*/ inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) { From 1e001e78cb52b35ddeb207db832b0625b81420d6 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 15 Oct 2020 09:39:18 +0300 Subject: [PATCH 40/53] add SIMD version of integrateVolumeUnit --- modules/rgbd/src/tsdf_functions.cpp | 148 ++++++++++++++++++++++++++++ 1 file changed, 148 insertions(+) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 1aa20cc28e2..94afa291195 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -132,6 +132,151 @@ void integrateVolumeUnit( const float dfac(1.f / depthFactor); TsdfVoxel* volDataStart = volume.ptr();; + +#if USE_INTRINSICS + auto IntegrateInvoker = [&](const Range& range) + { + // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; + Point3f zStepPt = Point3f(vol2cam.matrix(0, 2), + vol2cam.matrix(1, 2), + vol2cam.matrix(2, 2)) * voxelSize; + + v_float32x4 zStep(zStepPt.x, zStepPt.y, zStepPt.z, 0); + v_float32x4 vfxy(proj.fx, proj.fy, 0.f, 0.f), vcxy(proj.cx, proj.cy, 0.f, 0.f); + 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++) + { + TsdfVoxel* volDataX = volDataStart + x * volStrides[0]; + 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) + Point3f basePt = vol2cam * (Point3f((float)x, (float)y, 0) * voxelSize); + v_float32x4 camSpacePt(basePt.x, basePt.y, basePt.z, 0); + + int startZ, endZ; + if (abs(zStepPt.z) > 1e-5) + { + int baseZ = (int)(-basePt.z / zStepPt.z); + if (zStepPt.z > 0) + { + startZ = baseZ; + endZ = volResolution.z; + } + else + { + startZ = 0; + endZ = baseZ; + } + } + else + { + if (basePt.z > 0) + { + startZ = 0; + endZ = volResolution.z; + } + else + { + // z loop shouldn't be performed + startZ = endZ = 0; + } + } + startZ = max(0, startZ); + endZ = min(volResolution.z, endZ); + for (int z = startZ; z < endZ; z++) + { + // optimization of the following: + //Point3f volPt = Point3f(x, y, z)*voxelSize; + //Point3f camSpacePt = vol2cam * volPt; + camSpacePt += zStep; + + float zCamSpace = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(camSpacePt))).get0(); + if (zCamSpace <= 0.f) + continue; + + v_float32x4 camPixVec = camSpacePt / v_setall_f32(zCamSpace); + v_float32x4 projected = v_muladd(camPixVec, vfxy, vcxy); + // leave only first 2 lanes + projected = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & + v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); + + depthType v; + // bilinearly interpolate depth at projected + { + const v_float32x4& pt = projected; + // check coords >= 0 and < imgSize + v_uint32x4 limits = v_reinterpret_as_u32(pt < v_setzero_f32()) | + v_reinterpret_as_u32(pt >= upLimits); + limits = limits | v_rotate_right<1>(limits); + if (limits.get0()) + continue; + + // xi, yi = floor(pt) + v_int32x4 ip = v_floor(pt); + v_int32x4 ipshift = ip; + int xi = ipshift.get0(); + ipshift = v_rotate_right<1>(ipshift); + int yi = ipshift.get0(); + + const depthType* row0 = depth[yi + 0]; + const depthType* row1 = depth[yi + 1]; + + // v001 = [v(xi + 0, yi + 0), v(xi + 1, yi + 0)] + v_float32x4 v001 = v_load_low(row0 + xi); + // v101 = [v(xi + 0, yi + 1), v(xi + 1, yi + 1)] + v_float32x4 v101 = v_load_low(row1 + xi); + + v_float32x4 vall = v_combine_low(v001, v101); + + // assume correct depth is positive + // don't fix missing data + if (v_check_all(vall > v_setzero_f32())) + { + v_float32x4 t = pt - v_cvt_f32(ip); + float tx = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + v_float32x4 ty = v_setall_f32(t.get0()); + // vx is y-interpolated between rows 0 and 1 + v_float32x4 vx = v001 + ty * (v101 - v001); + float v0 = vx.get0(); + vx = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vx))); + float v1 = vx.get0(); + v = v0 + tx * (v1 - v0); + } + else + continue; + } + + // norm(camPixVec) produces double which is too slow + int _u = (int)projected.get0(); + int _v = (int)v_rotate_right<1>(projected).get0(); + if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows)) + continue; + float pixNorm = pixNorms.at(_v, _u); + // float pixNorm = sqrt(v_reduce_sum(camPixVec*camPixVec)); + // difference between distances of point and of surface to camera + float sdf = pixNorm * (v * dfac - zCamSpace); + // possible alternative is: + // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); + if (sdf >= -truncDist) + { + TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv)); + + TsdfVoxel& voxel = volDataY[z * volStrides[2]]; + WeightType& weight = voxel.weight; + TsdfType& value = voxel.tsdf; + + // update TSDF + value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); + weight = (weight + 1) < maxWeight ? (weight + 1) : maxWeight; + } + } + } + } + }; +#else auto IntegrateInvoker = [&](const Range& range) { for (int x = range.start; x < range.end; x++) @@ -223,7 +368,10 @@ void integrateVolumeUnit( } } }; +#endif + parallel_for_(integrateRange, IntegrateInvoker); + } } // namespace kinfu From 5f0118fd3300b73e4ec98caad089559a7959571b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 15 Oct 2020 11:24:56 +0300 Subject: [PATCH 41/53] fix something :) --- modules/rgbd/src/hash_tsdf.cpp | 5 ++--- modules/rgbd/src/hash_tsdf.hpp | 1 + 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 754cbdc43bc..a5b932a7cd9 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -579,9 +579,8 @@ Point3f HashTSDFVolumeCPU::getNormalVoxel(const Point3f &point) const return nv < 0.0001f ? nan3 : normal / nv; } -void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray _points, - cv::OutputArray _normals) const +void HashTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, + OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 920d1542dce..4bf6e448a4b 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -57,6 +57,7 @@ struct VolumeUnit cv::Vec3i coord; volumeIndex index; cv::Matx44f pose; + int lastVisibleIndex = 0; bool isActive; }; From 77c4f6a7d86a3663574289649c5feb37b8432506 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 15 Oct 2020 11:59:47 +0300 Subject: [PATCH 42/53] docs fix --- modules/rgbd/src/hash_tsdf.cpp | 4 +--- modules/rgbd/src/tsdf.cpp | 2 +- modules/rgbd/src/tsdf_functions.cpp | 3 +-- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a5b932a7cd9..f6b7fedd5e4 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -27,7 +27,6 @@ namespace kinfu HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) - : Volume(_voxelSize, _pose, _raycastStepFactor), maxWeight(_maxWeight), truncateThreshold(_truncateThreshold), @@ -37,7 +36,6 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca volDims(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution) { truncDist = std::max(_truncDist, 4.0f * voxelSize); - } HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, @@ -197,7 +195,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma } } }); - + Vec6f newParams((float)depth.rows, (float)depth.cols, intrinsics.fx, intrinsics.fy, intrinsics.cx, intrinsics.cy); diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index ac31b2a679d..42dbd29a82a 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -374,7 +374,7 @@ void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44 CV_Assert(_depth.type() == DEPTH_TYPE); CV_Assert(!_depth.empty()); Depth depth = _depth.getMat(); - + Vec6f newParams((float)depth.rows, (float)depth.cols, intrinsics.fx, intrinsics.fy, intrinsics.cx, intrinsics.cy); diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 94afa291195..fe3abc9e31c 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -132,7 +132,6 @@ void integrateVolumeUnit( const float dfac(1.f / depthFactor); TsdfVoxel* volDataStart = volume.ptr();; - #if USE_INTRINSICS auto IntegrateInvoker = [&](const Range& range) { @@ -369,7 +368,7 @@ void integrateVolumeUnit( } }; #endif - + parallel_for_(integrateRange, IntegrateInvoker); } From 07ee90effc63e22c94e9aad3e93c91d306f5d1b6 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 15 Oct 2020 14:10:13 +0300 Subject: [PATCH 43/53] warning 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 fe3abc9e31c..6e765d422b7 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -269,7 +269,7 @@ void integrateVolumeUnit( // update TSDF value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); - weight = (weight + 1) < maxWeight ? (weight + 1) : maxWeight; + weight = (weight + 1) < maxWeight ? (weight + 1) : (WeightType) maxWeight; } } } From 7a706c2fa6086fae372678a2098338480bdeb789 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 09:50:45 +0300 Subject: [PATCH 44/53] add degub asserts --- modules/rgbd/src/hash_tsdf.cpp | 38 +++++++--------------------------- 1 file changed, 8 insertions(+), 30 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index f6b7fedd5e4..1d038cb42e0 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -254,18 +254,10 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(const cv::Point3f& point) const inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex indx) const { //! Out of bounds - /* - //volResolution = volDims - if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || - (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || - (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) - { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.0f); - dummy.weight = 0; - return dummy; - } - */ + CV_DbgAssert((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || + (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || + (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) + const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2]; @@ -280,15 +272,8 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const cvFloor(volumeIdx[2] / volumeUnitResolution)); VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); - /* - if (it == volumeUnits.end()) - { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.f); - dummy.weight = 0; - return dummy; - } - */ + + CV_DbgAssert (it == volumeUnits.end()) cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, volumeUnitIdx[2] * volumeUnitResolution); @@ -303,16 +288,9 @@ TsdfVoxel HashTSDFVolumeCPU::at(const Point3f& point) const { cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); - /* - if (it == volumeUnits.end()) - { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.f); - dummy.weight = 0; - return dummy; - } - */ + CV_DbgAssert (it == volumeUnits.end()) + cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = From 3d2c0e24ce6ed1cf6d8802b1ecadd1b6801aba46 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 10:19:53 +0300 Subject: [PATCH 45/53] replace vars initialization with reset() --- modules/rgbd/src/hash_tsdf.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 1d038cb42e0..8e409afe86f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -58,8 +58,7 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, flo } volStrides = Vec4i(xdim, ydim, zdim); - lastVolIndex = 0; - volUnitsData = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); + reset(); } HashTSDFVolumeCPU::HashTSDFVolumeCPU(const VolumeParams& _params, bool _zFirstMemOrder) From ffeec52e39016f7e9dbf8391cf81679d160b82b5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 10:33:26 +0300 Subject: [PATCH 46/53] remove volDims var --- modules/rgbd/src/hash_tsdf.cpp | 15 +++++++-------- modules/rgbd/src/hash_tsdf.hpp | 1 - modules/rgbd/src/tsdf_functions.cpp | 20 ++++++++++---------- modules/rgbd/src/tsdf_functions.hpp | 2 +- 4 files changed, 18 insertions(+), 20 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 8e409afe86f..99ded08e827 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -32,8 +32,7 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca truncateThreshold(_truncateThreshold), volumeUnitResolution(_volumeUnitRes), volumeUnitSize(voxelSize* volumeUnitResolution), - zFirstMemOrder(_zFirstMemOrder), - volDims(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution) + zFirstMemOrder(_zFirstMemOrder) { truncDist = std::max(_truncDist, 4.0f * voxelSize); } @@ -46,15 +45,15 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, flo int xdim, ydim, zdim; if (zFirstMemOrder) { - xdim = volDims.z * volDims.y; - ydim = volDims.z; + xdim = volumeUnitResolution * volumeUnitResolution; + ydim = volumeUnitResolution; zdim = 1; } else { xdim = 1; - ydim = volDims.x; - zdim = volDims.x * volDims.y; + ydim = volumeUnitResolution; + zdim = volumeUnitResolution * volumeUnitResolution; } volStrides = Vec4i(xdim, ydim, zdim); @@ -71,7 +70,7 @@ void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); lastVolIndex = 0; - volUnitsData = cv::Mat(VOLUMES_SIZE, volDims.x * volDims.y * volDims.z, rawType()); + volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); } void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) @@ -217,7 +216,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, volDims, volStrides, depth, + integrateVolumeUnit(truncDist, voxelSize, maxWeight, volumeUnit.pose, 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/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 4bf6e448a4b..ab8e6092a4f 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -33,7 +33,6 @@ class HashTSDFVolume : public Volume int volumeUnitResolution; float volumeUnitSize; bool zFirstMemOrder; - Point3i volDims; }; //! Spatial hashing diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 6e765d422b7..ff0ab67a60f 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, Point3i volResolution, Vec4i volStrides, + cv::Matx44f _pose, float 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.x); + Range integrateRange(0, volResolution); 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; y++) + for (int y = 0; y < volResolution; 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.z; + endZ = volResolution; } else { @@ -174,7 +174,7 @@ void integrateVolumeUnit( if (basePt.z > 0) { startZ = 0; - endZ = volResolution.z; + endZ = volResolution; } else { @@ -183,7 +183,7 @@ void integrateVolumeUnit( } } startZ = max(0, startZ); - endZ = min(volResolution.z, endZ); + endZ = min(int(volResolution), 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; y++) + for (int y = 0; y < volResolution; 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.z; + endZ = volResolution; } else { @@ -312,7 +312,7 @@ void integrateVolumeUnit( if (basePt.z > 0) { startZ = 0; - endZ = volResolution.z; + endZ = volResolution; } else { @@ -321,7 +321,7 @@ void integrateVolumeUnit( } } startZ = max(0, startZ); - endZ = min(volResolution.z, endZ); + endZ = min(int(volResolution), 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 6d86595118f..4bbdca5097a 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, Point3i volResolution, Vec4i volStrides, + cv::Matx44f _pose, float volResolution, Vec4i volStrides, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); From ca6d445485b66776bf0b5aec3d234bd5f4f13549 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 10:41:33 +0300 Subject: [PATCH 47/53] new resize buffer --- 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 99ded08e827..a8e098f41c2 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -145,7 +145,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma vu.index = lastVolIndex; lastVolIndex++; if (lastVolIndex > volumeIndex(volUnitsData.size().height)) { - volUnitsData.resize(lastVolIndex - 1 + VOLUMES_SIZE); + volUnitsData.resize((lastVolIndex - 1) * 2); } volUnitsData.row(vu.index).forEach([](VecTsdfVoxel& vv, const int* /* position */) { From 1be50f7d60a28c43a71c742d0a2303389676614a Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 10:46:42 +0300 Subject: [PATCH 48/53] minor vars name fix --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- modules/rgbd/src/hash_tsdf.hpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a8e098f41c2..d17dc965c4f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -143,7 +143,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma vu.pose = subvolumePose; vu.index = lastVolIndex; lastVolIndex++; - if (lastVolIndex > volumeIndex(volUnitsData.size().height)) + if (lastVolIndex > VolumeIndex(volUnitsData.size().height)) { volUnitsData.resize((lastVolIndex - 1) * 2); } @@ -249,7 +249,7 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(const cv::Point3f& point) const cvFloor(point.z * voxelSizeInv)); } -inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, volumeIndex indx) const +inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const { //! Out of bounds CV_DbgAssert((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index ab8e6092a4f..6abf444b4ef 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -50,11 +50,11 @@ struct tsdf_hash } }; -typedef unsigned int volumeIndex; +typedef unsigned int VolumeIndex; struct VolumeUnit { cv::Vec3i coord; - volumeIndex index; + VolumeIndex index; cv::Matx44f pose; int lastVisibleIndex = 0; bool isActive; @@ -90,7 +90,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) virtual TsdfVoxel at(const cv::Point3f& point) const; - virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, volumeIndex indx) const; + virtual TsdfVoxel _at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const; TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const; @@ -112,7 +112,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume Mat pixNorms; VolumeUnitIndexes volumeUnits; cv::Mat volUnitsData; - volumeIndex lastVolIndex; + VolumeIndex lastVolIndex; }; From 8ab9316a669fc74bd28527d81a7666a74708a680 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 10:55:17 +0300 Subject: [PATCH 49/53] docs fix --- 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 d17dc965c4f..31dc54c8f07 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -255,7 +255,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, VolumeIndex CV_DbgAssert((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) - + const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2]; From e754f64d195a31be89ba09807e9f5c34245e1a66 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 12:35:22 +0300 Subject: [PATCH 50/53] warning fix --- modules/rgbd/src/hash_tsdf.hpp | 1 - modules/rgbd/src/tsdf_functions.cpp | 2 +- modules/rgbd/src/tsdf_functions.hpp | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 6abf444b4ef..31bf026785b 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -113,7 +113,6 @@ class HashTSDFVolumeCPU : public HashTSDFVolume VolumeUnitIndexes volumeUnits; cv::Mat volUnitsData; VolumeIndex lastVolIndex; - }; template diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index ff0ab67a60f..3eb27742c1e 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, float volResolution, Vec4i volStrides, + cv::Matx44f _pose, int volResolution, Vec4i volStrides, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume) { diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 4bbdca5097a..28f776d752f 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, float volResolution, Vec4i volStrides, + cv::Matx44f _pose, int volResolution, Vec4i volStrides, InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); From bae7dbff07b81f8d1da4d25c31d137ae2b526733 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 16:49:09 +0300 Subject: [PATCH 51/53] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 31dc54c8f07..f57ab401b3c 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -253,8 +253,8 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, VolumeIndex { //! Out of bounds CV_DbgAssert((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || - (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || - (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) + (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || + (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)); const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = @@ -271,7 +271,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); - CV_DbgAssert (it == volumeUnits.end()) + CV_DbgAssert(it == volumeUnits.end()); cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, volumeUnitIdx[2] * volumeUnitResolution); @@ -287,7 +287,7 @@ TsdfVoxel HashTSDFVolumeCPU::at(const Point3f& point) const cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); - CV_DbgAssert (it == volumeUnits.end()) + CV_DbgAssert(it == volumeUnits.end()); cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); From ffb71778e64fc9f102f8524aa468ce5e4ce8205b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 17:02:15 +0300 Subject: [PATCH 52/53] minor fix 1 --- modules/rgbd/src/hash_tsdf.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index f57ab401b3c..27ac2743907 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -252,9 +252,9 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(const cv::Point3f& point) const inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const { //! Out of bounds - CV_DbgAssert((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || - (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || - (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)); + CV_DbgAssert((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || + (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || + (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)); const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = From a82f5ec64a8c0f8330469d9f5a2b540106445c24 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 16 Oct 2020 19:00:00 +0300 Subject: [PATCH 53/53] remove dbg asserts --- modules/rgbd/src/hash_tsdf.cpp | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 27ac2743907..3c5d2d5d43d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -252,9 +252,15 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(const cv::Point3f& point) const inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const { //! Out of bounds - CV_DbgAssert((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || + if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || - (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)); + (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) + { + TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.0f); + dummy.weight = 0; + return dummy; + } const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = @@ -271,7 +277,14 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); - CV_DbgAssert(it == volumeUnits.end()); + if (it == volumeUnits.end()) + { + TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.f); + dummy.weight = 0; + return dummy; + } + cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, volumeUnitIdx[2] * volumeUnitResolution); @@ -287,7 +300,13 @@ TsdfVoxel HashTSDFVolumeCPU::at(const Point3f& point) const cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); - CV_DbgAssert(it == volumeUnits.end()); + if (it == volumeUnits.end()) + { + TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.f); + dummy.weight = 0; + return dummy; + } cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos);