From 1a487723de82272e838ace90baad04301bbe42aa Mon Sep 17 00:00:00 2001 From: Saratovtsev Date: Fri, 30 Oct 2020 15:42:04 +0300 Subject: [PATCH 001/216] create new class --- modules/rgbd/src/hash_tsdf.cpp | 48 ++++++++++++++++++++++++++++++++++ modules/rgbd/src/hash_tsdf.hpp | 25 ++++++++++++++++++ 2 files changed, 73 insertions(+) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 35838352d43..d4c96048bdc 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -817,5 +817,53 @@ int HashTSDFVolumeCPU::getVisibleBlocks(int currFrameId, int frameThreshold) con return numVisibleBlocks; } + +///////// GPU implementation ///////// + +#ifdef HAVE_OPENCL +/* +HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const 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) + { + xdim = volumeUnitResolution * volumeUnitResolution; + ydim = volumeUnitResolution; + zdim = 1; + } + else + { + xdim = 1; + ydim = volumeUnitResolution; + zdim = volumeUnitResolution * volumeUnitResolution; + } + volStrides = Vec4i(xdim, ydim, zdim); + + reset(); +} + +HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstMemOrder) + : HashTSDFVolume(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) +{ +} +// zero volume, leave rest params the same +void HashTSDFVolumeGPU::reset() +{ + CV_TRACE_FUNCTION(); + lastVolIndex = 0; + volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); +} +*/ + +#endif + + + + + + } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 31bf026785b..a35b0ef6448 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -115,6 +115,31 @@ class HashTSDFVolumeCPU : public HashTSDFVolume VolumeIndex lastVolIndex; }; +#ifdef HAVE_OPENCL +class HashTSDFVolumeGPU : public HashTSDFVolume +{ + HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); + + HashTSDFVolumeGPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); + + void reset() override; + + void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, + const int frameId = 0) override; + void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, + OutputArray normals) const override; + +public: + Vec4i volStrides; + Vec6f frameParams; + Mat pixNorms; + VolumeUnitIndexes volumeUnits; + cv::Mat volUnitsData; + VolumeIndex lastVolIndex; +}; +#endif + template Ptr makeHashTSDFVolume(const VolumeParams& _volumeParams) { From 8f2b15f5880b65ef5df3ec206327b81f51691f7e Mon Sep 17 00:00:00 2001 From: Saratovtsev Date: Mon, 2 Nov 2020 09:34:28 +0300 Subject: [PATCH 002/216] rewrite makeHashTSDFVolume --- modules/rgbd/src/hash_tsdf.cpp | 3 +-- modules/rgbd/src/hash_tsdf.hpp | 36 ++++++++++++++++++++++++++-------- modules/rgbd/src/volume.cpp | 7 ++++--- 3 files changed, 33 insertions(+), 13 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d4c96048bdc..f852da0b5b2 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -821,7 +821,7 @@ int HashTSDFVolumeCPU::getVisibleBlocks(int currFrameId, int frameThreshold) con ///////// GPU implementation ///////// #ifdef HAVE_OPENCL -/* + HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder) :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, zFirstMemOrder) @@ -856,7 +856,6 @@ void HashTSDFVolumeGPU::reset() lastVolIndex = 0; volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); } -*/ #endif diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index a35b0ef6448..28e3b456160 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -116,6 +116,10 @@ class HashTSDFVolumeCPU : public HashTSDFVolume }; #ifdef HAVE_OPENCL + +typedef cv::Mat _VolumeUnitIndexSet; +typedef cv::Mat _VolumeUnitIndexes; + class HashTSDFVolumeGPU : public HashTSDFVolume { HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, @@ -125,10 +129,9 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void reset() override; - void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, - const int frameId = 0) override; - void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, - OutputArray normals) const override; + + //void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; + //void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; public: Vec4i volStrides; @@ -137,22 +140,39 @@ class HashTSDFVolumeGPU : public HashTSDFVolume VolumeUnitIndexes volumeUnits; cv::Mat volUnitsData; VolumeIndex lastVolIndex; + + VolumeIndex _lastVolIndex; + cv::Mat indexes; + cv::Mat poses; + cv::Mat activities; + cv::Mat lastVisibleIndexes; + cv::Mat _volUnitsData; }; #endif -template +//template Ptr makeHashTSDFVolume(const VolumeParams& _volumeParams) { - return makePtr(_volumeParams); +#ifdef HAVE_OPENCL + if (ocl::useOpenCL()) + return makePtr(_volumeParams); +#endif + return makePtr(_volumeParams); } -template +//template Ptr makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16) { - return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, +#ifdef HAVE_OPENCL + if (ocl::useOpenCL()) + return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, + volumeUnitResolution); +#endif + return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, volumeUnitResolution); } + } // namespace kinfu } // namespace cv #endif diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index 8177213e8ab..70e77fd7086 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -68,7 +68,8 @@ Ptr makeVolume(const VolumeParams& _volumeParams) if(_volumeParams.type == VolumeType::TSDF) return kinfu::makeTSDFVolume(_volumeParams); else if(_volumeParams.type == VolumeType::HASHTSDF) - return kinfu::makeHashTSDFVolume(_volumeParams); + //return kinfu::makeHashTSDFVolume(_volumeParams); + return kinfu::makeHashTSDFVolume(_volumeParams); CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } @@ -84,8 +85,8 @@ Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, } else if (_volumeType == VolumeType::HASHTSDF) { - return makeHashTSDFVolume( - _voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); + //return makeHashTSDFVolume( + return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); } CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } From 003b80c626a8ee08238e1b138d7c24bd6e427ac5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 2 Nov 2020 10:01:55 +0300 Subject: [PATCH 003/216] it builds --- modules/rgbd/src/hash_tsdf.cpp | 1310 ++++++++++++++++++++++++++++++++ modules/rgbd/src/hash_tsdf.hpp | 59 +- 2 files changed, 1348 insertions(+), 21 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index f852da0b5b2..a73a2ee0390 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -857,10 +857,1320 @@ void HashTSDFVolumeGPU::reset() volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); } + +static inline bool _find(cv::Mat v, Vec3i tsdf_idx) +{ + //bool res = false; + for (int i = 0; i < v.size().height; i++) + { + auto p = v.at(i, 0); + if (p == tsdf_idx) + { + return true; + } + } + //v.forEach([&](Vec3i& v, const int*) {if (v == tsdf_idx) res = true; }); + return false; +} + +static inline int find_idx(cv::Mat v, Vec3i tsdf_idx) +{ + //bool res = false; + for (int i = 0; i < v.size().height; i++) + { + Vec3i p = v.at(i, 0); + //std::cout << p << " " << tsdf_idx << std::endl; + if (p == tsdf_idx) + { + return i; + } + } + //v.forEach([&](Vec3i& v, const int*) {if (v == tsdf_idx) res = true; }); + return -1; +} + +void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + Depth depth = _depth.getMat(); + + //! Compute volumes to be allocated + const int depthStride = int(log2(volumeUnitResolution)); + const float invDepthFactor = 1.f / depthFactor; + const Intr::Reprojector reproj(intrinsics.makeReprojector()); + const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); + const Point3f truncPt(truncDist, truncDist, truncDist); + VolumeUnitIndexSet newIndices; + _VolumeUnitIndexSet _newIndices = cv::Mat(VOLUMES_SIZE, 1, rawType()); + Mutex mutex; + Range allocateRange(0, depth.rows); + int loc_vol_idx = 0; + int vol_idx = 0; + + auto AllocateVolumeUnitsInvoker = [&](const Range& range) { + VolumeUnitIndexSet localAccessVolUnits; + _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(VOLUMES_SIZE, 1, rawType()); + + + for (int y = range.start; y < range.end; y += depthStride) + { + const depthType* depthRow = depth[y]; + for (int x = 0; x < depth.cols; x += depthStride) + { + depthType z = depthRow[x] * invDepthFactor; + if (z <= 0 || z > this->truncateThreshold) + continue; + Point3f camPoint = reproj(Point3f((float)x, (float)y, z)); + Point3f volPoint = cam2vol * camPoint; + //! Find accessed TSDF volume unit for valid 3D vertex + Vec3i lower_bound = this->volumeToVolumeUnitIdx(volPoint - truncPt); + Vec3i upper_bound = this->volumeToVolumeUnitIdx(volPoint + truncPt); + + for (int i = lower_bound[0]; i <= upper_bound[0]; i++) + for (int j = lower_bound[1]; j <= upper_bound[1]; j++) + for (int k = lower_bound[2]; k <= lower_bound[2]; k++) + { + const Vec3i tsdf_idx = Vec3i(i, j, k); + if (!localAccessVolUnits.count(tsdf_idx)) + { + //! This volume unit will definitely be required for current integration + localAccessVolUnits.emplace(tsdf_idx); + } + + if (!_find(_localAccessVolUnits, tsdf_idx)) + { + _localAccessVolUnits.at(loc_vol_idx, 0) = tsdf_idx; + //std::cout << tsdf_idx << std::endl; + loc_vol_idx++; + } + + } + } + } + + //mutex.lock(); + for (const auto& tsdf_idx : localAccessVolUnits) + { + //! If the insert into the global set passes + if (!this->volumeUnits.count(tsdf_idx)) + { + // Volume allocation can be performed outside of the lock + this->volumeUnits.emplace(tsdf_idx, VolumeUnit()); + newIndices.emplace(tsdf_idx); + } + } + + + for (int i = 0; i < loc_vol_idx; i++) + { + Vec3i idx = _localAccessVolUnits.at(i, 0); + if (!_find(indexes, idx)) + { + //std::cout << idx << std::endl; + if (_lastVolIndex >= VolumeIndex(indexes.size().height)) + { + indexes.resize(_lastVolIndex * 2); + _volUnitsData.resize(_lastVolIndex * 2); + poses.resize(_lastVolIndex * 2); + activities.resize(_lastVolIndex * 2); + lastVisibleIndexes.resize(_lastVolIndex * 2); + } + + this->indexes.at(_lastVolIndex, 0) = idx; + //std::cout << this->indexes.at(_lastVolIndex, 0) << std::endl; + _newIndices.at(vol_idx, 0) = _lastVolIndex; + + vol_idx++; + _lastVolIndex++; + + } + } + //std::cout << this->indexes.at(_lastVolIndex-1, 0) << std::endl; + //mutex.unlock(); + }; + + //parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); + AllocateVolumeUnitsInvoker(allocateRange); + + //! Perform the allocation + for (auto idx : newIndices) + { + VolumeUnit& vu = volumeUnits[idx]; + Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; + + vu.pose = subvolumePose; + vu.index = lastVolIndex; lastVolIndex++; + if (lastVolIndex > VolumeIndex(volUnitsData.size().height)) + { + volUnitsData.resize((lastVolIndex - 1) * 2); + } + volUnitsData.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.lastVisibleIndex = frameId; + vu.isActive = true; + } + + + for (int i = 0; i < vol_idx; i++) + { + /* + if (_lastVolIndex >= VolumeIndex(_volUnitsData.size().height)) + { + _volUnitsData.resize(_lastVolIndex * 2); + poses.resize(_lastVolIndex * 2); + activities.resize(_lastVolIndex * 2); + lastVisibleIndexes.resize(_lastVolIndex * 2); + } + */ + + VolumeIndex idx = _newIndices.at(i, 0); + + Vec3i tsdf_idx = indexes.at(idx, 0); + //std::cout << idx << " " << tsdf_idx << std::endl; + Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; + + poses.at(idx, 0) = subvolumePose; + activities.at(idx, 0) = true; + lastVisibleIndexes.at(idx, 0) = frameId; + + _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) + { + TsdfVoxel& v = reinterpret_cast(vv); + v.tsdf = floatToTsdf(0.0f); v.weight = 0; + }); + + } + + + //! Get keys for all the allocated volume Units + std::vector totalVolUnits; + for (const auto& keyvalue : volumeUnits) { totalVolUnits.push_back(keyvalue.first); } + std::vector _totalVolUnits; + for (int i = 0; i < indexes.size().height; i++) { _totalVolUnits.push_back(indexes.at(i, 0)); } + + + //! Mark volumes in the camera frustum as active + Range inFrustumRange(0, (int)volumeUnits.size()); + parallel_for_(inFrustumRange, [&](const Range& range) { + const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + const Intr::Projector proj(intrinsics.makeProjector()); + + for (int i = range.start; i < range.end; ++i) + { + Vec3i tsdf_idx = totalVolUnits[i]; + VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx); + if (it == volumeUnits.end()) + return; + + Point3f volumeUnitPos = volumeUnitIdxToVolume(it->first); + Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; + if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) + { + it->second.isActive = false; + return; + } + Point2f cameraPoint = proj(volUnitInCamSpace); + if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) + { + assert(it != volumeUnits.end()); + it->second.lastVisibleIndex = frameId; + it->second.isActive = true; + } + } + }); + + Range _inFrustumRange(0, (int)_totalVolUnits.size()); + auto markActivities = [&](const Range& range) { + const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + const Intr::Projector proj(intrinsics.makeProjector()); + + for (int i = range.start; i < range.end; ++i) + { + Vec3i tsdf_idx = _totalVolUnits[i]; + VolumeIndex idx = find_idx(indexes, tsdf_idx); + //std::cout << _lastVolIndex << " " << idx << " " << tsdf_idx << std::endl; + if (idx < 0 || idx == _lastVolIndex - 1) return; + + Point3f volumeUnitPos = volumeUnitIdxToVolume(poses.at(idx, 0)); + Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; + + if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) + { + activities.at(idx, 0) = false; + return; + } + Point2f cameraPoint = proj(volUnitInCamSpace); + if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) + { + assert(idx == _lastVolIndex - 1); + lastVisibleIndexes.at(idx, 0) = frameId; + activities.at(idx, 0) = true; + } + } + }; + //parallel_for_(_inFrustumRange, markActivities); + markActivities(_inFrustumRange); + + Vec6f newParams((float)depth.rows, (float)depth.cols, + intrinsics.fx, intrinsics.fy, + intrinsics.cx, intrinsics.cy); + if (!(frameParams == newParams)) + { + frameParams = newParams; + 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]; + VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx); + if (it == volumeUnits.end()) + return; + + VolumeUnit& volumeUnit = it->second; + if (volumeUnit.isActive) + { + //! The volume unit should already be added into the Volume from the allocator + + integrateVolumeUnit(truncDist, voxelSize, maxWeight, volumeUnit.pose, + Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, + depthFactor, cameraPose, intrinsics, pixNorms, volUnitsData.row(volumeUnit.index)); + + //! Ensure all active volumeUnits are set to inactive for next integration + volumeUnit.isActive = false; + } + } + }); + + parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) { + for (int i = range.start; i < range.end; i++) + { + Vec3i tsdf_idx = totalVolUnits[i]; + VolumeIndex idx = find_idx(indexes, tsdf_idx); + if (idx < 0 || idx == _lastVolIndex - 1) return; + + bool& isActive = activities.at(idx, 0); + if (isActive) + { + //! The volume unit should already be added into the Volume from the allocator + Matx44f _pose = poses.at(idx, 0); + + integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, + Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, + depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); + + //! Ensure all active volumeUnits are set to inactive for next integration + isActive = false; + } + } + }); +} + +cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const +{ + return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), + cvFloor(p.z / volumeUnitSize)); +} + +cv::Point3f HashTSDFVolumeGPU::volumeUnitIdxToVolume(const cv::Vec3i& volumeUnitIdx) const +{ + return cv::Point3f(volumeUnitIdx[0] * volumeUnitSize, volumeUnitIdx[1] * volumeUnitSize, + volumeUnitIdx[2] * volumeUnitSize); +} + +cv::Point3f HashTSDFVolumeGPU::voxelCoordToVolume(const cv::Vec3i& voxelIdx) const +{ + return cv::Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); +} + +cv::Vec3i HashTSDFVolumeGPU::volumeToVoxelCoord(const cv::Point3f& point) const +{ + return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), + cvFloor(point.z * voxelSizeInv)); +} + +inline TsdfVoxel HashTSDFVolumeGPU::_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const +{ + //! Out of bounds + if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || + (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 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 = + volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2]; + return volData[coordBase]; +} +inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const +{ + //! Out of bounds + if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || + (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 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 = + volumeIdx[0] * volStrides[0] + + volumeIdx[1] * volStrides[1] + + volumeIdx[2] * volStrides[2]; + return volData[coordBase]; +} + +inline TsdfVoxel HashTSDFVolumeGPU::at(const cv::Vec3i& volumeIdx) const + +{ + Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), + cvFloor(volumeIdx[1] / volumeUnitResolution), + 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::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, + volumeUnitIdx[1] * volumeUnitResolution, + volumeUnitIdx[2] * volumeUnitResolution); + + volUnitLocalIdx = + cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + return _at(volUnitLocalIdx, it->second.index); + +} + +TsdfVoxel HashTSDFVolumeGPU::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::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); + cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); + volUnitLocalIdx = + cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + return _at(volUnitLocalIdx, it->second.index); +} +/* +static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) +{ + if (!(vuRes & (vuRes - 1))) + { + // vuRes is a power of 2, let's get this power + const int p2 = trailingZeros32(vuRes); + return Vec3i(pt[0] >> p2, pt[1] >> p2, pt[2] >> p2); + } + else + { + return Vec3i(cvFloor(float(pt[0]) / vuRes), + cvFloor(float(pt[1]) / vuRes), + cvFloor(float(pt[2]) / vuRes)); + } +} +*/ +TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const +{ + if (indx < 0 || indx > _lastVolIndex - 1) + { + TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.f); + dummy.weight = 0; + return dummy; + } + Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; + + // expanding at(), removing bounds check + const TsdfVoxel* volData = _volUnitsData.ptr(indx); + int coordBase = volUnitLocalIdx[0] * volStrides[0] + + volUnitLocalIdx[1] * volStrides[1] + + volUnitLocalIdx[2] * volStrides[2]; + return volData[coordBase]; +} + +TsdfVoxel HashTSDFVolumeGPU::atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const +{ + if (it == volumeUnits.end()) + { + TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.f); + dummy.weight = 0; + return dummy; + } + Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; + + // expanding at(), removing bounds check + 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]; +} + + +/* +#if USE_INTRINSICS +inline float interpolate(float tx, float ty, float tz, float vx[8]) +{ + v_float32x4 v0246, v1357; + v_load_deinterleave(vx, v0246, v1357); + + v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); + + v_float32x4 v00_10 = vxx; + v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx))); + + v_float32x4 v0_1 = v00_10 + v_setall_f32(ty) * (v01_11 - v00_10); + float v0 = v0_1.get0(); + v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); + float v1 = v0_1.get0(); + + return v0 + tx * (v1 - v0); +} + +#else +inline float interpolate(float tx, float ty, float tz, float vx[8]) +{ + float v00 = vx[0] + tz * (vx[1] - vx[0]); + float v01 = vx[2] + tz * (vx[3] - vx[2]); + float v10 = vx[4] + tz * (vx[5] - vx[4]); + float v11 = vx[6] + tz * (vx[7] - vx[6]); + + float v0 = v00 + ty * (v01 - v00); + float v1 = v10 + ty * (v11 - v10); + + return v0 + tx * (v1 - v0); +} +#endif +*/ +float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const +{ + const Vec3i neighbourCoords[] = { {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, + {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1} }; + + // A small hash table to reduce a number of find() calls + bool queried[8]; + VolumeUnitIndexes::const_iterator iterMap[8]; + for (int i = 0; i < 8; i++) + { + iterMap[i] = volumeUnits.end(); + queried[i] = false; + } + + int ix = cvFloor(point.x); + int iy = cvFloor(point.y); + int iz = cvFloor(point.z); + + float tx = point.x - ix; + float ty = point.y - iy; + float tz = point.z - iz; + + Vec3i iv(ix, iy, iz); + float vx[8]; + for (int i = 0; i < 8; i++) + { + Vec3i pt = iv + neighbourCoords[i]; + + Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); + int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; + auto it = iterMap[dictIdx]; + if (!queried[dictIdx]) + { + it = volumeUnits.find(volumeUnitIdx); + iterMap[dictIdx] = it; + queried[dictIdx] = true; + } + + vx[i] = atVolumeUnit(pt, volumeUnitIdx, it).tsdf; + } + + return interpolate(tx, ty, tz, vx); +} + +inline float HashTSDFVolumeGPU::interpolateVoxel(const cv::Point3f& point) const +{ + return interpolateVoxelPoint(point * voxelSizeInv); +} + + +Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const +{ + Vec3f normal = Vec3f(0, 0, 0); + + Point3f ptVox = point * voxelSizeInv; + Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); + + // A small hash table to reduce a number of find() calls + bool queried[8]; + VolumeUnitIndexes::const_iterator iterMap[8]; + for (int i = 0; i < 8; i++) + { + iterMap[i] = volumeUnits.end(); + queried[i] = false; + } + +#if !USE_INTERPOLATION_IN_GETNORMAL + const Vec3i offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3 + { 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7 + }; + const int nVals = 6; + +#else + const Vec3i offsets[] = { { 0, 0, 0}, { 0, 0, 1}, { 0, 1, 0}, { 0, 1, 1}, // 0-3 + { 1, 0, 0}, { 1, 0, 1}, { 1, 1, 0}, { 1, 1, 1}, // 4-7 + {-1, 0, 0}, {-1, 0, 1}, {-1, 1, 0}, {-1, 1, 1}, // 8-11 + { 2, 0, 0}, { 2, 0, 1}, { 2, 1, 0}, { 2, 1, 1}, // 12-15 + { 0, -1, 0}, { 0, -1, 1}, { 1, -1, 0}, { 1, -1, 1}, // 16-19 + { 0, 2, 0}, { 0, 2, 1}, { 1, 2, 0}, { 1, 2, 1}, // 20-23 + { 0, 0, -1}, { 0, 1, -1}, { 1, 0, -1}, { 1, 1, -1}, // 24-27 + { 0, 0, 2}, { 0, 1, 2}, { 1, 0, 2}, { 1, 1, 2}, // 28-31 + }; + const int nVals = 32; +#endif + + float vals[nVals]; + for (int i = 0; i < nVals; i++) + { + Vec3i pt = iptVox + offsets[i]; + + Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); + + int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; + auto it = iterMap[dictIdx]; + if (!queried[dictIdx]) + { + it = volumeUnits.find(volumeUnitIdx); + iterMap[dictIdx] = it; + queried[dictIdx] = true; + } + + vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it).tsdf); + } + +#if !USE_INTERPOLATION_IN_GETNORMAL + for (int c = 0; c < 3; c++) + { + normal[c] = vals[c * 2] - vals[c * 2 + 1]; + } +#else + + float cxv[8], cyv[8], czv[8]; + + // How these numbers were obtained: + // 1. Take the basic interpolation sequence: + // 000, 001, 010, 011, 100, 101, 110, 111 + // where each digit corresponds to shift by x, y, z axis respectively. + // 2. Add +1 for next or -1 for prev to each coordinate to corresponding axis + // 3. Search corresponding values in offsets + const int idxxp[8] = { 8, 9, 10, 11, 0, 1, 2, 3 }; + const int idxxn[8] = { 4, 5, 6, 7, 12, 13, 14, 15 }; + const int idxyp[8] = { 16, 17, 0, 1, 18, 19, 4, 5 }; + const int idxyn[8] = { 2, 3, 20, 21, 6, 7, 22, 23 }; + const int idxzp[8] = { 24, 0, 25, 2, 26, 4, 27, 6 }; + const int idxzn[8] = { 1, 28, 3, 29, 5, 30, 7, 31 }; + +#if !USE_INTRINSICS + for (int i = 0; i < 8; i++) + { + cxv[i] = vals[idxxn[i]] - vals[idxxp[i]]; + cyv[i] = vals[idxyn[i]] - vals[idxyp[i]]; + czv[i] = vals[idxzn[i]] - vals[idxzp[i]]; + } +#else + +# if CV_SIMD >= 32 + v_float32x8 cxp = v_lut(vals, idxxp); + v_float32x8 cxn = v_lut(vals, idxxn); + + v_float32x8 cyp = v_lut(vals, idxyp); + v_float32x8 cyn = v_lut(vals, idxyn); + + v_float32x8 czp = v_lut(vals, idxzp); + v_float32x8 czn = v_lut(vals, idxzn); + + v_float32x8 vcxv = cxn - cxp; + v_float32x8 vcyv = cyn - cyp; + v_float32x8 vczv = czn - czp; + + v_store(cxv, vcxv); + v_store(cyv, vcyv); + v_store(czv, vczv); +# else + v_float32x4 cxp0 = v_lut(vals, idxxp + 0); v_float32x4 cxp1 = v_lut(vals, idxxp + 4); + v_float32x4 cxn0 = v_lut(vals, idxxn + 0); v_float32x4 cxn1 = v_lut(vals, idxxn + 4); + + v_float32x4 cyp0 = v_lut(vals, idxyp + 0); v_float32x4 cyp1 = v_lut(vals, idxyp + 4); + v_float32x4 cyn0 = v_lut(vals, idxyn + 0); v_float32x4 cyn1 = v_lut(vals, idxyn + 4); + + v_float32x4 czp0 = v_lut(vals, idxzp + 0); v_float32x4 czp1 = v_lut(vals, idxzp + 4); + v_float32x4 czn0 = v_lut(vals, idxzn + 0); v_float32x4 czn1 = v_lut(vals, idxzn + 4); + + v_float32x4 cxv0 = cxn0 - cxp0; v_float32x4 cxv1 = cxn1 - cxp1; + v_float32x4 cyv0 = cyn0 - cyp0; v_float32x4 cyv1 = cyn1 - cyp1; + v_float32x4 czv0 = czn0 - czp0; v_float32x4 czv1 = czn1 - czp1; + + v_store(cxv + 0, cxv0); v_store(cxv + 4, cxv1); + v_store(cyv + 0, cyv0); v_store(cyv + 4, cyv1); + v_store(czv + 0, czv0); v_store(czv + 4, czv1); +#endif + +#endif + + float tx = ptVox.x - iptVox[0]; + float ty = ptVox.y - iptVox[1]; + float tz = ptVox.z - iptVox[2]; + + normal[0] = interpolate(tx, ty, tz, cxv); + normal[1] = interpolate(tx, ty, tz, cyv); + normal[2] = interpolate(tx, ty, tz, czv); +#endif + + float nv = sqrt(normal[0] * normal[0] + + normal[1] * normal[1] + + normal[2] * normal[2]); + return nv < 0.0001f ? nan3 : normal / nv; +} + +Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const +{ + Vec3f normal = Vec3f(0, 0, 0); + + Point3f ptVox = point * voxelSizeInv; + Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); + + // A small hash table to reduce a number of find() calls + bool queried[8]; + //VolumeUnitIndexes::const_iterator iterMap[8]; + VolumeIndex iterMap[8]; + + for (int i = 0; i < 8; i++) + { + //iterMap[i] = volumeUnits.end(); + iterMap[i] = _lastVolIndex - 1; + queried[i] = false; + } + +#if !USE_INTERPOLATION_IN_GETNORMAL + const Vec3i offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3 + { 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7 + }; + const int nVals = 6; + +#else + const Vec3i offsets[] = { { 0, 0, 0}, { 0, 0, 1}, { 0, 1, 0}, { 0, 1, 1}, // 0-3 + { 1, 0, 0}, { 1, 0, 1}, { 1, 1, 0}, { 1, 1, 1}, // 4-7 + {-1, 0, 0}, {-1, 0, 1}, {-1, 1, 0}, {-1, 1, 1}, // 8-11 + { 2, 0, 0}, { 2, 0, 1}, { 2, 1, 0}, { 2, 1, 1}, // 12-15 + { 0, -1, 0}, { 0, -1, 1}, { 1, -1, 0}, { 1, -1, 1}, // 16-19 + { 0, 2, 0}, { 0, 2, 1}, { 1, 2, 0}, { 1, 2, 1}, // 20-23 + { 0, 0, -1}, { 0, 1, -1}, { 1, 0, -1}, { 1, 1, -1}, // 24-27 + { 0, 0, 2}, { 0, 1, 2}, { 1, 0, 2}, { 1, 1, 2}, // 28-31 + }; + const int nVals = 32; +#endif + + float vals[nVals]; + for (int i = 0; i < nVals; i++) + { + Vec3i pt = iptVox + offsets[i]; + + Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); + + int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; + auto it = iterMap[dictIdx]; + + if (!queried[dictIdx]) + { + //it = volumeUnits.find(volumeUnitIdx); + it = find_idx(indexes, volumeUnitIdx); + if (it >= 0 || it < _lastVolIndex - 1) + { + iterMap[dictIdx] = it; + queried[dictIdx] = true; + } + } + + vals[i] = tsdfToFloat(new_atVolumeUnit(pt, volumeUnitIdx, it).tsdf); + } + +#if !USE_INTERPOLATION_IN_GETNORMAL + for (int c = 0; c < 3; c++) + { + normal[c] = vals[c * 2] - vals[c * 2 + 1]; + } +#else + + float cxv[8], cyv[8], czv[8]; + + // How these numbers were obtained: + // 1. Take the basic interpolation sequence: + // 000, 001, 010, 011, 100, 101, 110, 111 + // where each digit corresponds to shift by x, y, z axis respectively. + // 2. Add +1 for next or -1 for prev to each coordinate to corresponding axis + // 3. Search corresponding values in offsets + const int idxxp[8] = { 8, 9, 10, 11, 0, 1, 2, 3 }; + const int idxxn[8] = { 4, 5, 6, 7, 12, 13, 14, 15 }; + const int idxyp[8] = { 16, 17, 0, 1, 18, 19, 4, 5 }; + const int idxyn[8] = { 2, 3, 20, 21, 6, 7, 22, 23 }; + const int idxzp[8] = { 24, 0, 25, 2, 26, 4, 27, 6 }; + const int idxzn[8] = { 1, 28, 3, 29, 5, 30, 7, 31 }; + +#if !USE_INTRINSICS + for (int i = 0; i < 8; i++) + { + cxv[i] = vals[idxxn[i]] - vals[idxxp[i]]; + cyv[i] = vals[idxyn[i]] - vals[idxyp[i]]; + czv[i] = vals[idxzn[i]] - vals[idxzp[i]]; + } +#else + +# if CV_SIMD >= 32 + v_float32x8 cxp = v_lut(vals, idxxp); + v_float32x8 cxn = v_lut(vals, idxxn); + + v_float32x8 cyp = v_lut(vals, idxyp); + v_float32x8 cyn = v_lut(vals, idxyn); + + v_float32x8 czp = v_lut(vals, idxzp); + v_float32x8 czn = v_lut(vals, idxzn); + + v_float32x8 vcxv = cxn - cxp; + v_float32x8 vcyv = cyn - cyp; + v_float32x8 vczv = czn - czp; + + v_store(cxv, vcxv); + v_store(cyv, vcyv); + v_store(czv, vczv); +# else + v_float32x4 cxp0 = v_lut(vals, idxxp + 0); v_float32x4 cxp1 = v_lut(vals, idxxp + 4); + v_float32x4 cxn0 = v_lut(vals, idxxn + 0); v_float32x4 cxn1 = v_lut(vals, idxxn + 4); + + v_float32x4 cyp0 = v_lut(vals, idxyp + 0); v_float32x4 cyp1 = v_lut(vals, idxyp + 4); + v_float32x4 cyn0 = v_lut(vals, idxyn + 0); v_float32x4 cyn1 = v_lut(vals, idxyn + 4); + + v_float32x4 czp0 = v_lut(vals, idxzp + 0); v_float32x4 czp1 = v_lut(vals, idxzp + 4); + v_float32x4 czn0 = v_lut(vals, idxzn + 0); v_float32x4 czn1 = v_lut(vals, idxzn + 4); + + v_float32x4 cxv0 = cxn0 - cxp0; v_float32x4 cxv1 = cxn1 - cxp1; + v_float32x4 cyv0 = cyn0 - cyp0; v_float32x4 cyv1 = cyn1 - cyp1; + v_float32x4 czv0 = czn0 - czp0; v_float32x4 czv1 = czn1 - czp1; + + v_store(cxv + 0, cxv0); v_store(cxv + 4, cxv1); + v_store(cyv + 0, cyv0); v_store(cyv + 4, cyv1); + v_store(czv + 0, czv0); v_store(czv + 4, czv1); +#endif + +#endif + + float tx = ptVox.x - iptVox[0]; + float ty = ptVox.y - iptVox[1]; + float tz = ptVox.z - iptVox[2]; + + normal[0] = interpolate(tx, ty, tz, cxv); + normal[1] = interpolate(tx, ty, tz, cyv); + normal[2] = interpolate(tx, ty, tz, czv); +#endif + + float nv = sqrt(normal[0] * normal[0] + + normal[1] * normal[1] + + normal[2] * normal[2]); + return nv < 0.0001f ? nan3 : normal / nv; + //return Point3f(0, 0, 0); +} + +void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, + OutputArray _points, OutputArray _normals) const +{ + 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); + + Points& new_points(points1); + //Normals& new_normals(normals1); + + const HashTSDFVolumeGPU& 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; + + auto HashRaycastInvoker = [&](const Range& range) + { + const Point3f cam2volTrans = cam2vol.translation(); + const Matx33f cam2volRot = cam2vol.rotation(); + const Matx33f vol2camRot = vol2cam.rotation(); + + const float blockSize = volume.volumeUnitSize; + + for (int y = range.start; y < range.end; y++) + { + ptype* ptsRow = points[y]; + ptype* nrmRow = normals[y]; + + for (int x = 0; x < points.cols; x++) + { + //! Initialize default value + Point3f point = nan3, normal = nan3; + + //! Ray origin and direction in the volume coordinate frame + Point3f orig = cam2volTrans; + Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); + + 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()); + + + float tprev = tcurr; + float prevTsdf = volume.truncDist; + Ptr currVolumeUnit; + while (tcurr < tmax) + { + 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; + cv::Vec3i volUnitLocalIdx; + + + //! The subvolume exists in hashtable + if (it != volume.volumeUnits.end()) + { + cv::Point3f currVolUnitPos = + volume.volumeUnitIdxToVolume(currVolumeUnitIdx); + volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); + + + //! TODO: Figure out voxel interpolation + TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index); + currTsdf = tsdfToFloat(currVoxel.tsdf); + currWeight = currVoxel.weight; + stepSize = tstep; + } + //! Surface crossing + if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) + { + float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); + if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) + { + Point3f pv = orig + tInterp * rayDirV; + Point3f nv = volume.getNormalVoxel(pv); + + if (!isNaN(nv)) + { + normal = vol2camRot * nv; + point = vol2cam * pv; + } + } + break; + } + prevVolumeUnitIdx = currVolumeUnitIdx; + prevTsdf = currTsdf; + tprev = tcurr; + tcurr += stepSize; + } + ptsRow[x] = toPtype(point); + nrmRow[x] = toPtype(normal); + } + } + }; + + parallel_for_(Range(0, points.rows), HashRaycastInvoker, nstripes); + + auto _truncDist = truncDist; + auto _HashRaycastInvoker = [&](const Range& range) + { + const Point3f cam2volTrans = cam2vol.translation(); + const Matx33f cam2volRot = cam2vol.rotation(); + const Matx33f vol2camRot = vol2cam.rotation(); + + const float blockSize = volume.volumeUnitSize; + + for (int y = range.start; y < range.end; y++) + { + //ptype* _ptsRow = new_points[y]; + //ptype* _nrmRow = new_normals[y]; + + for (int x = 0; x < new_points.cols; x++) + { + //! Initialize default value + Point3f point = nan3, normal = nan3; + + //! Ray origin and direction in the volume coordinate frame + Point3f orig = cam2volTrans; + Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); + + 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()); + + float tprev = tcurr; + float prevTsdf = _truncDist; + Ptr currVolumeUnit; + + while (tcurr < tmax) + { + tmax -= 1; + + Point3f currRayPos = orig + tcurr * rayDirV; + cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); + + + //VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); + VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); + + float currTsdf = prevTsdf; + int currWeight = 0; + float stepSize = 0.5f * blockSize; + cv::Vec3i volUnitLocalIdx; + + + //! The subvolume exists in hashtable + //if (it != volume.volumeUnits.end()) + if (idx != _lastVolIndex - 1 && idx >= 0) + { + cv::Point3f currVolUnitPos = + volume.volumeUnitIdxToVolume(currVolumeUnitIdx); + volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); + + + //! TODO: Figure out voxel interpolation + //std::cout << indexes << std::endl; + //std::cout << currVolumeUnitIdx << " " << idx << " " << _volUnitsData.size().height << std::endl; + + TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); + currTsdf = tsdfToFloat(currVoxel.tsdf); + currWeight = currVoxel.weight; + stepSize = tstep; + } + + //! Surface crossing + if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) + { + float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); + if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) + { + Point3f pv = orig + tInterp * rayDirV; + Point3f nv = volume.getNormalVoxel(pv); + + if (!isNaN(nv)) + { + normal = vol2camRot * nv; + point = vol2cam * pv; + } + } + break; + } + + prevVolumeUnitIdx = currVolumeUnitIdx; + prevTsdf = currTsdf; + tprev = tcurr; + tcurr += stepSize; + + } + //_ptsRow[x] = toPtype(point); + //_nrmRow[x] = toPtype(normal); + + } + + } + + }; + + parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); + //_HashRaycastInvoker(Range(0, new_points.rows)); +} + +void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const +{ + CV_TRACE_FUNCTION(); + + if (_points.needed()) + { + std::vector> pVecs, nVecs; + + std::vector totalVolUnits; + for (const auto& keyvalue : volumeUnits) + { + totalVolUnits.push_back(keyvalue.first); + } + + std::vector> _pVecs, _nVecs; + + std::vector _totalVolUnits; + for (int i = 0; i < indexes.size().height; i++) + { + _totalVolUnits.push_back(indexes.at(i, 0)); + } + + Range fetchRange(0, (int)totalVolUnits.size()); + Range _fetchRange(0, (int)_totalVolUnits.size()); + + const int nstripes = -1; + + const HashTSDFVolumeGPU& volume(*this); + bool needNormals(_normals.needed()); + Mutex mutex; + + + auto HashFetchPointsNormalsInvoker = [&](const Range& range) + { + std::vector points, normals; + for (int i = range.start; i < range.end; i++) + { + cv::Vec3i tsdf_idx = totalVolUnits[i]; + + + VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(tsdf_idx); + Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); + if (it != volume.volumeUnits.end()) + { + 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 = _at(voxelIdx, it->second.index); + + if (voxel.tsdf != -128 && voxel.weight != 0) + { + 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); + } + } + }; + + parallel_for_(fetchRange, HashFetchPointsNormalsInvoker, nstripes); + + + + auto _HashFetchPointsNormalsInvoker = [&](const Range& range) + { + std::vector points, normals; + for (int i = range.start; i < range.end; i++) + { + cv::Vec3i tsdf_idx = _totalVolUnits[i]; + + + //VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(tsdf_idx); + VolumeIndex idx = find_idx(indexes, tsdf_idx); + + Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); + + + if (idx >= 0 || idx < _lastVolIndex - 1) + { + 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 = new_at(voxelIdx, idx); + + if (voxel.tsdf != -128 && voxel.weight != 0) + { + Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); + localPoints.push_back(toPtype(point)); + if (needNormals) + { + Point3f normal = volume._getNormalVoxel(point); + //Point3f normal(0,0,0); + localNormals.push_back(toPtype(normal)); + } + } + } + + AutoLock al(mutex); + _pVecs.push_back(localPoints); + _nVecs.push_back(localNormals); + //pVecs.push_back(localPoints); + //nVecs.push_back(localNormals); + } + } + }; + + parallel_for_(_fetchRange, _HashFetchPointsNormalsInvoker, nstripes); + + + + std::vector points, normals; + for (size_t i = 0; i < pVecs.size(); i++) + { + points.insert(points.end(), pVecs[i].begin(), pVecs[i].end()); + normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end()); + } + + _points.create((int)points.size(), 1, POINT_TYPE); + if (!points.empty()) + Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat()); + + if (_normals.needed()) + { + _normals.create((int)normals.size(), 1, POINT_TYPE); + if (!normals.empty()) + Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat()); + } + } +} + +void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const +{ + CV_TRACE_FUNCTION(); + + if (_normals.needed()) + { + Points points = _points.getMat(); + CV_Assert(points.type() == POINT_TYPE); + + _normals.createSameSize(_points, _points.type()); + Normals normals = _normals.getMat(); + + const HashTSDFVolumeGPU& _volume = *this; + auto HashPushNormals = [&](const ptype& point, const int* position) { + const HashTSDFVolumeGPU& volume(_volume); + Affine3f invPose(volume.pose.inv()); + Point3f p = fromPtype(point); + Point3f n = nan3; + if (!isNaN(p)) + { + Point3f voxelPoint = invPose * p; + n = volume.pose.rotation() * volume.getNormalVoxel(voxelPoint); + } + normals(position[0], position[1]) = toPtype(n); + }; + points.forEach(HashPushNormals); + } + /* + if (_normals.needed()) + { + Points points = _points.getMat(); + CV_Assert(points.type() == POINT_TYPE); + _normals.createSameSize(_points, _points.type()); + Normals normals = _normals.getMat(); + const HashTSDFVolumeCPU& _volume = *this; + auto HashPushNormals = [&](const ptype& point, const int* position) { + const HashTSDFVolumeCPU& volume(_volume); + Affine3f invPose(volume.pose.inv()); + Point3f p = fromPtype(point); + Point3f n = nan3; + if (!isNaN(p)) + { + Point3f voxelPoint = invPose * p; + n = volume.pose.rotation() * volume._getNormalVoxel(voxelPoint); + } + normals(position[0], position[1]) = toPtype(n); + }; + points.forEach(HashPushNormals); + } + */ +} + +int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) const +{ + int numVisibleBlocks = 0; + //! TODO: Iterate over map parallely? + for (const auto& keyvalue : volumeUnits) + { + const VolumeUnit& volumeUnit = keyvalue.second; + if (volumeUnit.lastVisibleIndex > (currFrameId - frameThreshold)) + numVisibleBlocks++; + } + return numVisibleBlocks; +} + + + #endif +//template +Ptr makeHashTSDFVolume(const VolumeParams& _volumeParams) +{ +#ifdef HAVE_OPENCL + if (ocl::useOpenCL()) + return makePtr(_volumeParams); +#endif + return makePtr(_volumeParams); +} + +//template +Ptr makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float truncateThreshold, int volumeUnitResolution) +{ +#ifdef HAVE_OPENCL + if (ocl::useOpenCL()) + return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, + volumeUnitResolution); +#endif + return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, + volumeUnitResolution); +} diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 28e3b456160..95ded0a9a07 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -122,6 +122,7 @@ typedef cv::Mat _VolumeUnitIndexes; class HashTSDFVolumeGPU : public HashTSDFVolume { +public: HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); @@ -129,9 +130,42 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void reset() override; + void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, + const int frameId = 0) override; + void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, + OutputArray normals) const override; + + void fetchNormals(InputArray points, OutputArray _normals) const override; + void fetchPointsNormals(OutputArray points, OutputArray normals) const override; + + size_t getTotalVolumeUnits() const { return volumeUnits.size(); } + int getVisibleBlocks(int currFrameId, int frameThreshold) const; + + //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) + TsdfVoxel at(const Vec3i& volumeIdx) const; + + //! 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 new_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const; + + TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const; + TsdfVoxel new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const; + + + float interpolateVoxelPoint(const Point3f& point) const; + float interpolateVoxel(const cv::Point3f& point) const; + Point3f getNormalVoxel(const cv::Point3f& p) const; + Point3f _getNormalVoxel(const cv::Point3f& p) const; + + //! Utility functions for coordinate transformations + Vec3i volumeToVolumeUnitIdx(const Point3f& point) const; + Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const; + + Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; + Vec3i volumeToVoxelCoord(const Point3f& point) const; - //void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; - //void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; public: Vec4i volStrides; @@ -151,27 +185,10 @@ class HashTSDFVolumeGPU : public HashTSDFVolume #endif //template -Ptr makeHashTSDFVolume(const VolumeParams& _volumeParams) -{ -#ifdef HAVE_OPENCL - if (ocl::useOpenCL()) - return makePtr(_volumeParams); -#endif - return makePtr(_volumeParams); -} - +Ptr makeHashTSDFVolume(const VolumeParams& _volumeParams); //template Ptr makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, - int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16) -{ -#ifdef HAVE_OPENCL - if (ocl::useOpenCL()) - return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, - volumeUnitResolution); -#endif - return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, - volumeUnitResolution); -} + int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16); } // namespace kinfu } // namespace cv From afe9448a2a19e3bff8af976b9071f46ba06b2b40 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 2 Nov 2020 10:37:43 +0300 Subject: [PATCH 004/216] minor fi --- modules/rgbd/src/hash_tsdf.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a73a2ee0390..5d76aa6f792 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -854,7 +854,13 @@ void HashTSDFVolumeGPU::reset() { CV_TRACE_FUNCTION(); lastVolIndex = 0; + _lastVolIndex = 0; volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + _volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + indexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); + poses = cv::Mat(VOLUMES_SIZE, 1, rawType()); + activities = cv::Mat(VOLUMES_SIZE, 1, rawType()); + lastVisibleIndexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); } From 1acff75f6a0a3c631958245a029f3512749370f2 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 2 Nov 2020 11:29:14 +0300 Subject: [PATCH 005/216] raycast - done --- modules/rgbd/src/hash_tsdf.cpp | 289 ++++----------------------------- modules/rgbd/src/hash_tsdf.hpp | 2 +- 2 files changed, 30 insertions(+), 261 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 5d76aa6f792..958d37869e6 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -908,7 +908,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma const Intr::Reprojector reproj(intrinsics.makeReprojector()); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Point3f truncPt(truncDist, truncDist, truncDist); - VolumeUnitIndexSet newIndices; _VolumeUnitIndexSet _newIndices = cv::Mat(VOLUMES_SIZE, 1, rawType()); Mutex mutex; Range allocateRange(0, depth.rows); @@ -916,7 +915,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma int vol_idx = 0; auto AllocateVolumeUnitsInvoker = [&](const Range& range) { - VolumeUnitIndexSet localAccessVolUnits; _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(VOLUMES_SIZE, 1, rawType()); @@ -939,11 +937,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma for (int k = lower_bound[2]; k <= lower_bound[2]; k++) { const Vec3i tsdf_idx = Vec3i(i, j, k); - if (!localAccessVolUnits.count(tsdf_idx)) - { - //! This volume unit will definitely be required for current integration - localAccessVolUnits.emplace(tsdf_idx); - } if (!_find(_localAccessVolUnits, tsdf_idx)) { @@ -957,17 +950,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //mutex.lock(); - for (const auto& tsdf_idx : localAccessVolUnits) - { - //! If the insert into the global set passes - if (!this->volumeUnits.count(tsdf_idx)) - { - // Volume allocation can be performed outside of the lock - this->volumeUnits.emplace(tsdf_idx, VolumeUnit()); - newIndices.emplace(tsdf_idx); - } - } - for (int i = 0; i < loc_vol_idx; i++) { @@ -1001,39 +983,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma AllocateVolumeUnitsInvoker(allocateRange); //! Perform the allocation - for (auto idx : newIndices) - { - VolumeUnit& vu = volumeUnits[idx]; - Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; - - vu.pose = subvolumePose; - vu.index = lastVolIndex; lastVolIndex++; - if (lastVolIndex > VolumeIndex(volUnitsData.size().height)) - { - volUnitsData.resize((lastVolIndex - 1) * 2); - } - volUnitsData.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.lastVisibleIndex = frameId; - vu.isActive = true; - } - for (int i = 0; i < vol_idx; i++) { - /* - if (_lastVolIndex >= VolumeIndex(_volUnitsData.size().height)) - { - _volUnitsData.resize(_lastVolIndex * 2); - poses.resize(_lastVolIndex * 2); - activities.resize(_lastVolIndex * 2); - lastVisibleIndexes.resize(_lastVolIndex * 2); - } - */ VolumeIndex idx = _newIndices.at(i, 0); @@ -1055,42 +1007,14 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! Get keys for all the allocated volume Units - std::vector totalVolUnits; - for (const auto& keyvalue : volumeUnits) { totalVolUnits.push_back(keyvalue.first); } std::vector _totalVolUnits; - for (int i = 0; i < indexes.size().height; i++) { _totalVolUnits.push_back(indexes.at(i, 0)); } + for (int i = 0; i < indexes.size().height; i++) + { + _totalVolUnits.push_back(indexes.at(i, 0)); + } //! Mark volumes in the camera frustum as active - Range inFrustumRange(0, (int)volumeUnits.size()); - parallel_for_(inFrustumRange, [&](const Range& range) { - const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); - const Intr::Projector proj(intrinsics.makeProjector()); - - for (int i = range.start; i < range.end; ++i) - { - Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx); - if (it == volumeUnits.end()) - return; - - Point3f volumeUnitPos = volumeUnitIdxToVolume(it->first); - Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; - if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) - { - it->second.isActive = false; - return; - } - Point2f cameraPoint = proj(volUnitInCamSpace); - if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) - { - assert(it != volumeUnits.end()); - it->second.lastVisibleIndex = frameId; - it->second.isActive = true; - } - } - }); - Range _inFrustumRange(0, (int)_totalVolUnits.size()); auto markActivities = [&](const Range& range) { const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); @@ -1133,33 +1057,10 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //! Integrate the correct volumeUnits - parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) { + parallel_for_(Range(0, (int)_totalVolUnits.size()), [&](const Range& range) { for (int i = range.start; i < range.end; i++) { - Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx); - if (it == volumeUnits.end()) - return; - - VolumeUnit& volumeUnit = it->second; - if (volumeUnit.isActive) - { - //! The volume unit should already be added into the Volume from the allocator - - integrateVolumeUnit(truncDist, voxelSize, maxWeight, volumeUnit.pose, - Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, - depthFactor, cameraPose, intrinsics, pixNorms, volUnitsData.row(volumeUnit.index)); - - //! Ensure all active volumeUnits are set to inactive for next integration - volumeUnit.isActive = false; - } - } - }); - - parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) { - for (int i = range.start; i < range.end; i++) - { - Vec3i tsdf_idx = totalVolUnits[i]; + Vec3i tsdf_idx = _totalVolUnits[i]; VolumeIndex idx = find_idx(indexes, tsdf_idx); if (idx < 0 || idx == _lastVolIndex - 1) return; @@ -1226,7 +1127,8 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeInd //! Out of bounds 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) || + indx > _lastVolIndex) { TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -1426,145 +1328,6 @@ inline float HashTSDFVolumeGPU::interpolateVoxel(const cv::Point3f& point) const return interpolateVoxelPoint(point * voxelSizeInv); } - -Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const -{ - Vec3f normal = Vec3f(0, 0, 0); - - Point3f ptVox = point * voxelSizeInv; - Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); - - // A small hash table to reduce a number of find() calls - bool queried[8]; - VolumeUnitIndexes::const_iterator iterMap[8]; - for (int i = 0; i < 8; i++) - { - iterMap[i] = volumeUnits.end(); - queried[i] = false; - } - -#if !USE_INTERPOLATION_IN_GETNORMAL - const Vec3i offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3 - { 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7 - }; - const int nVals = 6; - -#else - const Vec3i offsets[] = { { 0, 0, 0}, { 0, 0, 1}, { 0, 1, 0}, { 0, 1, 1}, // 0-3 - { 1, 0, 0}, { 1, 0, 1}, { 1, 1, 0}, { 1, 1, 1}, // 4-7 - {-1, 0, 0}, {-1, 0, 1}, {-1, 1, 0}, {-1, 1, 1}, // 8-11 - { 2, 0, 0}, { 2, 0, 1}, { 2, 1, 0}, { 2, 1, 1}, // 12-15 - { 0, -1, 0}, { 0, -1, 1}, { 1, -1, 0}, { 1, -1, 1}, // 16-19 - { 0, 2, 0}, { 0, 2, 1}, { 1, 2, 0}, { 1, 2, 1}, // 20-23 - { 0, 0, -1}, { 0, 1, -1}, { 1, 0, -1}, { 1, 1, -1}, // 24-27 - { 0, 0, 2}, { 0, 1, 2}, { 1, 0, 2}, { 1, 1, 2}, // 28-31 - }; - const int nVals = 32; -#endif - - float vals[nVals]; - for (int i = 0; i < nVals; i++) - { - Vec3i pt = iptVox + offsets[i]; - - Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); - - int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; - auto it = iterMap[dictIdx]; - if (!queried[dictIdx]) - { - it = volumeUnits.find(volumeUnitIdx); - iterMap[dictIdx] = it; - queried[dictIdx] = true; - } - - vals[i] = tsdfToFloat(atVolumeUnit(pt, volumeUnitIdx, it).tsdf); - } - -#if !USE_INTERPOLATION_IN_GETNORMAL - for (int c = 0; c < 3; c++) - { - normal[c] = vals[c * 2] - vals[c * 2 + 1]; - } -#else - - float cxv[8], cyv[8], czv[8]; - - // How these numbers were obtained: - // 1. Take the basic interpolation sequence: - // 000, 001, 010, 011, 100, 101, 110, 111 - // where each digit corresponds to shift by x, y, z axis respectively. - // 2. Add +1 for next or -1 for prev to each coordinate to corresponding axis - // 3. Search corresponding values in offsets - const int idxxp[8] = { 8, 9, 10, 11, 0, 1, 2, 3 }; - const int idxxn[8] = { 4, 5, 6, 7, 12, 13, 14, 15 }; - const int idxyp[8] = { 16, 17, 0, 1, 18, 19, 4, 5 }; - const int idxyn[8] = { 2, 3, 20, 21, 6, 7, 22, 23 }; - const int idxzp[8] = { 24, 0, 25, 2, 26, 4, 27, 6 }; - const int idxzn[8] = { 1, 28, 3, 29, 5, 30, 7, 31 }; - -#if !USE_INTRINSICS - for (int i = 0; i < 8; i++) - { - cxv[i] = vals[idxxn[i]] - vals[idxxp[i]]; - cyv[i] = vals[idxyn[i]] - vals[idxyp[i]]; - czv[i] = vals[idxzn[i]] - vals[idxzp[i]]; - } -#else - -# if CV_SIMD >= 32 - v_float32x8 cxp = v_lut(vals, idxxp); - v_float32x8 cxn = v_lut(vals, idxxn); - - v_float32x8 cyp = v_lut(vals, idxyp); - v_float32x8 cyn = v_lut(vals, idxyn); - - v_float32x8 czp = v_lut(vals, idxzp); - v_float32x8 czn = v_lut(vals, idxzn); - - v_float32x8 vcxv = cxn - cxp; - v_float32x8 vcyv = cyn - cyp; - v_float32x8 vczv = czn - czp; - - v_store(cxv, vcxv); - v_store(cyv, vcyv); - v_store(czv, vczv); -# else - v_float32x4 cxp0 = v_lut(vals, idxxp + 0); v_float32x4 cxp1 = v_lut(vals, idxxp + 4); - v_float32x4 cxn0 = v_lut(vals, idxxn + 0); v_float32x4 cxn1 = v_lut(vals, idxxn + 4); - - v_float32x4 cyp0 = v_lut(vals, idxyp + 0); v_float32x4 cyp1 = v_lut(vals, idxyp + 4); - v_float32x4 cyn0 = v_lut(vals, idxyn + 0); v_float32x4 cyn1 = v_lut(vals, idxyn + 4); - - v_float32x4 czp0 = v_lut(vals, idxzp + 0); v_float32x4 czp1 = v_lut(vals, idxzp + 4); - v_float32x4 czn0 = v_lut(vals, idxzn + 0); v_float32x4 czn1 = v_lut(vals, idxzn + 4); - - v_float32x4 cxv0 = cxn0 - cxp0; v_float32x4 cxv1 = cxn1 - cxp1; - v_float32x4 cyv0 = cyn0 - cyp0; v_float32x4 cyv1 = cyn1 - cyp1; - v_float32x4 czv0 = czn0 - czp0; v_float32x4 czv1 = czn1 - czp1; - - v_store(cxv + 0, cxv0); v_store(cxv + 4, cxv1); - v_store(cyv + 0, cyv0); v_store(cyv + 4, cyv1); - v_store(czv + 0, czv0); v_store(czv + 4, czv1); -#endif - -#endif - - float tx = ptVox.x - iptVox[0]; - float ty = ptVox.y - iptVox[1]; - float tz = ptVox.z - iptVox[2]; - - normal[0] = interpolate(tx, ty, tz, cxv); - normal[1] = interpolate(tx, ty, tz, cyv); - normal[2] = interpolate(tx, ty, tz, czv); -#endif - - float nv = sqrt(normal[0] * normal[0] + - normal[1] * normal[1] + - normal[2] * normal[2]); - return nv < 0.0001f ? nan3 : normal / nv; -} - Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const { Vec3f normal = Vec3f(0, 0, 0); @@ -1723,11 +1486,11 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Points points1 = _points.getMat(); Normals normals1 = _normals.getMat(); - Points& points(points1); - Normals& normals(normals1); + //Points& points(points1); + //Normals& normals(normals1); Points& new_points(points1); - //Normals& new_normals(normals1); + Normals& new_normals(normals1); const HashTSDFVolumeGPU& volume(*this); const float tstep(volume.truncDist * volume.raycastStepFactor); @@ -1736,7 +1499,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in const Intr::Reprojector reproj(intrinsics.makeReprojector()); const int nstripes = -1; - + /* auto HashRaycastInvoker = [&](const Range& range) { const Point3f cam2volTrans = cam2vol.translation(); @@ -1828,10 +1591,11 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in }; parallel_for_(Range(0, points.rows), HashRaycastInvoker, nstripes); - + */ auto _truncDist = truncDist; auto _HashRaycastInvoker = [&](const Range& range) { + const Point3f cam2volTrans = cam2vol.translation(); const Matx33f cam2volRot = cam2vol.rotation(); const Matx33f vol2camRot = vol2cam.rotation(); @@ -1840,8 +1604,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in for (int y = range.start; y < range.end; y++) { - //ptype* _ptsRow = new_points[y]; - //ptype* _nrmRow = new_normals[y]; + ptype* _ptsRow = new_points[y]; + ptype* _nrmRow = new_normals[y]; for (int x = 0; x < new_points.cols; x++) { @@ -1895,6 +1659,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in //std::cout << currVolumeUnitIdx << " " << idx << " " << _volUnitsData.size().height << std::endl; TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); + //TsdfVoxel currVoxel; currVoxel.tsdf = floatToTsdf(1.0f); currVoxel.weight = 0; + currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -1907,8 +1673,9 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { Point3f pv = orig + tInterp * rayDirV; - Point3f nv = volume.getNormalVoxel(pv); - + Point3f nv = volume._getNormalVoxel(pv); + //Point3f nv = nan3; + if (!isNaN(nv)) { normal = vol2camRot * nv; @@ -1924,13 +1691,14 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in tcurr += stepSize; } - //_ptsRow[x] = toPtype(point); - //_nrmRow[x] = toPtype(normal); + + _ptsRow[x] = toPtype(point); + _nrmRow[x] = toPtype(normal); } } - + }; parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); @@ -1968,7 +1736,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor bool needNormals(_normals.needed()); Mutex mutex; - + /* auto HashFetchPointsNormalsInvoker = [&](const Range& range) { std::vector points, normals; @@ -2010,7 +1778,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor }; parallel_for_(fetchRange, HashFetchPointsNormalsInvoker, nstripes); - + */ auto _HashFetchPointsNormalsInvoker = [&](const Range& range) @@ -2087,7 +1855,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); - + /* if (_normals.needed()) { Points points = _points.getMat(); @@ -2111,6 +1879,7 @@ void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) c }; points.forEach(HashPushNormals); } + */ /* if (_normals.needed()) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 95ded0a9a07..ae8fa1dfd21 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -156,7 +156,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume float interpolateVoxelPoint(const Point3f& point) const; float interpolateVoxel(const cv::Point3f& point) const; - Point3f getNormalVoxel(const cv::Point3f& p) const; + //Point3f getNormalVoxel(const cv::Point3f& p) const; Point3f _getNormalVoxel(const cv::Point3f& p) const; //! Utility functions for coordinate transformations From 6ba4dbac64df6f3eb089ff02870023cfa6c67194 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 2 Nov 2020 11:50:51 +0300 Subject: [PATCH 006/216] all code builds --- modules/rgbd/src/hash_tsdf.cpp | 83 +++------------------------------- 1 file changed, 7 insertions(+), 76 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 958d37869e6..ef06c6634de 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1736,50 +1736,6 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor bool needNormals(_normals.needed()); Mutex mutex; - /* - auto HashFetchPointsNormalsInvoker = [&](const Range& range) - { - std::vector points, normals; - for (int i = range.start; i < range.end; i++) - { - cv::Vec3i tsdf_idx = totalVolUnits[i]; - - - VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(tsdf_idx); - Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); - if (it != volume.volumeUnits.end()) - { - 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 = _at(voxelIdx, it->second.index); - - if (voxel.tsdf != -128 && voxel.weight != 0) - { - 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); - } - } - }; - - parallel_for_(fetchRange, HashFetchPointsNormalsInvoker, nstripes); - */ - auto _HashFetchPointsNormalsInvoker = [&](const Range& range) { @@ -1820,10 +1776,10 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor } AutoLock al(mutex); - _pVecs.push_back(localPoints); - _nVecs.push_back(localNormals); - //pVecs.push_back(localPoints); - //nVecs.push_back(localNormals); + //_pVecs.push_back(localPoints); + //_nVecs.push_back(localNormals); + pVecs.push_back(localPoints); + nVecs.push_back(localNormals); } } }; @@ -1855,41 +1811,16 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); - /* + if (_normals.needed()) { Points points = _points.getMat(); CV_Assert(points.type() == POINT_TYPE); - _normals.createSameSize(_points, _points.type()); Normals normals = _normals.getMat(); - const HashTSDFVolumeGPU& _volume = *this; - auto HashPushNormals = [&](const ptype& point, const int* position) { - const HashTSDFVolumeGPU& volume(_volume); - Affine3f invPose(volume.pose.inv()); - Point3f p = fromPtype(point); - Point3f n = nan3; - if (!isNaN(p)) - { - Point3f voxelPoint = invPose * p; - n = volume.pose.rotation() * volume.getNormalVoxel(voxelPoint); - } - normals(position[0], position[1]) = toPtype(n); - }; - points.forEach(HashPushNormals); - } - */ - /* - if (_normals.needed()) - { - Points points = _points.getMat(); - CV_Assert(points.type() == POINT_TYPE); - _normals.createSameSize(_points, _points.type()); - Normals normals = _normals.getMat(); - const HashTSDFVolumeCPU& _volume = *this; auto HashPushNormals = [&](const ptype& point, const int* position) { - const HashTSDFVolumeCPU& volume(_volume); + const HashTSDFVolumeGPU& volume(_volume); Affine3f invPose(volume.pose.inv()); Point3f p = fromPtype(point); Point3f n = nan3; @@ -1902,7 +1833,7 @@ void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) c }; points.forEach(HashPushNormals); } - */ + } int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) const From be30c1ad95c4eb20dbcc64396d07d5c2a647ddd1 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 2 Nov 2020 14:46:58 +0300 Subject: [PATCH 007/216] Docs fix --- modules/rgbd/src/hash_tsdf.cpp | 20 ++++++++++---------- modules/rgbd/src/hash_tsdf.hpp | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ef06c6634de..197450c5f95 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1008,9 +1008,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! Get keys for all the allocated volume Units std::vector _totalVolUnits; - for (int i = 0; i < indexes.size().height; i++) - { - _totalVolUnits.push_back(indexes.at(i, 0)); + for (int i = 0; i < indexes.size().height; i++) + { + _totalVolUnits.push_back(indexes.at(i, 0)); } @@ -1595,7 +1595,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in auto _truncDist = truncDist; auto _HashRaycastInvoker = [&](const Range& range) { - + const Point3f cam2volTrans = cam2vol.translation(); const Matx33f cam2volRot = cam2vol.rotation(); const Matx33f vol2camRot = vol2cam.rotation(); @@ -1660,7 +1660,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); //TsdfVoxel currVoxel; currVoxel.tsdf = floatToTsdf(1.0f); currVoxel.weight = 0; - + currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -1675,7 +1675,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Point3f pv = orig + tInterp * rayDirV; Point3f nv = volume._getNormalVoxel(pv); //Point3f nv = nan3; - + if (!isNaN(nv)) { normal = vol2camRot * nv; @@ -1691,14 +1691,14 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in tcurr += stepSize; } - + _ptsRow[x] = toPtype(point); _nrmRow[x] = toPtype(normal); } } - + }; parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); @@ -1811,7 +1811,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); - + if (_normals.needed()) { Points points = _points.getMat(); @@ -1833,7 +1833,7 @@ void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) c }; points.forEach(HashPushNormals); } - + } int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) const diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index ae8fa1dfd21..51f998486fa 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -127,7 +127,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); HashTSDFVolumeGPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); - + void reset() override; void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, From 868955c8e3edc3a96f5cf0002b5b38ff3d3df060 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 3 Nov 2020 08:30:48 +0300 Subject: [PATCH 008/216] =?UTF-8?q?=C2=96fetch=5Fnormals=20fix?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 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 197450c5f95..b04d5881a21 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1380,7 +1380,7 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const { //it = volumeUnits.find(volumeUnitIdx); it = find_idx(indexes, volumeUnitIdx); - if (it >= 0 || it < _lastVolIndex - 1) + if (it >= 0 || it < _lastVolIndex) { iterMap[dictIdx] = it; queried[dictIdx] = true; From ad795f404e51ea046e091dbde83b1586552eb540 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 4 Nov 2020 14:10:51 +0300 Subject: [PATCH 009/216] I want it to work --- modules/rgbd/src/hash_tsdf.cpp | 125 +++++--------------------------- modules/rgbd/src/hash_tsdf.hpp | 4 +- modules/rgbd/test/test_tsdf.cpp | 6 +- 3 files changed, 26 insertions(+), 109 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b04d5881a21..6b4813909c2 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -879,13 +879,13 @@ static inline bool _find(cv::Mat v, Vec3i tsdf_idx) return false; } -static inline int find_idx(cv::Mat v, Vec3i tsdf_idx) +inline int HashTSDFVolumeGPU::find_idx(cv::Mat v, Vec3i tsdf_idx, bool f) const { //bool res = false; - for (int i = 0; i < v.size().height; i++) + for (int i = 0; i < _lastVolIndex; i++) { Vec3i p = v.at(i, 0); - //std::cout << p << " " << tsdf_idx << std::endl; + if (f) std::cout << " - "<< p << " " << tsdf_idx << std::endl; if (p == tsdf_idx) { return i; @@ -1128,7 +1128,7 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeInd if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0) || - indx > _lastVolIndex) + (indx < 0 || indx > _lastVolIndex-1) ) { TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -1243,7 +1243,6 @@ TsdfVoxel HashTSDFVolumeGPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum return volData[coordBase]; } - /* #if USE_INTRINSICS inline float interpolate(float tx, float ty, float tz, float vx[8]) @@ -1279,6 +1278,7 @@ inline float interpolate(float tx, float ty, float tz, float vx[8]) } #endif */ + float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const { const Vec3i neighbourCoords[] = { {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, @@ -1331,6 +1331,7 @@ inline float HashTSDFVolumeGPU::interpolateVoxel(const cv::Point3f& point) const Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const { Vec3f normal = Vec3f(0, 0, 0); + //std::cout << 111 << std::endl; Point3f ptVox = point * voxelSizeInv; Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); @@ -1388,6 +1389,7 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const } vals[i] = tsdfToFloat(new_atVolumeUnit(pt, volumeUnitIdx, it).tsdf); + //std::cout << vals[i] << std::endl; } #if !USE_INTERPOLATION_IN_GETNORMAL @@ -1499,103 +1501,11 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in const Intr::Reprojector reproj(intrinsics.makeReprojector()); const int nstripes = -1; - /* - auto HashRaycastInvoker = [&](const Range& range) - { - const Point3f cam2volTrans = cam2vol.translation(); - const Matx33f cam2volRot = cam2vol.rotation(); - const Matx33f vol2camRot = vol2cam.rotation(); - - const float blockSize = volume.volumeUnitSize; - - for (int y = range.start; y < range.end; y++) - { - ptype* ptsRow = points[y]; - ptype* nrmRow = normals[y]; - - for (int x = 0; x < points.cols; x++) - { - //! Initialize default value - Point3f point = nan3, normal = nan3; - - //! Ray origin and direction in the volume coordinate frame - Point3f orig = cam2volTrans; - Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); - - 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()); - - - float tprev = tcurr; - float prevTsdf = volume.truncDist; - Ptr currVolumeUnit; - while (tcurr < tmax) - { - 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; - cv::Vec3i volUnitLocalIdx; - - - //! The subvolume exists in hashtable - if (it != volume.volumeUnits.end()) - { - cv::Point3f currVolUnitPos = - volume.volumeUnitIdxToVolume(currVolumeUnitIdx); - volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); - - - //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index); - currTsdf = tsdfToFloat(currVoxel.tsdf); - currWeight = currVoxel.weight; - stepSize = tstep; - } - //! Surface crossing - if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) - { - float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); - if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) - { - Point3f pv = orig + tInterp * rayDirV; - Point3f nv = volume.getNormalVoxel(pv); - - if (!isNaN(nv)) - { - normal = vol2camRot * nv; - point = vol2cam * pv; - } - } - break; - } - prevVolumeUnitIdx = currVolumeUnitIdx; - prevTsdf = currTsdf; - tprev = tcurr; - tcurr += stepSize; - } - ptsRow[x] = toPtype(point); - nrmRow[x] = toPtype(normal); - } - } - }; - - parallel_for_(Range(0, points.rows), HashRaycastInvoker, nstripes); - */ + auto _truncDist = truncDist; auto _HashRaycastInvoker = [&](const Range& range) { - + //std::cout << "_HashRaycastInvoker" << std::endl; const Point3f cam2volTrans = cam2vol.translation(); const Matx33f cam2volRot = cam2vol.rotation(); const Matx33f vol2camRot = vol2cam.rotation(); @@ -1625,20 +1535,22 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in std::numeric_limits::min()); float tprev = tcurr; - float prevTsdf = _truncDist; + //float prevTsdf = _truncDist; + float prevTsdf = volume.truncDist; Ptr currVolumeUnit; while (tcurr < tmax) { - tmax -= 1; + //tmax -= 1; Point3f currRayPos = orig + tcurr * rayDirV; cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); //VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); + //VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx, true); VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); - + //std::cout << "=====" << currVolumeUnitIdx <<" "<< idx << std::endl; float currTsdf = prevTsdf; int currWeight = 0; float stepSize = 0.5f * blockSize; @@ -1647,7 +1559,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in //! The subvolume exists in hashtable //if (it != volume.volumeUnits.end()) - if (idx != _lastVolIndex - 1 && idx >= 0) + if (idx < _lastVolIndex && idx >= 0) { cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); @@ -1678,6 +1590,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in if (!isNaN(nv)) { + //std::cout << 1 << std::endl; normal = vol2camRot * nv; point = vol2cam * pv; } @@ -1701,8 +1614,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in }; - parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); - //_HashRaycastInvoker(Range(0, new_points.rows)); + //parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); + _HashRaycastInvoker(Range(0, new_points.rows)); } void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const @@ -1751,7 +1664,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); - if (idx >= 0 || idx < _lastVolIndex - 1) + if (idx >= 0 || idx < _lastVolIndex) { std::vector localPoints; std::vector localNormals; diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 51f998486fa..84fba6e67f4 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -50,7 +50,7 @@ struct tsdf_hash } }; -typedef unsigned int VolumeIndex; +typedef int VolumeIndex; struct VolumeUnit { cv::Vec3i coord; @@ -165,7 +165,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; Vec3i volumeToVoxelCoord(const Point3f& point) const; - + int find_idx(cv::Mat v, Vec3i tsdf_idx, bool f = false) const; public: Vec4i volStrides; diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 99da9579697..b598691db45 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -466,7 +466,11 @@ void valid_points_test(bool isHashTSDF) waitKey(20000); } - float percentValidity = float(profile) / float(anfas); + //float percentValidity = anfas == 0 ? 0 : float(profile) / float(anfas); + float percentValidity; + if (profile == 0) percentValidity = -0.5; + else if (anfas == 0) percentValidity = 0; + else percentValidity = float(profile) / float(anfas); ASSERT_LT(0.5 - percentValidity, 0.3); } From 8878cf4b1d3b42f841a474dc4436fbac1b7e3068 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 4 Nov 2020 15:42:21 +0300 Subject: [PATCH 010/216] 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 6b4813909c2..c22cf9b0554 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1501,8 +1501,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in const Intr::Reprojector reproj(intrinsics.makeReprojector()); const int nstripes = -1; - - auto _truncDist = truncDist; + + //auto _truncDist = truncDist; auto _HashRaycastInvoker = [&](const Range& range) { //std::cout << "_HashRaycastInvoker" << std::endl; @@ -1614,8 +1614,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in }; - //parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); - _HashRaycastInvoker(Range(0, new_points.rows)); + parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); + //_HashRaycastInvoker(Range(0, new_points.rows)); } void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const From fb512164a31f42927c1a1c786927d1d292d0a92c Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 4 Nov 2020 23:51:22 +0300 Subject: [PATCH 011/216] valid points checking passed --- modules/rgbd/src/hash_tsdf.cpp | 76 +++++++++++++++++++++------------- modules/rgbd/src/hash_tsdf.hpp | 1 + 2 files changed, 49 insertions(+), 28 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index c22cf9b0554..e796f82898a 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -864,10 +864,10 @@ void HashTSDFVolumeGPU::reset() } -static inline bool _find(cv::Mat v, Vec3i tsdf_idx) +static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) { //bool res = false; - for (int i = 0; i < v.size().height; i++) + for (int i = 0; i < _lastVolIndex+1; i++) { auto p = v.at(i, 0); if (p == tsdf_idx) @@ -882,6 +882,7 @@ static inline bool _find(cv::Mat v, Vec3i tsdf_idx) inline int HashTSDFVolumeGPU::find_idx(cv::Mat v, Vec3i tsdf_idx, bool f) const { //bool res = false; + //std::cout << " find_idx " << tsdf_idx << std::endl; for (int i = 0; i < _lastVolIndex; i++) { Vec3i p = v.at(i, 0); @@ -938,7 +939,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { const Vec3i tsdf_idx = Vec3i(i, j, k); - if (!_find(_localAccessVolUnits, tsdf_idx)) + if (!_find(_localAccessVolUnits, tsdf_idx, loc_vol_idx)) { _localAccessVolUnits.at(loc_vol_idx, 0) = tsdf_idx; //std::cout << tsdf_idx << std::endl; @@ -954,7 +955,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = 0; i < loc_vol_idx; i++) { Vec3i idx = _localAccessVolUnits.at(i, 0); - if (!_find(indexes, idx)) + if (find_idx(indexes, idx)==-1) { //std::cout << idx << std::endl; if (_lastVolIndex >= VolumeIndex(indexes.size().height)) @@ -1079,6 +1080,11 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } }); + + + //std::cout << "indexes" << std::endl; + //indexes.forEach([](Vec3i& vv, const int*) {std::cout << "-" << vv << std::endl; }); + } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const @@ -1127,8 +1133,8 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeInd //! Out of bounds if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || - (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0) || - (indx < 0 || indx > _lastVolIndex-1) ) + (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) + //|| (indx < 0 || indx > _lastVolIndex-1) ) { TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -1626,23 +1632,16 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { std::vector> pVecs, nVecs; - std::vector totalVolUnits; - for (const auto& keyvalue : volumeUnits) - { - totalVolUnits.push_back(keyvalue.first); - } - - std::vector> _pVecs, _nVecs; - std::vector _totalVolUnits; for (int i = 0; i < indexes.size().height; i++) { _totalVolUnits.push_back(indexes.at(i, 0)); } - Range fetchRange(0, (int)totalVolUnits.size()); Range _fetchRange(0, (int)_totalVolUnits.size()); - + //std::cout << "indexes" << std::endl; + //indexes.forEach([](Vec3i& vv, const int*){std::cout << "-" << vv << std::endl;}); + const int nstripes = -1; const HashTSDFVolumeGPU& volume(*this); @@ -1659,13 +1658,16 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor //VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(tsdf_idx); + //VolumeIndex idx = find_idx(indexes, tsdf_idx, true); VolumeIndex idx = find_idx(indexes, tsdf_idx); - + //std::cout << idx<<" " ; Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); - if (idx >= 0 || idx < _lastVolIndex) + if (idx >= 0 && idx < _lastVolIndex) { + //<< std::endl; + std::vector localPoints; std::vector localNormals; for (int x = 0; x < volume.volumeUnitResolution; x++) @@ -1673,33 +1675,48 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); + //std::cout << idx << std::endl; TsdfVoxel voxel = new_at(voxelIdx, idx); if (voxel.tsdf != -128 && voxel.weight != 0) { Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); + //std::cout << point << std::endl; + localPoints.push_back(toPtype(point)); + //std::cout << Mat(localPoints) << std::endl; if (needNormals) { - Point3f normal = volume._getNormalVoxel(point); - //Point3f normal(0,0,0); - localNormals.push_back(toPtype(normal)); + //Point3f normal = volume._getNormalVoxel(point); + Point3f normal(1,0,0); + if (normal.x == 0 && normal.y == 0 && normal.z) std::cout<<"looooooooooooool"< points, normals; for (size_t i = 0; i < pVecs.size(); i++) @@ -1707,7 +1724,8 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor points.insert(points.end(), pVecs[i].begin(), pVecs[i].end()); normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end()); } - + //std::cout << "points" << std::endl; + //std::cout << Mat(points) << std::endl; _points.create((int)points.size(), 1, POINT_TYPE); if (!points.empty()) Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat()); @@ -1724,7 +1742,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); - + if (_normals.needed()) { Points points = _points.getMat(); @@ -1744,7 +1762,9 @@ void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) c } normals(position[0], position[1]) = toPtype(n); }; - points.forEach(HashPushNormals); + //std::cout << "points" << std::endl; + //std::cout << points << std::endl; + //points.forEach(HashPushNormals); } } diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 84fba6e67f4..01edd82dec1 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -166,6 +166,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; Vec3i volumeToVoxelCoord(const Point3f& point) const; int find_idx(cv::Mat v, Vec3i tsdf_idx, bool f = false) const; + //bool _find(cv::Mat v, Vec3i tsdf_idx) const; public: Vec4i volStrides; From c15ea281c5654fbe599b1d2d13634d9557dc4dbc Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 5 Nov 2020 10:14:16 +0300 Subject: [PATCH 012/216] it works! --- modules/rgbd/src/hash_tsdf.cpp | 23 ++++++++++++++++------- modules/rgbd/test/test_tsdf.cpp | 1 + 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index e796f82898a..aeed437c983 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1475,10 +1475,18 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const normal[1] = interpolate(tx, ty, tz, cyv); normal[2] = interpolate(tx, ty, tz, czv); #endif - + //if (normal[0] == 0 && normal[1] == 0 && normal[2] == 0)return nan3; + //std::cout << "kek" << std::endl; float nv = sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]); + //if (nv < 0.0001f)return nan3; + //auto _normal = normal / nv; + //if (normal[0] == 0 && normal[1] == 0 && normal[2] == 0) + //if (_normal[0] == 0 && _normal[1] == 0 && _normal[2] == 0) + // std::cout << "kek" << std::endl; + //std::cout << normal / nv << std::endl; + //return normal / nv; return nv < 0.0001f ? nan3 : normal / nv; //return Point3f(0, 0, 0); } @@ -1687,12 +1695,12 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor //std::cout << Mat(localPoints) << std::endl; if (needNormals) { - //Point3f normal = volume._getNormalVoxel(point); - Point3f normal(1,0,0); - if (normal.x == 0 && normal.y == 0 && normal.z) std::cout<<"looooooooooooool"<()) std::cout << vector << std::endl; ASSERT_LT(abs(1 - length), 0.0001f); } } From 057c451ee903aa96969dd2146d5cb5d25da35c7c Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 5 Nov 2020 11:54:02 +0300 Subject: [PATCH 013/216] minor fixes --- modules/rgbd/src/hash_tsdf.cpp | 158 ++------------------------------ modules/rgbd/src/hash_tsdf.hpp | 3 +- modules/rgbd/test/test_tsdf.cpp | 2 - 3 files changed, 10 insertions(+), 153 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index aeed437c983..c6b42a2713f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -866,7 +866,6 @@ void HashTSDFVolumeGPU::reset() static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) { - //bool res = false; for (int i = 0; i < _lastVolIndex+1; i++) { auto p = v.at(i, 0); @@ -875,24 +874,19 @@ static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) return true; } } - //v.forEach([&](Vec3i& v, const int*) {if (v == tsdf_idx) res = true; }); return false; } -inline int HashTSDFVolumeGPU::find_idx(cv::Mat v, Vec3i tsdf_idx, bool f) const +inline int HashTSDFVolumeGPU::find_idx(cv::Mat v, Vec3i tsdf_idx) const { - //bool res = false; - //std::cout << " find_idx " << tsdf_idx << std::endl; for (int i = 0; i < _lastVolIndex; i++) { Vec3i p = v.at(i, 0); - if (f) std::cout << " - "<< p << " " << tsdf_idx << std::endl; if (p == tsdf_idx) { return i; } } - //v.forEach([&](Vec3i& v, const int*) {if (v == tsdf_idx) res = true; }); return -1; } @@ -942,7 +936,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (!_find(_localAccessVolUnits, tsdf_idx, loc_vol_idx)) { _localAccessVolUnits.at(loc_vol_idx, 0) = tsdf_idx; - //std::cout << tsdf_idx << std::endl; loc_vol_idx++; } @@ -957,7 +950,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Vec3i idx = _localAccessVolUnits.at(i, 0); if (find_idx(indexes, idx)==-1) { - //std::cout << idx << std::endl; if (_lastVolIndex >= VolumeIndex(indexes.size().height)) { indexes.resize(_lastVolIndex * 2); @@ -968,7 +960,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } this->indexes.at(_lastVolIndex, 0) = idx; - //std::cout << this->indexes.at(_lastVolIndex, 0) << std::endl; _newIndices.at(vol_idx, 0) = _lastVolIndex; vol_idx++; @@ -976,7 +967,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } - //std::cout << this->indexes.at(_lastVolIndex-1, 0) << std::endl; //mutex.unlock(); }; @@ -991,7 +981,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma VolumeIndex idx = _newIndices.at(i, 0); Vec3i tsdf_idx = indexes.at(idx, 0); - //std::cout << idx << " " << tsdf_idx << std::endl; Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; poses.at(idx, 0) = subvolumePose; @@ -1025,7 +1014,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { Vec3i tsdf_idx = _totalVolUnits[i]; VolumeIndex idx = find_idx(indexes, tsdf_idx); - //std::cout << _lastVolIndex << " " << idx << " " << tsdf_idx << std::endl; if (idx < 0 || idx == _lastVolIndex - 1) return; Point3f volumeUnitPos = volumeUnitIdxToVolume(poses.at(idx, 0)); @@ -1080,11 +1068,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } }); - - - //std::cout << "indexes" << std::endl; - //indexes.forEach([](Vec3i& vv, const int*) {std::cout << "-" << vv << std::endl; }); - } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const @@ -1134,7 +1117,6 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeInd if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) - //|| (indx < 0 || indx > _lastVolIndex-1) ) { TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -1196,23 +1178,7 @@ TsdfVoxel HashTSDFVolumeGPU::at(const Point3f& point) const cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); return _at(volUnitLocalIdx, it->second.index); } -/* -static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) -{ - if (!(vuRes & (vuRes - 1))) - { - // vuRes is a power of 2, let's get this power - const int p2 = trailingZeros32(vuRes); - return Vec3i(pt[0] >> p2, pt[1] >> p2, pt[2] >> p2); - } - else - { - return Vec3i(cvFloor(float(pt[0]) / vuRes), - cvFloor(float(pt[1]) / vuRes), - cvFloor(float(pt[2]) / vuRes)); - } -} -*/ + TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const { if (indx < 0 || indx > _lastVolIndex - 1) @@ -1249,42 +1215,6 @@ TsdfVoxel HashTSDFVolumeGPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum return volData[coordBase]; } -/* -#if USE_INTRINSICS -inline float interpolate(float tx, float ty, float tz, float vx[8]) -{ - v_float32x4 v0246, v1357; - v_load_deinterleave(vx, v0246, v1357); - - v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); - - v_float32x4 v00_10 = vxx; - v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx))); - - v_float32x4 v0_1 = v00_10 + v_setall_f32(ty) * (v01_11 - v00_10); - float v0 = v0_1.get0(); - v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); - float v1 = v0_1.get0(); - - return v0 + tx * (v1 - v0); -} - -#else -inline float interpolate(float tx, float ty, float tz, float vx[8]) -{ - float v00 = vx[0] + tz * (vx[1] - vx[0]); - float v01 = vx[2] + tz * (vx[3] - vx[2]); - float v10 = vx[4] + tz * (vx[5] - vx[4]); - float v11 = vx[6] + tz * (vx[7] - vx[6]); - - float v0 = v00 + ty * (v01 - v00); - float v1 = v10 + ty * (v11 - v10); - - return v0 + tx * (v1 - v0); -} -#endif -*/ - float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const { const Vec3i neighbourCoords[] = { {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, @@ -1337,19 +1267,16 @@ inline float HashTSDFVolumeGPU::interpolateVoxel(const cv::Point3f& point) const Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const { Vec3f normal = Vec3f(0, 0, 0); - //std::cout << 111 << std::endl; Point3f ptVox = point * voxelSizeInv; Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); // A small hash table to reduce a number of find() calls bool queried[8]; - //VolumeUnitIndexes::const_iterator iterMap[8]; VolumeIndex iterMap[8]; for (int i = 0; i < 8; i++) { - //iterMap[i] = volumeUnits.end(); iterMap[i] = _lastVolIndex - 1; queried[i] = false; } @@ -1385,7 +1312,6 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const if (!queried[dictIdx]) { - //it = volumeUnits.find(volumeUnitIdx); it = find_idx(indexes, volumeUnitIdx); if (it >= 0 || it < _lastVolIndex) { @@ -1395,7 +1321,6 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const } vals[i] = tsdfToFloat(new_atVolumeUnit(pt, volumeUnitIdx, it).tsdf); - //std::cout << vals[i] << std::endl; } #if !USE_INTERPOLATION_IN_GETNORMAL @@ -1475,20 +1400,10 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const normal[1] = interpolate(tx, ty, tz, cyv); normal[2] = interpolate(tx, ty, tz, czv); #endif - //if (normal[0] == 0 && normal[1] == 0 && normal[2] == 0)return nan3; - //std::cout << "kek" << std::endl; float nv = sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]); - //if (nv < 0.0001f)return nan3; - //auto _normal = normal / nv; - //if (normal[0] == 0 && normal[1] == 0 && normal[2] == 0) - //if (_normal[0] == 0 && _normal[1] == 0 && _normal[2] == 0) - // std::cout << "kek" << std::endl; - //std::cout << normal / nv << std::endl; - //return normal / nv; return nv < 0.0001f ? nan3 : normal / nv; - //return Point3f(0, 0, 0); } void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, @@ -1502,8 +1417,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Points points1 = _points.getMat(); Normals normals1 = _normals.getMat(); - //Points& points(points1); - //Normals& normals(normals1); Points& new_points(points1); Normals& new_normals(normals1); @@ -1516,10 +1429,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in const int nstripes = -1; - //auto _truncDist = truncDist; auto _HashRaycastInvoker = [&](const Range& range) { - //std::cout << "_HashRaycastInvoker" << std::endl; const Point3f cam2volTrans = cam2vol.translation(); const Matx33f cam2volRot = cam2vol.rotation(); const Matx33f vol2camRot = vol2cam.rotation(); @@ -1549,22 +1460,15 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in std::numeric_limits::min()); float tprev = tcurr; - //float prevTsdf = _truncDist; float prevTsdf = volume.truncDist; Ptr currVolumeUnit; while (tcurr < tmax) { - //tmax -= 1; - Point3f currRayPos = orig + tcurr * rayDirV; cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - - //VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); - //VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx, true); VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); - //std::cout << "=====" << currVolumeUnitIdx <<" "<< idx << std::endl; float currTsdf = prevTsdf; int currWeight = 0; float stepSize = 0.5f * blockSize; @@ -1572,7 +1476,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in //! The subvolume exists in hashtable - //if (it != volume.volumeUnits.end()) if (idx < _lastVolIndex && idx >= 0) { cv::Point3f currVolUnitPos = @@ -1581,12 +1484,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in //! TODO: Figure out voxel interpolation - //std::cout << indexes << std::endl; - //std::cout << currVolumeUnitIdx << " " << idx << " " << _volUnitsData.size().height << std::endl; - TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); - //TsdfVoxel currVoxel; currVoxel.tsdf = floatToTsdf(1.0f); currVoxel.weight = 0; - currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -1600,11 +1498,9 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in { Point3f pv = orig + tInterp * rayDirV; Point3f nv = volume._getNormalVoxel(pv); - //Point3f nv = nan3; if (!isNaN(nv)) { - //std::cout << 1 << std::endl; normal = vol2camRot * nv; point = vol2cam * pv; } @@ -1647,9 +1543,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor } Range _fetchRange(0, (int)_totalVolUnits.size()); - //std::cout << "indexes" << std::endl; - //indexes.forEach([](Vec3i& vv, const int*){std::cout << "-" << vv << std::endl;}); - + const int nstripes = -1; const HashTSDFVolumeGPU& volume(*this); @@ -1664,18 +1558,11 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { cv::Vec3i tsdf_idx = _totalVolUnits[i]; - - //VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(tsdf_idx); - //VolumeIndex idx = find_idx(indexes, tsdf_idx, true); VolumeIndex idx = find_idx(indexes, tsdf_idx); - //std::cout << idx<<" " ; Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); - if (idx >= 0 && idx < _lastVolIndex) { - //<< std::endl; - std::vector localPoints; std::vector localNormals; for (int x = 0; x < volume.volumeUnitResolution; x++) @@ -1683,48 +1570,31 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - //std::cout << idx << std::endl; TsdfVoxel voxel = new_at(voxelIdx, idx); if (voxel.tsdf != -128 && voxel.weight != 0) { Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); - //std::cout << point << std::endl; localPoints.push_back(toPtype(point)); - //std::cout << Mat(localPoints) << std::endl; if (needNormals) { Point3f normal = volume._getNormalVoxel(point); - //Point3f normal(1,0,0); - //if (normal.x == 0 && normal.y == 0 && normal.z==0) std::cout<<"looooooooooooool"< points, normals; for (size_t i = 0; i < pVecs.size(); i++) @@ -1732,8 +1602,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor points.insert(points.end(), pVecs[i].begin(), pVecs[i].end()); normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end()); } - //std::cout << "points" << std::endl; - //std::cout << Mat(points) << std::endl; + _points.create((int)points.size(), 1, POINT_TYPE); if (!points.empty()) Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat()); @@ -1750,14 +1619,13 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); - + if (_normals.needed()) { Points points = _points.getMat(); CV_Assert(points.type() == POINT_TYPE); _normals.createSameSize(_points, _points.type()); Normals normals = _normals.getMat(); - //std::cout << normals << std::endl; const HashTSDFVolumeGPU& _volume = *this; auto HashPushNormals = [&](const ptype& point, const int* position) { const HashTSDFVolumeGPU& volume(_volume); @@ -1771,8 +1639,6 @@ void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) c } normals(position[0], position[1]) = toPtype(n); }; - //std::cout << "points" << std::endl; - //std::cout << points << std::endl; points.forEach(HashPushNormals); } @@ -1791,12 +1657,8 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con return numVisibleBlocks; } - - #endif - - //template Ptr makeHashTSDFVolume(const VolumeParams& _volumeParams) { @@ -1820,7 +1682,5 @@ Ptr makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _r volumeUnitResolution); } - - } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 01edd82dec1..370169795b4 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -165,8 +165,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; Vec3i volumeToVoxelCoord(const Point3f& point) const; - int find_idx(cv::Mat v, Vec3i tsdf_idx, bool f = false) const; - //bool _find(cv::Mat v, Vec3i tsdf_idx) const; + int find_idx(cv::Mat v, Vec3i tsdf_idx) const; public: Vec4i volStrides; diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index d0f30d203ca..3c96f0ee868 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -288,7 +288,6 @@ void normalsCheck(Mat normals) float length = vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]; - //if (length == 0 && pvector == normals.begin()) std::cout << vector << std::endl; ASSERT_LT(abs(1 - length), 0.0001f); } } @@ -467,7 +466,6 @@ void valid_points_test(bool isHashTSDF) waitKey(20000); } - //float percentValidity = anfas == 0 ? 0 : float(profile) / float(anfas); float percentValidity; if (profile == 0) percentValidity = -0.5; else if (anfas == 0) percentValidity = 0; From b8b88be53a0dfa32cf2342722f19c5e2737a2975 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 5 Nov 2020 12:37:43 +0300 Subject: [PATCH 014/216] warning fix --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index c6b42a2713f..3ab24e2901d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -823,8 +823,8 @@ int HashTSDFVolumeCPU::getVisibleBlocks(int currFrameId, int frameThreshold) con #ifdef HAVE_OPENCL HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder) - :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, zFirstMemOrder) + float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) + :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { int xdim, ydim, zdim; if (zFirstMemOrder) From d35ce44ead2855c5ccc9ec9b7d1ef1cc13cd4975 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 5 Nov 2020 16:11:36 +0300 Subject: [PATCH 015/216] last replacements --- modules/rgbd/src/hash_tsdf.cpp | 110 +++++---------------------------- modules/rgbd/src/hash_tsdf.hpp | 13 +--- 2 files changed, 18 insertions(+), 105 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 3ab24e2901d..ff3b70de821 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -853,9 +853,7 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstM void HashTSDFVolumeGPU::reset() { CV_TRACE_FUNCTION(); - lastVolIndex = 0; _lastVolIndex = 0; - volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); _volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); indexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); poses = cv::Mat(VOLUMES_SIZE, 1, rawType()); @@ -1093,24 +1091,6 @@ cv::Vec3i HashTSDFVolumeGPU::volumeToVoxelCoord(const cv::Point3f& point) const cvFloor(point.z * voxelSizeInv)); } -inline TsdfVoxel HashTSDFVolumeGPU::_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const -{ - //! Out of bounds - if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || - (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 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 = - volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2]; - return volData[coordBase]; -} inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const { //! Out of bounds @@ -1132,53 +1112,6 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeInd return volData[coordBase]; } -inline TsdfVoxel HashTSDFVolumeGPU::at(const cv::Vec3i& volumeIdx) const - -{ - Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), - cvFloor(volumeIdx[1] / volumeUnitResolution), - 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::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, - volumeUnitIdx[1] * volumeUnitResolution, - volumeUnitIdx[2] * volumeUnitResolution); - - volUnitLocalIdx = - cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index); - -} - -TsdfVoxel HashTSDFVolumeGPU::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::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); - cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); - volUnitLocalIdx = - cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return _at(volUnitLocalIdx, it->second.index); -} - TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const { if (indx < 0 || indx > _lastVolIndex - 1) @@ -1198,23 +1131,6 @@ TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& v return volData[coordBase]; } -TsdfVoxel HashTSDFVolumeGPU::atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const -{ - if (it == volumeUnits.end()) - { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.f); - dummy.weight = 0; - return dummy; - } - Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; - - // expanding at(), removing bounds check - 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]; -} - float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const { const Vec3i neighbourCoords[] = { {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, @@ -1222,10 +1138,12 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const // A small hash table to reduce a number of find() calls bool queried[8]; - VolumeUnitIndexes::const_iterator iterMap[8]; + //VolumeUnitIndexes::const_iterator iterMap[8]; + VolumeIndex iterMap[8]; for (int i = 0; i < 8; i++) { - iterMap[i] = volumeUnits.end(); + //iterMap[i] = volumeUnits.end(); + iterMap[i] = _lastVolIndex - 1; queried[i] = false; } @@ -1248,12 +1166,19 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const auto it = iterMap[dictIdx]; if (!queried[dictIdx]) { - it = volumeUnits.find(volumeUnitIdx); - iterMap[dictIdx] = it; - queried[dictIdx] = true; + it = find_idx(indexes, volumeUnitIdx); + if (it >= 0 || it < _lastVolIndex) + { + iterMap[dictIdx] = it; + queried[dictIdx] = true; + } + //it = volumeUnits.find(volumeUnitIdx); + //iterMap[dictIdx] = it; + //queried[dictIdx] = true; } - vx[i] = atVolumeUnit(pt, volumeUnitIdx, it).tsdf; + //vx[i] = atVolumeUnit(pt, volumeUnitIdx, it).tsdf; + vx[i] = new_atVolumeUnit(pt, volumeUnitIdx, it).tsdf; } return interpolate(tx, ty, tz, vx); @@ -1648,10 +1573,9 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con { int numVisibleBlocks = 0; //! TODO: Iterate over map parallely? - for (const auto& keyvalue : volumeUnits) + for (int i = 0; i < _lastVolIndex; i++) { - const VolumeUnit& volumeUnit = keyvalue.second; - if (volumeUnit.lastVisibleIndex > (currFrameId - frameThreshold)) + if (lastVisibleIndexes.at(i, 0) > (currFrameId - frameThreshold)); numVisibleBlocks++; } return numVisibleBlocks; diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 370169795b4..8e652e5410b 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -138,25 +138,17 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; - size_t getTotalVolumeUnits() const { return volumeUnits.size(); } + VolumeIndex getTotalVolumeUnits() const { return _lastVolIndex; } int getVisibleBlocks(int currFrameId, int frameThreshold) const; - //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) - TsdfVoxel at(const Vec3i& volumeIdx) const; - //! 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 new_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const; - - TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const; TsdfVoxel new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const; float interpolateVoxelPoint(const Point3f& point) const; float interpolateVoxel(const cv::Point3f& point) const; - //Point3f getNormalVoxel(const cv::Point3f& p) const; Point3f _getNormalVoxel(const cv::Point3f& p) const; //! Utility functions for coordinate transformations @@ -171,9 +163,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Vec4i volStrides; Vec6f frameParams; Mat pixNorms; - VolumeUnitIndexes volumeUnits; - cv::Mat volUnitsData; - VolumeIndex lastVolIndex; VolumeIndex _lastVolIndex; cv::Mat indexes; From 191c2e48c6d01b3cff6fff12db4e3df42f57a9ec Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 5 Nov 2020 16:37:12 +0300 Subject: [PATCH 016/216] warning 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 ff3b70de821..4babbca0c2a 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1575,7 +1575,7 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con //! TODO: Iterate over map parallely? for (int i = 0; i < _lastVolIndex; i++) { - if (lastVisibleIndexes.at(i, 0) > (currFrameId - frameThreshold)); + if (lastVisibleIndexes.at(i, 0) > (currFrameId - frameThreshold)) numVisibleBlocks++; } return numVisibleBlocks; From 8e149044b170cd70e2a5fb2050393c1db386b0ef Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 6 Nov 2020 12:20:47 +0300 Subject: [PATCH 017/216] create hash_tsdf.cl --- modules/rgbd/src/opencl/hash_tsdf.cl | 193 +++++++++++++++++++++++++++ 1 file changed, 193 insertions(+) create mode 100644 modules/rgbd/src/opencl/hash_tsdf.cl diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl new file mode 100644 index 00000000000..1bb5cbd6c84 --- /dev/null +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -0,0 +1,193 @@ +// 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 + +typedef __INT8_TYPE__ int8_t; + +typedef int8_t TsdfType; +typedef uchar WeightType; + +struct TsdfVoxel +{ + TsdfType tsdf; + WeightType weight; +}; + +static inline TsdfType floatToTsdf(float num) +{ + int8_t res = (int8_t) ( (num * (-128)) ); + res = res ? res : (num < 0 ? 1 : -1); + return res; +} + +static inline float tsdfToFloat(TsdfType num) +{ + return ( (float) num ) / (-128); +} + +__kernel void preCalculationPixNorm (__global float * pixNorms, + const __global float * xx, + const __global float * yy, + int width) +{ + int i = get_global_id(0); + int j = get_global_id(1); + int idx = i*width + j; + pixNorms[idx] = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); +} + +__kernel void integrate(__global const char * depthptr, + int depth_step, int depth_offset, + int depth_rows, int depth_cols, + __global struct TsdfVoxel * volumeptr, + const float16 vol2camMatrix, + const float voxelSize, + const int4 volResolution4, + const int4 volDims4, + const float2 fxy, + const float2 cxy, + const float dfac, + const float truncDist, + const int maxWeight, + const __global float * pixNorms) +{ + int x = get_global_id(0); + int y = get_global_id(1); + + const int3 volResolution = volResolution4.xyz; + + if(x >= volResolution.x || y >= volResolution.y) + return; + + // coord-independent constants + const int3 volDims = volDims4.xyz; + const float2 limits = (float2)(depth_cols-1, depth_rows-1); + + const float4 vol2cam0 = vol2camMatrix.s0123; + const float4 vol2cam1 = vol2camMatrix.s4567; + const float4 vol2cam2 = vol2camMatrix.s89ab; + + const float truncDistInv = 1.f/truncDist; + + // optimization of camSpace transformation (vector addition instead of matmul at each z) + float4 inPt = (float4)(x*voxelSize, y*voxelSize, 0, 1); + float3 basePt = (float3)(dot(vol2cam0, inPt), + dot(vol2cam1, inPt), + dot(vol2cam2, inPt)); + + float3 camSpacePt = basePt; + + // zStep == vol2cam*(float3(x, y, 1)*voxelSize) - basePt; + float3 zStep = ((float3)(vol2cam0.z, vol2cam1.z, vol2cam2.z))*voxelSize; + + int volYidx = x*volDims.x + y*volDims.y; + + int startZ, endZ; + if(fabs(zStep.z) > 1e-5) + { + int baseZ = convert_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; + return; + } + } + + startZ = max(0, startZ); + endZ = min(volResolution.z, endZ); + + for(int z = startZ; z < endZ; z++) + { + // optimization of the following: + //float3 camSpacePt = vol2cam * ((float3)(x, y, z)*voxelSize); + camSpacePt += zStep; + + if(camSpacePt.z <= 0) + continue; + + float3 camPixVec = camSpacePt / camSpacePt.z; + float2 projected = mad(camPixVec.xy, fxy, cxy); + + float v; + // bilinearly interpolate depth at projected + if(all(projected >= 0) && all(projected < limits)) + { + float2 ip = floor(projected); + int xi = ip.x, yi = ip.y; + + __global const float* row0 = (__global const float*)(depthptr + depth_offset + + (yi+0)*depth_step); + __global const float* row1 = (__global const float*)(depthptr + depth_offset + + (yi+1)*depth_step); + + float v00 = row0[xi+0]; + float v01 = row0[xi+1]; + float v10 = row1[xi+0]; + float v11 = row1[xi+1]; + float4 vv = (float4)(v00, v01, v10, v11); + + // assume correct depth is positive + if(all(vv > 0)) + { + float2 t = projected - ip; + float2 vf = mix(vv.xz, vv.yw, t.x); + v = mix(vf.s0, vf.s1, t.y); + } + else + continue; + } + else + continue; + + if(v == 0) + continue; + + int idx = projected.y * depth_rows + projected.x; + float pixNorm = pixNorms[idx]; + //float pixNorm = length(camPixVec); + + // difference between distances of point and of surface to camera + float sdf = pixNorm*(v*dfac - camSpacePt.z); + // possible alternative is: + // float sdf = length(camSpacePt)*(v*dfac/camSpacePt.z - 1.0); + + if(sdf >= -truncDist) + { + float tsdf = fmin(1.0f, sdf * truncDistInv); + int volIdx = volYidx + z*volDims.z; + + struct TsdfVoxel voxel = volumeptr[volIdx]; + float value = tsdfToFloat(voxel.tsdf); + int weight = voxel.weight; + + // update TSDF + value = (value*weight + tsdf) / (weight + 1); + weight = min(weight + 1, maxWeight); + + voxel.tsdf = floatToTsdf(value); + voxel.weight = weight; + volumeptr[volIdx] = voxel; + } + } +} + From 0603845e9fb75f18731883158258fa09104a39d6 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 6 Nov 2020 13:13:58 +0300 Subject: [PATCH 018/216] add preCalculationPixNormGPU --- modules/rgbd/src/hash_tsdf.cpp | 43 ++++++++++++++++++++++++++++++++++ modules/rgbd/src/hash_tsdf.hpp | 1 + 2 files changed, 44 insertions(+) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 4babbca0c2a..d73311197b8 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -15,6 +15,7 @@ #include "opencv2/core/utility.hpp" #include "opencv2/core/utils/trace.hpp" #include "utils.hpp" +#include "opencl_kernels_rgbd.hpp" #define USE_INTERPOLATION_IN_GETNORMAL 1 #define VOLUMES_SIZE 1024 @@ -888,6 +889,48 @@ inline int HashTSDFVolumeGPU::find_idx(cv::Mat v, Vec3i tsdf_idx) const return -1; } +static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f fxy, Vec2f cxy) +{ + Mat x(1, depth_cols, CV_32F); + Mat y(1, depth_rows, CV_32F); + Mat _pixNorm(1, depth_rows * depth_cols, CV_32F); + + for (int i = 0; i < depth_cols; i++) + x.at(0, i) = (i - cxy[0]) / fxy[0]; + for (int i = 0; i < depth_rows; i++) + y.at(0, i) = (i - cxy[1]) / fxy[1]; + + cv::String errorStr; + cv::String name = "preCalculationPixNorm"; + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + cv::String options = "-cl-mad-enable"; + ocl::Kernel kk; + kk.create(name.c_str(), source, options, &errorStr); + + + if (kk.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); + + AccessFlag af = ACCESS_READ; + UMat pixNorm = _pixNorm.getUMat(af); + UMat xx = x.getUMat(af); + UMat yy = y.getUMat(af); + + kk.args(ocl::KernelArg::PtrReadWrite(pixNorm), + ocl::KernelArg::PtrReadOnly(xx), + ocl::KernelArg::PtrReadOnly(yy), + depth_cols); + + size_t globalSize[2]; + globalSize[0] = depth_rows; + globalSize[1] = depth_cols; + + if (!kk.run(2, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); + + return pixNorm; +} + void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 8e652e5410b..c1bb98f0484 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -163,6 +163,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Vec4i volStrides; Vec6f frameParams; Mat pixNorms; + UMat _pixNorms; VolumeIndex _lastVolIndex; cv::Mat indexes; From eb3cf3e4c96928da3b697851dd9967e86bc30279 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 6 Nov 2020 15:13:55 +0300 Subject: [PATCH 019/216] add integrateVolumeUnitGPU --- modules/rgbd/src/hash_tsdf.cpp | 63 ++++++++++++++++++++++++++-- modules/rgbd/src/hash_tsdf.hpp | 8 +++- modules/rgbd/src/opencl/hash_tsdf.cl | 2 +- 3 files changed, 68 insertions(+), 5 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d73311197b8..7d0cc23b53d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -856,13 +856,14 @@ void HashTSDFVolumeGPU::reset() CV_TRACE_FUNCTION(); _lastVolIndex = 0; _volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + //volUnitsData = cv::Mat(VOLUMES_SIZE, 1, rawType()); indexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); poses = cv::Mat(VOLUMES_SIZE, 1, rawType()); activities = cv::Mat(VOLUMES_SIZE, 1, rawType()); lastVisibleIndexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); } - static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) { for (int i = 0; i < _lastVolIndex+1; i++) @@ -931,6 +932,54 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f return pixNorm; } +void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFactor, + const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx) +{ + CV_TRACE_FUNCTION(); + CV_Assert(!_depth.empty()); + + UMat depth = _depth.getUMat(); + + String errorStr; + String name = "integrateVolumeUnit"; + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); + + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); + + Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + float dfac = 1.f / depthFactor; + Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); + Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); + + // TODO: optimization possible + // Use sampler for depth (mask needed) + k.args(ocl::KernelArg::ReadOnly(depth), + //ocl::KernelArg::PtrReadWrite(volUnitsData.row(idx).getUMat(ACCESS_RW)), + ocl::KernelArg::PtrReadWrite(_volUnitsData.row(idx).getUMat(ACCESS_RW)), + ocl::KernelArg::Constant(vol2cam.matrix.val, + sizeof(vol2cam.matrix.val)), + voxelSize, + volResGpu.val, + volStrides.val, + fxy.val, + cxy.val, + dfac, + truncDist, + int(maxWeight), + ocl::KernelArg::PtrReadOnly(_pixNorms)); + + size_t globalSize[2]; + globalSize[0] = (size_t)volumeUnitResolution; + globalSize[1] = (size_t)volumeUnitResolution; + + if (!k.run(2, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); +} + void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); @@ -995,6 +1044,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { indexes.resize(_lastVolIndex * 2); _volUnitsData.resize(_lastVolIndex * 2); + volUnitsData.resize(_lastVolIndex * 2); poses.resize(_lastVolIndex * 2); activities.resize(_lastVolIndex * 2); lastVisibleIndexes.resize(_lastVolIndex * 2); @@ -1033,7 +1083,12 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma TsdfVoxel& v = reinterpret_cast(vv); v.tsdf = floatToTsdf(0.0f); v.weight = 0; }); - + volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) + { + TsdfVoxel& v = reinterpret_cast(vv); + v.tsdf = floatToTsdf(0.0f); v.weight = 0; + }); + //volUnitsData.at(idx, 0) = cv::UMat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); } @@ -1084,6 +1139,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { frameParams = newParams; pixNorms = preCalculationPixNorm(depth, intrinsics); + Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); + _pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); } //! Integrate the correct volumeUnits @@ -1103,7 +1160,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); - + integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); //! Ensure all active volumeUnits are set to inactive for next integration isActive = false; } diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index c1bb98f0484..f0f35fb81e3 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -130,6 +130,9 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void reset() override; + void integrateVolumeUnitGPU(InputArray _depth, float depthFactor, + const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx); + void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, @@ -163,7 +166,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Vec4i volStrides; Vec6f frameParams; Mat pixNorms; - UMat _pixNorms; + VolumeIndex _lastVolIndex; cv::Mat indexes; @@ -171,6 +174,9 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::Mat activities; cv::Mat lastVisibleIndexes; cv::Mat _volUnitsData; + + cv::UMat _pixNorms; + cv::Mat volUnitsData; }; #endif diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 1bb5cbd6c84..6375ed617e4 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -38,7 +38,7 @@ __kernel void preCalculationPixNorm (__global float * pixNorms, pixNorms[idx] = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); } -__kernel void integrate(__global const char * depthptr, +__kernel void integrateVolumeUnit(__global const char * depthptr, int depth_step, int depth_offset, int depth_rows, int depth_cols, __global struct TsdfVoxel * volumeptr, From a4894ba8cff5cc98cbe4d7a9ea043683dea5fcdb Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 6 Nov 2020 16:57:44 +0300 Subject: [PATCH 020/216] Docs 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 f0f35fb81e3..b1d72a4ccca 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -166,7 +166,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Vec4i volStrides; Vec6f frameParams; Mat pixNorms; - + VolumeIndex _lastVolIndex; cv::Mat indexes; From db6f8671dd0957bdb78f41d0ed0c39bb0796ac52 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 9 Nov 2020 12:21:58 +0300 Subject: [PATCH 021/216] create simple volume table --- modules/rgbd/src/tsdf_functions.cpp | 120 ++++++++++++++++++++++++++++ 1 file changed, 120 insertions(+) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index ff40dce248d..3fc8a7ed330 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -373,5 +373,125 @@ void integrateVolumeUnit( } + +struct Volume_NODE +{ + Vec3i* idx; + int* row; + struct Volume_NODE* nextVolume; +}; + +struct Volumes_HEAD +{ + int* n; + struct Volume_NODE* firstVolume; +}; + +typedef Volume_NODE* head; + +Volume_NODE* create_Volume_NODE(Vec3i indx) +{ + Volume_NODE* new_volume_node = (Volume_NODE*)malloc(sizeof(Volume_NODE)); + *(new_volume_node->idx) = indx; + *(new_volume_node->row) = -1; + + return new_volume_node; +} + +Volume_NODE* create_Volume_NODE(Vec3i indx, int row) +{ + Volume_NODE* new_volume_node = (Volume_NODE*)malloc(sizeof(Volume_NODE)); + *(new_volume_node->idx) = indx; + *(new_volume_node->row) = row; + + return new_volume_node; +} + +Volumes_HEAD* create_Volumes_HEAD() +{ + Volumes_HEAD* new_volume_head = (Volumes_HEAD*)malloc(sizeof(Volumes_HEAD)); + *(new_volume_head->n) = 0; + new_volume_head->firstVolume = NULL; + + return new_volume_head; +} + +int find_Volume(Volumes_HEAD* head, Vec3i indx) +{ + //TODO: add difference between returning values; + if (head->n == 0) + return -1; + Volume_NODE* cursor = head->firstVolume; + while (cursor != NULL) + { + if (*(cursor->idx) == indx) + return *(cursor->row); + cursor = cursor->nextVolume; + } + return -1; +} + +//typedef cv::Mat VolumesTable; +//VolumesTable createVolumeTable() + +class VolumesTable +{ +public: + int hash_divisor; + cv::Mat table; + + VolumesTable(int rows); + ~VolumesTable(); + + void update(size_t hash, Vec3i indx); + void update(size_t hash, Vec3i indx, int row); +}; + +VolumesTable::VolumesTable(int rows) +{ + hash_divisor = rows; + table = cv::Mat(rows, 1, rawType()); +} + +VolumesTable::~VolumesTable() {} + +void VolumesTable::update(size_t hash, Vec3i indx) +{ + int i = hash % this->hash_divisor; + Volumes_HEAD* head = this->table.at(i, 0); + if (head->n == 0) + { + head->firstVolume = create_Volume_NODE(indx); + } + else + { + Volume_NODE* cursor = head->firstVolume; + while (cursor != NULL) + { + cursor = cursor->nextVolume; + } + cursor->nextVolume = create_Volume_NODE(indx); + } +} + +void VolumesTable::update(size_t hash, Vec3i indx, int row) +{ + int i = hash % this->hash_divisor; + Volumes_HEAD* head = this->table.at(i, 0); + if (head->n == 0) + { + head->firstVolume = create_Volume_NODE(indx, row); + } + else + { + Volume_NODE* cursor = head->firstVolume; + while (cursor != NULL) + { + cursor = cursor->nextVolume; + } + cursor->nextVolume = create_Volume_NODE(indx, row); + } +} + } // namespace kinfu } // namespace cv From adc0d21d7cece22b5ab39455dccb79c28f1756a7 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 10 Nov 2020 09:51:00 +0300 Subject: [PATCH 022/216] add functionality to the table --- modules/rgbd/src/tsdf_functions.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 3fc8a7ed330..51b36ef4870 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -403,6 +403,7 @@ Volume_NODE* create_Volume_NODE(Vec3i indx, int row) Volume_NODE* new_volume_node = (Volume_NODE*)malloc(sizeof(Volume_NODE)); *(new_volume_node->idx) = indx; *(new_volume_node->row) = row; + new_volume_node->nextVolume = NULL; return new_volume_node; } @@ -468,6 +469,8 @@ void VolumesTable::update(size_t hash, Vec3i indx) Volume_NODE* cursor = head->firstVolume; while (cursor != NULL) { + if (*(cursor->idx) == indx) + return; cursor = cursor->nextVolume; } cursor->nextVolume = create_Volume_NODE(indx); @@ -487,6 +490,11 @@ void VolumesTable::update(size_t hash, Vec3i indx, int row) Volume_NODE* cursor = head->firstVolume; while (cursor != NULL) { + if (*(cursor->idx) == indx) + { + *(cursor->idx) = row; + return; + } cursor = cursor->nextVolume; } cursor->nextVolume = create_Volume_NODE(indx, row); From 6ce9add459e9d056942e660afcce61ec7448a11d Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 10 Nov 2020 10:56:31 +0300 Subject: [PATCH 023/216] add new functions --- modules/rgbd/src/tsdf_functions.cpp | 25 ++++++++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 51b36ef4870..7c9884cd9e7 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -417,11 +417,12 @@ Volumes_HEAD* create_Volumes_HEAD() return new_volume_head; } -int find_Volume(Volumes_HEAD* head, Vec3i indx) +int _find_Volume(Volumes_HEAD* head, Vec3i indx) { //TODO: add difference between returning values; + // -2 if there is not needful value if (head->n == 0) - return -1; + return -2; Volume_NODE* cursor = head->firstVolume; while (cursor != NULL) { @@ -429,7 +430,7 @@ int find_Volume(Volumes_HEAD* head, Vec3i indx) return *(cursor->row); cursor = cursor->nextVolume; } - return -1; + return -2; } //typedef cv::Mat VolumesTable; @@ -446,6 +447,8 @@ class VolumesTable void update(size_t hash, Vec3i indx); void update(size_t hash, Vec3i indx, int row); + int find_Volume(size_t hash, Vec3i indx); + bool isExist(size_t hash, Vec3i indx); }; VolumesTable::VolumesTable(int rows) @@ -501,5 +504,21 @@ void VolumesTable::update(size_t hash, Vec3i indx, int row) } } +int VolumesTable::find_Volume(size_t hash, Vec3i indx) +{ + int i = hash % this->hash_divisor; + Volumes_HEAD* head = this->table.at(i, 0); + int row = _find_Volume(head, indx); + return row; +} + +bool VolumesTable::isExist(size_t hash, Vec3i indx) +{ + if (this->find_Volume(hash, indx) == -2) + return false; + return true; +} + + } // namespace kinfu } // namespace cv From 9f30f12cfb7180861815cf653ba454af8de22679 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 10 Nov 2020 12:53:47 +0300 Subject: [PATCH 024/216] create new hash table implementation --- modules/rgbd/src/tsdf_functions.cpp | 142 +++++++++++++++++++++++++++- 1 file changed, 139 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 7c9884cd9e7..39c5f5c9b87 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -373,7 +373,7 @@ void integrateVolumeUnit( } - +/* struct Volume_NODE { Vec3i* idx; @@ -463,7 +463,7 @@ void VolumesTable::update(size_t hash, Vec3i indx) { int i = hash % this->hash_divisor; Volumes_HEAD* head = this->table.at(i, 0); - if (head->n == 0) + if (*(head->n) == 0) { head->firstVolume = create_Volume_NODE(indx); } @@ -477,6 +477,7 @@ void VolumesTable::update(size_t hash, Vec3i indx) cursor = cursor->nextVolume; } cursor->nextVolume = create_Volume_NODE(indx); + *(head->n)++; } } @@ -484,7 +485,7 @@ void VolumesTable::update(size_t hash, Vec3i indx, int row) { int i = hash % this->hash_divisor; Volumes_HEAD* head = this->table.at(i, 0); - if (head->n == 0) + if (*(head->n) == 0) { head->firstVolume = create_Volume_NODE(indx, row); } @@ -501,7 +502,142 @@ void VolumesTable::update(size_t hash, Vec3i indx, int row) cursor = cursor->nextVolume; } cursor->nextVolume = create_Volume_NODE(indx, row); + *(head->n)++; + } +} + +int VolumesTable::find_Volume(size_t hash, Vec3i indx) +{ + int i = hash % this->hash_divisor; + Volumes_HEAD* head = this->table.at(i, 0); + int row = _find_Volume(head, indx); + return row; +} + +bool VolumesTable::isExist(size_t hash, Vec3i indx) +{ + if (this->find_Volume(hash, indx) == -2) + return false; + return true; +} +*/ + +struct Volume_NODE +{ + Vec3i* idx; + int* row; + bool* isActive; + struct Volume_NODE* nextVolume; +}; + +struct Volumes_HEAD +{ + struct Volume_NODE* firstVolume; +}; + +typedef Volume_NODE* head; + +Volume_NODE* create_Volume_NODE() +{ + Volume_NODE* new_volume_node = (Volume_NODE*)malloc(sizeof(Volume_NODE)); + *(new_volume_node->idx) = nan3; + *(new_volume_node->row) = -1; + *(new_volume_node->isActive) = false; + new_volume_node->nextVolume = NULL; + + return new_volume_node; +} + +Volumes_HEAD* create_Volumes_HEAD() +{ + Volumes_HEAD* new_volume_head = (Volumes_HEAD*)malloc(sizeof(Volumes_HEAD)); + new_volume_head->firstVolume = NULL; + + return new_volume_head; +} + +int _find_Volume(Volumes_HEAD* head, Vec3i indx) +{ + //TODO: add difference between returning values; + // -2 if there is not needful value + Volume_NODE* cursor = head->firstVolume; + while (cursor != NULL || *(cursor->isActive)) + { + if (*(cursor->idx) == indx) + return *(cursor->row); + cursor = cursor->nextVolume; + } + return -2; +} + +class VolumesTable +{ +public: + int hash_divisor; + int list_size; + cv::Mat table; + + VolumesTable(int rows); + ~VolumesTable(); + + void update(size_t hash, Vec3i indx); + void update(size_t hash, Vec3i indx, int row); + int find_Volume(size_t hash, Vec3i indx); + bool isExist(size_t hash, Vec3i indx); +}; + +VolumesTable::VolumesTable(int rows) +{ + hash_divisor = rows; + list_size = 100; + table = cv::Mat(rows, 1, rawType()); + table.forEach([&](Volumes_HEAD* h, const int*) + { + Volume_NODE* cursor = h->firstVolume; + for (int i = 0; i < this->list_size; i++) + { + cursor->nextVolume = create_Volume_NODE(); + cursor = cursor->nextVolume; + } + }); +} + +VolumesTable::~VolumesTable() {} + +void VolumesTable::update(size_t hash, Vec3i indx) +{ + int i = hash % this->hash_divisor; + Volumes_HEAD* head = this->table.at(i, 0); + + Volume_NODE* cursor = head->firstVolume; + while (cursor != NULL || *(cursor->isActive)) + { + if (*(cursor->idx) == indx) + return; + cursor = cursor->nextVolume; + } + *(cursor->isActive) = true; + *(cursor->idx) = indx; +} + +void VolumesTable::update(size_t hash, Vec3i indx, int row) +{ + int i = hash % this->hash_divisor; + Volumes_HEAD* head = this->table.at(i, 0); + + Volume_NODE* cursor = head->firstVolume; + while (cursor != NULL || *(cursor->isActive)) + { + if (*(cursor->idx) == indx) + { + *(cursor->row) = row; + return; + } + cursor = cursor->nextVolume; } + *(cursor->isActive) = true; + *(cursor->idx) = indx; + *(cursor->row) = row; } int VolumesTable::find_Volume(size_t hash, Vec3i indx) From 6dae10de6e9284ad373a182be63fe1d2e2c3d3f3 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 11 Nov 2020 15:48:01 +0300 Subject: [PATCH 025/216] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 1 + modules/rgbd/src/hash_tsdf.hpp | 2 +- modules/rgbd/src/tsdf_functions.cpp | 64 ++++++++++++++++++++++------- modules/rgbd/src/tsdf_functions.hpp | 38 +++++++++++++++++ 4 files changed, 90 insertions(+), 15 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 7d0cc23b53d..470f9775ba7 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -862,6 +862,7 @@ void HashTSDFVolumeGPU::reset() poses = cv::Mat(VOLUMES_SIZE, 1, rawType()); activities = cv::Mat(VOLUMES_SIZE, 1, rawType()); lastVisibleIndexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); + volumeUnits = VolumesTable(VOLUMES_SIZE); } static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index b1d72a4ccca..05b873fb308 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -167,7 +167,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Vec6f frameParams; Mat pixNorms; - VolumeIndex _lastVolIndex; cv::Mat indexes; cv::Mat poses; @@ -177,6 +176,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::UMat _pixNorms; cv::Mat volUnitsData; + VolumesTable volumeUnits; }; #endif diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 39c5f5c9b87..8bf768dff33 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -521,7 +521,7 @@ bool VolumesTable::isExist(size_t hash, Vec3i indx) return true; } */ - +/* struct Volume_NODE { Vec3i* idx; @@ -534,9 +534,7 @@ struct Volumes_HEAD { struct Volume_NODE* firstVolume; }; - -typedef Volume_NODE* head; - +*/ Volume_NODE* create_Volume_NODE() { Volume_NODE* new_volume_node = (Volume_NODE*)malloc(sizeof(Volume_NODE)); @@ -570,6 +568,18 @@ int _find_Volume(Volumes_HEAD* head, Vec3i indx) return -2; } +size_t calc_hash(Vec3i x) +{ + size_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + for (uint16_t i = 0; i < 3; i++) + { + seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + } + return seed; +} + +/* class VolumesTable { public: @@ -585,21 +595,47 @@ class VolumesTable int find_Volume(size_t hash, Vec3i indx); bool isExist(size_t hash, Vec3i indx); }; +*/ +/* +VolumesTable::VolumesTable() +{ + hash_divisor = 1024; + list_size = 100; + table = cv::Mat(1024, 1, rawType()); + + for (int i = 0; i < this->hash_divisor; i++) + { + Volume_NODE* cursor = table.at(i, 0)->firstVolume; + for (int i = 0; i < this->list_size; i++) + { + cursor->nextVolume = create_Volume_NODE(); + cursor = cursor->nextVolume; + } + } + +} VolumesTable::VolumesTable(int rows) { hash_divisor = rows; list_size = 100; table = cv::Mat(rows, 1, rawType()); - table.forEach([&](Volumes_HEAD* h, const int*) - { - Volume_NODE* cursor = h->firstVolume; - for (int i = 0; i < this->list_size; i++) - { - cursor->nextVolume = create_Volume_NODE(); - cursor = cursor->nextVolume; - } - }); + + for (int i = 0; i < this->hash_divisor; i++) + { + Volumes_HEAD* h = table.at(i, 0); + h = create_Volumes_HEAD(); + Volume_NODE* cursor = h->firstVolume; + cursor->nextVolume;// + cursor = create_Volume_NODE(); + //for (int i = 0; i < this->list_size; i++) + //{ + //cursor->nextVolume = create_Volume_NODE(); + //cursor = cursor->nextVolume; + //} + + } + } VolumesTable::~VolumesTable() {} @@ -654,7 +690,7 @@ bool VolumesTable::isExist(size_t hash, Vec3i indx) return false; return true; } - +*/ } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 6d86595118f..0dec00326c1 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -43,6 +43,44 @@ void integrateVolumeUnit( InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); + +struct Volume_NODE +{ + Vec3i* idx; + int* row; + bool* isActive; + struct Volume_NODE* nextVolume; +}; + +struct Volumes_HEAD +{ + struct Volume_NODE* firstVolume; +}; + +Volume_NODE* create_Volume_NODE(); +Volumes_HEAD* create_Volumes_HEAD(); +int _find_Volume(Volumes_HEAD* head, Vec3i indx); +size_t calc_hash(Vec3i x); + +class VolumesTable +{ +public: + int hash_divisor; + int list_size; + cv::Mat table; + + VolumesTable(); + VolumesTable(int rows); + ~VolumesTable(); + + void update(size_t hash, Vec3i indx); + void update(size_t hash, Vec3i indx, int row); + int find_Volume(size_t hash, Vec3i indx); + bool isExist(size_t hash, Vec3i indx); +}; + + + } // namespace kinfu } // namespace cv #endif From eaed8d2227465788eddb56ac44c4b3df446c8187 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 11 Nov 2020 17:02:01 +0300 Subject: [PATCH 026/216] create new hash tsdf implementaton --- modules/rgbd/src/hash_tsdf.cpp | 2 +- modules/rgbd/src/tsdf_functions.cpp | 330 ++++------------------------ modules/rgbd/src/tsdf_functions.hpp | 35 ++- 3 files changed, 61 insertions(+), 306 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 470f9775ba7..aa6bfeb7ffa 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -862,7 +862,7 @@ void HashTSDFVolumeGPU::reset() poses = cv::Mat(VOLUMES_SIZE, 1, rawType()); activities = cv::Mat(VOLUMES_SIZE, 1, rawType()); lastVisibleIndexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); - volumeUnits = VolumesTable(VOLUMES_SIZE); + volumeUnits = VolumesTable(); } static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 8bf768dff33..1db2840915e 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -373,201 +373,6 @@ void integrateVolumeUnit( } -/* -struct Volume_NODE -{ - Vec3i* idx; - int* row; - struct Volume_NODE* nextVolume; -}; - -struct Volumes_HEAD -{ - int* n; - struct Volume_NODE* firstVolume; -}; - -typedef Volume_NODE* head; - -Volume_NODE* create_Volume_NODE(Vec3i indx) -{ - Volume_NODE* new_volume_node = (Volume_NODE*)malloc(sizeof(Volume_NODE)); - *(new_volume_node->idx) = indx; - *(new_volume_node->row) = -1; - - return new_volume_node; -} - -Volume_NODE* create_Volume_NODE(Vec3i indx, int row) -{ - Volume_NODE* new_volume_node = (Volume_NODE*)malloc(sizeof(Volume_NODE)); - *(new_volume_node->idx) = indx; - *(new_volume_node->row) = row; - new_volume_node->nextVolume = NULL; - - return new_volume_node; -} - -Volumes_HEAD* create_Volumes_HEAD() -{ - Volumes_HEAD* new_volume_head = (Volumes_HEAD*)malloc(sizeof(Volumes_HEAD)); - *(new_volume_head->n) = 0; - new_volume_head->firstVolume = NULL; - - return new_volume_head; -} - -int _find_Volume(Volumes_HEAD* head, Vec3i indx) -{ - //TODO: add difference between returning values; - // -2 if there is not needful value - if (head->n == 0) - return -2; - Volume_NODE* cursor = head->firstVolume; - while (cursor != NULL) - { - if (*(cursor->idx) == indx) - return *(cursor->row); - cursor = cursor->nextVolume; - } - return -2; -} - -//typedef cv::Mat VolumesTable; -//VolumesTable createVolumeTable() - -class VolumesTable -{ -public: - int hash_divisor; - cv::Mat table; - - VolumesTable(int rows); - ~VolumesTable(); - - void update(size_t hash, Vec3i indx); - void update(size_t hash, Vec3i indx, int row); - int find_Volume(size_t hash, Vec3i indx); - bool isExist(size_t hash, Vec3i indx); -}; - -VolumesTable::VolumesTable(int rows) -{ - hash_divisor = rows; - table = cv::Mat(rows, 1, rawType()); -} - -VolumesTable::~VolumesTable() {} - -void VolumesTable::update(size_t hash, Vec3i indx) -{ - int i = hash % this->hash_divisor; - Volumes_HEAD* head = this->table.at(i, 0); - if (*(head->n) == 0) - { - head->firstVolume = create_Volume_NODE(indx); - } - else - { - Volume_NODE* cursor = head->firstVolume; - while (cursor != NULL) - { - if (*(cursor->idx) == indx) - return; - cursor = cursor->nextVolume; - } - cursor->nextVolume = create_Volume_NODE(indx); - *(head->n)++; - } -} - -void VolumesTable::update(size_t hash, Vec3i indx, int row) -{ - int i = hash % this->hash_divisor; - Volumes_HEAD* head = this->table.at(i, 0); - if (*(head->n) == 0) - { - head->firstVolume = create_Volume_NODE(indx, row); - } - else - { - Volume_NODE* cursor = head->firstVolume; - while (cursor != NULL) - { - if (*(cursor->idx) == indx) - { - *(cursor->idx) = row; - return; - } - cursor = cursor->nextVolume; - } - cursor->nextVolume = create_Volume_NODE(indx, row); - *(head->n)++; - } -} - -int VolumesTable::find_Volume(size_t hash, Vec3i indx) -{ - int i = hash % this->hash_divisor; - Volumes_HEAD* head = this->table.at(i, 0); - int row = _find_Volume(head, indx); - return row; -} - -bool VolumesTable::isExist(size_t hash, Vec3i indx) -{ - if (this->find_Volume(hash, indx) == -2) - return false; - return true; -} -*/ -/* -struct Volume_NODE -{ - Vec3i* idx; - int* row; - bool* isActive; - struct Volume_NODE* nextVolume; -}; - -struct Volumes_HEAD -{ - struct Volume_NODE* firstVolume; -}; -*/ -Volume_NODE* create_Volume_NODE() -{ - Volume_NODE* new_volume_node = (Volume_NODE*)malloc(sizeof(Volume_NODE)); - *(new_volume_node->idx) = nan3; - *(new_volume_node->row) = -1; - *(new_volume_node->isActive) = false; - new_volume_node->nextVolume = NULL; - - return new_volume_node; -} - -Volumes_HEAD* create_Volumes_HEAD() -{ - Volumes_HEAD* new_volume_head = (Volumes_HEAD*)malloc(sizeof(Volumes_HEAD)); - new_volume_head->firstVolume = NULL; - - return new_volume_head; -} - -int _find_Volume(Volumes_HEAD* head, Vec3i indx) -{ - //TODO: add difference between returning values; - // -2 if there is not needful value - Volume_NODE* cursor = head->firstVolume; - while (cursor != NULL || *(cursor->isActive)) - { - if (*(cursor->idx) == indx) - return *(cursor->row); - cursor = cursor->nextVolume; - } - return -2; -} - size_t calc_hash(Vec3i x) { size_t seed = 0; @@ -579,118 +384,77 @@ size_t calc_hash(Vec3i x) return seed; } -/* -class VolumesTable -{ -public: - int hash_divisor; - int list_size; - cv::Mat table; - - VolumesTable(int rows); - ~VolumesTable(); - - void update(size_t hash, Vec3i indx); - void update(size_t hash, Vec3i indx, int row); - int find_Volume(size_t hash, Vec3i indx); - bool isExist(size_t hash, Vec3i indx); -}; -*/ -/* VolumesTable::VolumesTable() { - hash_divisor = 1024; - list_size = 100; - table = cv::Mat(1024, 1, rawType()); - - for (int i = 0; i < this->hash_divisor; i++) - { - Volume_NODE* cursor = table.at(i, 0)->firstVolume; - for (int i = 0; i < this->list_size; i++) - { - cursor->nextVolume = create_Volume_NODE(); - cursor = cursor->nextVolume; - } - } - + this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType()); } -VolumesTable::VolumesTable(int rows) +void VolumesTable::update(Vec3i indx) { - hash_divisor = rows; - list_size = 100; - table = cv::Mat(rows, 1, rawType()); - - for (int i = 0; i < this->hash_divisor; i++) + size_t i = calc_hash(indx) / hash_divisor; + size_t a = i * list_size; + size_t b = (i + 1) * list_size; + while(a < b || volumes.at(a, 0).idx != indx) { - Volumes_HEAD* h = table.at(i, 0); - h = create_Volumes_HEAD(); - Volume_NODE* cursor = h->firstVolume; - cursor->nextVolume;// - cursor = create_Volume_NODE(); - //for (int i = 0; i < this->list_size; i++) - //{ - //cursor->nextVolume = create_Volume_NODE(); - //cursor = cursor->nextVolume; - //} - - } - -} - -VolumesTable::~VolumesTable() {} - -void VolumesTable::update(size_t hash, Vec3i indx) -{ - int i = hash % this->hash_divisor; - Volumes_HEAD* head = this->table.at(i, 0); - - Volume_NODE* cursor = head->firstVolume; - while (cursor != NULL || *(cursor->isActive)) - { - if (*(cursor->idx) == indx) + Volume_NODE& v = volumes.at(a, 0); + //find nan cheking for int or Vec3i + if (isnan(float(v.idx[0]))) + { + v.idx = indx; return; - cursor = cursor->nextVolume; + } + a++; } - *(cursor->isActive) = true; - *(cursor->idx) = indx; } -void VolumesTable::update(size_t hash, Vec3i indx, int row) +void VolumesTable::update(Vec3i indx, int row) { - int i = hash % this->hash_divisor; - Volumes_HEAD* head = this->table.at(i, 0); - - Volume_NODE* cursor = head->firstVolume; - while (cursor != NULL || *(cursor->isActive)) + size_t i = calc_hash(indx) / hash_divisor; + size_t a = i * list_size; + size_t b = (i + 1) * list_size; + while (a < b) { - if (*(cursor->idx) == indx) + Volume_NODE& v = volumes.at(a, 0); + if (v.idx == indx) + { + v.row = row; + return; + } + //find nan cheking for int or Vec3i + if (isnan(float(v.idx[0]))) { - *(cursor->row) = row; + v.idx = indx; + v.row = row; return; } - cursor = cursor->nextVolume; + a++; } - *(cursor->isActive) = true; - *(cursor->idx) = indx; - *(cursor->row) = row; } -int VolumesTable::find_Volume(size_t hash, Vec3i indx) +int VolumesTable::find_Volume(Vec3i indx) { - int i = hash % this->hash_divisor; - Volumes_HEAD* head = this->table.at(i, 0); - int row = _find_Volume(head, indx); - return row; + size_t i = calc_hash(indx) / hash_divisor; + size_t a = i * list_size; + size_t b = (i + 1) * list_size; + while (a < b) + { + Volume_NODE& v = volumes.at(a, 0); + if (v.idx == indx) + return v.row; + //find nan cheking for int or Vec3i + if (isnan(float(v.idx[0]))) + return -2; + a++; + } + return -2; } - -bool VolumesTable::isExist(size_t hash, Vec3i indx) +bool VolumesTable::isExist(Vec3i indx) { - if (this->find_Volume(hash, indx) == -2) + if (this->find_Volume(indx) == -2) return false; return true; } -*/ + } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 0dec00326c1..e89a538a154 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -46,37 +46,28 @@ void integrateVolumeUnit( struct Volume_NODE { - Vec3i* idx; - int* row; - bool* isActive; - struct Volume_NODE* nextVolume; + Vec3i idx = nan3; + int row = -1; }; -struct Volumes_HEAD -{ - struct Volume_NODE* firstVolume; -}; - -Volume_NODE* create_Volume_NODE(); -Volumes_HEAD* create_Volumes_HEAD(); -int _find_Volume(Volumes_HEAD* head, Vec3i indx); size_t calc_hash(Vec3i x); class VolumesTable { public: - int hash_divisor; - int list_size; - cv::Mat table; - + int hash_divisor = 1024; + int list_size = 128; + + cv::Mat volumes; + VolumesTable(); - VolumesTable(int rows); - ~VolumesTable(); + ~VolumesTable() {}; + - void update(size_t hash, Vec3i indx); - void update(size_t hash, Vec3i indx, int row); - int find_Volume(size_t hash, Vec3i indx); - bool isExist(size_t hash, Vec3i indx); + void update(Vec3i indx); + void update(Vec3i indx, int row); + int find_Volume(Vec3i indx); + bool isExist(Vec3i indx); }; From cd2c3e23c08d80f3a7b35d7853d753f61f3e2598 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 12 Nov 2020 10:32:40 +0300 Subject: [PATCH 027/216] add expand feature --- modules/rgbd/src/tsdf_functions.cpp | 59 +++++++++++++++++++++++------ modules/rgbd/src/tsdf_functions.hpp | 8 +++- 2 files changed, 53 insertions(+), 14 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 1db2840915e..be6f204c895 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -391,30 +391,37 @@ VolumesTable::VolumesTable() void VolumesTable::update(Vec3i indx) { - size_t i = calc_hash(indx) / hash_divisor; - size_t a = i * list_size; - size_t b = (i + 1) * list_size; - while(a < b || volumes.at(a, 0).idx != indx) + size_t hash = calc_hash(indx) / hash_divisor; + int num = 1; + size_t start = hash * num * list_size; + size_t i = start; + + while (i != -1) { - Volume_NODE& v = volumes.at(a, 0); + Volume_NODE& v = volumes.at(i, 0); + if (v.idx == indx) + return; //find nan cheking for int or Vec3i if (isnan(float(v.idx[0]))) { v.idx = indx; + v.nextVolumeRow = getNextVolume(hash, num, i); return; } - a++; + i = v.nextVolumeRow; } } void VolumesTable::update(Vec3i indx, int row) { - size_t i = calc_hash(indx) / hash_divisor; - size_t a = i * list_size; - size_t b = (i + 1) * list_size; - while (a < b) + size_t hash = calc_hash(indx) / hash_divisor; + int num = 1; + size_t start = hash * num * list_size; + size_t i = start; + + while (i != -1) { - Volume_NODE& v = volumes.at(a, 0); + Volume_NODE& v = volumes.at(i, 0); if (v.idx == indx) { v.row = row; @@ -425,12 +432,40 @@ void VolumesTable::update(Vec3i indx, int row) { v.idx = indx; v.row = row; + v.nextVolumeRow = getNextVolume(hash, num, i); return; } - a++; + i = v.nextVolumeRow; } } +int VolumesTable::getNextVolume(int hash, int& num, int i) +{ + if (i % list_size == 0) + { + if (num <= buffferNums) + { + num++; + } + else + { + this->expand(); + num++; + } + return hash * num * list_size; + } + else + { + return i++; + } +} + +void VolumesTable::expand() +{ + this->volumes.resize(hash_divisor * (buffferNums + 1)); + this->buffferNums++; +} + int VolumesTable::find_Volume(Vec3i indx) { size_t i = calc_hash(indx) / hash_divisor; diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index e89a538a154..3b2b5526a06 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -48,6 +48,7 @@ struct Volume_NODE { Vec3i idx = nan3; int row = -1; + int nextVolumeRow = -1; }; size_t calc_hash(Vec3i x); @@ -55,8 +56,9 @@ size_t calc_hash(Vec3i x); class VolumesTable { public: - int hash_divisor = 1024; - int list_size = 128; + int hash_divisor = 32768; + int list_size = 4; + int buffferNums = 1; cv::Mat volumes; @@ -66,6 +68,8 @@ class VolumesTable void update(Vec3i indx); void update(Vec3i indx, int row); + void expand(); + int getNextVolume(int hash, int& num, int i); int find_Volume(Vec3i indx); bool isExist(Vec3i indx); }; From b2f64c8b040b7058700b62c7558009f2ba73b8f6 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 12 Nov 2020 15:26:25 +0300 Subject: [PATCH 028/216] minorfix --- modules/rgbd/src/tsdf_functions.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index be6f204c895..fd65362a073 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -468,18 +468,19 @@ void VolumesTable::expand() int VolumesTable::find_Volume(Vec3i indx) { - size_t i = calc_hash(indx) / hash_divisor; - size_t a = i * list_size; - size_t b = (i + 1) * list_size; - while (a < b) + size_t hash = calc_hash(indx) / hash_divisor; + int num = 1; + size_t i = hash * num * list_size; + + while (i != -1) { - Volume_NODE& v = volumes.at(a, 0); + Volume_NODE& v = volumes.at(i, 0); if (v.idx == indx) return v.row; //find nan cheking for int or Vec3i if (isnan(float(v.idx[0]))) return -2; - a++; + i = v.nextVolumeRow; } return -2; } From d00fe0f6063834d9804bd44ac1fdf1fe93ea50ea Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 13 Nov 2020 09:22:37 +0300 Subject: [PATCH 029/216] error fix --- modules/rgbd/src/tsdf_functions.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index fd65362a073..4274cd9da15 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -402,7 +402,7 @@ void VolumesTable::update(Vec3i indx) if (v.idx == indx) return; //find nan cheking for int or Vec3i - if (isnan(float(v.idx[0]))) + if (isNaN(Point3i(v.idx))) { v.idx = indx; v.nextVolumeRow = getNextVolume(hash, num, i); @@ -428,7 +428,7 @@ void VolumesTable::update(Vec3i indx, int row) return; } //find nan cheking for int or Vec3i - if (isnan(float(v.idx[0]))) + if (isNaN(Point3i(v.idx))) { v.idx = indx; v.row = row; @@ -478,7 +478,7 @@ int VolumesTable::find_Volume(Vec3i indx) if (v.idx == indx) return v.row; //find nan cheking for int or Vec3i - if (isnan(float(v.idx[0]))) + if (isNaN(Point3i(v.idx))) return -2; i = v.nextVolumeRow; } From 8ccf21d3b372b8c289f6c35d9cdc7c6a38716285 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 13 Nov 2020 12:16:08 +0300 Subject: [PATCH 030/216] it builds --- modules/rgbd/src/hash_tsdf.cpp | 68 +++++++++++++++++------------ modules/rgbd/src/hash_tsdf.hpp | 2 +- modules/rgbd/src/tsdf_functions.cpp | 31 ++++++++++--- modules/rgbd/src/tsdf_functions.hpp | 3 +- 4 files changed, 68 insertions(+), 36 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index aa6bfeb7ffa..e497f5a7daf 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -862,7 +862,7 @@ void HashTSDFVolumeGPU::reset() poses = cv::Mat(VOLUMES_SIZE, 1, rawType()); activities = cv::Mat(VOLUMES_SIZE, 1, rawType()); lastVisibleIndexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); - volumeUnits = VolumesTable(); + _indexes = VolumesTable(); } static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) @@ -995,6 +995,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Point3f truncPt(truncDist, truncDist, truncDist); _VolumeUnitIndexSet _newIndices = cv::Mat(VOLUMES_SIZE, 1, rawType()); + cv::Mat newIndices = cv::Mat(VOLUMES_SIZE, 1, rawType()); Mutex mutex; Range allocateRange(0, depth.rows); int loc_vol_idx = 0; @@ -1039,9 +1040,14 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = 0; i < loc_vol_idx; i++) { Vec3i idx = _localAccessVolUnits.at(i, 0); - if (find_idx(indexes, idx)==-1) + + //if (find_idx(indexes, idx)==-1) + //std::cout << idx << " " << _indexes.isExist(idx) << std::endl; + if (_indexes.isExist(idx)) { - if (_lastVolIndex >= VolumeIndex(indexes.size().height)) + //std::cout << " 888 " << std::endl; + + if (_lastVolIndex >= VolumeIndex(_volUnitsData.size().height)) { indexes.resize(_lastVolIndex * 2); _volUnitsData.resize(_lastVolIndex * 2); @@ -1052,27 +1058,32 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } this->indexes.at(_lastVolIndex, 0) = idx; + _indexes.update(idx, _lastVolIndex); _newIndices.at(vol_idx, 0) = _lastVolIndex; - + newIndices.at(vol_idx, 0) = idx; vol_idx++; _lastVolIndex++; - + } + } //mutex.unlock(); }; - + //parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); AllocateVolumeUnitsInvoker(allocateRange); //! Perform the allocation - + //std::cout << "Perform the allocation" << std::endl; for (int i = 0; i < vol_idx; i++) { - - VolumeIndex idx = _newIndices.at(i, 0); - - Vec3i tsdf_idx = indexes.at(idx, 0); + //VolumeIndex idx = _newIndices.at(i, 0); + //Vec3i tsdf_idx = indexes.at(idx, 0); + Vec3i tsdf_idx = newIndices.at(i, 0); + VolumeIndex idx = _indexes.find_Volume(idx); + //Vec3i tsdf_idx = Vec3i(1, 1, 1); + //VolumeIndex idx = 0; + //std::cout << tsdf_idx << " " << idx << std::endl; Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; poses.at(idx, 0) = subvolumePose; @@ -1094,11 +1105,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! Get keys for all the allocated volume Units - std::vector _totalVolUnits; - for (int i = 0; i < indexes.size().height; i++) - { - _totalVolUnits.push_back(indexes.at(i, 0)); - } + std::vector _totalVolUnits = _indexes.indexes; + //for (int i = 0; i < indexes.size().height; i++){_totalVolUnits.push_back(indexes.at(i, 0));} //! Mark volumes in the camera frustum as active @@ -1110,7 +1118,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = range.start; i < range.end; ++i) { Vec3i tsdf_idx = _totalVolUnits[i]; - VolumeIndex idx = find_idx(indexes, tsdf_idx); + //VolumeIndex idx = find_idx(indexes, tsdf_idx); + VolumeIndex idx = _indexes.find_Volume(idx); if (idx < 0 || idx == _lastVolIndex - 1) return; Point3f volumeUnitPos = volumeUnitIdxToVolume(poses.at(idx, 0)); @@ -1143,13 +1152,14 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); _pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); } - + //std::cout << "ertyu\n"; //! 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]; - VolumeIndex idx = find_idx(indexes, tsdf_idx); + //VolumeIndex idx = find_idx(indexes, tsdf_idx); + VolumeIndex idx = _indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == _lastVolIndex - 1) return; bool& isActive = activities.at(idx, 0); @@ -1267,7 +1277,8 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const auto it = iterMap[dictIdx]; if (!queried[dictIdx]) { - it = find_idx(indexes, volumeUnitIdx); + //it = find_idx(indexes, volumeUnitIdx); + it = _indexes.find_Volume(volumeUnitIdx); if (it >= 0 || it < _lastVolIndex) { iterMap[dictIdx] = it; @@ -1338,7 +1349,8 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const if (!queried[dictIdx]) { - it = find_idx(indexes, volumeUnitIdx); + //it = find_idx(indexes, volumeUnitIdx); + it = _indexes.find_Volume(volumeUnitIdx); if (it >= 0 || it < _lastVolIndex) { iterMap[dictIdx] = it; @@ -1494,7 +1506,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Point3f currRayPos = orig + tcurr * rayDirV; cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); + //VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); + VolumeIndex idx = _indexes.find_Volume(currVolumeUnitIdx); float currTsdf = prevTsdf; int currWeight = 0; float stepSize = 0.5f * blockSize; @@ -1562,11 +1575,9 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { std::vector> pVecs, nVecs; - std::vector _totalVolUnits; - for (int i = 0; i < indexes.size().height; i++) - { - _totalVolUnits.push_back(indexes.at(i, 0)); - } + //std::vector _totalVolUnits; + std::vector _totalVolUnits = _indexes.indexes; + //for (int i = 0; i < indexes.size().height; i++){_totalVolUnits.push_back(indexes.at(i, 0));} Range _fetchRange(0, (int)_totalVolUnits.size()); @@ -1584,7 +1595,8 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { cv::Vec3i tsdf_idx = _totalVolUnits[i]; - VolumeIndex idx = find_idx(indexes, tsdf_idx); + //VolumeIndex idx = find_idx(indexes, tsdf_idx); + VolumeIndex idx = _indexes.find_Volume(tsdf_idx); Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); if (idx >= 0 && idx < _lastVolIndex) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 05b873fb308..896319f07f9 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -176,7 +176,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::UMat _pixNorms; cv::Mat volUnitsData; - VolumesTable volumeUnits; + VolumesTable _indexes; }; #endif diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 4274cd9da15..f66300ddc3c 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -387,11 +387,26 @@ size_t calc_hash(Vec3i x) VolumesTable::VolumesTable() { this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType()); + for (int i = 0; i < volumes.size().height; i++) + { + Volume_NODE& v = volumes.at(i, 0); + v.idx = nan3; + v.row = -1; + v.nextVolumeRow = -1; + } + /* + this->volumes.forEach([](Volume_NODE& v, const int*) + { + v.idx = nan3; + v.row = -1; + v.nextVolumeRow = -1; + }); + */ } void VolumesTable::update(Vec3i indx) { - size_t hash = calc_hash(indx) / hash_divisor; + size_t hash = calc_hash(indx) % hash_divisor; int num = 1; size_t start = hash * num * list_size; size_t i = start; @@ -406,6 +421,7 @@ void VolumesTable::update(Vec3i indx) { v.idx = indx; v.nextVolumeRow = getNextVolume(hash, num, i); + indexes.push_back(indx); return; } i = v.nextVolumeRow; @@ -414,7 +430,7 @@ void VolumesTable::update(Vec3i indx) void VolumesTable::update(Vec3i indx, int row) { - size_t hash = calc_hash(indx) / hash_divisor; + size_t hash = calc_hash(indx) % hash_divisor; int num = 1; size_t start = hash * num * list_size; size_t i = start; @@ -433,6 +449,7 @@ void VolumesTable::update(Vec3i indx, int row) v.idx = indx; v.row = row; v.nextVolumeRow = getNextVolume(hash, num, i); + indexes.push_back(indx); return; } i = v.nextVolumeRow; @@ -466,15 +483,17 @@ void VolumesTable::expand() this->buffferNums++; } -int VolumesTable::find_Volume(Vec3i indx) +int VolumesTable::find_Volume(Vec3i indx) const { - size_t hash = calc_hash(indx) / hash_divisor; + size_t hash = calc_hash(indx) % hash_divisor; int num = 1; size_t i = hash * num * list_size; - + //std::cout << "1" << std::endl; while (i != -1) { - Volume_NODE& v = volumes.at(i, 0); + //std::cout << "i = " << i << std::endl; + Volume_NODE v = volumes.at(i, 0); + //std::cout << " " << v.idx << std::endl; if (v.idx == indx) return v.row; //find nan cheking for int or Vec3i diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 3b2b5526a06..cb130910009 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -61,6 +61,7 @@ class VolumesTable int buffferNums = 1; cv::Mat volumes; + std::vector indexes; VolumesTable(); ~VolumesTable() {}; @@ -70,7 +71,7 @@ class VolumesTable void update(Vec3i indx, int row); void expand(); int getNextVolume(int hash, int& num, int i); - int find_Volume(Vec3i indx); + int find_Volume(Vec3i indx) const; bool isExist(Vec3i indx); }; From b75cdf6db2330b475697643593c09af8dd7cfb30 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 16 Nov 2020 10:45:37 +0300 Subject: [PATCH 031/216] new hash table work --- modules/rgbd/src/hash_tsdf.cpp | 59 +++++++++++++++++++++-------- modules/rgbd/src/tsdf_functions.cpp | 36 ++++++++---------- modules/rgbd/src/tsdf_functions.hpp | 2 +- 3 files changed, 60 insertions(+), 37 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index e497f5a7daf..a39738b8e06 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -983,6 +983,7 @@ void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFa void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { + //std::cout << "integrate" << std::endl; CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); @@ -1036,19 +1037,25 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //mutex.lock(); + //std::cout << "< =========== isExist " << std::endl; + //std::cout << "lol" << std::endl; + //std::cout << loc_vol_idx << std::endl; for (int i = 0; i < loc_vol_idx; i++) { + //std::cout << i; Vec3i idx = _localAccessVolUnits.at(i, 0); - + //std::cout << " " << idx << std::endl; //if (find_idx(indexes, idx)==-1) - //std::cout << idx << " " << _indexes.isExist(idx) << std::endl; - if (_indexes.isExist(idx)) + //VolumeIndex _idx = _indexes.find_Volume(idx); + //std::cout << "{}" << std::endl; + if (!_indexes.isExist(idx)) { - //std::cout << " 888 " << std::endl; + //std::cout << " 8 " << std::endl; if (_lastVolIndex >= VolumeIndex(_volUnitsData.size().height)) { + //std::cout << " extend " << std::endl; indexes.resize(_lastVolIndex * 2); _volUnitsData.resize(_lastVolIndex * 2); volUnitsData.resize(_lastVolIndex * 2); @@ -1056,16 +1063,16 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma activities.resize(_lastVolIndex * 2); lastVisibleIndexes.resize(_lastVolIndex * 2); } - + //std::cout << " 88 " << std::endl; this->indexes.at(_lastVolIndex, 0) = idx; _indexes.update(idx, _lastVolIndex); _newIndices.at(vol_idx, 0) = _lastVolIndex; newIndices.at(vol_idx, 0) = idx; vol_idx++; _lastVolIndex++; - + //std::cout << " 888 " << std::endl; } - + //std::cout << " 8888 "<< std::endl; } //mutex.unlock(); }; @@ -1075,21 +1082,29 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! Perform the allocation //std::cout << "Perform the allocation" << std::endl; + //std::cout << "< =========== find_Volume " << std::endl; + //std::cout << nan3 << " " << Vec3i(nan3) << std::endl; + //std::cout << vol_idx << std::endl; + //std::cout << "lol 1" << std::endl; + for (int i = 0; i < vol_idx; i++) { //VolumeIndex idx = _newIndices.at(i, 0); //Vec3i tsdf_idx = indexes.at(idx, 0); Vec3i tsdf_idx = newIndices.at(i, 0); - VolumeIndex idx = _indexes.find_Volume(idx); + //std::cout << tsdf_idx << " " << i ; + VolumeIndex idx = _indexes.find_Volume(tsdf_idx); + //std::cout << "{}" << idx << std::endl; //Vec3i tsdf_idx = Vec3i(1, 1, 1); //VolumeIndex idx = 0; - //std::cout << tsdf_idx << " " << idx << std::endl; + //std::cout << " " << idx; + Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; poses.at(idx, 0) = subvolumePose; activities.at(idx, 0) = true; lastVisibleIndexes.at(idx, 0) = frameId; - + //std::cout << " 1 "; _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) { TsdfVoxel& v = reinterpret_cast(vv); @@ -1100,15 +1115,17 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma TsdfVoxel& v = reinterpret_cast(vv); v.tsdf = floatToTsdf(0.0f); v.weight = 0; }); + //std::cout << " 2 "; //volUnitsData.at(idx, 0) = cv::UMat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + //std::cout << std::endl; } - + // //! Get keys for all the allocated volume Units std::vector _totalVolUnits = _indexes.indexes; //for (int i = 0; i < indexes.size().height; i++){_totalVolUnits.push_back(indexes.at(i, 0));} - + //std::cout << "lol 2" << std::endl; //! Mark volumes in the camera frustum as active Range _inFrustumRange(0, (int)_totalVolUnits.size()); auto markActivities = [&](const Range& range) { @@ -1152,17 +1169,22 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); _pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); } + //std::cout << "ertyu\n"; //! Integrate the correct volumeUnits - parallel_for_(Range(0, (int)_totalVolUnits.size()), [&](const Range& range) { + auto Integrate = [&](const Range& range) { for (int i = range.start; i < range.end; i++) { Vec3i tsdf_idx = _totalVolUnits[i]; + //std::cout << tsdf_idx; //VolumeIndex idx = find_idx(indexes, tsdf_idx); VolumeIndex idx = _indexes.find_Volume(tsdf_idx); + //std::cout << " " << idx;// << std::endl; + if (idx < 0 || idx == _lastVolIndex - 1) return; - + bool& isActive = activities.at(idx, 0); + if (isActive) { //! The volume unit should already be added into the Volume from the allocator @@ -1171,12 +1193,17 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); - integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); + + //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); //! Ensure all active volumeUnits are set to inactive for next integration isActive = false; } + //std::cout << " integrated"<< std::endl; } - }); + }; + parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); + //Integrate(Range(0, (int)_totalVolUnits.size())); + //std::cout << "kek" << std::endl; } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index f66300ddc3c..54cddac2920 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -394,14 +394,6 @@ VolumesTable::VolumesTable() v.row = -1; v.nextVolumeRow = -1; } - /* - this->volumes.forEach([](Volume_NODE& v, const int*) - { - v.idx = nan3; - v.row = -1; - v.nextVolumeRow = -1; - }); - */ } void VolumesTable::update(Vec3i indx) @@ -417,10 +409,11 @@ void VolumesTable::update(Vec3i indx) if (v.idx == indx) return; //find nan cheking for int or Vec3i - if (isNaN(Point3i(v.idx))) + //if (isNaN(Point3i(v.idx))) + if (v.idx == Vec3i(nan3)) { v.idx = indx; - v.nextVolumeRow = getNextVolume(hash, num, i); + v.nextVolumeRow = getNextVolume(hash, num, i, start); indexes.push_back(indx); return; } @@ -444,11 +437,12 @@ void VolumesTable::update(Vec3i indx, int row) return; } //find nan cheking for int or Vec3i - if (isNaN(Point3i(v.idx))) + //if (isNaN(Point3i(v.idx))) + if (v.idx == Vec3i(nan3)) { v.idx = indx; v.row = row; - v.nextVolumeRow = getNextVolume(hash, num, i); + v.nextVolumeRow = getNextVolume(hash, num, i, start); indexes.push_back(indx); return; } @@ -456,11 +450,11 @@ void VolumesTable::update(Vec3i indx, int row) } } -int VolumesTable::getNextVolume(int hash, int& num, int i) +int VolumesTable::getNextVolume(int hash, int& num, int i, size_t start) { - if (i % list_size == 0) + if (i != start && i % list_size == 0) { - if (num <= buffferNums) + if (num < buffferNums) { num++; } @@ -473,7 +467,7 @@ int VolumesTable::getNextVolume(int hash, int& num, int i) } else { - return i++; + return i+1; } } @@ -485,19 +479,20 @@ void VolumesTable::expand() int VolumesTable::find_Volume(Vec3i indx) const { + //std::cout << "find_Volume -> "; size_t hash = calc_hash(indx) % hash_divisor; int num = 1; size_t i = hash * num * list_size; - //std::cout << "1" << std::endl; + //std::cout << " [find_Volume]"; // << std::endl; while (i != -1) { - //std::cout << "i = " << i << std::endl; Volume_NODE v = volumes.at(i, 0); - //std::cout << " " << v.idx << std::endl; + //std::cout << " | i = " << i << " idx=" << v.idx << " row=" << v.row << " next=" << v.nextVolumeRow << std::endl; if (v.idx == indx) return v.row; //find nan cheking for int or Vec3i - if (isNaN(Point3i(v.idx))) + //if (isNaN(Point3i(v.idx))) + if (v.idx == Vec3i(nan3)) return -2; i = v.nextVolumeRow; } @@ -505,6 +500,7 @@ int VolumesTable::find_Volume(Vec3i indx) const } bool VolumesTable::isExist(Vec3i indx) { + //std::cout << "isExist -> "; if (this->find_Volume(indx) == -2) return false; return true; diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index cb130910009..f1281e282f9 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -70,7 +70,7 @@ class VolumesTable void update(Vec3i indx); void update(Vec3i indx, int row); void expand(); - int getNextVolume(int hash, int& num, int i); + int getNextVolume(int hash, int& num, int i, size_t start); int find_Volume(Vec3i indx) const; bool isExist(Vec3i indx); }; From ad61d80802f37ce09ffbcd6a75af2454da75a495 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 16 Nov 2020 11:17:31 +0300 Subject: [PATCH 032/216] Docs fix --- modules/rgbd/src/hash_tsdf.cpp | 12 ++++++------ modules/rgbd/src/tsdf_functions.cpp | 10 +++++----- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a39738b8e06..91d34e0592b 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1076,7 +1076,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //mutex.unlock(); }; - + //parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); AllocateVolumeUnitsInvoker(allocateRange); @@ -1098,7 +1098,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //Vec3i tsdf_idx = Vec3i(1, 1, 1); //VolumeIndex idx = 0; //std::cout << " " << idx; - + Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; poses.at(idx, 0) = subvolumePose; @@ -1180,11 +1180,11 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //VolumeIndex idx = find_idx(indexes, tsdf_idx); VolumeIndex idx = _indexes.find_Volume(tsdf_idx); //std::cout << " " << idx;// << std::endl; - + if (idx < 0 || idx == _lastVolIndex - 1) return; - + bool& isActive = activities.at(idx, 0); - + if (isActive) { //! The volume unit should already be added into the Volume from the allocator @@ -1193,7 +1193,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); - + //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); //! Ensure all active volumeUnits are set to inactive for next integration isActive = false; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 54cddac2920..8da298b2c57 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -402,13 +402,13 @@ void VolumesTable::update(Vec3i indx) int num = 1; size_t start = hash * num * list_size; size_t i = start; - + while (i != -1) { Volume_NODE& v = volumes.at(i, 0); if (v.idx == indx) return; - //find nan cheking for int or Vec3i + //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) if (v.idx == Vec3i(nan3)) { @@ -436,7 +436,7 @@ void VolumesTable::update(Vec3i indx, int row) v.row = row; return; } - //find nan cheking for int or Vec3i + //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) if (v.idx == Vec3i(nan3)) { @@ -458,7 +458,7 @@ int VolumesTable::getNextVolume(int hash, int& num, int i, size_t start) { num++; } - else + else { this->expand(); num++; @@ -490,7 +490,7 @@ int VolumesTable::find_Volume(Vec3i indx) const //std::cout << " | i = " << i << " idx=" << v.idx << " row=" << v.row << " next=" << v.nextVolumeRow << std::endl; if (v.idx == indx) return v.row; - //find nan cheking for int or Vec3i + //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) if (v.idx == Vec3i(nan3)) return -2; From ca82124aaddb30f162976a7cbfa7b10cec6cc8a3 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 16 Nov 2020 12:15:15 +0300 Subject: [PATCH 033/216] warnings fix --- modules/rgbd/src/tsdf_functions.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 8da298b2c57..0ff5b0b9e96 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -401,7 +401,7 @@ void VolumesTable::update(Vec3i indx) size_t hash = calc_hash(indx) % hash_divisor; int num = 1; size_t start = hash * num * list_size; - size_t i = start; + int i = (int) start; while (i != -1) { @@ -426,7 +426,7 @@ void VolumesTable::update(Vec3i indx, int row) size_t hash = calc_hash(indx) % hash_divisor; int num = 1; size_t start = hash * num * list_size; - size_t i = start; + int i = (int) start; while (i != -1) { @@ -482,7 +482,7 @@ int VolumesTable::find_Volume(Vec3i indx) const //std::cout << "find_Volume -> "; size_t hash = calc_hash(indx) % hash_divisor; int num = 1; - size_t i = hash * num * list_size; + int i = int(hash) * num * list_size; //std::cout << " [find_Volume]"; // << std::endl; while (i != -1) { From c19ef2e0b69ded87dc557e2ff3af449a071499e4 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 16 Nov 2020 13:48:46 +0300 Subject: [PATCH 034/216] warning fix 1 --- 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 0ff5b0b9e96..250435d4fbe 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -452,7 +452,7 @@ void VolumesTable::update(Vec3i indx, int row) int VolumesTable::getNextVolume(int hash, int& num, int i, size_t start) { - if (i != start && i % list_size == 0) + if (i != int(start) && i % list_size == 0) { if (num < buffferNums) { From f87e0d3f41e436394f13f8ddbfb92a0f5a6deb36 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 16 Nov 2020 14:48:06 +0300 Subject: [PATCH 035/216] warning fix 2 --- modules/rgbd/src/tsdf_functions.cpp | 20 ++++++++++---------- modules/rgbd/src/tsdf_functions.hpp | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 250435d4fbe..28d28066f03 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -398,10 +398,10 @@ VolumesTable::VolumesTable() void VolumesTable::update(Vec3i indx) { - size_t hash = calc_hash(indx) % hash_divisor; + int hash = int(calc_hash(indx) % hash_divisor); int num = 1; - size_t start = hash * num * list_size; - int i = (int) start; + int start = hash * num * list_size; + int i = start; while (i != -1) { @@ -423,10 +423,10 @@ void VolumesTable::update(Vec3i indx) void VolumesTable::update(Vec3i indx, int row) { - size_t hash = calc_hash(indx) % hash_divisor; + int hash = int(calc_hash(indx) % hash_divisor); int num = 1; - size_t start = hash * num * list_size; - int i = (int) start; + int start = hash * num * list_size; + int i = start; while (i != -1) { @@ -450,9 +450,9 @@ void VolumesTable::update(Vec3i indx, int row) } } -int VolumesTable::getNextVolume(int hash, int& num, int i, size_t start) +int VolumesTable::getNextVolume(int hash, int& num, int i, int start) { - if (i != int(start) && i % list_size == 0) + if (i != start && i % list_size == 0) { if (num < buffferNums) { @@ -480,9 +480,9 @@ void VolumesTable::expand() int VolumesTable::find_Volume(Vec3i indx) const { //std::cout << "find_Volume -> "; - size_t hash = calc_hash(indx) % hash_divisor; + int hash = int(calc_hash(indx) % hash_divisor); int num = 1; - int i = int(hash) * num * list_size; + int i = hash * num * list_size; //std::cout << " [find_Volume]"; // << std::endl; while (i != -1) { diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index f1281e282f9..08a374df909 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -70,7 +70,7 @@ class VolumesTable void update(Vec3i indx); void update(Vec3i indx, int row); void expand(); - int getNextVolume(int hash, int& num, int i, size_t start); + int getNextVolume(int hash, int& num, int i, int start); int find_Volume(Vec3i indx) const; bool isExist(Vec3i indx); }; From a3254efd2c1e9bad8a4d917440da81a9a49c29cf Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 17 Nov 2020 08:51:16 +0300 Subject: [PATCH 036/216] warning 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 91d34e0592b..39d2e1734aa 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1136,7 +1136,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { Vec3i tsdf_idx = _totalVolUnits[i]; //VolumeIndex idx = find_idx(indexes, tsdf_idx); - VolumeIndex idx = _indexes.find_Volume(idx); + VolumeIndex idx = _indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == _lastVolIndex - 1) return; Point3f volumeUnitPos = volumeUnitIdxToVolume(poses.at(idx, 0)); From b6af0a01d7c4ad678844831e194d6bb2630e72c0 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 17 Nov 2020 09:38:26 +0300 Subject: [PATCH 037/216] remove extra coments --- modules/rgbd/src/hash_tsdf.cpp | 54 ++++++---------------------------- 1 file changed, 9 insertions(+), 45 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 39d2e1734aa..9766c6e493e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -860,7 +860,7 @@ void HashTSDFVolumeGPU::reset() //volUnitsData = cv::Mat(VOLUMES_SIZE, 1, rawType()); indexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); poses = cv::Mat(VOLUMES_SIZE, 1, rawType()); - activities = cv::Mat(VOLUMES_SIZE, 1, rawType()); + isActive = cv::Mat(VOLUMES_SIZE, 1, rawType()); lastVisibleIndexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); _indexes = VolumesTable(); } @@ -1037,42 +1037,29 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //mutex.lock(); - //std::cout << "< =========== isExist " << std::endl; - //std::cout << "lol" << std::endl; - //std::cout << loc_vol_idx << std::endl; for (int i = 0; i < loc_vol_idx; i++) { - //std::cout << i; Vec3i idx = _localAccessVolUnits.at(i, 0); - //std::cout << " " << idx << std::endl; - //if (find_idx(indexes, idx)==-1) - //VolumeIndex _idx = _indexes.find_Volume(idx); - //std::cout << "{}" << std::endl; + if (!_indexes.isExist(idx)) { - //std::cout << " 8 " << std::endl; - if (_lastVolIndex >= VolumeIndex(_volUnitsData.size().height)) { - //std::cout << " extend " << std::endl; indexes.resize(_lastVolIndex * 2); _volUnitsData.resize(_lastVolIndex * 2); volUnitsData.resize(_lastVolIndex * 2); poses.resize(_lastVolIndex * 2); - activities.resize(_lastVolIndex * 2); + isActive.resize(_lastVolIndex * 2); lastVisibleIndexes.resize(_lastVolIndex * 2); } - //std::cout << " 88 " << std::endl; this->indexes.at(_lastVolIndex, 0) = idx; _indexes.update(idx, _lastVolIndex); _newIndices.at(vol_idx, 0) = _lastVolIndex; newIndices.at(vol_idx, 0) = idx; vol_idx++; _lastVolIndex++; - //std::cout << " 888 " << std::endl; } - //std::cout << " 8888 "<< std::endl; } //mutex.unlock(); }; @@ -1081,28 +1068,16 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma AllocateVolumeUnitsInvoker(allocateRange); //! Perform the allocation - //std::cout << "Perform the allocation" << std::endl; - //std::cout << "< =========== find_Volume " << std::endl; - //std::cout << nan3 << " " << Vec3i(nan3) << std::endl; - //std::cout << vol_idx << std::endl; - //std::cout << "lol 1" << std::endl; for (int i = 0; i < vol_idx; i++) { - //VolumeIndex idx = _newIndices.at(i, 0); - //Vec3i tsdf_idx = indexes.at(idx, 0); Vec3i tsdf_idx = newIndices.at(i, 0); - //std::cout << tsdf_idx << " " << i ; VolumeIndex idx = _indexes.find_Volume(tsdf_idx); - //std::cout << "{}" << idx << std::endl; - //Vec3i tsdf_idx = Vec3i(1, 1, 1); - //VolumeIndex idx = 0; - //std::cout << " " << idx; Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; poses.at(idx, 0) = subvolumePose; - activities.at(idx, 0) = true; + isActive.at(idx, 0) = true; lastVisibleIndexes.at(idx, 0) = frameId; //std::cout << " 1 "; _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) @@ -1115,9 +1090,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma TsdfVoxel& v = reinterpret_cast(vv); v.tsdf = floatToTsdf(0.0f); v.weight = 0; }); - //std::cout << " 2 "; //volUnitsData.at(idx, 0) = cv::UMat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); - //std::cout << std::endl; } // @@ -1125,7 +1098,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma std::vector _totalVolUnits = _indexes.indexes; //for (int i = 0; i < indexes.size().height; i++){_totalVolUnits.push_back(indexes.at(i, 0));} - //std::cout << "lol 2" << std::endl; //! Mark volumes in the camera frustum as active Range _inFrustumRange(0, (int)_totalVolUnits.size()); auto markActivities = [&](const Range& range) { @@ -1135,7 +1107,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = range.start; i < range.end; ++i) { Vec3i tsdf_idx = _totalVolUnits[i]; - //VolumeIndex idx = find_idx(indexes, tsdf_idx); VolumeIndex idx = _indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == _lastVolIndex - 1) return; @@ -1144,7 +1115,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { - activities.at(idx, 0) = false; + isActive.at(idx, 0) = false; return; } Point2f cameraPoint = proj(volUnitInCamSpace); @@ -1152,7 +1123,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { assert(idx == _lastVolIndex - 1); lastVisibleIndexes.at(idx, 0) = frameId; - activities.at(idx, 0) = true; + isActive.at(idx, 0) = true; } } }; @@ -1170,22 +1141,17 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma _pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); } - //std::cout << "ertyu\n"; //! Integrate the correct volumeUnits auto Integrate = [&](const Range& range) { for (int i = range.start; i < range.end; i++) { Vec3i tsdf_idx = _totalVolUnits[i]; - //std::cout << tsdf_idx; - //VolumeIndex idx = find_idx(indexes, tsdf_idx); VolumeIndex idx = _indexes.find_Volume(tsdf_idx); - //std::cout << " " << idx;// << std::endl; - if (idx < 0 || idx == _lastVolIndex - 1) return; - bool& isActive = activities.at(idx, 0); + bool& _isActive = isActive.at(idx, 0); - if (isActive) + if (_isActive) { //! The volume unit should already be added into the Volume from the allocator Matx44f _pose = poses.at(idx, 0); @@ -1196,14 +1162,12 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); //! Ensure all active volumeUnits are set to inactive for next integration - isActive = false; + _isActive = false; } - //std::cout << " integrated"<< std::endl; } }; parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); //Integrate(Range(0, (int)_totalVolUnits.size())); - //std::cout << "kek" << std::endl; } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const From 40f3f1182d4e6d79631ab3afd86b49cac21510fd Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 17 Nov 2020 10:21:38 +0300 Subject: [PATCH 038/216] create new function integrateAllVolumeUnitsGPU --- modules/rgbd/src/hash_tsdf.cpp | 34 +++++++++++++++++++++++++++- modules/rgbd/src/hash_tsdf.hpp | 3 ++- modules/rgbd/src/opencl/hash_tsdf.cl | 5 ++++ 3 files changed, 40 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 9766c6e493e..a0325895225 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -981,6 +981,36 @@ void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFa throw std::runtime_error("Failed to run kernel"); } +void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth) +{ + CV_TRACE_FUNCTION(); + CV_Assert(!_depth.empty()); + + UMat depth = _depth.getUMat(); + + String errorStr; + String name = "integrateAllVolumeUnits"; + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); + + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); + + k.args(ocl::KernelArg::ReadOnly(depth)); + + int resol = 50; + size_t globalSize[3]; + globalSize[0] = (size_t)resol; + globalSize[1] = (size_t)resol; + globalSize[2] = (size_t)resol; + + if (!k.run(3, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); + +} + void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { //std::cout << "integrate" << std::endl; @@ -1093,7 +1123,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //volUnitsData.at(idx, 0) = cv::UMat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); } - // //! Get keys for all the allocated volume Units std::vector _totalVolUnits = _indexes.indexes; //for (int i = 0; i < indexes.size().height; i++){_totalVolUnits.push_back(indexes.at(i, 0));} @@ -1168,6 +1197,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma }; parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); //Integrate(Range(0, (int)_totalVolUnits.size())); + + integrateAllVolumeUnitsGPU(depth); + } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 896319f07f9..ef75655ae7f 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -132,6 +132,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void integrateVolumeUnitGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx); + void integrateAllVolumeUnitsGPU(InputArray _depth); void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; @@ -170,7 +171,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume VolumeIndex _lastVolIndex; cv::Mat indexes; cv::Mat poses; - cv::Mat activities; + cv::Mat isActive; cv::Mat lastVisibleIndexes; cv::Mat _volUnitsData; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 6375ed617e4..166584a2d56 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -191,3 +191,8 @@ __kernel void integrateVolumeUnit(__global const char * depthptr, } } +__kernel void integrateAllVolumeUnits(__global const char * depthptr, + int depth_step, int depth_offset, + int depth_rows, int depth_cols) +{ +} \ No newline at end of file From 3dcfd5a907fdeb0282f36fde911a541facd9d6a5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 17 Nov 2020 16:54:25 +0300 Subject: [PATCH 039/216] add hash table function --- modules/rgbd/src/hash_tsdf.cpp | 14 +++++----- modules/rgbd/src/hash_tsdf.hpp | 2 +- modules/rgbd/src/opencl/hash_tsdf.cl | 38 +++++++++++++++++++++++++++- modules/rgbd/src/tsdf_functions.cpp | 6 ++--- modules/rgbd/src/tsdf_functions.hpp | 2 +- 5 files changed, 49 insertions(+), 13 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a0325895225..8be54171e5d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -971,7 +971,7 @@ void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFa dfac, truncDist, int(maxWeight), - ocl::KernelArg::PtrReadOnly(_pixNorms)); + ocl::KernelArg::PtrReadOnly(_pixNorms)); size_t globalSize[2]; globalSize[0] = (size_t)volumeUnitResolution; @@ -981,7 +981,7 @@ void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFa throw std::runtime_error("Failed to run kernel"); } -void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth) +void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics) { CV_TRACE_FUNCTION(); CV_Assert(!_depth.empty()); @@ -997,10 +997,11 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth) if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); + std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; + k.args(ocl::KernelArg::ReadOnly(depth), _indexes.volumes.getUMat(ACCESS_RW), + _indexes.list_size, _indexes.bufferNums, _indexes.hash_divisor); - k.args(ocl::KernelArg::ReadOnly(depth)); - - int resol = 50; + int resol = 2; size_t globalSize[3]; globalSize[0] = (size_t)resol; globalSize[1] = (size_t)resol; @@ -1008,7 +1009,6 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth) if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); - } void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) @@ -1198,7 +1198,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); //Integrate(Range(0, (int)_totalVolUnits.size())); - integrateAllVolumeUnitsGPU(depth); + integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); } diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index ef75655ae7f..6b36ae322e8 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -132,7 +132,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void integrateVolumeUnitGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx); - void integrateAllVolumeUnitsGPU(InputArray _depth); + void integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics); void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 166584a2d56..2a00f0644fd 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -4,6 +4,8 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +#define NAN_NUM -2147483648; + typedef __INT8_TYPE__ int8_t; typedef int8_t TsdfType; @@ -15,6 +17,13 @@ struct TsdfVoxel WeightType weight; }; +struct Volume_NODE +{ + int3 idx; + int row; + int nextVolumeRow; +}; + static inline TsdfType floatToTsdf(float num) { int8_t res = (int8_t) ( (num * (-128)) ); @@ -191,8 +200,35 @@ __kernel void integrateVolumeUnit(__global const char * depthptr, } } +uint calc_hash(int3 x) +{ + unsigned int seed = 0; + uint GOLDEN_RATIO = 0x9e3779b9; + seed += x.x + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed += x.y + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed += x.z + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + return seed; +} + +int findVolume(__global struct Volume_NODE * hash_table, int3 indx, int hash_divisor) +{ + int hash = calc_hash(indx) % hash_divisor; + printf("%d \n", hash); + return 1; +} + __kernel void integrateAllVolumeUnits(__global const char * depthptr, int depth_step, int depth_offset, - int depth_rows, int depth_cols) + int depth_rows, int depth_cols, + __global struct Volume_NODE * hash_table, + int list_size, const int bufferNums, const int hash_divisor) { + printf(" = %d , %d , %d \n", list_size, bufferNums, hash_divisor ); + + findVolume(hash_table, hash_table->idx, hash_divisor); + hash_table->idx.x = 10; + findVolume(hash_table, hash_table->idx, hash_divisor); + //printf("%d \n", hash_table->nextVolumeRow); + //printf("%d \n", hash_table->row); + //printf("%d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); } \ No newline at end of file diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 28d28066f03..c7fab77139d 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -454,7 +454,7 @@ int VolumesTable::getNextVolume(int hash, int& num, int i, int start) { if (i != start && i % list_size == 0) { - if (num < buffferNums) + if (num < bufferNums) { num++; } @@ -473,8 +473,8 @@ int VolumesTable::getNextVolume(int hash, int& num, int i, int start) void VolumesTable::expand() { - this->volumes.resize(hash_divisor * (buffferNums + 1)); - this->buffferNums++; + this->volumes.resize(hash_divisor * (bufferNums + 1)); + this->bufferNums++; } int VolumesTable::find_Volume(Vec3i indx) const diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 08a374df909..dfda44f1d69 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -58,7 +58,7 @@ class VolumesTable public: int hash_divisor = 32768; int list_size = 4; - int buffferNums = 1; + int bufferNums = 1; cv::Mat volumes; std::vector indexes; From f2bea7cdea014c3945de4ac2b9144714c32c1666 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 18 Nov 2020 11:56:01 +0300 Subject: [PATCH 040/216] extend kernel signature --- modules/rgbd/src/hash_tsdf.cpp | 29 +++++++++- modules/rgbd/src/opencl/hash_tsdf.cl | 87 +++++++++++++++++----------- 2 files changed, 81 insertions(+), 35 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 8be54171e5d..31c85ed1f2b 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -997,9 +997,34 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); + + //Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + float dfac = 1.f / depthFactor; + Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); + Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); + + std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; - k.args(ocl::KernelArg::ReadOnly(depth), _indexes.volumes.getUMat(ACCESS_RW), - _indexes.list_size, _indexes.bufferNums, _indexes.hash_divisor); + k.args(ocl::KernelArg::ReadOnly(depth), + _indexes.volumes.getUMat(ACCESS_RW), + (int)_indexes.list_size, + (int)_indexes.bufferNums, + (int)_indexes.hash_divisor, + // ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)), + // _volUnitsData.getUMat(ACCESS_RW), + // isActive.getUMat(ACCESS_READ), + lastVisibleIndexes.getUMat(ACCESS_READ), + // ocl::KernelArg::PtrReadOnly(_pixNorms), + voxelSize, + volResGpu.val, + volStrides.val, + fxy.val, + cxy.val, + dfac, + truncDist, + int(maxWeight) + // ,ocl::KernelArg::PtrReadOnly(_pixNorms) + ); int resol = 2; size_t globalSize[3]; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 2a00f0644fd..c48dfbab356 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -47,6 +47,60 @@ __kernel void preCalculationPixNorm (__global float * pixNorms, pixNorms[idx] = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); } +uint calc_hash(int3 x) +{ + unsigned int seed = 0; + uint GOLDEN_RATIO = 0x9e3779b9; + seed += x.x + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed += x.y + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed += x.z + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + return seed; +} + +int findVolume(__global struct Volume_NODE * hash_table, int3 indx, int hash_divisor) +{ + int hash = calc_hash(indx) % hash_divisor; + printf("%d \n", hash); + return 1; +} + +__kernel void integrateAllVolumeUnits( + __global const char * depthptr, + int depth_step, int depth_offset, + int depth_rows, int depth_cols, + __global struct Volume_NODE * hash_table, + const int list_size, + const int bufferNums, + const int hash_divisor, + // const float16 vol2camMatrix, + // const __global struct TsdfVoxel * volUnitsData, + // const __global bool * isActive, + // const __global int * lastVisibleIndexes, + // const __global float * pixNorms, + const float voxelSize, + const int4 volResolution4, + const int4 volDims4, + const float2 fxy, + const float2 cxy, + const float dfac, + const float truncDist, + const int maxWeight + // ,const __global float * pixNorms + ) +{ + //printf(" = %d , %d , %d \n", hash_params.x, hash_params.y, hash_params.z); + printf(" = %d , %d , %d \n", list_size, bufferNums, hash_divisor ); + //findVolume(hash_table, hash_table->idx, hash_divisor); + //hash_table->idx.x = 10; + //findVolume(hash_table, hash_table->idx, hash_divisor); + + //printf("%d \n", hash_table->nextVolumeRow); + //printf("%d \n", hash_table->row); + //printf("%d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); + + +} + __kernel void integrateVolumeUnit(__global const char * depthptr, int depth_step, int depth_offset, int depth_rows, int depth_cols, @@ -198,37 +252,4 @@ __kernel void integrateVolumeUnit(__global const char * depthptr, volumeptr[volIdx] = voxel; } } -} - -uint calc_hash(int3 x) -{ - unsigned int seed = 0; - uint GOLDEN_RATIO = 0x9e3779b9; - seed += x.x + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed += x.y + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed += x.z + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - return seed; -} - -int findVolume(__global struct Volume_NODE * hash_table, int3 indx, int hash_divisor) -{ - int hash = calc_hash(indx) % hash_divisor; - printf("%d \n", hash); - return 1; -} - -__kernel void integrateAllVolumeUnits(__global const char * depthptr, - int depth_step, int depth_offset, - int depth_rows, int depth_cols, - __global struct Volume_NODE * hash_table, - int list_size, const int bufferNums, const int hash_divisor) -{ - printf(" = %d , %d , %d \n", list_size, bufferNums, hash_divisor ); - - findVolume(hash_table, hash_table->idx, hash_divisor); - hash_table->idx.x = 10; - findVolume(hash_table, hash_table->idx, hash_divisor); - //printf("%d \n", hash_table->nextVolumeRow); - //printf("%d \n", hash_table->row); - //printf("%d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); } \ No newline at end of file From 0b298314e251937c0ecc51cf6fe7ee70132c61db Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 18 Nov 2020 11:57:48 +0300 Subject: [PATCH 041/216] minor 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 31c85ed1f2b..61ee4314525 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1013,7 +1013,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept // ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)), // _volUnitsData.getUMat(ACCESS_RW), // isActive.getUMat(ACCESS_READ), - lastVisibleIndexes.getUMat(ACCESS_READ), + // lastVisibleIndexes.getUMat(ACCESS_READ), // ocl::KernelArg::PtrReadOnly(_pixNorms), voxelSize, volResGpu.val, From e3bd4238c8bf7f070fffef2f91644a7ac8f5422a Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 19 Nov 2020 12:54:43 +0300 Subject: [PATCH 042/216] extent Volume_NODE signature and add functionality --- modules/rgbd/src/hash_tsdf.cpp | 17 +++++++--- modules/rgbd/src/hash_tsdf.hpp | 3 +- modules/rgbd/src/opencl/hash_tsdf.cl | 49 ++++++++++++++++++++++------ modules/rgbd/src/tsdf_functions.cpp | 29 ++++++++++++++++ modules/rgbd/src/tsdf_functions.hpp | 4 +++ 5 files changed, 85 insertions(+), 17 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 61ee4314525..4df3abf9b4a 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -932,7 +932,7 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f return pixNorm; } - +/* void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx) { @@ -980,7 +980,7 @@ void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFa if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } - +*/ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics) { CV_TRACE_FUNCTION(); @@ -1004,7 +1004,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; + //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; k.args(ocl::KernelArg::ReadOnly(depth), _indexes.volumes.getUMat(ACCESS_RW), (int)_indexes.list_size, @@ -1012,7 +1012,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept (int)_indexes.hash_divisor, // ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)), // _volUnitsData.getUMat(ACCESS_RW), - // isActive.getUMat(ACCESS_READ), + // isActive.getUMat(ACCESS_RW), // lastVisibleIndexes.getUMat(ACCESS_READ), // ocl::KernelArg::PtrReadOnly(_pixNorms), voxelSize, @@ -1134,7 +1134,12 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma poses.at(idx, 0) = subvolumePose; isActive.at(idx, 0) = true; lastVisibleIndexes.at(idx, 0) = frameId; - //std::cout << " 1 "; + //Affine3f cam2vol(Affine3f(subvolumePose) * Affine3f(cameraPose)); + Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + ocl::KernelArg pose = ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)); + //ocl::KernelArg pose; + _indexes.update(idx, true, frameId, pose); + _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) { TsdfVoxel& v = reinterpret_cast(vv); @@ -1161,7 +1166,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = range.start; i < range.end; ++i) { Vec3i tsdf_idx = _totalVolUnits[i]; + VolumeIndex idx = _indexes.find_Volume(tsdf_idx); + //std::cout << tsdf_idx << " " << idx << std::endl; if (idx < 0 || idx == _lastVolIndex - 1) return; Point3f volumeUnitPos = volumeUnitIdxToVolume(poses.at(idx, 0)); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 6b36ae322e8..92692fd13f5 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -130,8 +130,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void reset() override; - void integrateVolumeUnitGPU(InputArray _depth, float depthFactor, - const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx); + //void integrateVolumeUnitGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx); void integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics); void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index c48dfbab356..16d036cff16 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -4,7 +4,7 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory -#define NAN_NUM -2147483648; +//#define NAN_NUM -2147483648; typedef __INT8_TYPE__ int8_t; @@ -22,6 +22,9 @@ struct Volume_NODE int3 idx; int row; int nextVolumeRow; + bool isActive; + int lastVisibleIndex; + float16 vol2camMatrix; }; static inline TsdfType floatToTsdf(float num) @@ -57,10 +60,27 @@ uint calc_hash(int3 x) return seed; } -int findVolume(__global struct Volume_NODE * hash_table, int3 indx, int hash_divisor) +int findVolume(__global struct Volume_NODE * hash_table, int3 indx, + int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; - printf("%d \n", hash); + //printf("%d \n", hash); + int num = 1; + int i = hash * num * list_size; + int NAN_NUM = -2147483648; + while (i != -1) + { + __global struct Volume_NODE* v = (hash_table + i); + //int3 tmp = v->idx + if (v->idx.x == indx.x && + v->idx.y == indx.y && + v->idx.z == indx.z) + return v->row; + if (v->idx.x == NAN_NUM) + return -2; + i = v->nextVolumeRow; + } + return 1; } @@ -74,7 +94,7 @@ __kernel void integrateAllVolumeUnits( const int hash_divisor, // const float16 vol2camMatrix, // const __global struct TsdfVoxel * volUnitsData, - // const __global bool * isActive, + //global bool * isActive, // const __global int * lastVisibleIndexes, // const __global float * pixNorms, const float voxelSize, @@ -89,15 +109,24 @@ __kernel void integrateAllVolumeUnits( ) { //printf(" = %d , %d , %d \n", hash_params.x, hash_params.y, hash_params.z); - printf(" = %d , %d , %d \n", list_size, bufferNums, hash_divisor ); - //findVolume(hash_table, hash_table->idx, hash_divisor); + //printf(" = %d , %d , %d \n", list_size, bufferNums, hash_divisor ); + //int tmp = findVolume(hash_table, hash_table->idx, hash_divisor); + + //int3 idx; idx.x = 7; idx.y=7;idx.z=1; + //int tmp = findVolume(hash_table, idx, list_size, bufferNums, hash_divisor); + //print("idx = %d %d %d | row = %d \n", idx.x, idx.y, idx.z, tmp); + //hash_table->idx.x = 10; //findVolume(hash_table, hash_table->idx, hash_divisor); - //printf("%d \n", hash_table->nextVolumeRow); - //printf("%d \n", hash_table->row); - //printf("%d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); - + //printf("\n"); + //printf("idx = %d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); + //printf("row = %d \n", hash_table->row); + //printf("nv = %d \n", hash_table->nextVolumeRow); + //printf("isA = %d \n", hash_table->isActive); + //printf("isA = %s \n", hash_table->isActive ? "true" : "false"); + //printf("lvi = %d \n", hash_table->lastVisibleIndex); + //printf("%f %f \n", hash_table->vol2camMatrix.x, hash_table->vol2camMatrix.y); } diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index c7fab77139d..6517a071c9d 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -450,6 +450,35 @@ void VolumesTable::update(Vec3i indx, int row) } } +void VolumesTable::update(Vec3i indx, bool isActive, int lastVisibleIndex, ocl::KernelArg pose) +{ + int hash = int(calc_hash(indx) % hash_divisor); + int num = 1; + int start = hash * num * list_size; + int i = start; + + while (i != -1) + { + Volume_NODE& v = volumes.at(i, 0); + if (v.idx == indx) + { + return; + } + //find nan cheking for int or Vec3i + if (v.idx == Vec3i(nan3)) + { + v.idx = indx; + v.isActive = isActive; + v.lastVisibleIndex = lastVisibleIndex; + v.pose = pose; + v.nextVolumeRow = getNextVolume(hash, num, i, start); + indexes.push_back(indx); + return; + } + i = v.nextVolumeRow; + } +} + int VolumesTable::getNextVolume(int hash, int& num, int i, int start) { if (i != start && i % list_size == 0) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index dfda44f1d69..0c53ff90af7 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -49,6 +49,9 @@ struct Volume_NODE Vec3i idx = nan3; int row = -1; int nextVolumeRow = -1; + bool isActive = true; + int lastVisibleIndex = -1; + ocl::KernelArg pose; }; size_t calc_hash(Vec3i x); @@ -69,6 +72,7 @@ class VolumesTable void update(Vec3i indx); void update(Vec3i indx, int row); + void update(Vec3i indx, bool isActive, int lastVisibleIndex, ocl::KernelArg pose); void expand(); int getNextVolume(int hash, int& num, int i, int start); int find_Volume(Vec3i indx) const; From ce19cf6e260cd893264dce95672c06bb61ac145f Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 20 Nov 2020 12:16:48 +0300 Subject: [PATCH 043/216] kernel args fix --- modules/rgbd/src/hash_tsdf.cpp | 8 +++++--- modules/rgbd/src/opencl/hash_tsdf.cl | 30 +++++++++++++++++----------- modules/rgbd/src/tsdf_functions.cpp | 17 ++++++++++------ modules/rgbd/src/tsdf_functions.hpp | 2 +- 4 files changed, 35 insertions(+), 22 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 4df3abf9b4a..925fe0ccbbb 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1003,10 +1003,12 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - + std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << + " | " << calc_hash(Vec3i(7, 7, 1)) % _indexes.hash_divisor<< std::endl; + std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; k.args(ocl::KernelArg::ReadOnly(depth), - _indexes.volumes.getUMat(ACCESS_RW), + ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), (int)_indexes.list_size, (int)_indexes.bufferNums, (int)_indexes.hash_divisor, @@ -1026,7 +1028,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept // ,ocl::KernelArg::PtrReadOnly(_pixNorms) ); - int resol = 2; + int resol = 1; size_t globalSize[3]; globalSize[0] = (size_t)resol; globalSize[1] = (size_t)resol; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 16d036cff16..8740b215e45 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -7,6 +7,7 @@ //#define NAN_NUM -2147483648; typedef __INT8_TYPE__ int8_t; +typedef __UINT32_TYPE__ uint32_t; typedef int8_t TsdfType; typedef uchar WeightType; @@ -52,11 +53,12 @@ __kernel void preCalculationPixNorm (__global float * pixNorms, uint calc_hash(int3 x) { - unsigned int seed = 0; - uint GOLDEN_RATIO = 0x9e3779b9; - seed += x.x + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed += x.y + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed += x.z + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + uint32_t seed = 0; + //uint GOLDEN_RATIO = 0x9e3779b9; + uint32_t GOLDEN_RATIO = 0x9e3779b9; + seed ^= x.x + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x.y + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x.z + GOLDEN_RATIO + (seed << 6) + (seed >> 2); return seed; } @@ -64,13 +66,17 @@ int findVolume(__global struct Volume_NODE * hash_table, int3 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; - //printf("%d \n", hash); + printf("hash = %d \n", hash); + + //printf("%d | %d \n", calc_hash(indx), hash); int num = 1; int i = hash * num * list_size; int NAN_NUM = -2147483648; while (i != -1) { __global struct Volume_NODE* v = (hash_table + i); + printf(" = %d %d %d \n", v->idx.x, v->idx.y, v->idx.z); + printf(" %d | %d \n", v->row , v->nextVolumeRow); //int3 tmp = v->idx if (v->idx.x == indx.x && v->idx.y == indx.y && @@ -114,18 +120,18 @@ __kernel void integrateAllVolumeUnits( //int3 idx; idx.x = 7; idx.y=7;idx.z=1; //int tmp = findVolume(hash_table, idx, list_size, bufferNums, hash_divisor); - //print("idx = %d %d %d | row = %d \n", idx.x, idx.y, idx.z, tmp); + //printf("idx = %d %d %d | row = %d \n", idx.x, idx.y, idx.z, tmp); //hash_table->idx.x = 10; //findVolume(hash_table, hash_table->idx, hash_divisor); //printf("\n"); - //printf("idx = %d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); - //printf("row = %d \n", hash_table->row); - //printf("nv = %d \n", hash_table->nextVolumeRow); - //printf("isA = %d \n", hash_table->isActive); + printf("idx = %d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); + printf("row = %d \n", hash_table->row); + printf("nv = %d \n", hash_table->nextVolumeRow); + printf("isA = %d \n", hash_table->isActive); //printf("isA = %s \n", hash_table->isActive ? "true" : "false"); - //printf("lvi = %d \n", hash_table->lastVisibleIndex); + printf("lvi = %d \n", hash_table->lastVisibleIndex); //printf("%f %f \n", hash_table->vol2camMatrix.x, hash_table->vol2camMatrix.y); } diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 6517a071c9d..2d5aa207770 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -375,12 +375,17 @@ void integrateVolumeUnit( size_t calc_hash(Vec3i x) { - size_t seed = 0; - constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; - for (uint16_t i = 0; i < 3; i++) - { - seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - } + uint32_t seed = 0; + //constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + uint32_t GOLDEN_RATIO = 0x9e3779b9; + seed ^= x[0] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x[1] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + //for (int i = 0; i < 3; i++) + //{ + // //seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + // seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + //} return seed; } diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 0c53ff90af7..19f72537cc1 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -49,7 +49,7 @@ struct Volume_NODE Vec3i idx = nan3; int row = -1; int nextVolumeRow = -1; - bool isActive = true; + bool isActive = false; int lastVisibleIndex = -1; ocl::KernelArg pose; }; From 386396c30565200cb75c8d87ab9f03d509daed01 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 20 Nov 2020 13:44:10 +0300 Subject: [PATCH 044/216] smth --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- modules/rgbd/src/opencl/hash_tsdf.cl | 34 ++++++++++++++++++++-------- modules/rgbd/src/tsdf_functions.cpp | 5 ++-- modules/rgbd/src/tsdf_functions.hpp | 7 +++--- 4 files changed, 34 insertions(+), 16 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 925fe0ccbbb..aecd7f9df40 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1005,7 +1005,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << " | " << calc_hash(Vec3i(7, 7, 1)) % _indexes.hash_divisor<< std::endl; - std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; + //std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), @@ -1140,7 +1140,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); ocl::KernelArg pose = ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)); //ocl::KernelArg pose; - _indexes.update(idx, true, frameId, pose); + //_indexes.update(idx, true, frameId, pose); _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) { diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 8740b215e45..f81ef9c350c 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -23,9 +23,10 @@ struct Volume_NODE int3 idx; int row; int nextVolumeRow; - bool isActive; - int lastVisibleIndex; - float16 vol2camMatrix; + //bool isActive; + //int lastVisibleIndex; + //float16 vol2camMatrix; + int tmp; }; static inline TsdfType floatToTsdf(float num) @@ -72,7 +73,7 @@ int findVolume(__global struct Volume_NODE * hash_table, int3 indx, int num = 1; int i = hash * num * list_size; int NAN_NUM = -2147483648; - while (i != -1) + while (i != NAN_NUM) { __global struct Volume_NODE* v = (hash_table + i); printf(" = %d %d %d \n", v->idx.x, v->idx.y, v->idx.z); @@ -126,12 +127,27 @@ __kernel void integrateAllVolumeUnits( //findVolume(hash_table, hash_table->idx, hash_divisor); //printf("\n"); - printf("idx = %d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); - printf("row = %d \n", hash_table->row); - printf("nv = %d \n", hash_table->nextVolumeRow); - printf("isA = %d \n", hash_table->isActive); + for (int i = 0; i < 10; i++) + { + printf("\n"); + int idx = i*3; + struct Volume_NODE v = hash_table[idx]; + printf("idx = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); + printf("row = %d \n", v.row); + printf("nv = %d \n", v.nextVolumeRow); + printf("tmp = %d \n", v.tmp); + + //printf("idx = %d %d %d \n", (hash_table[idx]).idx.x, (hash_table[idx]).idx.y, (hash_table[idx]).idx.z); + //printf("idx = %d %d %d \n", (hash_table+idx)->idx.x, (hash_table+idx)->idx.y, (hash_table+idx)->idx.z); + + } + + //printf("idx = %d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); + //printf("row = %d \n", hash_table->row); + //printf("nv = %d \n", hash_table->nextVolumeRow); + //printf("isA = %d \n", hash_table->isActive); //printf("isA = %s \n", hash_table->isActive ? "true" : "false"); - printf("lvi = %d \n", hash_table->lastVisibleIndex); + //printf("lvi = %d \n", hash_table->lastVisibleIndex); //printf("%f %f \n", hash_table->vol2camMatrix.x, hash_table->vol2camMatrix.y); } diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 2d5aa207770..1da6c43871e 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -398,6 +398,7 @@ VolumesTable::VolumesTable() v.idx = nan3; v.row = -1; v.nextVolumeRow = -1; + v.tmp = i; } } @@ -454,7 +455,7 @@ void VolumesTable::update(Vec3i indx, int row) i = v.nextVolumeRow; } } - +/* void VolumesTable::update(Vec3i indx, bool isActive, int lastVisibleIndex, ocl::KernelArg pose) { int hash = int(calc_hash(indx) % hash_divisor); @@ -483,7 +484,7 @@ void VolumesTable::update(Vec3i indx, bool isActive, int lastVisibleIndex, ocl:: i = v.nextVolumeRow; } } - +*/ int VolumesTable::getNextVolume(int hash, int& num, int i, int start) { if (i != start && i % list_size == 0) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 19f72537cc1..1999cc62de2 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -49,9 +49,10 @@ struct Volume_NODE Vec3i idx = nan3; int row = -1; int nextVolumeRow = -1; - bool isActive = false; - int lastVisibleIndex = -1; - ocl::KernelArg pose; + //bool isActive = false; + //int lastVisibleIndex = -1; + //ocl::KernelArg pose; + int tmp; }; size_t calc_hash(Vec3i x); From 036c0dbacdaca7a222b9d9e97bc2fc0757293129 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 23 Nov 2020 08:59:28 +0300 Subject: [PATCH 045/216] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 12 +++++++----- modules/rgbd/src/opencl/hash_tsdf.cl | 23 +++++++++++++++++++---- modules/rgbd/src/tsdf_functions.hpp | 6 +++--- 3 files changed, 29 insertions(+), 12 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index aecd7f9df40..153602661aa 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1003,10 +1003,12 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << - " | " << calc_hash(Vec3i(7, 7, 1)) % _indexes.hash_divisor<< std::endl; + //std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << + // " | " << calc_hash(Vec3i(7, 7, 1)) % _indexes.hash_divisor<< std::endl; + //std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; + k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), (int)_indexes.list_size, @@ -1030,9 +1032,9 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept int resol = 1; size_t globalSize[3]; - globalSize[0] = (size_t)resol; - globalSize[1] = (size_t)resol; - globalSize[2] = (size_t)resol; + globalSize[0] = (size_t)resol; // volResolution.x + globalSize[1] = (size_t)resol; // volResolution.y + globalSize[2] = (size_t)resol; // num of voxels if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index f81ef9c350c..b93890cca67 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -8,6 +8,7 @@ typedef __INT8_TYPE__ int8_t; typedef __UINT32_TYPE__ uint32_t; +typedef __INT32_TYPE__ int32_t; typedef int8_t TsdfType; typedef uchar WeightType; @@ -21,12 +22,12 @@ struct TsdfVoxel struct Volume_NODE { int3 idx; - int row; - int nextVolumeRow; + int32_t row; + int32_t nextVolumeRow; //bool isActive; //int lastVisibleIndex; //float16 vol2camMatrix; - int tmp; + int32_t tmp; }; static inline TsdfType floatToTsdf(float num) @@ -115,6 +116,12 @@ __kernel void integrateAllVolumeUnits( // ,const __global float * pixNorms ) { + + //int i = get_global_id(0); + //int j = get_global_id(1); + //int k = get_global_id(2); + //printf("[i, j, k] = [%d , %d , %d] \n", i, j, k); + //printf(" = %d , %d , %d \n", hash_params.x, hash_params.y, hash_params.z); //printf(" = %d , %d , %d \n", list_size, bufferNums, hash_divisor ); //int tmp = findVolume(hash_table, hash_table->idx, hash_divisor); @@ -127,10 +134,18 @@ __kernel void integrateAllVolumeUnits( //findVolume(hash_table, hash_table->idx, hash_divisor); //printf("\n"); + + //char* tmp = (char) hash_table; + //for (int i = 0; i < 100; i++) + //{ + // printf("|%c", tmp[i] ); + //} + + for (int i = 0; i < 10; i++) { printf("\n"); - int idx = i*3; + int idx = i; struct Volume_NODE v = hash_table[idx]; printf("idx = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); printf("row = %d \n", v.row); diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 1999cc62de2..e05602c56ec 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -47,12 +47,12 @@ void integrateVolumeUnit( struct Volume_NODE { Vec3i idx = nan3; - int row = -1; - int nextVolumeRow = -1; + int32_t row = -1; + int32_t nextVolumeRow = -1; //bool isActive = false; //int lastVisibleIndex = -1; //ocl::KernelArg pose; - int tmp; + int32_t tmp; }; size_t calc_hash(Vec3i x); From 06a501384c32017273708d30e265d541d43c28c9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 23 Nov 2020 09:49:55 +0300 Subject: [PATCH 046/216] kernal args fix --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- modules/rgbd/src/opencl/hash_tsdf.cl | 2 +- modules/rgbd/src/tsdf_functions.cpp | 30 ++++++++++++++++------------ modules/rgbd/src/tsdf_functions.hpp | 7 ++++--- 4 files changed, 24 insertions(+), 19 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 153602661aa..1e80d8269a7 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1003,8 +1003,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - //std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << - // " | " << calc_hash(Vec3i(7, 7, 1)) % _indexes.hash_divisor<< std::endl; + std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << + " | " << calc_hash(Vec4i(7, 7, 1, 0)) % _indexes.hash_divisor<< std::endl; //std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index b93890cca67..3bd954b604e 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -21,7 +21,7 @@ struct TsdfVoxel struct Volume_NODE { - int3 idx; + int4 idx; int32_t row; int32_t nextVolumeRow; //bool isActive; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 1da6c43871e..06e2803cd45 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -373,7 +373,7 @@ void integrateVolumeUnit( } -size_t calc_hash(Vec3i x) +size_t calc_hash(Vec4i x) { uint32_t seed = 0; //constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; @@ -395,16 +395,18 @@ VolumesTable::VolumesTable() for (int i = 0; i < volumes.size().height; i++) { Volume_NODE& v = volumes.at(i, 0); - v.idx = nan3; + v.idx = nan4; v.row = -1; v.nextVolumeRow = -1; v.tmp = i; + v.noneed = -555; } } void VolumesTable::update(Vec3i indx) { - int hash = int(calc_hash(indx) % hash_divisor); + Vec4i idx = (indx[0], indx[0], indx[0], -2147483648); + int hash = int(calc_hash(idx) % hash_divisor); int num = 1; int start = hash * num * list_size; int i = start; @@ -412,13 +414,13 @@ void VolumesTable::update(Vec3i indx) while (i != -1) { Volume_NODE& v = volumes.at(i, 0); - if (v.idx == indx) + if (v.idx == idx) return; //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx == Vec3i(nan3)) + if (v.idx == nan4) { - v.idx = indx; + v.idx = idx; v.nextVolumeRow = getNextVolume(hash, num, i, start); indexes.push_back(indx); return; @@ -429,7 +431,8 @@ void VolumesTable::update(Vec3i indx) void VolumesTable::update(Vec3i indx, int row) { - int hash = int(calc_hash(indx) % hash_divisor); + Vec4i idx = (indx[0], indx[0], indx[0], -2147483648); + int hash = int(calc_hash(idx) % hash_divisor); int num = 1; int start = hash * num * list_size; int i = start; @@ -437,16 +440,16 @@ void VolumesTable::update(Vec3i indx, int row) while (i != -1) { Volume_NODE& v = volumes.at(i, 0); - if (v.idx == indx) + if (v.idx == idx) { v.row = row; return; } //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx == Vec3i(nan3)) + if (v.idx == nan4) { - v.idx = indx; + v.idx = idx; v.row = row; v.nextVolumeRow = getNextVolume(hash, num, i, start); indexes.push_back(indx); @@ -514,8 +517,9 @@ void VolumesTable::expand() int VolumesTable::find_Volume(Vec3i indx) const { + Vec4i idx = (indx[0], indx[0], indx[0], -2147483648); //std::cout << "find_Volume -> "; - int hash = int(calc_hash(indx) % hash_divisor); + int hash = int(calc_hash(idx) % hash_divisor); int num = 1; int i = hash * num * list_size; //std::cout << " [find_Volume]"; // << std::endl; @@ -523,11 +527,11 @@ int VolumesTable::find_Volume(Vec3i indx) const { Volume_NODE v = volumes.at(i, 0); //std::cout << " | i = " << i << " idx=" << v.idx << " row=" << v.row << " next=" << v.nextVolumeRow << std::endl; - if (v.idx == indx) + if (v.idx == idx) return v.row; //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx == Vec3i(nan3)) + if (v.idx == nan4) return -2; i = v.nextVolumeRow; } diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index e05602c56ec..2dfea4d25e9 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -46,16 +46,17 @@ void integrateVolumeUnit( struct Volume_NODE { - Vec3i idx = nan3; + Vec4i idx = Vec4i(-2147483648); int32_t row = -1; int32_t nextVolumeRow = -1; //bool isActive = false; //int lastVisibleIndex = -1; //ocl::KernelArg pose; int32_t tmp; + int32_t noneed; }; -size_t calc_hash(Vec3i x); +size_t calc_hash(Vec4i x); class VolumesTable { @@ -66,7 +67,7 @@ class VolumesTable cv::Mat volumes; std::vector indexes; - + cv::Vec4i nan4 = cv::Vec4i(-2147483648, -2147483647, -2147483647, -2147483647); VolumesTable(); ~VolumesTable() {}; From abfb7c7a7b815e080646ab361c0b7ef4d591f184 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 23 Nov 2020 11:04:30 +0300 Subject: [PATCH 047/216] try to fix reading bug --- modules/rgbd/src/opencl/hash_tsdf.cl | 43 +++++++++++++++------------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 3bd954b604e..675d50fd8a4 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -68,7 +68,7 @@ int findVolume(__global struct Volume_NODE * hash_table, int3 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; - printf("hash = %d \n", hash); + printf("\nhash = %d \n", hash); //printf("%d | %d \n", calc_hash(indx), hash); int num = 1; @@ -76,17 +76,17 @@ int findVolume(__global struct Volume_NODE * hash_table, int3 indx, int NAN_NUM = -2147483648; while (i != NAN_NUM) { - __global struct Volume_NODE* v = (hash_table + i); - printf(" = %d %d %d \n", v->idx.x, v->idx.y, v->idx.z); - printf(" %d | %d \n", v->row , v->nextVolumeRow); + struct Volume_NODE v = hash_table[i]; + printf(" = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); + //printf(" %d | %d \n", v->row , v->nextVolumeRow); //int3 tmp = v->idx - if (v->idx.x == indx.x && - v->idx.y == indx.y && - v->idx.z == indx.z) - return v->row; - if (v->idx.x == NAN_NUM) + if (v.idx.x == indx.x && + v.idx.y == indx.y && + v.idx.z == indx.z) + return v.row; + if (v.idx.x == NAN_NUM) return -2; - i = v->nextVolumeRow; + i = v.nextVolumeRow; } return 1; @@ -142,19 +142,22 @@ __kernel void integrateAllVolumeUnits( //} - for (int i = 0; i < 10; i++) + for (int i = 0; i < 16000; i++) { - printf("\n"); + int idx = i; struct Volume_NODE v = hash_table[idx]; - printf("idx = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); - printf("row = %d \n", v.row); - printf("nv = %d \n", v.nextVolumeRow); - printf("tmp = %d \n", v.tmp); - - //printf("idx = %d %d %d \n", (hash_table[idx]).idx.x, (hash_table[idx]).idx.y, (hash_table[idx]).idx.z); - //printf("idx = %d %d %d \n", (hash_table+idx)->idx.x, (hash_table+idx)->idx.y, (hash_table+idx)->idx.z); - + if (v.idx.x != -2147483648) + { + printf("\n"); + printf("idx = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); + printf("row = %d \n", v.row); + printf("nv = %d \n", v.nextVolumeRow); + printf("tmp = %d \n", v.tmp); + + //printf("idx = %d %d %d \n", (hash_table[idx]).idx.x, (hash_table[idx]).idx.y, (hash_table[idx]).idx.z); + //printf("idx = %d %d %d \n", (hash_table+idx)->idx.x, (hash_table+idx)->idx.y, (hash_table+idx)->idx.z); + } } //printf("idx = %d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); From ad0019edd76ba972cd83085226168d8fa6fdc2f4 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 24 Nov 2020 09:57:49 +0300 Subject: [PATCH 048/216] hash table fix --- modules/rgbd/src/hash_tsdf.cpp | 22 +++++++++++++--- modules/rgbd/src/opencl/hash_tsdf.cl | 23 ++++++++-------- modules/rgbd/src/tsdf_functions.cpp | 39 ++++++++++++++++------------ modules/rgbd/src/tsdf_functions.hpp | 5 ++-- 4 files changed, 55 insertions(+), 34 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 1e80d8269a7..bc8e5a6b022 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1008,9 +1008,23 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept //std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; - + /* + for (int i = 5060; i < 5070; i++) + { + Volume_NODE v = _indexes.volumes.at(i, 0); + //if (v.idx[0] != -2147483648) + //{ + printf("\n"); + printf("idx = %d %d %d \n", v.idx[0], v.idx[1], v.idx[2]); + printf("row = %d \n", v.row); + printf("nv = %d \n", v.nextVolumeRow); + printf("tmp = %d \n", v.tmp); + //} + } + */ k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), + //ocl::KernelArg::PtrReadWrite(_indexes.volumes), (int)_indexes.list_size, (int)_indexes.bufferNums, (int)_indexes.hash_divisor, @@ -1032,8 +1046,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept int resol = 1; size_t globalSize[3]; - globalSize[0] = (size_t)resol; // volResolution.x - globalSize[1] = (size_t)resol; // volResolution.y + globalSize[0] = (size_t)volumeUnitResolution; // volResolution.x + globalSize[1] = (size_t)volumeUnitResolution; // volResolution.y globalSize[2] = (size_t)resol; // num of voxels if (!k.run(3, globalSize, NULL, true)) @@ -1160,7 +1174,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! Get keys for all the allocated volume Units std::vector _totalVolUnits = _indexes.indexes; //for (int i = 0; i < indexes.size().height; i++){_totalVolUnits.push_back(indexes.at(i, 0));} - + //std::cout << "lol\n"; //! Mark volumes in the camera frustum as active Range _inFrustumRange(0, (int)_totalVolUnits.size()); auto markActivities = [&](const Range& range) { diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 675d50fd8a4..e49057833b9 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -4,7 +4,7 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory -//#define NAN_NUM -2147483648; +//#define NAN_NUM -2147483647; typedef __INT8_TYPE__ int8_t; typedef __UINT32_TYPE__ uint32_t; @@ -68,16 +68,16 @@ int findVolume(__global struct Volume_NODE * hash_table, int3 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; - printf("\nhash = %d \n", hash); + //printf("\nhash = %d \n", hash); //printf("%d | %d \n", calc_hash(indx), hash); int num = 1; int i = hash * num * list_size; - int NAN_NUM = -2147483648; + int NAN_NUM = -2147483647; while (i != NAN_NUM) { struct Volume_NODE v = hash_table[i]; - printf(" = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); + //printf(" = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); //printf(" %d | %d \n", v->row , v->nextVolumeRow); //int3 tmp = v->idx if (v.idx.x == indx.x && @@ -126,9 +126,9 @@ __kernel void integrateAllVolumeUnits( //printf(" = %d , %d , %d \n", list_size, bufferNums, hash_divisor ); //int tmp = findVolume(hash_table, hash_table->idx, hash_divisor); - //int3 idx; idx.x = 7; idx.y=7;idx.z=1; - //int tmp = findVolume(hash_table, idx, list_size, bufferNums, hash_divisor); - //printf("idx = %d %d %d | row = %d \n", idx.x, idx.y, idx.z, tmp); + int3 idx; idx.x = 7; idx.y=7;idx.z=1; + int tmp = findVolume(hash_table, idx, list_size, bufferNums, hash_divisor); + printf("idx = %d %d %d | row = %d \n", idx.x, idx.y, idx.z, tmp); //hash_table->idx.x = 10; //findVolume(hash_table, hash_table->idx, hash_divisor); @@ -141,13 +141,13 @@ __kernel void integrateAllVolumeUnits( // printf("|%c", tmp[i] ); //} - + /* for (int i = 0; i < 16000; i++) { int idx = i; struct Volume_NODE v = hash_table[idx]; - if (v.idx.x != -2147483648) + if (v.idx.x != -2147483647) { printf("\n"); printf("idx = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); @@ -159,7 +159,8 @@ __kernel void integrateAllVolumeUnits( //printf("idx = %d %d %d \n", (hash_table+idx)->idx.x, (hash_table+idx)->idx.y, (hash_table+idx)->idx.z); } } - + */ + //printf("idx = %d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); //printf("row = %d \n", hash_table->row); //printf("nv = %d \n", hash_table->nextVolumeRow); @@ -170,7 +171,7 @@ __kernel void integrateAllVolumeUnits( } -__kernel void integrateVolumeUnit(__global const char * depthptr, +void integrateVolumeUnit(__global const char * depthptr, int depth_step, int depth_offset, int depth_rows, int depth_cols, __global struct TsdfVoxel * volumeptr, diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 06e2803cd45..da1efd08fae 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -376,16 +376,18 @@ void integrateVolumeUnit( size_t calc_hash(Vec4i x) { uint32_t seed = 0; - //constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; - uint32_t GOLDEN_RATIO = 0x9e3779b9; - seed ^= x[0] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed ^= x[1] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - //for (int i = 0; i < 3; i++) - //{ - // //seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - // seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - //} + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + //uint32_t GOLDEN_RATIO = 0x9e3779b9; + //seed ^= x[0] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + //seed ^= x[1] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + //seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + //std::cout << " lol " << x[0]<()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + //std::cout << x[i] << "|" << seed << std::endl; + } return seed; } @@ -396,6 +398,7 @@ VolumesTable::VolumesTable() { Volume_NODE& v = volumes.at(i, 0); v.idx = nan4; + //v.idx = cv::Vec4i(-2147483648, -2147483648, -2147483648, -2147483648); v.row = -1; v.nextVolumeRow = -1; v.tmp = i; @@ -405,7 +408,7 @@ VolumesTable::VolumesTable() void VolumesTable::update(Vec3i indx) { - Vec4i idx = (indx[0], indx[0], indx[0], -2147483648); + Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); int num = 1; int start = hash * num * list_size; @@ -418,11 +421,12 @@ void VolumesTable::update(Vec3i indx) return; //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx == nan4) + if (v.idx[0] == -2147483647) { v.idx = idx; v.nextVolumeRow = getNextVolume(hash, num, i, start); indexes.push_back(indx); + indexesGPU.push_back(idx); return; } i = v.nextVolumeRow; @@ -431,7 +435,7 @@ void VolumesTable::update(Vec3i indx) void VolumesTable::update(Vec3i indx, int row) { - Vec4i idx = (indx[0], indx[0], indx[0], -2147483648); + Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); int num = 1; int start = hash * num * list_size; @@ -447,12 +451,13 @@ void VolumesTable::update(Vec3i indx, int row) } //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx == nan4) + if (v.idx[0] == -2147483647) { v.idx = idx; v.row = row; v.nextVolumeRow = getNextVolume(hash, num, i, start); indexes.push_back(indx); + indexesGPU.push_back(idx); return; } i = v.nextVolumeRow; @@ -517,12 +522,12 @@ void VolumesTable::expand() int VolumesTable::find_Volume(Vec3i indx) const { - Vec4i idx = (indx[0], indx[0], indx[0], -2147483648); + Vec4i idx(indx[0], indx[1], indx[2], 0); //std::cout << "find_Volume -> "; int hash = int(calc_hash(idx) % hash_divisor); int num = 1; int i = hash * num * list_size; - //std::cout << " [find_Volume]"; // << std::endl; + //std::cout <<"[ "<< idx<<" ]= " << calc_hash(idx) <<" = "<< hash << std::endl; while (i != -1) { Volume_NODE v = volumes.at(i, 0); @@ -531,7 +536,7 @@ int VolumesTable::find_Volume(Vec3i indx) const return v.row; //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx == nan4) + if (v.idx[0] == -2147483648) return -2; i = v.nextVolumeRow; } diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 2dfea4d25e9..56b3847f6ef 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -46,7 +46,7 @@ void integrateVolumeUnit( struct Volume_NODE { - Vec4i idx = Vec4i(-2147483648); + Vec4i idx = Vec4i(-2147483647); int32_t row = -1; int32_t nextVolumeRow = -1; //bool isActive = false; @@ -67,7 +67,8 @@ class VolumesTable cv::Mat volumes; std::vector indexes; - cv::Vec4i nan4 = cv::Vec4i(-2147483648, -2147483647, -2147483647, -2147483647); + std::vector indexesGPU; + cv::Vec4i nan4 = cv::Vec4i(-2147483647); VolumesTable(); ~VolumesTable() {}; From a363277996a8709c95d846420b0ee00b7f20571a Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 24 Nov 2020 11:57:55 +0300 Subject: [PATCH 049/216] add totalVolUnits feature --- modules/rgbd/src/hash_tsdf.cpp | 63 ++++++++++++++-------------- modules/rgbd/src/hash_tsdf.hpp | 2 + modules/rgbd/src/opencl/hash_tsdf.cl | 16 +++---- 3 files changed, 42 insertions(+), 39 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index bc8e5a6b022..8abb2c0bb6f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -855,13 +855,15 @@ void HashTSDFVolumeGPU::reset() { CV_TRACE_FUNCTION(); _lastVolIndex = 0; - _volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); - volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + degree = 15; + buff_lvl = pow(2, degree); + _volUnitsData = cv::Mat(buff_lvl, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + volUnitsData = cv::Mat(buff_lvl, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); //volUnitsData = cv::Mat(VOLUMES_SIZE, 1, rawType()); - indexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); - poses = cv::Mat(VOLUMES_SIZE, 1, rawType()); - isActive = cv::Mat(VOLUMES_SIZE, 1, rawType()); - lastVisibleIndexes = cv::Mat(VOLUMES_SIZE, 1, rawType()); + indexes = cv::Mat(buff_lvl, 1, rawType()); + poses = cv::Mat(buff_lvl, 1, rawType()); + isActive = cv::Mat(buff_lvl, 1, rawType()); + lastVisibleIndexes = cv::Mat(buff_lvl, 1, rawType()); _indexes = VolumesTable(); } @@ -1003,31 +1005,22 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); + int totalVolUnitsSize = _indexes.indexesGPU.size(); + Mat totalVolUnits(_indexes.indexesGPU, rawType()); + std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << " | " << calc_hash(Vec4i(7, 7, 1, 0)) % _indexes.hash_divisor<< std::endl; //std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; - /* - for (int i = 5060; i < 5070; i++) - { - Volume_NODE v = _indexes.volumes.at(i, 0); - //if (v.idx[0] != -2147483648) - //{ - printf("\n"); - printf("idx = %d %d %d \n", v.idx[0], v.idx[1], v.idx[2]); - printf("row = %d \n", v.row); - printf("nv = %d \n", v.nextVolumeRow); - printf("tmp = %d \n", v.tmp); - //} - } - */ + k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), //ocl::KernelArg::PtrReadWrite(_indexes.volumes), (int)_indexes.list_size, (int)_indexes.bufferNums, (int)_indexes.hash_divisor, + ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), // ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)), // _volUnitsData.getUMat(ACCESS_RW), // isActive.getUMat(ACCESS_RW), @@ -1046,9 +1039,9 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept int resol = 1; size_t globalSize[3]; - globalSize[0] = (size_t)volumeUnitResolution; // volResolution.x - globalSize[1] = (size_t)volumeUnitResolution; // volResolution.y - globalSize[2] = (size_t)resol; // num of voxels + globalSize[0] = (size_t)resol; // volResolution.x + globalSize[1] = (size_t)resol; // volResolution.y + globalSize[2] = (size_t)totalVolUnitsSize; // num of voxels if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); @@ -1110,21 +1103,24 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //mutex.lock(); - + //std::cout << "lol\n"; for (int i = 0; i < loc_vol_idx; i++) { Vec3i idx = _localAccessVolUnits.at(i, 0); if (!_indexes.isExist(idx)) { - if (_lastVolIndex >= VolumeIndex(_volUnitsData.size().height)) + if (_lastVolIndex >= buff_lvl) { - indexes.resize(_lastVolIndex * 2); - _volUnitsData.resize(_lastVolIndex * 2); - volUnitsData.resize(_lastVolIndex * 2); - poses.resize(_lastVolIndex * 2); - isActive.resize(_lastVolIndex * 2); - lastVisibleIndexes.resize(_lastVolIndex * 2); + //std::cout << "kek " << _lastVolIndex << " " << buff_lvl << std::endl; + degree++; + buff_lvl = pow(2, degree); + indexes.resize(buff_lvl); + _volUnitsData.resize(buff_lvl); + volUnitsData.resize(buff_lvl); + poses.resize(buff_lvl); + isActive.resize(buff_lvl); + lastVisibleIndexes.resize(buff_lvl); } this->indexes.at(_lastVolIndex, 0) = idx; _indexes.update(idx, _lastVolIndex); @@ -1141,7 +1137,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma AllocateVolumeUnitsInvoker(allocateRange); //! Perform the allocation - + //std::cout << "lol\n"; for (int i = 0; i < vol_idx; i++) { Vec3i tsdf_idx = newIndices.at(i, 0); @@ -1248,8 +1244,11 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); //Integrate(Range(0, (int)_totalVolUnits.size())); + //std::cout << "lol\n"; + integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); + //std::cout << "lol\n"; } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 92692fd13f5..3b1b7321dde 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -166,6 +166,8 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Vec4i volStrides; Vec6f frameParams; Mat pixNorms; + int degree; + int buff_lvl; VolumeIndex _lastVolIndex; cv::Mat indexes; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index e49057833b9..0b0e7a8252c 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -100,6 +100,7 @@ __kernel void integrateAllVolumeUnits( const int list_size, const int bufferNums, const int hash_divisor, + __global const int4 * totalVolUnits, // const float16 vol2camMatrix, // const __global struct TsdfVoxel * volUnitsData, //global bool * isActive, @@ -117,18 +118,19 @@ __kernel void integrateAllVolumeUnits( ) { - //int i = get_global_id(0); - //int j = get_global_id(1); - //int k = get_global_id(2); - //printf("[i, j, k] = [%d , %d , %d] \n", i, j, k); + int i = get_global_id(0); + int j = get_global_id(1); + int k = get_global_id(2); + int4 v = totalVolUnits[k]; + printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d]\n", i, j, k, v[0], v[1], v[2]); //printf(" = %d , %d , %d \n", hash_params.x, hash_params.y, hash_params.z); //printf(" = %d , %d , %d \n", list_size, bufferNums, hash_divisor ); //int tmp = findVolume(hash_table, hash_table->idx, hash_divisor); - int3 idx; idx.x = 7; idx.y=7;idx.z=1; - int tmp = findVolume(hash_table, idx, list_size, bufferNums, hash_divisor); - printf("idx = %d %d %d | row = %d \n", idx.x, idx.y, idx.z, tmp); + //int3 idx; idx.x = 7; idx.y=7;idx.z=1; + //int tmp = findVolume(hash_table, idx, list_size, bufferNums, hash_divisor); + //printf("idx = %d %d %d | row = %d \n", idx.x, idx.y, idx.z, tmp); //hash_table->idx.x = 10; //findVolume(hash_table, hash_table->idx, hash_divisor); From 7db18175fe2602326ff125001c4c917e1acd4f6b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 25 Nov 2020 11:59:46 +0300 Subject: [PATCH 050/216] it builds --- modules/rgbd/src/hash_tsdf.cpp | 41 +++--- modules/rgbd/src/hash_tsdf.hpp | 1 + modules/rgbd/src/opencl/hash_tsdf.cl | 200 ++++++++++++++------------- modules/rgbd/src/tsdf.cpp | 2 +- modules/rgbd/src/tsdf_functions.cpp | 54 ++++++-- modules/rgbd/src/tsdf_functions.hpp | 11 +- 6 files changed, 175 insertions(+), 134 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 8abb2c0bb6f..139b7a4d93a 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -865,6 +865,7 @@ void HashTSDFVolumeGPU::reset() isActive = cv::Mat(buff_lvl, 1, rawType()); lastVisibleIndexes = cv::Mat(buff_lvl, 1, rawType()); _indexes = VolumesTable(); + posesGPU = cv::Mat(buff_lvl, 1, rawType()); } static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) @@ -1008,12 +1009,12 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept int totalVolUnitsSize = _indexes.indexesGPU.size(); Mat totalVolUnits(_indexes.indexesGPU, rawType()); - std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << - " | " << calc_hash(Vec4i(7, 7, 1, 0)) % _indexes.hash_divisor<< std::endl; + //std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << + // " | " << calc_hash(Vec4i(7, 7, 1, 0)) % _indexes.hash_divisor<< std::endl; //std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; - + //std::cout << "maxWeight == " << maxWeight << std::endl; k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), //ocl::KernelArg::PtrReadWrite(_indexes.volumes), @@ -1021,11 +1022,10 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept (int)_indexes.bufferNums, (int)_indexes.hash_divisor, ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), - // ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)), - // _volUnitsData.getUMat(ACCESS_RW), - // isActive.getUMat(ACCESS_RW), - // lastVisibleIndexes.getUMat(ACCESS_READ), - // ocl::KernelArg::PtrReadOnly(_pixNorms), + ocl::KernelArg::PtrReadWrite(_volUnitsData.getUMat(ACCESS_RW)), + ocl::KernelArg::PtrReadOnly(_pixNorms), + ocl::KernelArg::PtrReadWrite(posesGPU.getUMat(ACCESS_RW)), + _lastVolIndex, voxelSize, volResGpu.val, volStrides.val, @@ -1034,13 +1034,16 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept dfac, truncDist, int(maxWeight) - // ,ocl::KernelArg::PtrReadOnly(_pixNorms) + ); - int resol = 1; + //int resol = 1; + int resol = volumeUnitResolution; size_t globalSize[3]; - globalSize[0] = (size_t)resol; // volResolution.x - globalSize[1] = (size_t)resol; // volResolution.y + globalSize[0] = (size_t)resol; // volumeUnitResolution + globalSize[1] = (size_t)resol; // volumeUnitResolution + //globalSize[0] = (size_t)depth.rows; // volResolution.x + //globalSize[1] = (size_t)depth.cols; // volResolution.y globalSize[2] = (size_t)totalVolUnitsSize; // num of voxels if (!k.run(3, globalSize, NULL, true)) @@ -1121,6 +1124,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma poses.resize(buff_lvl); isActive.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); + posesGPU.resize(buff_lvl); } this->indexes.at(_lastVolIndex, 0) = idx; _indexes.update(idx, _lastVolIndex); @@ -1151,8 +1155,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //Affine3f cam2vol(Affine3f(subvolumePose) * Affine3f(cameraPose)); Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); ocl::KernelArg pose = ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)); + posesGPU.at(idx, 0) = pose; //ocl::KernelArg pose; - //_indexes.update(idx, true, frameId, pose); _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) { @@ -1169,8 +1173,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! Get keys for all the allocated volume Units std::vector _totalVolUnits = _indexes.indexes; - //for (int i = 0; i < indexes.size().height; i++){_totalVolUnits.push_back(indexes.at(i, 0));} - //std::cout << "lol\n"; + //! Mark volumes in the camera frustum as active Range _inFrustumRange(0, (int)_totalVolUnits.size()); auto markActivities = [&](const Range& range) { @@ -1191,6 +1194,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { isActive.at(idx, 0) = false; + _indexes.updateActive(tsdf_idx, false); return; } Point2f cameraPoint = proj(volUnitInCamSpace); @@ -1199,6 +1203,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma assert(idx == _lastVolIndex - 1); lastVisibleIndexes.at(idx, 0) = frameId; isActive.at(idx, 0) = true; + _indexes.update(tsdf_idx, true, frameId); } } }; @@ -1231,9 +1236,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! The volume unit should already be added into the Volume from the allocator Matx44f _pose = poses.at(idx, 0); - integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, - Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, - depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); + //integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, + // Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, + // depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); //! 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 3b1b7321dde..bee1a307a76 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -176,6 +176,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::Mat lastVisibleIndexes; cv::Mat _volUnitsData; + cv::Mat posesGPU; cv::UMat _pixNorms; cv::Mat volUnitsData; VolumesTable _indexes; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 0b0e7a8252c..abd165c7d9f 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -24,10 +24,8 @@ struct Volume_NODE int4 idx; int32_t row; int32_t nextVolumeRow; - //bool isActive; - //int lastVisibleIndex; - //float16 vol2camMatrix; - int32_t tmp; + int32_t isActive; + int32_t lastVisibleIndex; }; static inline TsdfType floatToTsdf(float num) @@ -53,18 +51,18 @@ __kernel void preCalculationPixNorm (__global float * pixNorms, pixNorms[idx] = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); } -uint calc_hash(int3 x) +uint calc_hash(int4 x) { uint32_t seed = 0; //uint GOLDEN_RATIO = 0x9e3779b9; uint32_t GOLDEN_RATIO = 0x9e3779b9; - seed ^= x.x + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed ^= x.y + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed ^= x.z + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x[0] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x[1] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); return seed; } -int findVolume(__global struct Volume_NODE * hash_table, int3 indx, +int findRow(__global struct Volume_NODE * hash_table, int4 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; @@ -77,106 +75,48 @@ int findVolume(__global struct Volume_NODE * hash_table, int3 indx, while (i != NAN_NUM) { struct Volume_NODE v = hash_table[i]; - //printf(" = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); - //printf(" %d | %d \n", v->row , v->nextVolumeRow); - //int3 tmp = v->idx - if (v.idx.x == indx.x && - v.idx.y == indx.y && - v.idx.z == indx.z) + if (v.idx[0] == indx[0] && + v.idx[1] == indx[1] && + v.idx[2] == indx[2]) return v.row; if (v.idx.x == NAN_NUM) return -2; i = v.nextVolumeRow; } - return 1; + return -2; } -__kernel void integrateAllVolumeUnits( - __global const char * depthptr, - int depth_step, int depth_offset, - int depth_rows, int depth_cols, - __global struct Volume_NODE * hash_table, - const int list_size, - const int bufferNums, - const int hash_divisor, - __global const int4 * totalVolUnits, - // const float16 vol2camMatrix, - // const __global struct TsdfVoxel * volUnitsData, - //global bool * isActive, - // const __global int * lastVisibleIndexes, - // const __global float * pixNorms, - const float voxelSize, - const int4 volResolution4, - const int4 volDims4, - const float2 fxy, - const float2 cxy, - const float dfac, - const float truncDist, - const int maxWeight - // ,const __global float * pixNorms - ) +struct Volume_NODE findVolume(__global struct Volume_NODE * hash_table, int4 indx, + int list_size, int bufferNums, int hash_divisor) { - - int i = get_global_id(0); - int j = get_global_id(1); - int k = get_global_id(2); - int4 v = totalVolUnits[k]; - printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d]\n", i, j, k, v[0], v[1], v[2]); - - //printf(" = %d , %d , %d \n", hash_params.x, hash_params.y, hash_params.z); - //printf(" = %d , %d , %d \n", list_size, bufferNums, hash_divisor ); - //int tmp = findVolume(hash_table, hash_table->idx, hash_divisor); - - //int3 idx; idx.x = 7; idx.y=7;idx.z=1; - //int tmp = findVolume(hash_table, idx, list_size, bufferNums, hash_divisor); - //printf("idx = %d %d %d | row = %d \n", idx.x, idx.y, idx.z, tmp); - - //hash_table->idx.x = 10; - //findVolume(hash_table, hash_table->idx, hash_divisor); - - //printf("\n"); - - //char* tmp = (char) hash_table; - //for (int i = 0; i < 100; i++) - //{ - // printf("|%c", tmp[i] ); - //} - - /* - for (int i = 0; i < 16000; i++) + int hash = calc_hash(indx) % hash_divisor; + int num = 1; + int i = hash * num * list_size; + int NAN_NUM = -2147483647; + struct Volume_NODE tmp; + while (i != NAN_NUM) { - - int idx = i; - struct Volume_NODE v = hash_table[idx]; - if (v.idx.x != -2147483647) - { - printf("\n"); - printf("idx = %d %d %d \n", v.idx.x, v.idx.y, v.idx.z); - printf("row = %d \n", v.row); - printf("nv = %d \n", v.nextVolumeRow); - printf("tmp = %d \n", v.tmp); - - //printf("idx = %d %d %d \n", (hash_table[idx]).idx.x, (hash_table[idx]).idx.y, (hash_table[idx]).idx.z); - //printf("idx = %d %d %d \n", (hash_table+idx)->idx.x, (hash_table+idx)->idx.y, (hash_table+idx)->idx.z); - } + struct Volume_NODE v = hash_table[i]; + if (v.idx[0] == indx[0] && + v.idx[1] == indx[1] && + v.idx[2] == indx[2]) + return v; + if (v.idx.x == NAN_NUM) + return tmp; + i = v.nextVolumeRow; } - */ - - //printf("idx = %d %d %d \n", hash_table->idx.x, hash_table->idx.y, hash_table->idx.z); - //printf("row = %d \n", hash_table->row); - //printf("nv = %d \n", hash_table->nextVolumeRow); - //printf("isA = %d \n", hash_table->isActive); - //printf("isA = %s \n", hash_table->isActive ? "true" : "false"); - //printf("lvi = %d \n", hash_table->lastVisibleIndex); - //printf("%f %f \n", hash_table->vol2camMatrix.x, hash_table->vol2camMatrix.y); + return tmp; } -void integrateVolumeUnit(__global const char * depthptr, +void integrateVolumeUnit( + int x, int y, + __global const char * depthptr, int depth_step, int depth_offset, int depth_rows, int depth_cols, __global struct TsdfVoxel * volumeptr, + const __global float * pixNorms, const float16 vol2camMatrix, const float voxelSize, const int4 volResolution4, @@ -185,11 +125,11 @@ void integrateVolumeUnit(__global const char * depthptr, const float2 cxy, const float dfac, const float truncDist, - const int maxWeight, - const __global float * pixNorms) + const int maxWeight + ) { - int x = get_global_id(0); - int y = get_global_id(1); + //int x = get_global_id(0); + //int y = get_global_id(1); const int3 volResolution = volResolution4.xyz; @@ -324,4 +264,70 @@ void integrateVolumeUnit(__global const char * depthptr, volumeptr[volIdx] = voxel; } } -} \ No newline at end of file + +} + +__kernel void integrateAllVolumeUnits( + __global const char * depthptr, + int depth_step, int depth_offset, + int depth_rows, int depth_cols, + __global struct Volume_NODE * hash_table, + const int list_size, + const int bufferNums, + const int hash_divisor, + __global const int4 * totalVolUnits, + __global struct TsdfVoxel * allVolumePtr, + __global const float * pixNorms, + __global const float16 * allVol2camMatrix, + const int lastVolIndex, + const float voxelSize, + const int4 volResolution4, + const int4 volDims4, + const float2 fxy, + const float2 cxy, + const float dfac, + const float truncDist, + const int maxWeight + ) +{ + + int i = get_global_id(0); + int j = get_global_id(1); + int k = get_global_id(2); + + int4 v = totalVolUnits[k]; + int row = findRow(hash_table, v, list_size, bufferNums, hash_divisor); + //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | row = %d \n", i, j, k, v[0], v[1], v[2], row); + //printf("maxWeight = %d \n", maxWeight); + int hash = calc_hash(v); + __global struct Volume_NODE * vu = (hash_table + hash); + + + if (vu->isActive == 1) + { + + int resol = volResolution4[0] * volResolution4[1] * volResolution4[2]; + __global struct TsdfVoxel * volumeptr = (allVolumePtr+(row*resol)); + const float16 vol2camMatrix = allVol2camMatrix[row]; + //printf("resol = %d \n", resol); + + integrateVolumeUnit( + i, j, + depthptr, + depth_step, depth_offset, + depth_rows, depth_cols, + volumeptr, + pixNorms, + vol2camMatrix, + voxelSize, + volResolution4, + volDims4, + fxy, + cxy, + dfac, + truncDist, + maxWeight + ); + vu->isActive = 0; + } +} diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 1e8704170f4..f7dab563aec 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -929,7 +929,7 @@ void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, size_t globalSize[2]; globalSize[0] = (size_t)volResolution.x; globalSize[1] = (size_t)volResolution.y; - + std::cout << "volResolution = "<(i, 0); v.idx = nan4; - //v.idx = cv::Vec4i(-2147483648, -2147483648, -2147483648, -2147483648); v.row = -1; v.nextVolumeRow = -1; - v.tmp = i; - v.noneed = -555; + v.isActive = 0; + v.lastVisibleIndex = -1; + //v.tmp = i; } } @@ -463,10 +463,11 @@ void VolumesTable::update(Vec3i indx, int row) i = v.nextVolumeRow; } } -/* -void VolumesTable::update(Vec3i indx, bool isActive, int lastVisibleIndex, ocl::KernelArg pose) + +void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) { - int hash = int(calc_hash(indx) % hash_divisor); + Vec4i idx(indx[0], indx[1], indx[2], 0); + int hash = int(calc_hash(idx) % hash_divisor); int num = 1; int start = hash * num * list_size; int i = start; @@ -474,25 +475,54 @@ void VolumesTable::update(Vec3i indx, bool isActive, int lastVisibleIndex, ocl:: while (i != -1) { Volume_NODE& v = volumes.at(i, 0); - if (v.idx == indx) + if (v.idx == idx) { + v.isActive = isActive; + v.lastVisibleIndex = lastVisibleIndex; return; } //find nan cheking for int or Vec3i - if (v.idx == Vec3i(nan3)) + //if (isNaN(Point3i(v.idx))) + if (v.idx[0] == -2147483647) { - v.idx = indx; + v.idx = idx; + v.nextVolumeRow = getNextVolume(hash, num, i, start); v.isActive = isActive; v.lastVisibleIndex = lastVisibleIndex; - v.pose = pose; - v.nextVolumeRow = getNextVolume(hash, num, i, start); indexes.push_back(indx); + indexesGPU.push_back(idx); + return; + } + i = v.nextVolumeRow; + } +} + +void VolumesTable::updateActive(Vec3i indx, int isActive) +{ + Vec4i idx(indx[0], indx[1], indx[2], 0); + int hash = int(calc_hash(idx) % hash_divisor); + int num = 1; + int start = hash * num * list_size; + int i = start; + + while (i != -1) + { + Volume_NODE& v = volumes.at(i, 0); + if (v.idx == idx) + { + v.isActive = isActive; + return; + } + //find nan cheking for int or Vec3i + //if (isNaN(Point3i(v.idx))) + if (v.idx[0] == -2147483647) + { return; } i = v.nextVolumeRow; } } -*/ + int VolumesTable::getNextVolume(int hash, int& num, int i, int start) { if (i != start && i % list_size == 0) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 56b3847f6ef..ac8b0ac088f 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -49,11 +49,9 @@ struct Volume_NODE Vec4i idx = Vec4i(-2147483647); int32_t row = -1; int32_t nextVolumeRow = -1; - //bool isActive = false; - //int lastVisibleIndex = -1; - //ocl::KernelArg pose; - int32_t tmp; - int32_t noneed; + int32_t isActive; + int32_t lastVisibleIndex; + //int32_t tmp; }; size_t calc_hash(Vec4i x); @@ -75,7 +73,8 @@ class VolumesTable void update(Vec3i indx); void update(Vec3i indx, int row); - void update(Vec3i indx, bool isActive, int lastVisibleIndex, ocl::KernelArg pose); + void update(Vec3i indx, int isActive, int lastVisibleIndex); + void updateActive(Vec3i indx, int isActive); void expand(); int getNextVolume(int hash, int& num, int i, int start); int find_Volume(Vec3i indx) const; From 168f19095c2ee1fd5852e87e82d7651037f5f866 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 26 Nov 2020 09:31:29 +0300 Subject: [PATCH 051/216] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 16 +++++--- modules/rgbd/src/opencl/hash_tsdf.cl | 59 ++++++++++++++++++++++------ 2 files changed, 59 insertions(+), 16 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 139b7a4d93a..e5ea6f60a2f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -986,6 +986,7 @@ void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFa */ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics) { + //std::cout << "integrateAllVolumeUnitsGPU" << std::endl; CV_TRACE_FUNCTION(); CV_Assert(!_depth.empty()); @@ -1037,8 +1038,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept ); - //int resol = 1; - int resol = volumeUnitResolution; + int resol = 1; + //int resol = volumeUnitResolution; size_t globalSize[3]; globalSize[0] = (size_t)resol; // volumeUnitResolution globalSize[1] = (size_t)resol; // volumeUnitResolution @@ -1046,6 +1047,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept //globalSize[1] = (size_t)depth.cols; // volResolution.y globalSize[2] = (size_t)totalVolUnitsSize; // num of voxels + //std::cout << "RUN" << std::endl; if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } @@ -1154,6 +1156,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma lastVisibleIndexes.at(idx, 0) = frameId; //Affine3f cam2vol(Affine3f(subvolumePose) * Affine3f(cameraPose)); Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + _indexes.updateActive(tsdf_idx, 1); ocl::KernelArg pose = ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)); posesGPU.at(idx, 0) = pose; //ocl::KernelArg pose; @@ -1194,7 +1197,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { isActive.at(idx, 0) = false; - _indexes.updateActive(tsdf_idx, false); + _indexes.updateActive(tsdf_idx, 0); return; } Point2f cameraPoint = proj(volUnitInCamSpace); @@ -1203,7 +1206,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma assert(idx == _lastVolIndex - 1); lastVisibleIndexes.at(idx, 0) = frameId; isActive.at(idx, 0) = true; - _indexes.update(tsdf_idx, true, frameId); + std::cout << " - " << tsdf_idx << std::endl; + _indexes.update(tsdf_idx, 1, frameId); } } }; @@ -1222,6 +1226,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //! Integrate the correct volumeUnits + /* auto Integrate = [&](const Range& range) { for (int i = range.start; i < range.end; i++) { @@ -1246,7 +1251,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } }; - parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); + */ + //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); //Integrate(Range(0, (int)_totalVolUnits.size())); //std::cout << "lol\n"; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index abd165c7d9f..96a93306d21 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -87,29 +87,56 @@ int findRow(__global struct Volume_NODE * hash_table, int4 indx, return -2; } -struct Volume_NODE findVolume(__global struct Volume_NODE * hash_table, int4 indx, +int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; int num = 1; int i = hash * num * list_size; int NAN_NUM = -2147483647; - struct Volume_NODE tmp; + //struct Volume_NODE tmp; while (i != NAN_NUM) { struct Volume_NODE v = hash_table[i]; + if (v.idx[0] == indx[0] && v.idx[1] == indx[1] && v.idx[2] == indx[2]) - return v; - if (v.idx.x == NAN_NUM) - return tmp; + { + //printf("idx = [%d %d %d] | idx = [%d %d %d] \n nextVolumeRow = %d \n", indx[0],indx[1],indx[2],v->idx[0], v->idx[1], v->idx[2], v->nextVolumeRow); + + return v.isActive; + } + if (v.idx[0] == NAN_NUM) + return 0; i = v.nextVolumeRow; } + return 0; +} + +void updateIsActive(__global struct Volume_NODE * hash_table, int4 indx, int isActive, + int list_size, int bufferNums, int hash_divisor) +{ + int hash = calc_hash(indx) % hash_divisor; + int num = 1; + int i = hash * num * list_size; + int NAN_NUM = -2147483647; + while (i != NAN_NUM) + { + __global struct Volume_NODE * v = (hash_table + i); - return tmp; + if (v->idx[0] == indx[0] && + v->idx[1] == indx[1] && + v->idx[2] == indx[2]) + v->isActive = isActive; + if (v->idx[0] == NAN_NUM) + return; + i = v->nextVolumeRow; + } + return; } + void integrateVolumeUnit( int x, int y, __global const char * depthptr, @@ -290,6 +317,7 @@ __kernel void integrateAllVolumeUnits( const int maxWeight ) { + //printf("start \n"); int i = get_global_id(0); int j = get_global_id(1); @@ -299,13 +327,20 @@ __kernel void integrateAllVolumeUnits( int row = findRow(hash_table, v, list_size, bufferNums, hash_divisor); //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | row = %d \n", i, j, k, v[0], v[1], v[2], row); //printf("maxWeight = %d \n", maxWeight); - int hash = calc_hash(v); - __global struct Volume_NODE * vu = (hash_table + hash); + //int hash = calc_hash(v); + + //printf("start_ \n"); + //__global struct Volume_NODE * vu = getVolume(hash_table, v, list_size, bufferNums, hash_divisor); + //printf("idx = [x, y, z] = [%d, %d, %d] | row = %d \n", vu->idx[0], vu->idx[0], vu->idx[0], vu->row); + //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | row = %d \n", v[0], v[1], v[2], vu->idx[0], vu->idx[0], vu->idx[0], vu->row); + int isActive = getIsActive(hash_table, v, list_size, bufferNums, hash_divisor); + //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | isActive = %d \n", i, j, k, v[0], v[1], v[2], isActive); - if (vu->isActive == 1) + + if (isActive == 1) { - + //printf("lol \n"); int resol = volResolution4[0] * volResolution4[1] * volResolution4[2]; __global struct TsdfVoxel * volumeptr = (allVolumePtr+(row*resol)); const float16 vol2camMatrix = allVol2camMatrix[row]; @@ -328,6 +363,8 @@ __kernel void integrateAllVolumeUnits( truncDist, maxWeight ); - vu->isActive = 0; + //isActive = 0; + updateIsActive(hash_table, v, 0, list_size, bufferNums, hash_divisor); } + } From a4d20a83b4035ea37b6261e19da998e8cbacbe9f Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 26 Nov 2020 13:41:04 +0300 Subject: [PATCH 052/216] integrate fix --- modules/rgbd/src/hash_tsdf.cpp | 16 ++++++---------- modules/rgbd/src/hash_tsdf.hpp | 1 - modules/rgbd/src/opencl/hash_tsdf.cl | 21 +++++++-------------- 3 files changed, 13 insertions(+), 25 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index e5ea6f60a2f..93d9a88fa40 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -862,7 +862,6 @@ void HashTSDFVolumeGPU::reset() //volUnitsData = cv::Mat(VOLUMES_SIZE, 1, rawType()); indexes = cv::Mat(buff_lvl, 1, rawType()); poses = cv::Mat(buff_lvl, 1, rawType()); - isActive = cv::Mat(buff_lvl, 1, rawType()); lastVisibleIndexes = cv::Mat(buff_lvl, 1, rawType()); _indexes = VolumesTable(); posesGPU = cv::Mat(buff_lvl, 1, rawType()); @@ -1025,6 +1024,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), ocl::KernelArg::PtrReadWrite(_volUnitsData.getUMat(ACCESS_RW)), ocl::KernelArg::PtrReadOnly(_pixNorms), + //ocl::KernelArg::PtrReadOnly(pixNorms.getUMat(ACCESS_RW)), ocl::KernelArg::PtrReadWrite(posesGPU.getUMat(ACCESS_RW)), _lastVolIndex, voxelSize, @@ -1038,16 +1038,16 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept ); - int resol = 1; - //int resol = volumeUnitResolution; + //int resol = 1; + int resol = volumeUnitResolution; size_t globalSize[3]; globalSize[0] = (size_t)resol; // volumeUnitResolution globalSize[1] = (size_t)resol; // volumeUnitResolution - //globalSize[0] = (size_t)depth.rows; // volResolution.x - //globalSize[1] = (size_t)depth.cols; // volResolution.y + //globalSize[0] = (size_t)volResGpu.val[0]; // volResolution.x + //globalSize[1] = (size_t)volResGpu.val[1]; // volResolution.y globalSize[2] = (size_t)totalVolUnitsSize; // num of voxels - //std::cout << "RUN" << std::endl; + //std::cout << "r = " << volumeUnitResolution << std::endl; if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } @@ -1124,7 +1124,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma _volUnitsData.resize(buff_lvl); volUnitsData.resize(buff_lvl); poses.resize(buff_lvl); - isActive.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); posesGPU.resize(buff_lvl); } @@ -1152,7 +1151,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; poses.at(idx, 0) = subvolumePose; - isActive.at(idx, 0) = true; lastVisibleIndexes.at(idx, 0) = frameId; //Affine3f cam2vol(Affine3f(subvolumePose) * Affine3f(cameraPose)); Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); @@ -1196,7 +1194,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { - isActive.at(idx, 0) = false; _indexes.updateActive(tsdf_idx, 0); return; } @@ -1205,7 +1202,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { assert(idx == _lastVolIndex - 1); lastVisibleIndexes.at(idx, 0) = frameId; - isActive.at(idx, 0) = true; std::cout << " - " << tsdf_idx << std::endl; _indexes.update(tsdf_idx, 1, frameId); } diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index bee1a307a76..85da938aa50 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -172,7 +172,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume VolumeIndex _lastVolIndex; cv::Mat indexes; cv::Mat poses; - cv::Mat isActive; cv::Mat lastVisibleIndexes; cv::Mat _volUnitsData; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 96a93306d21..ee44a7d0017 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -157,7 +157,7 @@ void integrateVolumeUnit( { //int x = get_global_id(0); //int y = get_global_id(1); - + //printf("integrateVolumeUnit \n"); const int3 volResolution = volResolution4.xyz; if(x >= volResolution.x || y >= volResolution.y) @@ -232,6 +232,7 @@ void integrateVolumeUnit( float v; // bilinearly interpolate depth at projected + //printf("bilinearly\n"); if(all(projected >= 0) && all(projected < limits)) { float2 ip = floor(projected); @@ -272,7 +273,7 @@ void integrateVolumeUnit( float sdf = pixNorm*(v*dfac - camSpacePt.z); // possible alternative is: // float sdf = length(camSpacePt)*(v*dfac/camSpacePt.z - 1.0); - + //printf("sdf \n"); if(sdf >= -truncDist) { float tsdf = fmin(1.0f, sdf * truncDistInv); @@ -281,9 +282,10 @@ void integrateVolumeUnit( struct TsdfVoxel voxel = volumeptr[volIdx]; float value = tsdfToFloat(voxel.tsdf); int weight = voxel.weight; - + //printf(" v = %f \n", value); // update TSDF value = (value*weight + tsdf) / (weight + 1); + //printf(" v = %f \n", value); weight = min(weight + 1, maxWeight); voxel.tsdf = floatToTsdf(value); @@ -325,26 +327,18 @@ __kernel void integrateAllVolumeUnits( int4 v = totalVolUnits[k]; int row = findRow(hash_table, v, list_size, bufferNums, hash_divisor); + if (row < 0) + return; //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | row = %d \n", i, j, k, v[0], v[1], v[2], row); //printf("maxWeight = %d \n", maxWeight); - //int hash = calc_hash(v); - - //printf("start_ \n"); - //__global struct Volume_NODE * vu = getVolume(hash_table, v, list_size, bufferNums, hash_divisor); - //printf("idx = [x, y, z] = [%d, %d, %d] | row = %d \n", vu->idx[0], vu->idx[0], vu->idx[0], vu->row); - //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | row = %d \n", v[0], v[1], v[2], vu->idx[0], vu->idx[0], vu->idx[0], vu->row); int isActive = getIsActive(hash_table, v, list_size, bufferNums, hash_divisor); - //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | isActive = %d \n", i, j, k, v[0], v[1], v[2], isActive); - if (isActive == 1) { - //printf("lol \n"); int resol = volResolution4[0] * volResolution4[1] * volResolution4[2]; __global struct TsdfVoxel * volumeptr = (allVolumePtr+(row*resol)); const float16 vol2camMatrix = allVol2camMatrix[row]; - //printf("resol = %d \n", resol); integrateVolumeUnit( i, j, @@ -363,7 +357,6 @@ __kernel void integrateAllVolumeUnits( truncDist, maxWeight ); - //isActive = 0; updateIsActive(hash_table, v, 0, list_size, bufferNums, hash_divisor); } From 1ace155b8332852265c2f6f154c93376b9dafe36 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 27 Nov 2020 12:38:56 +0300 Subject: [PATCH 053/216] fix work with table --- modules/rgbd/src/hash_tsdf.cpp | 7 ++++--- modules/rgbd/src/opencl/hash_tsdf.cl | 20 ++++++++++++++++---- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 93d9a88fa40..f7cf85dd080 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1017,15 +1017,15 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept //std::cout << "maxWeight == " << maxWeight << std::endl; k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), - //ocl::KernelArg::PtrReadWrite(_indexes.volumes), (int)_indexes.list_size, (int)_indexes.bufferNums, (int)_indexes.hash_divisor, ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), - ocl::KernelArg::PtrReadWrite(_volUnitsData.getUMat(ACCESS_RW)), + //ocl::KernelArg::PtrReadWrite(_volUnitsData.getUMat(ACCESS_RW)), + ocl::KernelArg::ReadWrite(_volUnitsData.getUMat(ACCESS_RW)), ocl::KernelArg::PtrReadOnly(_pixNorms), //ocl::KernelArg::PtrReadOnly(pixNorms.getUMat(ACCESS_RW)), - ocl::KernelArg::PtrReadWrite(posesGPU.getUMat(ACCESS_RW)), + ocl::KernelArg::ReadOnly(posesGPU.getUMat(ACCESS_RW)), _lastVolIndex, voxelSize, volResGpu.val, @@ -1037,6 +1037,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept int(maxWeight) ); + //std::cout << _indexes //int resol = 1; int resol = volumeUnitResolution; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index ee44a7d0017..9d4174a1215 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -301,13 +301,18 @@ __kernel void integrateAllVolumeUnits( int depth_step, int depth_offset, int depth_rows, int depth_cols, __global struct Volume_NODE * hash_table, + const int list_size, const int bufferNums, const int hash_divisor, __global const int4 * totalVolUnits, __global struct TsdfVoxel * allVolumePtr, + int table_step, int table_offset, + int table_rows, int table_cols, __global const float * pixNorms, __global const float16 * allVol2camMatrix, + int val2cam_step, int val2cam_offset, + int val2cam_rows, int val2cam_cols, const int lastVolIndex, const float voxelSize, const int4 volResolution4, @@ -320,7 +325,9 @@ __kernel void integrateAllVolumeUnits( ) { //printf("start \n"); - + //printf(" | step, offset = %d %d \n | rows, cols = %d %d \n", table_step, table_offset, table_rows, table_cols); + //printf(" | step, offset = %d %d \n | rows, cols = %d %d \n", val2cam_step, val2cam_offset, val2cam_rows, val2cam_cols); + int i = get_global_id(0); int j = get_global_id(1); int k = get_global_id(2); @@ -334,11 +341,16 @@ __kernel void integrateAllVolumeUnits( int isActive = getIsActive(hash_table, v, list_size, bufferNums, hash_divisor); + if (isActive == 1) { int resol = volResolution4[0] * volResolution4[1] * volResolution4[2]; - __global struct TsdfVoxel * volumeptr = (allVolumePtr+(row*resol)); - const float16 vol2camMatrix = allVol2camMatrix[row]; + //__global struct TsdfVoxel * volumeptr = (allVolumePtr+(row*resol)); + __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) + (allVolumePtr + table_offset + + (row)*table_step); + //const float16 vol2camMatrix = allVol2camMatrix[row]; + const float16 vol2camMatrix = allVol2camMatrix[val2cam_offset + (row) * val2cam_step]; integrateVolumeUnit( i, j, @@ -359,5 +371,5 @@ __kernel void integrateAllVolumeUnits( ); updateIsActive(hash_table, v, 0, list_size, bufferNums, hash_divisor); } - + } From 208968482e2cf1a98bdb5c0eebb5a0ea05109327 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 30 Nov 2020 10:59:05 +0300 Subject: [PATCH 054/216] minor table fi --- modules/rgbd/src/hash_tsdf.cpp | 18 ++++++++++-------- modules/rgbd/src/tsdf_functions.cpp | 20 +++++++++++++++++++- modules/rgbd/src/tsdf_functions.hpp | 5 +++-- 3 files changed, 32 insertions(+), 11 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index f7cf85dd080..06c2610495a 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1223,7 +1223,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //! Integrate the correct volumeUnits - /* + auto Integrate = [&](const Range& range) { for (int i = range.start; i < range.end; i++) { @@ -1231,30 +1231,32 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma VolumeIndex idx = _indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == _lastVolIndex - 1) return; - bool& _isActive = isActive.at(idx, 0); + //bool& _isActive = isActive.at(idx, 0); + bool _isActive = _indexes.getActive(tsdf_idx); if (_isActive) { //! The volume unit should already be added into the Volume from the allocator Matx44f _pose = poses.at(idx, 0); - //integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, - // Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, - // depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); + integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, + Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, + depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); //! Ensure all active volumeUnits are set to inactive for next integration - _isActive = false; + //_isActive = false; + _indexes.updateActive(tsdf_idx, 0); } } }; - */ + //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); //Integrate(Range(0, (int)_totalVolUnits.size())); //std::cout << "lol\n"; - integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); + //integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); //std::cout << "lol\n"; } diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 8e591cb7405..4a156cbb4b4 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -523,6 +523,24 @@ void VolumesTable::updateActive(Vec3i indx, int isActive) } } +bool VolumesTable::getActive(Vec3i indx) const +{ + Vec4i idx(indx[0], indx[1], indx[2], 0); + int hash = int(calc_hash(idx) % hash_divisor); + int num = 1; + int i = hash * num * list_size; + while (i != -1) + { + Volume_NODE v = volumes.at(i, 0); + if (v.idx == idx) + return v.isActive; + if (v.idx[0] == -2147483647) + return false; + i = v.nextVolumeRow; + } + return false; +} + int VolumesTable::getNextVolume(int hash, int& num, int i, int start) { if (i != start && i % list_size == 0) @@ -566,7 +584,7 @@ int VolumesTable::find_Volume(Vec3i indx) const return v.row; //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx[0] == -2147483648) + if (v.idx[0] == -2147483647) return -2; i = v.nextVolumeRow; } diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index ac8b0ac088f..ce72069d4a4 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -49,8 +49,8 @@ struct Volume_NODE Vec4i idx = Vec4i(-2147483647); int32_t row = -1; int32_t nextVolumeRow = -1; - int32_t isActive; - int32_t lastVisibleIndex; + int32_t isActive = 0; + int32_t lastVisibleIndex = -1; //int32_t tmp; }; @@ -76,6 +76,7 @@ class VolumesTable void update(Vec3i indx, int isActive, int lastVisibleIndex); void updateActive(Vec3i indx, int isActive); void expand(); + bool getActive(Vec3i indx) const; int getNextVolume(int hash, int& num, int i, int start); int find_Volume(Vec3i indx) const; bool isExist(Vec3i indx); From 66c09bd66dd0349658befd6d928c19136dcc8f97 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 30 Nov 2020 12:29:39 +0300 Subject: [PATCH 055/216] I am stumped --- modules/rgbd/src/hash_tsdf.cpp | 60 ++++++++++++++-------------- modules/rgbd/src/opencl/hash_tsdf.cl | 3 +- 2 files changed, 33 insertions(+), 30 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 06c2610495a..7c6f1c3a196 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1223,40 +1223,42 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //! Integrate the correct volumeUnits - - auto Integrate = [&](const Range& range) { - for (int i = range.start; i < range.end; i++) - { - Vec3i tsdf_idx = _totalVolUnits[i]; - VolumeIndex idx = _indexes.find_Volume(tsdf_idx); - if (idx < 0 || idx == _lastVolIndex - 1) return; - - //bool& _isActive = isActive.at(idx, 0); - bool _isActive = _indexes.getActive(tsdf_idx); - - if (_isActive) + if (false) + { + auto Integrate = [&](const Range& range) { + for (int i = range.start; i < range.end; i++) { - //! The volume unit should already be added into the Volume from the allocator - Matx44f _pose = poses.at(idx, 0); - - integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, - Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, - depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); + Vec3i tsdf_idx = _totalVolUnits[i]; + VolumeIndex idx = _indexes.find_Volume(tsdf_idx); + if (idx < 0 || idx == _lastVolIndex - 1) return; - //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); - //! Ensure all active volumeUnits are set to inactive for next integration - //_isActive = false; - _indexes.updateActive(tsdf_idx, 0); + //bool& _isActive = isActive.at(idx, 0); + bool _isActive = _indexes.getActive(tsdf_idx); + //std::cout << _isActive; + if (_isActive) + { + //std::cout << " lol "; + //! The volume unit should already be added into the Volume from the allocator + Matx44f _pose = poses.at(idx, 0); + + integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, + Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, + depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); + + //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); + //! Ensure all active volumeUnits are set to inactive for next integration + //_isActive = false; + _indexes.updateActive(tsdf_idx, 0); + } } - } - }; - - //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); - //Integrate(Range(0, (int)_totalVolUnits.size())); + }; + //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); + Integrate(Range(0, (int)_totalVolUnits.size())); + } //std::cout << "lol\n"; - - //integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); + else + integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); //std::cout << "lol\n"; } diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 9d4174a1215..5ccef1f8b87 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -334,7 +334,7 @@ __kernel void integrateAllVolumeUnits( int4 v = totalVolUnits[k]; int row = findRow(hash_table, v, list_size, bufferNums, hash_divisor); - if (row < 0) + if (row < 0 || row > lastVolIndex-1) return; //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | row = %d \n", i, j, k, v[0], v[1], v[2], row); //printf("maxWeight = %d \n", maxWeight); @@ -344,6 +344,7 @@ __kernel void integrateAllVolumeUnits( if (isActive == 1) { + printf("lol"); int resol = volResolution4[0] * volResolution4[1] * volResolution4[2]; //__global struct TsdfVoxel * volumeptr = (allVolumePtr+(row*resol)); __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) From a1aa11a05cd6f99f8575d69cd01e9b4d5fa47b1f Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 30 Nov 2020 15:38:37 +0300 Subject: [PATCH 056/216] try to fix --- modules/rgbd/src/hash_tsdf.cpp | 16 +++++++++++++++- modules/rgbd/src/opencl/hash_tsdf.cl | 2 +- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 7c6f1c3a196..5721751683f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1015,6 +1015,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept //std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; //std::cout << "maxWeight == " << maxWeight << std::endl; + UMat U_volUnitsData = _volUnitsData.getUMat(ACCESS_RW); + k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), (int)_indexes.list_size, @@ -1022,7 +1024,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept (int)_indexes.hash_divisor, ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), //ocl::KernelArg::PtrReadWrite(_volUnitsData.getUMat(ACCESS_RW)), - ocl::KernelArg::ReadWrite(_volUnitsData.getUMat(ACCESS_RW)), + //ocl::KernelArg::ReadWrite(_volUnitsData.getUMat(ACCESS_RW)), + ocl::KernelArg::ReadWrite(U_volUnitsData), ocl::KernelArg::PtrReadOnly(_pixNorms), //ocl::KernelArg::PtrReadOnly(pixNorms.getUMat(ACCESS_RW)), ocl::KernelArg::ReadOnly(posesGPU.getUMat(ACCESS_RW)), @@ -1051,6 +1054,17 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept //std::cout << "r = " << volumeUnitResolution << std::endl; if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); + + //Mat _tmp = _volUnitsData; + (U_volUnitsData.getMat(ACCESS_RW)).copyTo(_volUnitsData); + //Mat tmp = U_volUnitsData.getMat(ACCESS_RW); + + //tmp.copyTo(_volUnitsData); + //U_volUnitsData.release(); + //tmp.release(); + //bool res = false; + //if (_tmp == _volUnitsData) res = true; + //std::cout << res << std::endl; } void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 5ccef1f8b87..0707abc0510 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -344,7 +344,7 @@ __kernel void integrateAllVolumeUnits( if (isActive == 1) { - printf("lol"); + //printf("lol"); int resol = volResolution4[0] * volResolution4[1] * volResolution4[2]; //__global struct TsdfVoxel * volumeptr = (allVolumePtr+(row*resol)); __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) From c43339e1b3ac33671fdaf79ec9959d38a613b890 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 1 Dec 2020 10:28:57 +0300 Subject: [PATCH 057/216] add checking to find error --- modules/rgbd/src/hash_tsdf.cpp | 49 +++++++++++++++++++++++++++++++--- 1 file changed, 45 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 5721751683f..a99483fbfaf 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -983,6 +983,33 @@ void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFa throw std::runtime_error("Failed to run kernel"); } */ + +bool matIsEqual(const cv::Mat Mat1, const cv::Mat Mat2) +{ + if (Mat1.dims == Mat2.dims && + Mat1.size == Mat2.size && + Mat1.elemSize() == Mat2.elemSize()) + { + if (Mat1.isContinuous() && Mat2.isContinuous()) + { + return 0 == memcmp(Mat1.ptr(), Mat2.ptr(), Mat1.total() * Mat1.elemSize()); + } + else + { + const cv::Mat* arrays[] = { &Mat1, &Mat2, 0 }; + uchar* ptrs[2]; + cv::NAryMatIterator it(arrays, ptrs, 2); + for (unsigned int p = 0; p < it.nplanes; p++, ++it) + if (0 != memcmp(it.ptrs[0], it.ptrs[1], it.size * Mat1.elemSize())) + return false; + + return true; + } + } + + return false; +} + void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics) { //std::cout << "integrateAllVolumeUnitsGPU" << std::endl; @@ -1015,7 +1042,10 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept //std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; //std::cout << "maxWeight == " << maxWeight << std::endl; - UMat U_volUnitsData = _volUnitsData.getUMat(ACCESS_RW); + Mat _tmp; + _volUnitsData.copyTo(_tmp); + UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); + //UMat U_volUnitsData = _volUnitsData.getUMat(ACCESS_RW); k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), @@ -1038,10 +1068,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept dfac, truncDist, int(maxWeight) - ); //std::cout << _indexes - //int resol = 1; int resol = volumeUnitResolution; size_t globalSize[3]; @@ -1055,8 +1083,21 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); + Mat checking; + _volUnitsData.copyTo(checking); + + _tmp = U_volUnitsData.getMat(ACCESS_RW); + _tmp.copyTo(_volUnitsData); + _tmp.release(); + + //cv::Mat diff; + //cv::compare(checking, _volUnitsData, diff, cv::CMP_NE); + //int nz = cv::countNonZero(diff); + + //if (nz == 0) std::cout << "compare = " << true << std::endl; + if (matIsEqual(checking, _volUnitsData)) std::cout << "compare = " << true << std::endl; //Mat _tmp = _volUnitsData; - (U_volUnitsData.getMat(ACCESS_RW)).copyTo(_volUnitsData); + //(U_volUnitsData.getMat(ACCESS_RW)).copyTo(_volUnitsData); //Mat tmp = U_volUnitsData.getMat(ACCESS_RW); //tmp.copyTo(_volUnitsData); From 230ac40f7cadcd301fc38b2a4803f2f7a9ce8e07 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 1 Dec 2020 15:20:32 +0300 Subject: [PATCH 058/216] add copyto --- modules/rgbd/src/hash_tsdf.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a99483fbfaf..87855f1373e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1095,7 +1095,11 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept //int nz = cv::countNonZero(diff); //if (nz == 0) std::cout << "compare = " << true << std::endl; - if (matIsEqual(checking, _volUnitsData)) std::cout << "compare = " << true << std::endl; + if (matIsEqual(checking, _volUnitsData)) + std::cout << "compare = " << true << std::endl; + else + std::cout << "compare = " << false << std::endl; + //Mat _tmp = _volUnitsData; //(U_volUnitsData.getMat(ACCESS_RW)).copyTo(_volUnitsData); //Mat tmp = U_volUnitsData.getMat(ACCESS_RW); From 59e6da47b358404285121ff92c5720fc9a09e29d Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 2 Dec 2020 11:44:25 +0300 Subject: [PATCH 059/216] vol2cam fix --- modules/rgbd/src/hash_tsdf.cpp | 32 +++++++++---- modules/rgbd/src/opencl/hash_tsdf.cl | 69 +++++++++++++++++++++++++--- 2 files changed, 85 insertions(+), 16 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 87855f1373e..f11b8d542e3 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -864,7 +864,7 @@ void HashTSDFVolumeGPU::reset() poses = cv::Mat(buff_lvl, 1, rawType()); lastVisibleIndexes = cv::Mat(buff_lvl, 1, rawType()); _indexes = VolumesTable(); - posesGPU = cv::Mat(buff_lvl, 1, rawType()); + posesGPU = cv::Mat(buff_lvl, 16, rawType()); } static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) @@ -1058,7 +1058,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept ocl::KernelArg::ReadWrite(U_volUnitsData), ocl::KernelArg::PtrReadOnly(_pixNorms), //ocl::KernelArg::PtrReadOnly(pixNorms.getUMat(ACCESS_RW)), - ocl::KernelArg::ReadOnly(posesGPU.getUMat(ACCESS_RW)), + ocl::KernelArg::ReadOnly(posesGPU.getUMat(ACCESS_READ)), + //ocl::KernelArg::ReadOnly(poses.getUMat(ACCESS_READ)), _lastVolIndex, voxelSize, volResGpu.val, @@ -1078,17 +1079,18 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept //globalSize[0] = (size_t)volResGpu.val[0]; // volResolution.x //globalSize[1] = (size_t)volResGpu.val[1]; // volResolution.y globalSize[2] = (size_t)totalVolUnitsSize; // num of voxels - - //std::cout << "r = " << volumeUnitResolution << std::endl; + //printf("CPU: fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); + //std::cout << "rmaxWeight == " << maxWeight << std::endl; if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); Mat checking; _volUnitsData.copyTo(checking); - _tmp = U_volUnitsData.getMat(ACCESS_RW); - _tmp.copyTo(_volUnitsData); - _tmp.release(); + U_volUnitsData.getMat(ACCESS_RW).copyTo(_volUnitsData); + //_tmp = U_volUnitsData.getMat(ACCESS_RW); + //_tmp.copyTo(_volUnitsData); + //_tmp.release(); //cv::Mat diff; //cv::compare(checking, _volUnitsData, diff, cv::CMP_NE); @@ -1215,9 +1217,19 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //Affine3f cam2vol(Affine3f(subvolumePose) * Affine3f(cameraPose)); Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); _indexes.updateActive(tsdf_idx, 1); - ocl::KernelArg pose = ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)); - posesGPU.at(idx, 0) = pose; - //ocl::KernelArg pose; + + auto vol2camMatrix = vol2cam.matrix.val; + for (int i = 0; i < 16; i++) + { + posesGPU.at(idx, i) = vol2camMatrix[i]; + } + /* + printf(" CPU | %f %f %f %f | %f %f %f %f | %f %f %f %f | %f %f %f %f |\n", + vol2camMatrix[0], vol2camMatrix[1], vol2camMatrix[2], vol2camMatrix[3], + vol2camMatrix[4], vol2camMatrix[5], vol2camMatrix[6], vol2camMatrix[7], + vol2camMatrix[8], vol2camMatrix[9], vol2camMatrix[10], vol2camMatrix[11], + vol2camMatrix[12], vol2camMatrix[13], vol2camMatrix[14], vol2camMatrix[15]); + */ _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) { diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 0707abc0510..cf92fbcb22d 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -163,6 +163,8 @@ void integrateVolumeUnit( if(x >= volResolution.x || y >= volResolution.y) return; + //printf("ok_1 \n"); + // coord-independent constants const int3 volDims = volDims4.xyz; const float2 limits = (float2)(depth_cols-1, depth_rows-1); @@ -189,26 +191,32 @@ void integrateVolumeUnit( int startZ, endZ; if(fabs(zStep.z) > 1e-5) { + //printf("test_1 \n"); int baseZ = convert_int(-basePt.z / zStep.z); if(zStep.z > 0) { + //printf("test_11 \n"); startZ = baseZ; endZ = volResolution.z; } else { + //printf("test_12 \n"); startZ = 0; endZ = baseZ; } } else { + //printf("test_2 \n"); if(basePt.z > 0) { + //printf("test_21 \n"); startZ = 0; endZ = volResolution.z; } else { + //printf("test_22 \n"); // z loop shouldn't be performed //startZ = endZ = 0; return; @@ -218,6 +226,8 @@ void integrateVolumeUnit( startZ = max(0, startZ); endZ = min(volResolution.z, endZ); + //printf("ok_2 \n"); + for(int z = startZ; z < endZ; z++) { // optimization of the following: @@ -229,12 +239,15 @@ void integrateVolumeUnit( float3 camPixVec = camSpacePt / camSpacePt.z; float2 projected = mad(camPixVec.xy, fxy, cxy); + //printf("fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); + float v; // bilinearly interpolate depth at projected - //printf("bilinearly\n"); + //printf("projected = [%f, %f] \n",projected[0],projected[1] ); if(all(projected >= 0) && all(projected < limits)) { + //printf("<----- lol -----> \n"); float2 ip = floor(projected); int xi = ip.x, yi = ip.y; @@ -273,7 +286,7 @@ void integrateVolumeUnit( float sdf = pixNorm*(v*dfac - camSpacePt.z); // possible alternative is: // float sdf = length(camSpacePt)*(v*dfac/camSpacePt.z - 1.0); - //printf("sdf \n"); + printf("sdf \n"); if(sdf >= -truncDist) { float tsdf = fmin(1.0f, sdf * truncDistInv); @@ -310,7 +323,8 @@ __kernel void integrateAllVolumeUnits( int table_step, int table_offset, int table_rows, int table_cols, __global const float * pixNorms, - __global const float16 * allVol2camMatrix, + //__global const float16 * allVol2camMatrix, + __global const float * allVol2camMatrix, int val2cam_step, int val2cam_offset, int val2cam_rows, int val2cam_cols, const int lastVolIndex, @@ -327,7 +341,8 @@ __kernel void integrateAllVolumeUnits( //printf("start \n"); //printf(" | step, offset = %d %d \n | rows, cols = %d %d \n", table_step, table_offset, table_rows, table_cols); //printf(" | step, offset = %d %d \n | rows, cols = %d %d \n", val2cam_step, val2cam_offset, val2cam_rows, val2cam_cols); - + //printf("fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); + int i = get_global_id(0); int j = get_global_id(1); int k = get_global_id(2); @@ -336,6 +351,7 @@ __kernel void integrateAllVolumeUnits( int row = findRow(hash_table, v, list_size, bufferNums, hash_divisor); if (row < 0 || row > lastVolIndex-1) return; + //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | row = %d \n", i, j, k, v[0], v[1], v[2], row); //printf("maxWeight = %d \n", maxWeight); @@ -350,9 +366,50 @@ __kernel void integrateAllVolumeUnits( __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) (allVolumePtr + table_offset + (row)*table_step); + + __global const float * p_vol2camMatrix = (__global const float *) + (allVol2camMatrix + val2cam_offset + (row) * val2cam_step); + + + const float16 vol2camMatrix = (float16) + (p_vol2camMatrix[0], p_vol2camMatrix[1], p_vol2camMatrix[2], p_vol2camMatrix[3], + p_vol2camMatrix[4], p_vol2camMatrix[5], p_vol2camMatrix[6], p_vol2camMatrix[7], + p_vol2camMatrix[8], p_vol2camMatrix[9], p_vol2camMatrix[10], p_vol2camMatrix[11], + p_vol2camMatrix[12], p_vol2camMatrix[13], p_vol2camMatrix[14], p_vol2camMatrix[15]); + /* + const float16 vol2camMatrix = (float16) + (0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0); + */ + + /* //const float16 vol2camMatrix = allVol2camMatrix[row]; - const float16 vol2camMatrix = allVol2camMatrix[val2cam_offset + (row) * val2cam_step]; - + //const float16 vol2camMatrix = allVol2camMatrix[val2cam_offset + (row) * val2cam_step]; + __global const float16 * p_vol2camMatrix = (__global const float16*) + (allVol2camMatrix + val2cam_offset + (row) * val2cam_step); + const float16 vol2camMatrix = *(p_vol2camMatrix); +*/ +/* + printf(" GPU | %f %f %f %f | %f %f %f %f | %f %f %f %f | %f %f %f %f |\n", + vol2camMatrix[0], vol2camMatrix[1], vol2camMatrix[2], vol2camMatrix[3], + vol2camMatrix[4], vol2camMatrix[5], vol2camMatrix[6], vol2camMatrix[7], + vol2camMatrix[8], vol2camMatrix[9], vol2camMatrix[10], vol2camMatrix[11], + vol2camMatrix[12], vol2camMatrix[13], vol2camMatrix[14], vol2camMatrix[15]); +*/ + + /* + const float4 vol2cam0 = vol2camMatrix.s0123; + const float4 vol2cam1 = vol2camMatrix.s4567; + const float4 vol2cam2 = vol2camMatrix.s89ab; + printf(" {GPU} | %f %f %f %f | %f %f %f %f | %f %f %f %f |\n", + vol2cam0[0], vol2cam0[1], vol2cam0[2], vol2cam0[3], + vol2cam1[0], vol2cam1[1], vol2cam1[2], vol2cam1[3]); + vol2cam2[0], vol2cam2[1], vol2cam2[2], vol2cam2[3]); + */ + + integrateVolumeUnit( i, j, depthptr, From 9015bef9d887bcc484720223e032127cdbec877c Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 2 Dec 2020 13:30:18 +0300 Subject: [PATCH 060/216] try to fix --- modules/rgbd/src/hash_tsdf.cpp | 3 ++- modules/rgbd/src/opencl/hash_tsdf.cl | 10 ++++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index f11b8d542e3..3b9b38e184c 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -864,7 +864,7 @@ void HashTSDFVolumeGPU::reset() poses = cv::Mat(buff_lvl, 1, rawType()); lastVisibleIndexes = cv::Mat(buff_lvl, 1, rawType()); _indexes = VolumesTable(); - posesGPU = cv::Mat(buff_lvl, 16, rawType()); + posesGPU = cv::Mat(buff_lvl, 16, rawType()); } static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) @@ -1223,6 +1223,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { posesGPU.at(idx, i) = vol2camMatrix[i]; } + /* printf(" CPU | %f %f %f %f | %f %f %f %f | %f %f %f %f | %f %f %f %f |\n", vol2camMatrix[0], vol2camMatrix[1], vol2camMatrix[2], vol2camMatrix[3], diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index cf92fbcb22d..2a93f85a9e1 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -238,13 +238,13 @@ void integrateVolumeUnit( continue; float3 camPixVec = camSpacePt / camSpacePt.z; - float2 projected = mad(camPixVec.xy, fxy, cxy); - //printf("fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); - + float2 projected = mad(camPixVec.xy, fxy, cxy); // mad(a,b,c) = a * b + c + printf("GPU: fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); + printf("GPU: camSpacePt = [%f, %f, %f] \n camPixVec = [%f, %f, %f] \n",camSpacePt[0], camSpacePt[1], camSpacePt[2], camPixVec[0], camPixVec[1], camPixVec[2]); float v; // bilinearly interpolate depth at projected - //printf("projected = [%f, %f] \n",projected[0],projected[1] ); + printf("projected = [%f, %f] \n",projected[0],projected[1] ); if(all(projected >= 0) && all(projected < limits)) { //printf("<----- lol -----> \n"); @@ -314,7 +314,6 @@ __kernel void integrateAllVolumeUnits( int depth_step, int depth_offset, int depth_rows, int depth_cols, __global struct Volume_NODE * hash_table, - const int list_size, const int bufferNums, const int hash_divisor, @@ -323,7 +322,6 @@ __kernel void integrateAllVolumeUnits( int table_step, int table_offset, int table_rows, int table_cols, __global const float * pixNorms, - //__global const float16 * allVol2camMatrix, __global const float * allVol2camMatrix, int val2cam_step, int val2cam_offset, int val2cam_rows, int val2cam_cols, From fc0ce5142beefce885570ae70b7c71e9564e3286 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 2 Dec 2020 17:40:17 +0300 Subject: [PATCH 061/216] we are the champions --- modules/rgbd/src/hash_tsdf.cpp | 5 +++-- modules/rgbd/src/opencl/hash_tsdf.cl | 13 ++++++++----- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 3b9b38e184c..6bc2b9a29a9 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1097,11 +1097,12 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept //int nz = cv::countNonZero(diff); //if (nz == 0) std::cout << "compare = " << true << std::endl; + /* if (matIsEqual(checking, _volUnitsData)) std::cout << "compare = " << true << std::endl; else std::cout << "compare = " << false << std::endl; - + */ //Mat _tmp = _volUnitsData; //(U_volUnitsData.getMat(ACCESS_RW)).copyTo(_volUnitsData); //Mat tmp = U_volUnitsData.getMat(ACCESS_RW); @@ -1215,7 +1216,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma poses.at(idx, 0) = subvolumePose; lastVisibleIndexes.at(idx, 0) = frameId; //Affine3f cam2vol(Affine3f(subvolumePose) * Affine3f(cameraPose)); - Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); _indexes.updateActive(tsdf_idx, 1); auto vol2camMatrix = vol2cam.matrix.val; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 2a93f85a9e1..b47789b970e 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -239,12 +239,15 @@ void integrateVolumeUnit( float3 camPixVec = camSpacePt / camSpacePt.z; float2 projected = mad(camPixVec.xy, fxy, cxy); // mad(a,b,c) = a * b + c - printf("GPU: fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); - printf("GPU: camSpacePt = [%f, %f, %f] \n camPixVec = [%f, %f, %f] \n",camSpacePt[0], camSpacePt[1], camSpacePt[2], camPixVec[0], camPixVec[1], camPixVec[2]); + //printf("GPU: fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); + //printf("GPU: camSpacePt = [%f, %f, %f] \n camPixVec = [%f, %f, %f] \n",camSpacePt[0], camSpacePt[1], camSpacePt[2], camPixVec[0], camPixVec[1], camPixVec[2]); + + //if(camPixVec[0] > 0 && camPixVec[1] > 0) printf("GPU: camSpacePt = [%f, %f, %f] \n camPixVec = [%f, %f, %f] \n",camSpacePt[0], camSpacePt[1], camSpacePt[2], camPixVec[0], camPixVec[1], camPixVec[2]); + float v; // bilinearly interpolate depth at projected - printf("projected = [%f, %f] \n",projected[0],projected[1] ); + //printf("projected = [%f, %f] \n",projected[0],projected[1] ); if(all(projected >= 0) && all(projected < limits)) { //printf("<----- lol -----> \n"); @@ -286,7 +289,7 @@ void integrateVolumeUnit( float sdf = pixNorm*(v*dfac - camSpacePt.z); // possible alternative is: // float sdf = length(camSpacePt)*(v*dfac/camSpacePt.z - 1.0); - printf("sdf \n"); + //printf("sdf \n"); if(sdf >= -truncDist) { float tsdf = fmin(1.0f, sdf * truncDistInv); @@ -395,7 +398,7 @@ __kernel void integrateAllVolumeUnits( vol2camMatrix[4], vol2camMatrix[5], vol2camMatrix[6], vol2camMatrix[7], vol2camMatrix[8], vol2camMatrix[9], vol2camMatrix[10], vol2camMatrix[11], vol2camMatrix[12], vol2camMatrix[13], vol2camMatrix[14], vol2camMatrix[15]); -*/ +*/ /* const float4 vol2cam0 = vol2camMatrix.s0123; From 307d5bac15f9853e9e6c178aad46f05f8cba7948 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 3 Dec 2020 11:52:45 +0300 Subject: [PATCH 062/216] remove some comments --- modules/rgbd/src/hash_tsdf.cpp | 131 +-------------------------------- 1 file changed, 4 insertions(+), 127 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 6bc2b9a29a9..967266d4f5d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -934,85 +934,9 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f return pixNorm; } -/* -void HashTSDFVolumeGPU::integrateVolumeUnitGPU( InputArray _depth, float depthFactor, - const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx) -{ - CV_TRACE_FUNCTION(); - CV_Assert(!_depth.empty()); - - UMat depth = _depth.getUMat(); - - String errorStr; - String name = "integrateVolumeUnit"; - ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; - String options = "-cl-mad-enable"; - ocl::Kernel k; - k.create(name.c_str(), source, options, &errorStr); - - if (k.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); - - Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); - float dfac = 1.f / depthFactor; - Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); - Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - - // TODO: optimization possible - // Use sampler for depth (mask needed) - k.args(ocl::KernelArg::ReadOnly(depth), - //ocl::KernelArg::PtrReadWrite(volUnitsData.row(idx).getUMat(ACCESS_RW)), - ocl::KernelArg::PtrReadWrite(_volUnitsData.row(idx).getUMat(ACCESS_RW)), - ocl::KernelArg::Constant(vol2cam.matrix.val, - sizeof(vol2cam.matrix.val)), - voxelSize, - volResGpu.val, - volStrides.val, - fxy.val, - cxy.val, - dfac, - truncDist, - int(maxWeight), - ocl::KernelArg::PtrReadOnly(_pixNorms)); - - size_t globalSize[2]; - globalSize[0] = (size_t)volumeUnitResolution; - globalSize[1] = (size_t)volumeUnitResolution; - - if (!k.run(2, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); -} -*/ - -bool matIsEqual(const cv::Mat Mat1, const cv::Mat Mat2) -{ - if (Mat1.dims == Mat2.dims && - Mat1.size == Mat2.size && - Mat1.elemSize() == Mat2.elemSize()) - { - if (Mat1.isContinuous() && Mat2.isContinuous()) - { - return 0 == memcmp(Mat1.ptr(), Mat2.ptr(), Mat1.total() * Mat1.elemSize()); - } - else - { - const cv::Mat* arrays[] = { &Mat1, &Mat2, 0 }; - uchar* ptrs[2]; - cv::NAryMatIterator it(arrays, ptrs, 2); - for (unsigned int p = 0; p < it.nplanes; p++, ++it) - if (0 != memcmp(it.ptrs[0], it.ptrs[1], it.size * Mat1.elemSize())) - return false; - - return true; - } - } - - return false; -} void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics) { - //std::cout << "integrateAllVolumeUnitsGPU" << std::endl; CV_TRACE_FUNCTION(); CV_Assert(!_depth.empty()); @@ -1028,7 +952,6 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); - //Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); float dfac = 1.f / depthFactor; Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); @@ -1036,16 +959,9 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept int totalVolUnitsSize = _indexes.indexesGPU.size(); Mat totalVolUnits(_indexes.indexesGPU, rawType()); - //std::cout << Vec3i(7,7,1) << " = " << _indexes.find_Volume(Vec3i(7, 7, 1)) << - // " | " << calc_hash(Vec4i(7, 7, 1, 0)) % _indexes.hash_divisor<< std::endl; - - //std::cout << calc_hash(Vec3i(7, 7, 1)) << std::endl; - //std::cout << " lol =" << _indexes.list_size<<" "<< _indexes.bufferNums << " " << _indexes.hash_divisor << std::endl; - //std::cout << "maxWeight == " << maxWeight << std::endl; Mat _tmp; _volUnitsData.copyTo(_tmp); UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); - //UMat U_volUnitsData = _volUnitsData.getUMat(ACCESS_RW); k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), @@ -1053,13 +969,9 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept (int)_indexes.bufferNums, (int)_indexes.hash_divisor, ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), - //ocl::KernelArg::PtrReadWrite(_volUnitsData.getUMat(ACCESS_RW)), - //ocl::KernelArg::ReadWrite(_volUnitsData.getUMat(ACCESS_RW)), ocl::KernelArg::ReadWrite(U_volUnitsData), ocl::KernelArg::PtrReadOnly(_pixNorms), - //ocl::KernelArg::PtrReadOnly(pixNorms.getUMat(ACCESS_RW)), ocl::KernelArg::ReadOnly(posesGPU.getUMat(ACCESS_READ)), - //ocl::KernelArg::ReadOnly(poses.getUMat(ACCESS_READ)), _lastVolIndex, voxelSize, volResGpu.val, @@ -1070,54 +982,21 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept truncDist, int(maxWeight) ); - //std::cout << _indexes - //int resol = 1; + int resol = volumeUnitResolution; size_t globalSize[3]; globalSize[0] = (size_t)resol; // volumeUnitResolution globalSize[1] = (size_t)resol; // volumeUnitResolution - //globalSize[0] = (size_t)volResGpu.val[0]; // volResolution.x - //globalSize[1] = (size_t)volResGpu.val[1]; // volResolution.y globalSize[2] = (size_t)totalVolUnitsSize; // num of voxels - //printf("CPU: fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); - //std::cout << "rmaxWeight == " << maxWeight << std::endl; + if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); - - Mat checking; - _volUnitsData.copyTo(checking); U_volUnitsData.getMat(ACCESS_RW).copyTo(_volUnitsData); - //_tmp = U_volUnitsData.getMat(ACCESS_RW); - //_tmp.copyTo(_volUnitsData); - //_tmp.release(); - - //cv::Mat diff; - //cv::compare(checking, _volUnitsData, diff, cv::CMP_NE); - //int nz = cv::countNonZero(diff); - - //if (nz == 0) std::cout << "compare = " << true << std::endl; - /* - if (matIsEqual(checking, _volUnitsData)) - std::cout << "compare = " << true << std::endl; - else - std::cout << "compare = " << false << std::endl; - */ - //Mat _tmp = _volUnitsData; - //(U_volUnitsData.getMat(ACCESS_RW)).copyTo(_volUnitsData); - //Mat tmp = U_volUnitsData.getMat(ACCESS_RW); - - //tmp.copyTo(_volUnitsData); - //U_volUnitsData.release(); - //tmp.release(); - //bool res = false; - //if (_tmp == _volUnitsData) res = true; - //std::cout << res << std::endl; } void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { - //std::cout << "integrate" << std::endl; CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); @@ -1139,7 +1018,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma auto AllocateVolumeUnitsInvoker = [&](const Range& range) { _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(VOLUMES_SIZE, 1, rawType()); - for (int y = range.start; y < range.end; y += depthStride) { const depthType* depthRow = depth[y]; @@ -1170,8 +1048,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } - //mutex.lock(); - //std::cout << "lol\n"; + mutex.lock(); for (int i = 0; i < loc_vol_idx; i++) { Vec3i idx = _localAccessVolUnits.at(i, 0); @@ -1198,7 +1075,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma _lastVolIndex++; } } - //mutex.unlock(); + mutex.unlock(); }; //parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); From 4fd73d381fc690097cf51b7bea775c8debfcaa0e Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 3 Dec 2020 12:22:21 +0300 Subject: [PATCH 063/216] remove last comments --- modules/rgbd/src/hash_tsdf.cpp | 25 ++-------- modules/rgbd/src/opencl/hash_tsdf.cl | 75 +--------------------------- 2 files changed, 4 insertions(+), 96 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 967266d4f5d..b9136c12407 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1057,7 +1057,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { if (_lastVolIndex >= buff_lvl) { - //std::cout << "kek " << _lastVolIndex << " " << buff_lvl << std::endl; degree++; buff_lvl = pow(2, degree); indexes.resize(buff_lvl); @@ -1082,7 +1081,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma AllocateVolumeUnitsInvoker(allocateRange); //! Perform the allocation - //std::cout << "lol\n"; for (int i = 0; i < vol_idx; i++) { Vec3i tsdf_idx = newIndices.at(i, 0); @@ -1092,7 +1090,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma poses.at(idx, 0) = subvolumePose; lastVisibleIndexes.at(idx, 0) = frameId; - //Affine3f cam2vol(Affine3f(subvolumePose) * Affine3f(cameraPose)); Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); _indexes.updateActive(tsdf_idx, 1); @@ -1102,14 +1099,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma posesGPU.at(idx, i) = vol2camMatrix[i]; } - /* - printf(" CPU | %f %f %f %f | %f %f %f %f | %f %f %f %f | %f %f %f %f |\n", - vol2camMatrix[0], vol2camMatrix[1], vol2camMatrix[2], vol2camMatrix[3], - vol2camMatrix[4], vol2camMatrix[5], vol2camMatrix[6], vol2camMatrix[7], - vol2camMatrix[8], vol2camMatrix[9], vol2camMatrix[10], vol2camMatrix[11], - vol2camMatrix[12], vol2camMatrix[13], vol2camMatrix[14], vol2camMatrix[15]); - */ - _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) { TsdfVoxel& v = reinterpret_cast(vv); @@ -1120,7 +1109,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma TsdfVoxel& v = reinterpret_cast(vv); v.tsdf = floatToTsdf(0.0f); v.weight = 0; }); - //volUnitsData.at(idx, 0) = cv::UMat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); } //! Get keys for all the allocated volume Units @@ -1137,7 +1125,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Vec3i tsdf_idx = _totalVolUnits[i]; VolumeIndex idx = _indexes.find_Volume(tsdf_idx); - //std::cout << tsdf_idx << " " << idx << std::endl; if (idx < 0 || idx == _lastVolIndex - 1) return; Point3f volumeUnitPos = volumeUnitIdxToVolume(poses.at(idx, 0)); @@ -1153,13 +1140,12 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { assert(idx == _lastVolIndex - 1); lastVisibleIndexes.at(idx, 0) = frameId; - std::cout << " - " << tsdf_idx << std::endl; _indexes.update(tsdf_idx, 1, frameId); } } }; - //parallel_for_(_inFrustumRange, markActivities); - markActivities(_inFrustumRange); + parallel_for_(_inFrustumRange, markActivities); + //markActivities(_inFrustumRange); Vec6f newParams((float)depth.rows, (float)depth.cols, intrinsics.fx, intrinsics.fy, @@ -1182,12 +1168,10 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma VolumeIndex idx = _indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == _lastVolIndex - 1) return; - //bool& _isActive = isActive.at(idx, 0); bool _isActive = _indexes.getActive(tsdf_idx); - //std::cout << _isActive; + if (_isActive) { - //std::cout << " lol "; //! The volume unit should already be added into the Volume from the allocator Matx44f _pose = poses.at(idx, 0); @@ -1197,7 +1181,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); //! Ensure all active volumeUnits are set to inactive for next integration - //_isActive = false; _indexes.updateActive(tsdf_idx, 0); } } @@ -1206,11 +1189,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); Integrate(Range(0, (int)_totalVolUnits.size())); } - //std::cout << "lol\n"; else integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); - //std::cout << "lol\n"; } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index b47789b970e..fae30314d33 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -66,9 +66,7 @@ int findRow(__global struct Volume_NODE * hash_table, int4 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; - //printf("\nhash = %d \n", hash); - //printf("%d | %d \n", calc_hash(indx), hash); int num = 1; int i = hash * num * list_size; int NAN_NUM = -2147483647; @@ -94,7 +92,7 @@ int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, int num = 1; int i = hash * num * list_size; int NAN_NUM = -2147483647; - //struct Volume_NODE tmp; + while (i != NAN_NUM) { struct Volume_NODE v = hash_table[i]; @@ -102,11 +100,7 @@ int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, if (v.idx[0] == indx[0] && v.idx[1] == indx[1] && v.idx[2] == indx[2]) - { - //printf("idx = [%d %d %d] | idx = [%d %d %d] \n nextVolumeRow = %d \n", indx[0],indx[1],indx[2],v->idx[0], v->idx[1], v->idx[2], v->nextVolumeRow); - return v.isActive; - } if (v.idx[0] == NAN_NUM) return 0; i = v.nextVolumeRow; @@ -155,16 +149,11 @@ void integrateVolumeUnit( const int maxWeight ) { - //int x = get_global_id(0); - //int y = get_global_id(1); - //printf("integrateVolumeUnit \n"); const int3 volResolution = volResolution4.xyz; if(x >= volResolution.x || y >= volResolution.y) return; - //printf("ok_1 \n"); - // coord-independent constants const int3 volDims = volDims4.xyz; const float2 limits = (float2)(depth_cols-1, depth_rows-1); @@ -191,32 +180,26 @@ void integrateVolumeUnit( int startZ, endZ; if(fabs(zStep.z) > 1e-5) { - //printf("test_1 \n"); int baseZ = convert_int(-basePt.z / zStep.z); if(zStep.z > 0) { - //printf("test_11 \n"); startZ = baseZ; endZ = volResolution.z; } else { - //printf("test_12 \n"); startZ = 0; endZ = baseZ; } } else { - //printf("test_2 \n"); if(basePt.z > 0) { - //printf("test_21 \n"); startZ = 0; endZ = volResolution.z; } else { - //printf("test_22 \n"); // z loop shouldn't be performed //startZ = endZ = 0; return; @@ -226,8 +209,6 @@ void integrateVolumeUnit( startZ = max(0, startZ); endZ = min(volResolution.z, endZ); - //printf("ok_2 \n"); - for(int z = startZ; z < endZ; z++) { // optimization of the following: @@ -239,18 +220,11 @@ void integrateVolumeUnit( float3 camPixVec = camSpacePt / camSpacePt.z; float2 projected = mad(camPixVec.xy, fxy, cxy); // mad(a,b,c) = a * b + c - //printf("GPU: fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); - //printf("GPU: camSpacePt = [%f, %f, %f] \n camPixVec = [%f, %f, %f] \n",camSpacePt[0], camSpacePt[1], camSpacePt[2], camPixVec[0], camPixVec[1], camPixVec[2]); - - //if(camPixVec[0] > 0 && camPixVec[1] > 0) printf("GPU: camSpacePt = [%f, %f, %f] \n camPixVec = [%f, %f, %f] \n",camSpacePt[0], camSpacePt[1], camSpacePt[2], camPixVec[0], camPixVec[1], camPixVec[2]); - float v; // bilinearly interpolate depth at projected - //printf("projected = [%f, %f] \n",projected[0],projected[1] ); if(all(projected >= 0) && all(projected < limits)) { - //printf("<----- lol -----> \n"); float2 ip = floor(projected); int xi = ip.x, yi = ip.y; @@ -289,7 +263,6 @@ void integrateVolumeUnit( float sdf = pixNorm*(v*dfac - camSpacePt.z); // possible alternative is: // float sdf = length(camSpacePt)*(v*dfac/camSpacePt.z - 1.0); - //printf("sdf \n"); if(sdf >= -truncDist) { float tsdf = fmin(1.0f, sdf * truncDistInv); @@ -298,10 +271,8 @@ void integrateVolumeUnit( struct TsdfVoxel voxel = volumeptr[volIdx]; float value = tsdfToFloat(voxel.tsdf); int weight = voxel.weight; - //printf(" v = %f \n", value); // update TSDF value = (value*weight + tsdf) / (weight + 1); - //printf(" v = %f \n", value); weight = min(weight + 1, maxWeight); voxel.tsdf = floatToTsdf(value); @@ -339,11 +310,6 @@ __kernel void integrateAllVolumeUnits( const int maxWeight ) { - //printf("start \n"); - //printf(" | step, offset = %d %d \n | rows, cols = %d %d \n", table_step, table_offset, table_rows, table_cols); - //printf(" | step, offset = %d %d \n | rows, cols = %d %d \n", val2cam_step, val2cam_offset, val2cam_rows, val2cam_cols); - //printf("fxy = [%f, %f] | cxy = [%f, %f] \n", fxy[0], fxy[1], cxy[0], cxy[1]); - int i = get_global_id(0); int j = get_global_id(1); int k = get_global_id(2); @@ -353,21 +319,15 @@ __kernel void integrateAllVolumeUnits( if (row < 0 || row > lastVolIndex-1) return; - //printf("[i, j, k] = [%d , %d , %d] | idx = [x, y, z] = [%d, %d, %d] | row = %d \n", i, j, k, v[0], v[1], v[2], row); - //printf("maxWeight = %d \n", maxWeight); - int isActive = getIsActive(hash_table, v, list_size, bufferNums, hash_divisor); if (isActive == 1) { - //printf("lol"); int resol = volResolution4[0] * volResolution4[1] * volResolution4[2]; - //__global struct TsdfVoxel * volumeptr = (allVolumePtr+(row*resol)); __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) (allVolumePtr + table_offset + (row)*table_step); - __global const float * p_vol2camMatrix = (__global const float *) (allVol2camMatrix + val2cam_offset + (row) * val2cam_step); @@ -377,39 +337,6 @@ __kernel void integrateAllVolumeUnits( p_vol2camMatrix[4], p_vol2camMatrix[5], p_vol2camMatrix[6], p_vol2camMatrix[7], p_vol2camMatrix[8], p_vol2camMatrix[9], p_vol2camMatrix[10], p_vol2camMatrix[11], p_vol2camMatrix[12], p_vol2camMatrix[13], p_vol2camMatrix[14], p_vol2camMatrix[15]); - /* - const float16 vol2camMatrix = (float16) - (0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0); - */ - - /* - //const float16 vol2camMatrix = allVol2camMatrix[row]; - //const float16 vol2camMatrix = allVol2camMatrix[val2cam_offset + (row) * val2cam_step]; - __global const float16 * p_vol2camMatrix = (__global const float16*) - (allVol2camMatrix + val2cam_offset + (row) * val2cam_step); - const float16 vol2camMatrix = *(p_vol2camMatrix); -*/ -/* - printf(" GPU | %f %f %f %f | %f %f %f %f | %f %f %f %f | %f %f %f %f |\n", - vol2camMatrix[0], vol2camMatrix[1], vol2camMatrix[2], vol2camMatrix[3], - vol2camMatrix[4], vol2camMatrix[5], vol2camMatrix[6], vol2camMatrix[7], - vol2camMatrix[8], vol2camMatrix[9], vol2camMatrix[10], vol2camMatrix[11], - vol2camMatrix[12], vol2camMatrix[13], vol2camMatrix[14], vol2camMatrix[15]); -*/ - - /* - const float4 vol2cam0 = vol2camMatrix.s0123; - const float4 vol2cam1 = vol2camMatrix.s4567; - const float4 vol2cam2 = vol2camMatrix.s89ab; - printf(" {GPU} | %f %f %f %f | %f %f %f %f | %f %f %f %f |\n", - vol2cam0[0], vol2cam0[1], vol2cam0[2], vol2cam0[3], - vol2cam1[0], vol2cam1[1], vol2cam1[2], vol2cam1[3]); - vol2cam2[0], vol2cam2[1], vol2cam2[2], vol2cam2[3]); - */ - integrateVolumeUnit( i, j, From 0260138c9715071ac3e8d783f917e998a5ac8ccb Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 3 Dec 2020 12:30:47 +0300 Subject: [PATCH 064/216] 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 b9136c12407..44d574b59e0 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -941,7 +941,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept CV_Assert(!_depth.empty()); UMat depth = _depth.getUMat(); - + String errorStr; String name = "integrateAllVolumeUnits"; ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; @@ -951,7 +951,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); - + float dfac = 1.f / depthFactor; Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); @@ -1092,7 +1092,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma lastVisibleIndexes.at(idx, 0) = frameId; Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); _indexes.updateActive(tsdf_idx, 1); - + auto vol2camMatrix = vol2cam.matrix.val; for (int i = 0; i < 16; i++) { @@ -1123,7 +1123,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = range.start; i < range.end; ++i) { Vec3i tsdf_idx = _totalVolUnits[i]; - + VolumeIndex idx = _indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == _lastVolIndex - 1) return; From 027f0518761960eed00c8fe18bcb9440f3b873ea Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 3 Dec 2020 13:30:07 +0300 Subject: [PATCH 065/216] warning fix --- modules/rgbd/src/hash_tsdf.cpp | 6 ++++-- modules/rgbd/src/tsdf_functions.cpp | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 44d574b59e0..d53bf6e5be3 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1094,9 +1094,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma _indexes.updateActive(tsdf_idx, 1); auto vol2camMatrix = vol2cam.matrix.val; - for (int i = 0; i < 16; i++) + for (int k = 0; k < 16; k++) { - posesGPU.at(idx, i) = vol2camMatrix[i]; + posesGPU.at(idx, k) = vol2camMatrix[k]; } _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) @@ -1159,6 +1159,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //! Integrate the correct volumeUnits + /* if (false) { auto Integrate = [&](const Range& range) { @@ -1190,6 +1191,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Integrate(Range(0, (int)_totalVolUnits.size())); } else + */ integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); } diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 4a156cbb4b4..2b0107a6108 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -533,7 +533,7 @@ bool VolumesTable::getActive(Vec3i indx) const { Volume_NODE v = volumes.at(i, 0); if (v.idx == idx) - return v.isActive; + return bool(v.isActive); if (v.idx[0] == -2147483647) return false; i = v.nextVolumeRow; From 5213c7809bfad8edd5cba04bde7cd42d603905b9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 3 Dec 2020 14:18:14 +0300 Subject: [PATCH 066/216] warning fix 1 --- 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 2b0107a6108..1a2e7a42f55 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -533,7 +533,7 @@ bool VolumesTable::getActive(Vec3i indx) const { Volume_NODE v = volumes.at(i, 0); if (v.idx == idx) - return bool(v.isActive); + return (bool(v.isActive)); if (v.idx[0] == -2147483647) return false; i = v.nextVolumeRow; From 06d2b263ce2f98f3938db834c60c67f7d34b5f23 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 3 Dec 2020 15:06:06 +0300 Subject: [PATCH 067/216] warning fix 2 --- 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 1a2e7a42f55..3cedad37099 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -533,7 +533,7 @@ bool VolumesTable::getActive(Vec3i indx) const { Volume_NODE v = volumes.at(i, 0); if (v.idx == idx) - return (bool(v.isActive)); + return (v.isActive == 1); if (v.idx[0] == -2147483647) return false; i = v.nextVolumeRow; From 85296cc9f6288e7fcb5e401164ac1a9ba82a6eec Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 4 Dec 2020 11:04:18 +0300 Subject: [PATCH 068/216] create raycast kernel --- modules/rgbd/src/hash_tsdf.cpp | 91 +++++++++++++++++++--------- modules/rgbd/src/opencl/hash_tsdf.cl | 14 +++++ 2 files changed, 76 insertions(+), 29 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d53bf6e5be3..925fc05d2c1 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1160,37 +1160,34 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! Integrate the correct volumeUnits /* - if (false) - { - auto Integrate = [&](const Range& range) { - for (int i = range.start; i < range.end; i++) - { - Vec3i tsdf_idx = _totalVolUnits[i]; - VolumeIndex idx = _indexes.find_Volume(tsdf_idx); - if (idx < 0 || idx == _lastVolIndex - 1) return; + auto Integrate = [&](const Range& range) { + for (int i = range.start; i < range.end; i++) + { + Vec3i tsdf_idx = _totalVolUnits[i]; + VolumeIndex idx = _indexes.find_Volume(tsdf_idx); + if (idx < 0 || idx == _lastVolIndex - 1) return; - bool _isActive = _indexes.getActive(tsdf_idx); + bool _isActive = _indexes.getActive(tsdf_idx); - if (_isActive) - { - //! The volume unit should already be added into the Volume from the allocator - Matx44f _pose = poses.at(idx, 0); + if (_isActive) + { + //! The volume unit should already be added into the Volume from the allocator + Matx44f _pose = poses.at(idx, 0); - integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, - Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, - depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); + integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, + Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, + depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); - //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); - //! Ensure all active volumeUnits are set to inactive for next integration - _indexes.updateActive(tsdf_idx, 0); - } + //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); + //! Ensure all active volumeUnits are set to inactive for next integration + _indexes.updateActive(tsdf_idx, 0); } - }; + } + }; + + //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); + Integrate(Range(0, (int)_totalVolUnits.size())); - //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); - Integrate(Range(0, (int)_totalVolUnits.size())); - } - else */ integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); @@ -1470,11 +1467,11 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in _points.create(frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE); - Points points1 = _points.getMat(); - Normals normals1 = _normals.getMat(); + Points points = _points.getMat(); + Normals normals = _normals.getMat(); - Points& new_points(points1); - Normals& new_normals(normals1); + Points& new_points(points); + Normals& new_normals(normals); const HashTSDFVolumeGPU& volume(*this); const float tstep(volume.truncDist * volume.raycastStepFactor); @@ -1582,6 +1579,42 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); //_HashRaycastInvoker(Range(0, new_points.rows)); + + + { + String errorStr; + String name = "raycast"; + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); + + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); + + int totalVolUnitsSize = _indexes.indexesGPU.size(); + Mat totalVolUnits(_indexes.indexesGPU, rawType()); + + k.args( + ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), + (int)_indexes.list_size, + (int)_indexes.bufferNums, + (int)_indexes.hash_divisor, + ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), + ocl::KernelArg::PtrReadWrite(points.getUMat(ACCESS_RW)), + ocl::KernelArg::PtrReadWrite(normals.getUMat(ACCESS_RW)) + ); + + //int resol = new_points.rows; + int resol = 1; + size_t globalSize[1]; + globalSize[0] = (size_t)resol; // num of points + + if (!k.run(1, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); + } + + } void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index fae30314d33..bbd4a3d8bb1 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -359,3 +359,17 @@ __kernel void integrateAllVolumeUnits( } } + + +__kernel void raycast( + __global struct Volume_NODE * hash_table, + const int list_size, + const int bufferNums, + const int hash_divisor, + __global const int4 * totalVolUnits, + __global float4 * points, + __global float4 * normals + ) +{ + printf("raycast_GPU \n"); +} \ No newline at end of file From df69ed75d5b29b477fdd7c18a42da7a70f3af652 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 4 Dec 2020 12:01:41 +0300 Subject: [PATCH 069/216] extend raycast gpu signature --- modules/rgbd/src/hash_tsdf.cpp | 13 +++++-------- modules/rgbd/src/opencl/hash_tsdf.cl | 11 +++++++++-- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 925fc05d2c1..b1539624ebe 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1159,7 +1159,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //! Integrate the correct volumeUnits - /* auto Integrate = [&](const Range& range) { for (int i = range.start; i < range.end; i++) { @@ -1186,11 +1185,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma }; //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); - Integrate(Range(0, (int)_totalVolUnits.size())); + //Integrate(Range(0, (int)_totalVolUnits.size())); - */ integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); - } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const @@ -1601,12 +1598,12 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in (int)_indexes.bufferNums, (int)_indexes.hash_divisor, ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), - ocl::KernelArg::PtrReadWrite(points.getUMat(ACCESS_RW)), - ocl::KernelArg::PtrReadWrite(normals.getUMat(ACCESS_RW)) + ocl::KernelArg::ReadWrite(points.getUMat(ACCESS_RW)), + ocl::KernelArg::ReadWrite(normals.getUMat(ACCESS_RW)) ); - //int resol = new_points.rows; - int resol = 1; + int resol = new_points.rows; + //int resol = 1; size_t globalSize[1]; globalSize[0] = (size_t)resol; // num of points diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index bbd4a3d8bb1..438d70ebb98 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -368,8 +368,15 @@ __kernel void raycast( const int hash_divisor, __global const int4 * totalVolUnits, __global float4 * points, - __global float4 * normals + int points_step, int points_offset, + int points_rows, int points_cols, + __global float4 * normals, + int normals_step, int normals_offset, + int normals_rows, int normals_cols ) { - printf("raycast_GPU \n"); + int i = get_global_id(0); + //printf("raycast_GPU \n"); + float4 point = points[points_offset + (i) * points_step]; + //printf("point (%d) = [%f, %f, %f, %f] \n", i, point[0], point[1], point[2], point[3]); } \ No newline at end of file From 5d530257093ae4ba8053846b9000849967ce5f18 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 8 Dec 2020 11:12:59 +0300 Subject: [PATCH 070/216] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 14 ++++++++++++-- modules/rgbd/src/opencl/hash_tsdf.cl | 21 ++++++++++++++------- modules/rgbd/test/test_tsdf.cpp | 2 +- 3 files changed, 27 insertions(+), 10 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b1539624ebe..fe35fe1b982 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -962,13 +962,19 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Mat _tmp; _volUnitsData.copyTo(_tmp); UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); + _tmp.release(); + + _indexes.volumes.copyTo(_tmp); + UMat U_hashtable = _tmp.getUMat(ACCESS_RW); + k.args(ocl::KernelArg::ReadOnly(depth), - ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), + //ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), + ocl::KernelArg::PtrReadWrite(U_hashtable), (int)_indexes.list_size, (int)_indexes.bufferNums, (int)_indexes.hash_divisor, - ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), + ocl::KernelArg::ReadWrite(totalVolUnits.getUMat(ACCESS_RW)), ocl::KernelArg::ReadWrite(U_volUnitsData), ocl::KernelArg::PtrReadOnly(_pixNorms), ocl::KernelArg::ReadOnly(posesGPU.getUMat(ACCESS_READ)), @@ -993,6 +999,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept throw std::runtime_error("Failed to run kernel"); U_volUnitsData.getMat(ACCESS_RW).copyTo(_volUnitsData); + U_hashtable.getMat(ACCESS_RW).copyTo(_indexes.volumes); } void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) @@ -1188,6 +1195,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //Integrate(Range(0, (int)_totalVolUnits.size())); integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); + } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const @@ -1455,6 +1463,8 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const return nv < 0.0001f ? nan3 : normal / nv; } + + void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals) const { diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 438d70ebb98..4594512839f 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -291,7 +291,9 @@ __kernel void integrateAllVolumeUnits( const int list_size, const int bufferNums, const int hash_divisor, - __global const int4 * totalVolUnits, + __global const int4 * totalVolUnits, + int totalVolUnits_step, int totalVolUnits_offset, + int totalVolUnits_rows, int totalVolUnits_cols, __global struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, int table_rows, int table_cols, @@ -320,23 +322,28 @@ __kernel void integrateAllVolumeUnits( return; int isActive = getIsActive(hash_table, v, list_size, bufferNums, hash_divisor); - if (isActive == 1) { - int resol = volResolution4[0] * volResolution4[1] * volResolution4[2]; __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) - (allVolumePtr + table_offset + - (row)*table_step); + (allVolumePtr + table_offset + (row)*table_step); __global const float * p_vol2camMatrix = (__global const float *) (allVol2camMatrix + val2cam_offset + (row) * val2cam_step); - const float16 vol2camMatrix = (float16) - (p_vol2camMatrix[0], p_vol2camMatrix[1], p_vol2camMatrix[2], p_vol2camMatrix[3], + const float16 vol2camMatrix = (float16) ( + p_vol2camMatrix[0], p_vol2camMatrix[1], p_vol2camMatrix[2], p_vol2camMatrix[3], + p_vol2camMatrix[4], p_vol2camMatrix[5], p_vol2camMatrix[6], p_vol2camMatrix[7], + p_vol2camMatrix[8], p_vol2camMatrix[9], p_vol2camMatrix[10], p_vol2camMatrix[11], + p_vol2camMatrix[12], p_vol2camMatrix[13], p_vol2camMatrix[14], p_vol2camMatrix[15]); + + /* + printf("GPU %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f \n", + p_vol2camMatrix[0], p_vol2camMatrix[1], p_vol2camMatrix[2], p_vol2camMatrix[3], p_vol2camMatrix[4], p_vol2camMatrix[5], p_vol2camMatrix[6], p_vol2camMatrix[7], p_vol2camMatrix[8], p_vol2camMatrix[9], p_vol2camMatrix[10], p_vol2camMatrix[11], p_vol2camMatrix[12], p_vol2camMatrix[13], p_vol2camMatrix[14], p_vol2camMatrix[15]); + */ integrateVolumeUnit( i, j, diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 3c96f0ee868..49671a74931 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -274,7 +274,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im } // ---------------------------- -static const bool display = false; +static const bool display = true; static const bool parallelCheck = false; void normalsCheck(Mat normals) From b5dc584524c2ff83eb1be7d3ec050a5e1e255fd2 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 9 Dec 2020 10:42:20 +0300 Subject: [PATCH 071/216] integrate all volUnits - done --- modules/rgbd/src/hash_tsdf.cpp | 1 + modules/rgbd/src/opencl/hash_tsdf.cl | 15 +++------------ modules/rgbd/src/tsdf_functions.cpp | 7 ------- modules/rgbd/test/test_tsdf.cpp | 2 +- 4 files changed, 5 insertions(+), 20 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index fe35fe1b982..560bf9e9185 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -998,6 +998,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); + // add updating of isActive for volUnits U_volUnitsData.getMat(ACCESS_RW).copyTo(_volUnitsData); U_hashtable.getMat(ACCESS_RW).copyTo(_indexes.volumes); } diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 4594512839f..ca473648ec2 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -326,10 +326,9 @@ __kernel void integrateAllVolumeUnits( if (isActive == 1) { __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) - (allVolumePtr + table_offset + (row)*table_step); + (allVolumePtr + table_offset + (row) * 16*16*16); __global const float * p_vol2camMatrix = (__global const float *) - (allVol2camMatrix + val2cam_offset + (row) * val2cam_step); - + (allVol2camMatrix + val2cam_offset + (row) * 16); const float16 vol2camMatrix = (float16) ( p_vol2camMatrix[0], p_vol2camMatrix[1], p_vol2camMatrix[2], p_vol2camMatrix[3], @@ -337,14 +336,6 @@ __kernel void integrateAllVolumeUnits( p_vol2camMatrix[8], p_vol2camMatrix[9], p_vol2camMatrix[10], p_vol2camMatrix[11], p_vol2camMatrix[12], p_vol2camMatrix[13], p_vol2camMatrix[14], p_vol2camMatrix[15]); - /* - printf("GPU %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f \n", - p_vol2camMatrix[0], p_vol2camMatrix[1], p_vol2camMatrix[2], p_vol2camMatrix[3], - p_vol2camMatrix[4], p_vol2camMatrix[5], p_vol2camMatrix[6], p_vol2camMatrix[7], - p_vol2camMatrix[8], p_vol2camMatrix[9], p_vol2camMatrix[10], p_vol2camMatrix[11], - p_vol2camMatrix[12], p_vol2camMatrix[13], p_vol2camMatrix[14], p_vol2camMatrix[15]); - */ - integrateVolumeUnit( i, j, depthptr, @@ -362,7 +353,7 @@ __kernel void integrateAllVolumeUnits( truncDist, maxWeight ); - updateIsActive(hash_table, v, 0, list_size, bufferNums, hash_divisor); + //updateIsActive(hash_table, v, 0, list_size, bufferNums, hash_divisor); } } diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 3cedad37099..0ee569a05c3 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -381,12 +381,9 @@ size_t calc_hash(Vec4i x) //seed ^= x[0] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); //seed ^= x[1] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); //seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - //std::cout << " lol " << x[0]<()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - //std::cout << x[i] << "|" << seed << std::endl; } return seed; } @@ -571,15 +568,12 @@ void VolumesTable::expand() int VolumesTable::find_Volume(Vec3i indx) const { Vec4i idx(indx[0], indx[1], indx[2], 0); - //std::cout << "find_Volume -> "; int hash = int(calc_hash(idx) % hash_divisor); int num = 1; int i = hash * num * list_size; - //std::cout <<"[ "<< idx<<" ]= " << calc_hash(idx) <<" = "<< hash << std::endl; while (i != -1) { Volume_NODE v = volumes.at(i, 0); - //std::cout << " | i = " << i << " idx=" << v.idx << " row=" << v.row << " next=" << v.nextVolumeRow << std::endl; if (v.idx == idx) return v.row; //find nan cheking for int or Vec3i @@ -592,7 +586,6 @@ int VolumesTable::find_Volume(Vec3i indx) const } bool VolumesTable::isExist(Vec3i indx) { - //std::cout << "isExist -> "; if (this->find_Volume(indx) == -2) return false; return true; diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 49671a74931..3c96f0ee868 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -274,7 +274,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im } // ---------------------------- -static const bool display = true; +static const bool display = false; static const bool parallelCheck = false; void normalsCheck(Mat normals) From edf0ad4b89e98f5b45158828ff54608b13ff66d7 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 9 Dec 2020 10:55:57 +0300 Subject: [PATCH 072/216] docs fix --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 560bf9e9185..7e3627516fa 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -963,7 +963,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept _volUnitsData.copyTo(_tmp); UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); _tmp.release(); - + _indexes.volumes.copyTo(_tmp); UMat U_hashtable = _tmp.getUMat(ACCESS_RW); @@ -1612,7 +1612,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in ocl::KernelArg::ReadWrite(points.getUMat(ACCESS_RW)), ocl::KernelArg::ReadWrite(normals.getUMat(ACCESS_RW)) ); - + int resol = new_points.rows; //int resol = 1; size_t globalSize[1]; From 2143ec62d1116254d709b65e7145417a928ae122 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 9 Dec 2020 11:30:19 +0300 Subject: [PATCH 073/216] warning fix --- modules/rgbd/src/hash_tsdf.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 7e3627516fa..85c1d99796f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1167,6 +1167,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //! Integrate the correct volumeUnits + /* auto Integrate = [&](const Range& range) { for (int i = range.start; i < range.end; i++) { @@ -1191,7 +1192,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } }; - + */ //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); //Integrate(Range(0, (int)_totalVolUnits.size())); @@ -1600,7 +1601,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); - int totalVolUnitsSize = _indexes.indexesGPU.size(); + //int totalVolUnitsSize = _indexes.indexesGPU.size(); Mat totalVolUnits(_indexes.indexesGPU, rawType()); k.args( From 7cfc39cf3a195ab21b0252f7a940895ebab13bd7 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 10 Dec 2020 10:36:18 +0300 Subject: [PATCH 074/216] extend raycast signature --- modules/rgbd/src/hash_tsdf.cpp | 248 ++++++++++++++++----------- modules/rgbd/src/hash_tsdf.hpp | 5 +- modules/rgbd/src/opencl/hash_tsdf.cl | 24 ++- 3 files changed, 173 insertions(+), 104 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 85c1d99796f..bfa90cd8a5b 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -842,6 +842,17 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, flo } volStrides = Vec4i(xdim, ydim, zdim); + neighbourCoords = Vec8i( + volStrides.dot(Vec4i(0, 0, 0)), + volStrides.dot(Vec4i(0, 0, 1)), + volStrides.dot(Vec4i(0, 1, 0)), + volStrides.dot(Vec4i(0, 1, 1)), + volStrides.dot(Vec4i(1, 0, 0)), + volStrides.dot(Vec4i(1, 0, 1)), + volStrides.dot(Vec4i(1, 1, 0)), + volStrides.dot(Vec4i(1, 1, 1)) + ); + reset(); } @@ -864,7 +875,8 @@ void HashTSDFVolumeGPU::reset() poses = cv::Mat(buff_lvl, 1, rawType()); lastVisibleIndexes = cv::Mat(buff_lvl, 1, rawType()); _indexes = VolumesTable(); - posesGPU = cv::Mat(buff_lvl, 16, rawType()); + allVol2cam = cv::Mat(buff_lvl, 16, rawType()); + allCam2vol = cv::Mat(buff_lvl, 16, rawType()); } static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) @@ -977,7 +989,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept ocl::KernelArg::ReadWrite(totalVolUnits.getUMat(ACCESS_RW)), ocl::KernelArg::ReadWrite(U_volUnitsData), ocl::KernelArg::PtrReadOnly(_pixNorms), - ocl::KernelArg::ReadOnly(posesGPU.getUMat(ACCESS_READ)), + ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), _lastVolIndex, voxelSize, volResGpu.val, @@ -1072,7 +1084,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma volUnitsData.resize(buff_lvl); poses.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); - posesGPU.resize(buff_lvl); + allVol2cam.resize(buff_lvl); + allCam2vol.resize(buff_lvl); } this->indexes.at(_lastVolIndex, 0) = idx; _indexes.update(idx, _lastVolIndex); @@ -1098,13 +1111,20 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma poses.at(idx, 0) = subvolumePose; lastVisibleIndexes.at(idx, 0) = frameId; - Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); _indexes.updateActive(tsdf_idx, 1); + Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); auto vol2camMatrix = vol2cam.matrix.val; for (int k = 0; k < 16; k++) { - posesGPU.at(idx, k) = vol2camMatrix[k]; + allVol2cam.at(idx, k) = vol2camMatrix[k]; + } + + Affine3f cam2vol(Affine3f(subvolumePose.inv()) * Affine3f(cameraPose)); + auto cam2volMatrix = vol2cam.matrix.val; + for (int k = 0; k < 16; k++) + { + allCam2vol.at(idx, k) = cam2volMatrix[k]; } _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) @@ -1472,124 +1492,129 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); + + if(true) + { - _points.create(frameSize, POINT_TYPE); - _normals.create(frameSize, POINT_TYPE); - - Points points = _points.getMat(); - Normals normals = _normals.getMat(); - - Points& new_points(points); - Normals& new_normals(normals); + _points.create(frameSize, POINT_TYPE); + _normals.create(frameSize, POINT_TYPE); - const HashTSDFVolumeGPU& 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()); + Points points = _points.getMat(); + Normals normals = _normals.getMat(); - const int nstripes = -1; + Points& new_points(points); + Normals& new_normals(normals); - auto _HashRaycastInvoker = [&](const Range& range) - { - const Point3f cam2volTrans = cam2vol.translation(); - const Matx33f cam2volRot = cam2vol.rotation(); - const Matx33f vol2camRot = vol2cam.rotation(); + const HashTSDFVolumeGPU& 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 float blockSize = volume.volumeUnitSize; + const int nstripes = -1; - for (int y = range.start; y < range.end; y++) + auto _HashRaycastInvoker = [&](const Range& range) { - ptype* _ptsRow = new_points[y]; - ptype* _nrmRow = new_normals[y]; + const Point3f cam2volTrans = cam2vol.translation(); + const Matx33f cam2volRot = cam2vol.rotation(); + const Matx33f vol2camRot = vol2cam.rotation(); + + const float blockSize = volume.volumeUnitSize; - for (int x = 0; x < new_points.cols; x++) + for (int y = range.start; y < range.end; y++) { - //! Initialize default value - Point3f point = nan3, normal = nan3; + ptype* _ptsRow = new_points[y]; + ptype* _nrmRow = new_normals[y]; - //! Ray origin and direction in the volume coordinate frame - Point3f orig = cam2volTrans; - Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); + for (int x = 0; x < new_points.cols; x++) + { + //! Initialize default value + Point3f point = nan3, normal = nan3; - float tmin = 0; - float tmax = volume.truncateThreshold; - float tcurr = tmin; + //! Ray origin and direction in the volume coordinate frame + Point3f orig = cam2volTrans; + Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); - cv::Vec3i prevVolumeUnitIdx = - cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), - std::numeric_limits::min()); + float tmin = 0; + float tmax = volume.truncateThreshold; + float tcurr = tmin; - float tprev = tcurr; - float prevTsdf = volume.truncDist; - Ptr currVolumeUnit; + cv::Vec3i prevVolumeUnitIdx = + cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), + std::numeric_limits::min()); - while (tcurr < tmax) - { - Point3f currRayPos = orig + tcurr * rayDirV; - cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); + float tprev = tcurr; + float prevTsdf = volume.truncDist; + Ptr currVolumeUnit; - //VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); - VolumeIndex idx = _indexes.find_Volume(currVolumeUnitIdx); - float currTsdf = prevTsdf; - int currWeight = 0; - float stepSize = 0.5f * blockSize; - cv::Vec3i volUnitLocalIdx; - - - //! The subvolume exists in hashtable - if (idx < _lastVolIndex && idx >= 0) + while (tcurr < tmax) { - cv::Point3f currVolUnitPos = - volume.volumeUnitIdxToVolume(currVolumeUnitIdx); - volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); + Point3f currRayPos = orig + tcurr * rayDirV; + cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); + //VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); + VolumeIndex idx = _indexes.find_Volume(currVolumeUnitIdx); + float currTsdf = prevTsdf; + int currWeight = 0; + float stepSize = 0.5f * blockSize; + cv::Vec3i volUnitLocalIdx; - //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); - currTsdf = tsdfToFloat(currVoxel.tsdf); - currWeight = currVoxel.weight; - stepSize = tstep; - } - //! Surface crossing - if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) - { - float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); - if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) + //! The subvolume exists in hashtable + if (idx < _lastVolIndex && idx >= 0) { - Point3f pv = orig + tInterp * rayDirV; - Point3f nv = volume._getNormalVoxel(pv); + cv::Point3f currVolUnitPos = + volume.volumeUnitIdxToVolume(currVolumeUnitIdx); + volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); - if (!isNaN(nv)) + + //! TODO: Figure out voxel interpolation + TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); + currTsdf = tsdfToFloat(currVoxel.tsdf); + currWeight = currVoxel.weight; + stepSize = tstep; + } + + //! Surface crossing + if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) + { + float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); + if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { - normal = vol2camRot * nv; - point = vol2cam * pv; + Point3f pv = orig + tInterp * rayDirV; + Point3f nv = volume._getNormalVoxel(pv); + + if (!isNaN(nv)) + { + normal = vol2camRot * nv; + point = vol2cam * pv; + } } + break; } - break; + + prevVolumeUnitIdx = currVolumeUnitIdx; + prevTsdf = currTsdf; + tprev = tcurr; + tcurr += stepSize; + } - prevVolumeUnitIdx = currVolumeUnitIdx; - prevTsdf = currTsdf; - tprev = tcurr; - tcurr += stepSize; + _ptsRow[x] = toPtype(point); + _nrmRow[x] = toPtype(normal); } - _ptsRow[x] = toPtype(point); - _nrmRow[x] = toPtype(normal); - } - } - - }; - - parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); - //_HashRaycastInvoker(Range(0, new_points.rows)); - + }; + parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); + //_HashRaycastInvoker(Range(0, new_points.rows)); + + } + + if (true) { String errorStr; String name = "raycast"; @@ -1603,23 +1628,50 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in //int totalVolUnitsSize = _indexes.indexesGPU.size(); Mat totalVolUnits(_indexes.indexesGPU, rawType()); + + _points.create(frameSize, CV_32FC4); + _normals.create(frameSize, CV_32FC4); + + UMat points = _points.getUMat(); + UMat normals = _normals.getUMat(); + + Intr::Reprojector r = intrinsics.makeReprojector(); + Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); + + Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, + volumeUnitSize - voxelSize, + volumeUnitSize - voxelSize); + + float tstep = truncDist * raycastStepFactor; + Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); k.args( ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), (int)_indexes.list_size, (int)_indexes.bufferNums, (int)_indexes.hash_divisor, - ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), - ocl::KernelArg::ReadWrite(points.getUMat(ACCESS_RW)), - ocl::KernelArg::ReadWrite(normals.getUMat(ACCESS_RW)) + //ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), + ocl::KernelArg::ReadWrite(points), + ocl::KernelArg::ReadWrite(normals), + frameSize, + ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), + ocl::KernelArg::ReadOnly(allCam2vol.getUMat(ACCESS_READ)), + finv.val, cxy.val, + boxMin.val, boxMax.val, + tstep, + voxelSize, + volResGpu.val, + volStrides.val, + neighbourCoords.val ); - int resol = new_points.rows; + int resol = points.rows; //int resol = 1; - size_t globalSize[1]; - globalSize[0] = (size_t)resol; // num of points + size_t globalSize[2]; + globalSize[0] = (size_t)frameSize.width; + globalSize[1] = (size_t)frameSize.height; - if (!k.run(1, globalSize, NULL, true)) + if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 85da938aa50..b25d52ccf7d 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -175,10 +175,13 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::Mat lastVisibleIndexes; cv::Mat _volUnitsData; - cv::Mat posesGPU; + cv::Mat allVol2cam; + cv::Mat allCam2vol; cv::UMat _pixNorms; cv::Mat volUnitsData; VolumesTable _indexes; + Vec8i neighbourCoords; + }; #endif diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index ca473648ec2..e5c3100fefb 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -364,13 +364,27 @@ __kernel void raycast( const int list_size, const int bufferNums, const int hash_divisor, - __global const int4 * totalVolUnits, + //__global const int4 * totalVolUnits, __global float4 * points, - int points_step, int points_offset, - int points_rows, int points_cols, + int points_step, int points_offset, + int points_rows, int points_cols, __global float4 * normals, - int normals_step, int normals_offset, - int normals_rows, int normals_cols + int normals_step, int normals_offset, + int normals_rows, int normals_cols, + const int2 frameSize, + __global const float * allVol2cam, + int val2cam_step, int val2cam_offset, + int val2cam_rows, int val2cam_cols, + __global const float * allCam2vol, + int cam2vol_step, int cam2vol_offset, + int cam2vol_rows, int cam2vol_cols, + const float2 fixy, const float2 cxy, + const float4 boxDown4, const float4 boxUp4, + const float tstep, + const float voxelSize, + const int4 volResolution4, + const int4 volDims4, + const int8 neighbourCoords ) { int i = get_global_id(0); From ea528bb172ba1f291b405dc30ce9deb4f3e0ad23 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 11 Dec 2020 15:59:18 +0300 Subject: [PATCH 075/216] raycast work --- modules/rgbd/src/hash_tsdf.cpp | 46 ++++++- modules/rgbd/src/opencl/hash_tsdf.cl | 183 ++++++++++++++++++++++++++- 2 files changed, 223 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index bfa90cd8a5b..a86e10c41f4 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1547,6 +1547,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in float prevTsdf = volume.truncDist; Ptr currVolumeUnit; + //std::cout << "CPU " << tcurr << " " << tmax << std::endl; + while (tcurr < tmax) { Point3f currRayPos = orig + tcurr * rayDirV; @@ -1645,6 +1647,27 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in float tstep = truncDist * raycastStepFactor; Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); + const HashTSDFVolumeGPU& volume(*this); + const Affine3f cam2vol(volume.pose.inv()* Affine3f(cameraPose)); + const Affine3f vol2cam(Affine3f(cameraPose.inv())* volume.pose); + + const Point3f cam2volTrans = cam2vol.translation(); + const Matx33f cam2volRot = cam2vol.rotation(); + const Matx33f vol2camRot = vol2cam.rotation(); + + Vec4f cam2volTransGPU(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0); + Matx44f cam2volRotGPU = cam2vol.matrix; + Matx44f vol2camRotGPU = vol2cam.matrix; + + UMat volPoseGpu, invPoseGpu; + Mat(pose.matrix).copyTo(volPoseGpu); + Mat(pose.inv().matrix).copyTo(invPoseGpu); + + //std::cout << cam2volTrans << std::endl; + //std::cout << Vec4f(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0) << std::endl; + //std::cout << cam2volRot << std::endl; + //std::cout << cam2volRotGPU << std::endl; + k.args( ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), (int)_indexes.list_size, @@ -1656,20 +1679,37 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in frameSize, ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), ocl::KernelArg::ReadOnly(allCam2vol.getUMat(ACCESS_READ)), + ocl::KernelArg::PtrReadOnly(volPoseGpu), + ocl::KernelArg::PtrReadOnly(invPoseGpu), + cam2volTransGPU, + cam2volRotGPU, + vol2camRotGPU, + float(volume.truncateThreshold), finv.val, cxy.val, boxMin.val, boxMax.val, tstep, voxelSize, volResGpu.val, volStrides.val, - neighbourCoords.val + neighbourCoords.val, + voxelSizeInv + //, int(123456789) ); int resol = points.rows; //int resol = 1; size_t globalSize[2]; - globalSize[0] = (size_t)frameSize.width; - globalSize[1] = (size_t)frameSize.height; + //globalSize[0] = (size_t)frameSize.width; + //globalSize[1] = (size_t)frameSize.height; + //globalSize[0] = (size_t)points.cols; + //globalSize[1] = (size_t)points.rows; + globalSize[0] = (size_t)10; + globalSize[1] = (size_t)10; + + std::cout << "CPU "<< "cam2volTransGPU " << cam2volTransGPU << + "\n | cam2volRotGPU " << cam2volRotGPU << + "\n | vol2camRotGPU " << vol2camRotGPU << + "\n | truncateThreshold " << volume.truncateThreshold << std::endl; if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index e5c3100fefb..c5d17ad4526 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -378,17 +378,194 @@ __kernel void raycast( __global const float * allCam2vol, int cam2vol_step, int cam2vol_offset, int cam2vol_rows, int cam2vol_cols, + __global const float * vol2camptr, + __global const float * cam2volptr, + float4 cam2volTransGPU, + float16 cam2volRotGPU, + float16 vol2camRotGPU, + float truncateThreshold, const float2 fixy, const float2 cxy, const float4 boxDown4, const float4 boxUp4, const float tstep, const float voxelSize, const int4 volResolution4, const int4 volDims4, - const int8 neighbourCoords + const int8 neighbourCoords, + float voxelSizeInv + //, int test ) { - int i = get_global_id(0); + int x = get_global_id(0); + int y = get_global_id(1); + + //printf(" %d \n", test); //printf("raycast_GPU \n"); - float4 point = points[points_offset + (i) * points_step]; + //float4 point = points[points_offset + (i) * points_step]; //printf("point (%d) = [%f, %f, %f, %f] \n", i, point[0], point[1], point[2], point[3]); + + /* + printf(" cam2volTransGPU [%f, %f, %f, %f] \n cam2volRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n vol2camRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n ", + cam2volTransGPU[0], cam2volTransGPU[1], cam2volTransGPU[2], cam2volTransGPU[3], + + cam2volRotGPU[0],cam2volRotGPU[1],cam2volRotGPU[2],cam2volRotGPU[3],cam2volRotGPU[4],cam2volRotGPU[5],cam2volRotGPU[6],cam2volRotGPU[7], + cam2volRotGPU[8],cam2volRotGPU[9],cam2volRotGPU[10],cam2volRotGPU[11],cam2volRotGPU[12],cam2volRotGPU[13],cam2volRotGPU[14],cam2volRotGPU[15], + + vol2camRotGPU[0],vol2camRotGPU[1],vol2camRotGPU[2],vol2camRotGPU[3],vol2camRotGPU[4],vol2camRotGPU[5],vol2camRotGPU[6],vol2camRotGPU[7], + vol2camRotGPU[8],vol2camRotGPU[9],vol2camRotGPU[10],vol2camRotGPU[11],vol2camRotGPU[12],vol2camRotGPU[13],vol2camRotGPU[14],vol2camRotGPU[15] + ); + */ + + + if(x >= frameSize.x || y >= frameSize.y) + return; + + + + __global const float* cm = cam2volptr; + const float3 camRot0 = vload4(0, cm).xyz; + const float3 camRot1 = vload4(1, cm).xyz; + const float3 camRot2 = vload4(2, cm).xyz; + const float3 camTrans = (float3)(cm[3], cm[7], cm[11]); + + __global const float* vm = vol2camptr; + const float3 volRot0 = vload4(0, vm).xyz; + const float3 volRot1 = vload4(1, vm).xyz; + const float3 volRot2 = vload4(2, vm).xyz; + const float3 volTrans = (float3)(vm[3], vm[7], vm[11]); + + /* + printf("camRot0 [%f, %f, %f] \ncamRot1 [%f, %f, %f] \ncamRot2 [%f, %f, %f] \ncamTrans [%f, %f, %f] \n", + camRot0[0], camRot0[1], camRot0[2], + camRot1[0], camRot1[1], camRot1[2], + camRot2[0], camRot2[1], camRot2[2], + camTrans[0], camTrans[1], camTrans[2] + ); + */ + + + float3 planed = (float3)(((float2)(x, y) - cxy)*fixy, 1.f); + planed = (float3)(dot(planed, camRot0), + dot(planed, camRot1), + dot(planed, camRot2)); + + float3 orig = (float3) (1, 1, 1); + float3 dir = fast_normalize(planed); + + float tmin = 0; + truncateThreshold = 4; + float tmax = truncateThreshold; + + float tcurr = tmin; + float tprev = tcurr; + + if (tcurr < tmax) + { + + float3 currRayPos = orig + tcurr * dir; + + int3 point = (int3) ( + (int) currRayPos.x * voxelSizeInv, + (int) currRayPos.y * voxelSizeInv, + (int) currRayPos.z * voxelSizeInv + ); + + printf("lol [%d, %d] tcurr=%d tmax=%d [%d, %d, %d] \n", x, y, tcurr, tmax, point.x, point.y, point.z); + + tprev = tcurr; + tcurr += tstep; + } + + + + + + // coordinate-independent constants + /* + __global const float* cm = cam2volptr; + const float3 camRot0 = vload4(0, cm).xyz; + const float3 camRot1 = vload4(1, cm).xyz; + const float3 camRot2 = vload4(2, cm).xyz; + const float3 camTrans = (float3)(cm[3], cm[7], cm[11]); + + __global const float* vm = vol2camptr; + const float3 volRot0 = vload4(0, vm).xyz; + const float3 volRot1 = vload4(1, vm).xyz; + const float3 volRot2 = vload4(2, vm).xyz; + const float3 volTrans = (float3)(vm[3], vm[7], vm[11]); + + const float3 boxDown = boxDown4.xyz; + const float3 boxUp = boxUp4.xyz; + const int3 volDims = volDims4.xyz; + + const int3 volResolution = volResolution4.xyz; + + const float invVoxelSize = native_recip(voxelSize); + + // kernel itself + + float3 point = nan((uint)0); + float3 normal = nan((uint)0); + + float3 orig = camTrans; + + // get direction through pixel in volume space: + // 1. reproject (x, y) on projecting plane where z = 1.f + float3 planed = (float3)(((float2)(x, y) - cxy)*fixy, 1.f); + + // 2. rotate to volume space + planed = (float3)(dot(planed, camRot0), + dot(planed, camRot1), + dot(planed, camRot2)); + + // 3. normalize + float3 dir = fast_normalize(planed); + + // compute intersection of ray with all six bbox planes + float3 rayinv = native_recip(dir); + float3 tbottom = rayinv*(boxDown - orig); //incorrect value + float3 ttop = rayinv*(boxUp - orig); //incorrect value + + // re-order intersections to find smallest and largest on each axis + float3 minAx = min(ttop, tbottom); + float3 maxAx = max(ttop, tbottom); + + // near clipping plane + const float clip = 0.f; + float tmin = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip); + float tmax = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z)); + + // precautions against getting coordinates out of bounds + tmin = tmin + tstep; + tmax = tmax - tstep; + + //printf("GPU [%d, %d] tmin=%f tmax=%f minAx(%f, %f, %f) maxAx(%f, %f, %f) \n", + // x, y, tmin, tmax, minAx.x, minAx.y, minAx.z, maxAx.x, maxAx.y, maxAx.z); + + //printf("GPU [%d, %d] rayinv(%f, %f, %f) tbottom(%f, %f, %f) ttop(%f, %f, %f) \n", + // x, y, rayinv.x, rayinv.y, rayinv.z, tbottom.x, tbottom.y, tbottom.z, ttop.x, ttop.y, ttop.z); + + printf("GPU [%d, %d] orig(%f, %f, %f) boxDown(%f, %f, %f) boxUp(%f, %f, %f) \n", + x, y, orig.x, orig.y, orig.z, boxDown.x, boxDown.y, boxDown.z, boxUp.x, boxUp.y, boxUp.z); + + + if(tmin < tmax) + { + // interpolation optimized a little + orig *= invVoxelSize; + dir *= invVoxelSize; + + float3 rayStep = dir*tstep; + float3 next = (orig + dir*tmin); + + float3 currRayPos = orig + tmin * dir; + + printf("[%f, %f, %f] \n",currRayPos[0], currRayPos[1], currRayPos[2]); + + } + */ + + + + + } \ No newline at end of file From 0a9f59fa130f6b93eb2af3c824114af07467c437 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 11 Dec 2020 18:07:16 +0300 Subject: [PATCH 076/216] bug fix --- modules/rgbd/src/hash_tsdf.cpp | 25 +++++------ modules/rgbd/src/opencl/hash_tsdf.cl | 64 +++++++++++++++++----------- 2 files changed, 48 insertions(+), 41 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a86e10c41f4..ac7ec849aaf 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1673,14 +1673,15 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in (int)_indexes.list_size, (int)_indexes.bufferNums, (int)_indexes.hash_divisor, + (int)_lastVolIndex, //ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), ocl::KernelArg::ReadWrite(points), ocl::KernelArg::ReadWrite(normals), frameSize, - ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), - ocl::KernelArg::ReadOnly(allCam2vol.getUMat(ACCESS_READ)), - ocl::KernelArg::PtrReadOnly(volPoseGpu), - ocl::KernelArg::PtrReadOnly(invPoseGpu), + //ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), + //ocl::KernelArg::ReadOnly(allCam2vol.getUMat(ACCESS_READ)), + //ocl::KernelArg::PtrReadOnly(volPoseGpu), + //ocl::KernelArg::PtrReadOnly(invPoseGpu), cam2volTransGPU, cam2volRotGPU, vol2camRotGPU, @@ -1696,21 +1697,15 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in //, int(123456789) ); - int resol = points.rows; - //int resol = 1; + int resol = 1; size_t globalSize[2]; + //globalSize[0] = (size_t) resol; + //globalSize[1] = (size_t) resol; //globalSize[0] = (size_t)frameSize.width; //globalSize[1] = (size_t)frameSize.height; - //globalSize[0] = (size_t)points.cols; - //globalSize[1] = (size_t)points.rows; - globalSize[0] = (size_t)10; - globalSize[1] = (size_t)10; + globalSize[0] = (size_t)points.cols; + globalSize[1] = (size_t)points.rows; - std::cout << "CPU "<< "cam2volTransGPU " << cam2volTransGPU << - "\n | cam2volRotGPU " << cam2volRotGPU << - "\n | vol2camRotGPU " << vol2camRotGPU << - "\n | truncateThreshold " << volume.truncateThreshold << std::endl; - if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index c5d17ad4526..f5420cc694b 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -364,6 +364,7 @@ __kernel void raycast( const int list_size, const int bufferNums, const int hash_divisor, + const int lastVolIndex, //__global const int4 * totalVolUnits, __global float4 * points, int points_step, int points_offset, @@ -372,14 +373,16 @@ __kernel void raycast( int normals_step, int normals_offset, int normals_rows, int normals_cols, const int2 frameSize, - __global const float * allVol2cam, - int val2cam_step, int val2cam_offset, - int val2cam_rows, int val2cam_cols, - __global const float * allCam2vol, - int cam2vol_step, int cam2vol_offset, - int cam2vol_rows, int cam2vol_cols, - __global const float * vol2camptr, - __global const float * cam2volptr, + + //__global const float * allVol2cam, + // int val2cam_step, int val2cam_offset, + // int val2cam_rows, int val2cam_cols, + //__global const float * allCam2vol, + // int cam2vol_step, int cam2vol_offset, + // int cam2vol_rows, int cam2vol_cols, + //__global const float * vol2camptr, + //__global const float * cam2volptr, + float4 cam2volTransGPU, float16 cam2volRotGPU, float16 vol2camRotGPU, @@ -392,6 +395,7 @@ __kernel void raycast( const int4 volDims4, const int8 neighbourCoords, float voxelSizeInv + //, int test ) { @@ -419,19 +423,15 @@ __kernel void raycast( if(x >= frameSize.x || y >= frameSize.y) return; + const float3 camRot0 = cam2volRotGPU.s012; + const float3 camRot1 = cam2volRotGPU.s456; + const float3 camRot2 = cam2volRotGPU.s89a; + const float3 camTrans = cam2volRotGPU.s37b; - - __global const float* cm = cam2volptr; - const float3 camRot0 = vload4(0, cm).xyz; - const float3 camRot1 = vload4(1, cm).xyz; - const float3 camRot2 = vload4(2, cm).xyz; - const float3 camTrans = (float3)(cm[3], cm[7], cm[11]); - - __global const float* vm = vol2camptr; - const float3 volRot0 = vload4(0, vm).xyz; - const float3 volRot1 = vload4(1, vm).xyz; - const float3 volRot2 = vload4(2, vm).xyz; - const float3 volTrans = (float3)(vm[3], vm[7], vm[11]); + const float3 volRot0 = vol2camRotGPU.s012; + const float3 volRot1 = vol2camRotGPU.s456; + const float3 volRot2 = vol2camRotGPU.s89a; + const float3 volTrans = vol2camRotGPU.s37b; /* printf("camRot0 [%f, %f, %f] \ncamRot1 [%f, %f, %f] \ncamRot2 [%f, %f, %f] \ncamTrans [%f, %f, %f] \n", @@ -448,17 +448,18 @@ __kernel void raycast( dot(planed, camRot1), dot(planed, camRot2)); - float3 orig = (float3) (1, 1, 1); + float3 orig = (float3) (cam2volTransGPU[0], cam2volTransGPU[1], cam2volTransGPU[2]); float3 dir = fast_normalize(planed); float tmin = 0; - truncateThreshold = 4; float tmax = truncateThreshold; float tcurr = tmin; float tprev = tcurr; - if (tcurr < tmax) + //printf("GPU [%d, %d] truncateThreshold=%f, orig=[%f, %f, %f] \n", x, y, truncateThreshold, orig[0], orig[1], orig[2]); + + while (tcurr < tmax) { float3 currRayPos = orig + tcurr * dir; @@ -466,10 +467,21 @@ __kernel void raycast( int3 point = (int3) ( (int) currRayPos.x * voxelSizeInv, (int) currRayPos.y * voxelSizeInv, - (int) currRayPos.z * voxelSizeInv - ); + (int) currRayPos.z * voxelSizeInv); + + int4 point4 = (int4) ( + (int) currRayPos.x * voxelSizeInv, + (int) currRayPos.y * voxelSizeInv, + (int) currRayPos.z * voxelSizeInv, 0); + + int row = findRow(hash_table, point4, list_size, bufferNums, hash_divisor); + if (row >= 0 && row <= lastVolIndex-1) { + printf("idx = [%d, %d, %d] row=%d \n", point4[0], point4[1], point4[2], row); + return; + } + - printf("lol [%d, %d] tcurr=%d tmax=%d [%d, %d, %d] \n", x, y, tcurr, tmax, point.x, point.y, point.z); + //printf("lol [%d, %d] tcurr=%f tmax=%f tstep=%f [%d, %d, %d] \n", x, y, tcurr, tmax, tstep, point.x, point.y, point.z); tprev = tcurr; tcurr += tstep; From a590ae37d4ca48193bd7ecd4cae7e7bd1e4c3162 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 14 Dec 2020 10:51:46 +0300 Subject: [PATCH 077/216] calc point4 fix --- modules/rgbd/src/hash_tsdf.cpp | 11 +++++++---- modules/rgbd/src/opencl/hash_tsdf.cl | 20 ++++++++++++++------ 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ac7ec849aaf..23f6fec0114 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1701,11 +1701,14 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in size_t globalSize[2]; //globalSize[0] = (size_t) resol; //globalSize[1] = (size_t) resol; - //globalSize[0] = (size_t)frameSize.width; - //globalSize[1] = (size_t)frameSize.height; - globalSize[0] = (size_t)points.cols; - globalSize[1] = (size_t)points.rows; + globalSize[0] = (size_t)frameSize.width; + globalSize[1] = (size_t)frameSize.height; + //globalSize[0] = (size_t)points.cols; + //globalSize[1] = (size_t)points.rows; + //std::cout << "voxelSizeInv = " << voxelSizeInv << std::endl; + //std::cout << totalVolUnits << std::endl; + if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index f5420cc694b..1a1fa0f5dc2 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -465,22 +465,30 @@ __kernel void raycast( float3 currRayPos = orig + tcurr * dir; int3 point = (int3) ( - (int) currRayPos.x * voxelSizeInv, - (int) currRayPos.y * voxelSizeInv, - (int) currRayPos.z * voxelSizeInv); + (int) (currRayPos.x * voxelSizeInv), + (int) (currRayPos.y * voxelSizeInv), + (int) (currRayPos.z * voxelSizeInv) ); int4 point4 = (int4) ( - (int) currRayPos.x * voxelSizeInv, - (int) currRayPos.y * voxelSizeInv, - (int) currRayPos.z * voxelSizeInv, 0); + (int) (currRayPos.x * voxelSizeInv), + (int) (currRayPos.y * voxelSizeInv), + (int) (currRayPos.z * voxelSizeInv), 0); int row = findRow(hash_table, point4, list_size, bufferNums, hash_divisor); + + /* if (row >= 0 && row <= lastVolIndex-1) { printf("idx = [%d, %d, %d] row=%d \n", point4[0], point4[1], point4[2], row); return; } + if ( point.x>-10 && point.x<10 && point.y>-10 && point.y<20 && point.z>-10 && point.z<10 ) + { + printf("[%d, %d] currRayPos=[%f, %f, %f] voxelSizeInv=%f point=[%d, %d, %d] \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, voxelSizeInv, point.x, point.y, point.z); + } + */ + //printf("[%d, %d] currRayPos=[%f, %f, %f] voxelSizeInv=%f point=[%d, %d, %d] \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, voxelSizeInv, point.x, point.y, point.z); //printf("lol [%d, %d] tcurr=%f tmax=%f tstep=%f [%d, %d, %d] \n", x, y, tcurr, tmax, tstep, point.x, point.y, point.z); tprev = tcurr; From 1f5a5e6c3de09ba510a42422b1cbf20025d0f5a9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 14 Dec 2020 14:42:54 +0300 Subject: [PATCH 078/216] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 24 ++++++++---- modules/rgbd/src/opencl/hash_tsdf.cl | 55 ++++++++++++++++++++++------ 2 files changed, 60 insertions(+), 19 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 23f6fec0114..d43ec0d2464 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1548,6 +1548,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Ptr currVolumeUnit; //std::cout << "CPU " << tcurr << " " << tmax << std::endl; + //printf("CPU [%d, %d] truncateThreshold=%f, orig=[%f, %f, %f] \n", x, y, truncateThreshold, orig.x, orig.y, orig.z); while (tcurr < tmax) { @@ -1561,10 +1562,12 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; - //! The subvolume exists in hashtable if (idx < _lastVolIndex && idx >= 0) { + + //std::cout << "CPU x,y=" << x << "," << y <<" currRayPos =" << currRayPos << " Idx=" << currVolumeUnitIdx <<" row=" << idx<< std::endl; + cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); @@ -1611,8 +1614,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in }; - parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); - //_HashRaycastInvoker(Range(0, new_points.rows)); + //parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); + _HashRaycastInvoker(Range(0, new_points.rows)); } @@ -1693,7 +1696,10 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in volResGpu.val, volStrides.val, neighbourCoords.val, - voxelSizeInv + voxelSizeInv, + volumeUnitSize + + //, int(123456789) ); @@ -1701,13 +1707,15 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in size_t globalSize[2]; //globalSize[0] = (size_t) resol; //globalSize[1] = (size_t) resol; - globalSize[0] = (size_t)frameSize.width; - globalSize[1] = (size_t)frameSize.height; - //globalSize[0] = (size_t)points.cols; - //globalSize[1] = (size_t)points.rows; + globalSize[0] = (size_t) frameSize.width; + globalSize[1] = (size_t) frameSize.height; + //globalSize[0] = (size_t) points.cols; + //globalSize[1] = (size_t) points.rows; //std::cout << "voxelSizeInv = " << voxelSizeInv << std::endl; //std::cout << totalVolUnits << std::endl; + //printf("CPU volumeUnitSize=%f \n", volumeUnitSize); + if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 1a1fa0f5dc2..8a29fb0cd23 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -394,7 +394,8 @@ __kernel void raycast( const int4 volResolution4, const int4 volDims4, const int8 neighbourCoords, - float voxelSizeInv + float voxelSizeInv, + float volumeUnitSize //, int test ) @@ -402,11 +403,15 @@ __kernel void raycast( int x = get_global_id(0); int y = get_global_id(1); + //x+=468; y+=29; + //printf(" %d \n", test); //printf("raycast_GPU \n"); //float4 point = points[points_offset + (i) * points_step]; //printf("point (%d) = [%f, %f, %f, %f] \n", i, point[0], point[1], point[2], point[3]); - + //printf("GPU volumeUnitSize=%f \n", volumeUnitSize); + //printf("GPU volumeUnitSize=%f \n", volumeUnitSize); + /* printf(" cam2volTransGPU [%f, %f, %f, %f] \n cam2volRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n vol2camRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n ", cam2volTransGPU[0], cam2volTransGPU[1], cam2volTransGPU[2], cam2volTransGPU[3], @@ -459,29 +464,34 @@ __kernel void raycast( //printf("GPU [%d, %d] truncateThreshold=%f, orig=[%f, %f, %f] \n", x, y, truncateThreshold, orig[0], orig[1], orig[2]); + //printf("GPU tcurr=%f", tcurr); + + float stepSize = 0.5 * volumeUnitSize; + while (tcurr < tmax) { float3 currRayPos = orig + tcurr * dir; int3 point = (int3) ( - (int) (currRayPos.x * voxelSizeInv), - (int) (currRayPos.y * voxelSizeInv), - (int) (currRayPos.z * voxelSizeInv) ); + (int) (currRayPos.x / volumeUnitSize), + (int) (currRayPos.y / volumeUnitSize), + (int) (currRayPos.z / volumeUnitSize) ); int4 point4 = (int4) ( - (int) (currRayPos.x * voxelSizeInv), - (int) (currRayPos.y * voxelSizeInv), - (int) (currRayPos.z * voxelSizeInv), 0); + (int) (currRayPos.x / volumeUnitSize), + (int) (currRayPos.y / volumeUnitSize), + (int) (currRayPos.z / volumeUnitSize), 0); int row = findRow(hash_table, point4, list_size, bufferNums, hash_divisor); - /* + if (row >= 0 && row <= lastVolIndex-1) { - printf("idx = [%d, %d, %d] row=%d \n", point4[0], point4[1], point4[2], row); + printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); return; } + /* if ( point.x>-10 && point.x<10 && point.y>-10 && point.y<20 && point.z>-10 && point.z<10 ) { printf("[%d, %d] currRayPos=[%f, %f, %f] voxelSizeInv=%f point=[%d, %d, %d] \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, voxelSizeInv, point.x, point.y, point.z); @@ -492,13 +502,36 @@ __kernel void raycast( //printf("lol [%d, %d] tcurr=%f tmax=%f tstep=%f [%d, %d, %d] \n", x, y, tcurr, tmax, tstep, point.x, point.y, point.z); tprev = tcurr; - tcurr += tstep; + tcurr += stepSize; } + + + + + + + + + + + + + + + + + + + + + + + // coordinate-independent constants /* __global const float* cm = cam2volptr; From 427667c71d5e2a53830ff8babf32d078d2dd10cc Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 16 Dec 2020 10:42:01 +0300 Subject: [PATCH 079/216] add _at function --- modules/rgbd/src/hash_tsdf.cpp | 23 +++- modules/rgbd/src/opencl/hash_tsdf.cl | 167 ++++++++++++++++++--------- 2 files changed, 136 insertions(+), 54 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d43ec0d2464..23f4ca4bff0 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1562,11 +1562,18 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; + //if (x == 468 && y == 29) + // { + // std::cout << "CPU x,y=[" << x << ", " << y << "] currRayPos =" << currRayPos << " Idx=" << currVolumeUnitIdx << " row=" << idx << std::endl; + // } + //! The subvolume exists in hashtable - if (idx < _lastVolIndex && idx >= 0) + if (idx >= 0 && idx < _lastVolIndex) { + //std::cout << "CPU x,y=" << x << "," << y <<" currRayPos =" << currRayPos << " Idx=" << currVolumeUnitIdx <<" row=" << idx<< std::endl; + //return; cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); @@ -1662,6 +1669,11 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Matx44f cam2volRotGPU = cam2vol.matrix; Matx44f vol2camRotGPU = vol2cam.matrix; + Mat _tmp; + _volUnitsData.copyTo(_tmp); + UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); + _tmp.release(); + UMat volPoseGpu, invPoseGpu; Mat(pose.matrix).copyTo(volPoseGpu); Mat(pose.inv().matrix).copyTo(invPoseGpu); @@ -1681,6 +1693,10 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in ocl::KernelArg::ReadWrite(points), ocl::KernelArg::ReadWrite(normals), frameSize, + + ocl::KernelArg::ReadWrite(U_volUnitsData), + + //ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), //ocl::KernelArg::ReadOnly(allCam2vol.getUMat(ACCESS_READ)), //ocl::KernelArg::PtrReadOnly(volPoseGpu), @@ -1697,7 +1713,10 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in volStrides.val, neighbourCoords.val, voxelSizeInv, - volumeUnitSize + volumeUnitSize, + volume.truncDist, + volumeUnitResolution, + volStrides //, int(123456789) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 8a29fb0cd23..c5a02b7e324 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -358,6 +358,29 @@ __kernel void integrateAllVolumeUnits( } +struct TsdfVoxel _at(int3 volumeIdx, int row, + int volumeUnitResolution, int4 volStrides, + __global struct TsdfVoxel * allVolumePtr, int table_offset) + +{ + //! Out of bounds + if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || + (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || + (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) + { + struct TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.0f); + dummy.weight = 0; + return dummy; + } + + __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) + (allVolumePtr + table_offset + (row) * 16*16*16); int coordBase = + volumeIdx[0] * volStrides[0] + + volumeIdx[1] * volStrides[1] + + volumeIdx[2] * volStrides[2]; + return volData[coordBase]; +} __kernel void raycast( __global struct Volume_NODE * hash_table, @@ -373,6 +396,10 @@ __kernel void raycast( int normals_step, int normals_offset, int normals_rows, int normals_cols, const int2 frameSize, + __global struct TsdfVoxel * allVolumePtr, + int table_step, int table_offset, + int table_rows, int table_cols, + //__global const float * allVol2cam, // int val2cam_step, int val2cam_offset, @@ -395,8 +422,10 @@ __kernel void raycast( const int4 volDims4, const int8 neighbourCoords, float voxelSizeInv, - float volumeUnitSize - + float volumeUnitSize, + float truncDist, + int volumeUnitResolution, + int4 volStrides //, int test ) { @@ -405,26 +434,6 @@ __kernel void raycast( //x+=468; y+=29; - //printf(" %d \n", test); - //printf("raycast_GPU \n"); - //float4 point = points[points_offset + (i) * points_step]; - //printf("point (%d) = [%f, %f, %f, %f] \n", i, point[0], point[1], point[2], point[3]); - //printf("GPU volumeUnitSize=%f \n", volumeUnitSize); - //printf("GPU volumeUnitSize=%f \n", volumeUnitSize); - - /* - printf(" cam2volTransGPU [%f, %f, %f, %f] \n cam2volRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n vol2camRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n ", - cam2volTransGPU[0], cam2volTransGPU[1], cam2volTransGPU[2], cam2volTransGPU[3], - - cam2volRotGPU[0],cam2volRotGPU[1],cam2volRotGPU[2],cam2volRotGPU[3],cam2volRotGPU[4],cam2volRotGPU[5],cam2volRotGPU[6],cam2volRotGPU[7], - cam2volRotGPU[8],cam2volRotGPU[9],cam2volRotGPU[10],cam2volRotGPU[11],cam2volRotGPU[12],cam2volRotGPU[13],cam2volRotGPU[14],cam2volRotGPU[15], - - vol2camRotGPU[0],vol2camRotGPU[1],vol2camRotGPU[2],vol2camRotGPU[3],vol2camRotGPU[4],vol2camRotGPU[5],vol2camRotGPU[6],vol2camRotGPU[7], - vol2camRotGPU[8],vol2camRotGPU[9],vol2camRotGPU[10],vol2camRotGPU[11],vol2camRotGPU[12],vol2camRotGPU[13],vol2camRotGPU[14],vol2camRotGPU[15] - ); - */ - - if(x >= frameSize.x || y >= frameSize.y) return; @@ -438,16 +447,6 @@ __kernel void raycast( const float3 volRot2 = vol2camRotGPU.s89a; const float3 volTrans = vol2camRotGPU.s37b; - /* - printf("camRot0 [%f, %f, %f] \ncamRot1 [%f, %f, %f] \ncamRot2 [%f, %f, %f] \ncamTrans [%f, %f, %f] \n", - camRot0[0], camRot0[1], camRot0[2], - camRot1[0], camRot1[1], camRot1[2], - camRot2[0], camRot2[1], camRot2[2], - camTrans[0], camTrans[1], camTrans[2] - ); - */ - - float3 planed = (float3)(((float2)(x, y) - cxy)*fixy, 1.f); planed = (float3)(dot(planed, camRot0), dot(planed, camRot1), @@ -458,46 +457,70 @@ __kernel void raycast( float tmin = 0; float tmax = truncateThreshold; - float tcurr = tmin; float tprev = tcurr; - - //printf("GPU [%d, %d] truncateThreshold=%f, orig=[%f, %f, %f] \n", x, y, truncateThreshold, orig[0], orig[1], orig[2]); - - //printf("GPU tcurr=%f", tcurr); + float prevTsdf = truncDist; float stepSize = 0.5 * volumeUnitSize; while (tcurr < tmax) { - float3 currRayPos = orig + tcurr * dir; - int3 point = (int3) ( + // VolumeToVolumeUnitIdx() + int3 currVolumeUnitIdx = (int3) ( (int) (currRayPos.x / volumeUnitSize), (int) (currRayPos.y / volumeUnitSize), (int) (currRayPos.z / volumeUnitSize) ); - + + // VolumeToVolumeUnitIdx4() int4 point4 = (int4) ( (int) (currRayPos.x / volumeUnitSize), (int) (currRayPos.y / volumeUnitSize), (int) (currRayPos.z / volumeUnitSize), 0); int row = findRow(hash_table, point4, list_size, bufferNums, hash_divisor); - - - if (row >= 0 && row <= lastVolIndex-1) { - printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); - return; - } - - /* - if ( point.x>-10 && point.x<10 && point.y>-10 && point.y<20 && point.z>-10 && point.z<10 ) - { - printf("[%d, %d] currRayPos=[%f, %f, %f] voxelSizeInv=%f point=[%d, %d, %d] \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, voxelSizeInv, point.x, point.y, point.z); + float currTsdf = prevTsdf; + int currWeight = 0; + float stepSize = 0.5 * volumeUnitSize; + int3 volUnitLocalIdx; + + if (row >= 0 && row < lastVolIndex) { + + + //TsdfVoxel currVoxel + // VolumeUnitIdxToVolume() + float3 currVolUnitPos = (float3) + (( (float) (currVolumeUnitIdx[0]) * volumeUnitSize), + ( (float) (currVolumeUnitIdx[1]) * volumeUnitSize), + ( (float) (currVolumeUnitIdx[2]) * volumeUnitSize) ); + + + float3 pos = currRayPos - currVolUnitPos; + volUnitLocalIdx = (int3) + (( (int) (point4[0] * voxelSizeInv) ), + ( (int) (point4[1] * voxelSizeInv) ), + ( (int) (point4[2] * voxelSizeInv) ) ); + + struct TsdfVoxel currVoxel = _at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); + + currTsdf = tsdfToFloat(currVoxel.tsdf); + currWeight = currVoxel.weight; + + if (currTsdf!=1) + printf("GPU [%d, %d] currTsdf=%f currWeight=%d \n", x, y, currTsdf, currWeight); + + //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d]\n", + // x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row, currVolUnitPos[0], currVolUnitPos[1], currVolUnitPos[2], volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2]); + + + stepSize = tstep; + + //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); + } - */ + //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); //printf("[%d, %d] currRayPos=[%f, %f, %f] voxelSizeInv=%f point=[%d, %d, %d] \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, voxelSizeInv, point.x, point.y, point.z); //printf("lol [%d, %d] tcurr=%f tmax=%f tstep=%f [%d, %d, %d] \n", x, y, tcurr, tmax, tstep, point.x, point.y, point.z); @@ -515,14 +538,54 @@ __kernel void raycast( + + //printf(" %d \n", test); + //printf("raycast_GPU \n"); + //float4 point = points[points_offset + (i) * points_step]; + //printf("point (%d) = [%f, %f, %f, %f] \n", i, point[0], point[1], point[2], point[3]); + //printf("GPU volumeUnitSize=%f \n", volumeUnitSize); + //printf("GPU volumeUnitSize=%f \n", volumeUnitSize); + /* + printf(" cam2volTransGPU [%f, %f, %f, %f] \n cam2volRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n vol2camRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n ", + cam2volTransGPU[0], cam2volTransGPU[1], cam2volTransGPU[2], cam2volTransGPU[3], + + cam2volRotGPU[0],cam2volRotGPU[1],cam2volRotGPU[2],cam2volRotGPU[3],cam2volRotGPU[4],cam2volRotGPU[5],cam2volRotGPU[6],cam2volRotGPU[7], + cam2volRotGPU[8],cam2volRotGPU[9],cam2volRotGPU[10],cam2volRotGPU[11],cam2volRotGPU[12],cam2volRotGPU[13],cam2volRotGPU[14],cam2volRotGPU[15], + + vol2camRotGPU[0],vol2camRotGPU[1],vol2camRotGPU[2],vol2camRotGPU[3],vol2camRotGPU[4],vol2camRotGPU[5],vol2camRotGPU[6],vol2camRotGPU[7], + vol2camRotGPU[8],vol2camRotGPU[9],vol2camRotGPU[10],vol2camRotGPU[11],vol2camRotGPU[12],vol2camRotGPU[13],vol2camRotGPU[14],vol2camRotGPU[15] + ); + */ + /* + printf("camRot0 [%f, %f, %f] \ncamRot1 [%f, %f, %f] \ncamRot2 [%f, %f, %f] \ncamTrans [%f, %f, %f] \n", + camRot0[0], camRot0[1], camRot0[2], + camRot1[0], camRot1[1], camRot1[2], + camRot2[0], camRot2[1], camRot2[2], + camTrans[0], camTrans[1], camTrans[2] + ); + */ + + //printf("GPU [%d, %d] truncateThreshold=%f, orig=[%f, %f, %f] \n", x, y, truncateThreshold, orig[0], orig[1], orig[2]); + //printf("GPU tcurr=%f", tcurr); + + //if (row >= 0 && row <= lastVolIndex-1) { + // printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); + // return; + //} + /* + if ( point.x>-10 && point.x<10 && point.y>-10 && point.y<20 && point.z>-10 && point.z<10 ) + { + printf("[%d, %d] currRayPos=[%f, %f, %f] voxelSizeInv=%f point=[%d, %d, %d] \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, voxelSizeInv, point.x, point.y, point.z); + } + */ From fcaaf2c957e48af3282b5c20e8c540ca9fd287da Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 16 Dec 2020 12:24:10 +0300 Subject: [PATCH 080/216] raycast done, but work incorrect --- modules/rgbd/src/hash_tsdf.cpp | 9 +- modules/rgbd/src/opencl/hash_tsdf.cl | 258 ++++++++++----------------- 2 files changed, 104 insertions(+), 163 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 23f4ca4bff0..ad4fe084533 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1493,7 +1493,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); - if(true) + if(false) { _points.create(frameSize, POINT_TYPE); @@ -1584,6 +1584,12 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; + + //if (x == 168 && y == 28) + // printf("CPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d \n", + // x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], idx, currVolUnitPos.x, currVolUnitPos.y, currVolUnitPos.z, volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); + + stepSize = tstep; } @@ -1735,6 +1741,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in //std::cout << totalVolUnits << std::endl; //printf("CPU volumeUnitSize=%f \n", volumeUnitSize); + //printf("CPU voxelSizeInv = %f", voxelSizeInv); if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index c5a02b7e324..82d28d78489 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -382,6 +382,50 @@ struct TsdfVoxel _at(int3 volumeIdx, int row, return volData[coordBase]; } +inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* volumePtr, + int3 volResolution, int3 volDims, int8 neighbourCoords) +{ + if(any(p < 1) || any(p >= convert_float3(volResolution - 2))) + return nan((uint)0); + + float3 fip = floor(p); + int3 ip = convert_int3(fip); + float3 t = p - fip; + + int3 cmul = volDims*ip; + int coordBase = cmul.x + cmul.y + cmul.z; + int nco[8]; + vstore8(neighbourCoords + coordBase, 0, nco); + + int arDims[3]; + vstore3(volDims, 0, arDims); + float an[3]; + for(int c = 0; c < 3; c++) + { + int dim = arDims[c]; + + float vaz[8]; + for(int i = 0; i < 8; i++) + vaz[i] = tsdfToFloat(volumePtr[nco[i] + dim].tsdf - + volumePtr[nco[i] - dim].tsdf); + + float8 vz = vload8(0, vaz); + + float4 vy = mix(vz.s0246, vz.s1357, t.z); + float2 vx = mix(vy.s02, vy.s13, t.y); + + an[c] = mix(vx.s0, vx.s1, t.x); + } + + //gradientDeltaFactor is fixed at 1.0 of voxel size + float3 n = vload3(0, an); + float Norm = sqrt(n.x*n.x + n.y*n.y + n.z*n.z); + return Norm < 0.0001f ? nan((uint)0) : n / Norm; + //return fast_normalize(vload3(0, an)); +} + +typedef float4 ptype; + __kernel void raycast( __global struct Volume_NODE * hash_table, const int list_size, @@ -433,10 +477,14 @@ __kernel void raycast( int y = get_global_id(1); //x+=468; y+=29; + x+=168; y+=28; if(x >= frameSize.x || y >= frameSize.y) return; + float3 point = nan((uint)0); + float3 normal = nan((uint)0); + const float3 camRot0 = cam2volRotGPU.s012; const float3 camRot1 = cam2volRotGPU.s456; const float3 camRot2 = cam2volRotGPU.s89a; @@ -495,28 +543,59 @@ __kernel void raycast( ( (float) (currVolumeUnitIdx[1]) * volumeUnitSize), ( (float) (currVolumeUnitIdx[2]) * volumeUnitSize) ); - + // VolumeToVoxelCoord() float3 pos = currRayPos - currVolUnitPos; volUnitLocalIdx = (int3) - (( (int) (point4[0] * voxelSizeInv) ), - ( (int) (point4[1] * voxelSizeInv) ), - ( (int) (point4[2] * voxelSizeInv) ) ); + (( (int) ( (float) (pos[0]) * voxelSizeInv) ), + ( (int) ( (float) (pos[1]) * voxelSizeInv) ), + ( (int) ( (float) (pos[2]) * voxelSizeInv) ) ); struct TsdfVoxel currVoxel = _at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; - - if (currTsdf!=1) - printf("GPU [%d, %d] currTsdf=%f currWeight=%d \n", x, y, currTsdf, currWeight); - - //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d]\n", - // x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row, currVolUnitPos[0], currVolUnitPos[1], currVolUnitPos[2], volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2]); - - stepSize = tstep; - + + //printf("GPU voxelSizeInv = %f", voxelSizeInv); + //if (currTsdf!=1) + // printf("GPU [%d, %d] currTsdf=%f currWeight=%d \n", x, y, currTsdf, currWeight); + //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d\n", + // x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row, currVolUnitPos[0], currVolUnitPos[1], currVolUnitPos[2], volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); + } + + + if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) + { + float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); + if ( !isnan(tInterp) && !isinf(tInterp) ) + { + //printf("tInterp=%f \n", tInterp); + + __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) + (allVolumePtr + table_offset + (row) * 16*16*16); + + int3 volResolution = (int3) (volResolution4[0], volResolution4[1], volResolution4[2]); + int3 volDims = (int3) (volDims4[0], volDims4[1], volDims4[2]); + + float3 pv = orig + tInterp * dir; + float3 nv = getNormalVoxel( pv, volumeptr, volResolution, volDims, neighbourCoords); + + if(!any(isnan(nv))) + { + printf("lol \n"); + //convert pv and nv to camera space + normal = (float3)(dot(nv, volRot0), + dot(nv, volRot1), + dot(nv, volRot2)); + // interpolation optimized a little + pv *= voxelSize; + point = (float3)(dot(pv, volRot0), + dot(pv, volRot1), + dot(pv, volRot2)) + volTrans; + } + + } } @@ -529,159 +608,14 @@ __kernel void raycast( } + __global float* pts = (__global float*)(points + points_offset + y*points_step + x*sizeof(ptype)); + __global float* nrm = (__global float*)(normals + normals_offset + y*normals_step + x*sizeof(ptype)); + vstore4((float4)(point, 0), 0, pts); + vstore4((float4)(normal, 0), 0, nrm); - - - - - //printf(" %d \n", test); - //printf("raycast_GPU \n"); - //float4 point = points[points_offset + (i) * points_step]; - //printf("point (%d) = [%f, %f, %f, %f] \n", i, point[0], point[1], point[2], point[3]); - //printf("GPU volumeUnitSize=%f \n", volumeUnitSize); - //printf("GPU volumeUnitSize=%f \n", volumeUnitSize); - - /* - printf(" cam2volTransGPU [%f, %f, %f, %f] \n cam2volRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n vol2camRotGPU | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | %f, %f, %f, %f | \n ", - cam2volTransGPU[0], cam2volTransGPU[1], cam2volTransGPU[2], cam2volTransGPU[3], - - cam2volRotGPU[0],cam2volRotGPU[1],cam2volRotGPU[2],cam2volRotGPU[3],cam2volRotGPU[4],cam2volRotGPU[5],cam2volRotGPU[6],cam2volRotGPU[7], - cam2volRotGPU[8],cam2volRotGPU[9],cam2volRotGPU[10],cam2volRotGPU[11],cam2volRotGPU[12],cam2volRotGPU[13],cam2volRotGPU[14],cam2volRotGPU[15], - - vol2camRotGPU[0],vol2camRotGPU[1],vol2camRotGPU[2],vol2camRotGPU[3],vol2camRotGPU[4],vol2camRotGPU[5],vol2camRotGPU[6],vol2camRotGPU[7], - vol2camRotGPU[8],vol2camRotGPU[9],vol2camRotGPU[10],vol2camRotGPU[11],vol2camRotGPU[12],vol2camRotGPU[13],vol2camRotGPU[14],vol2camRotGPU[15] - ); - */ - - - /* - printf("camRot0 [%f, %f, %f] \ncamRot1 [%f, %f, %f] \ncamRot2 [%f, %f, %f] \ncamTrans [%f, %f, %f] \n", - camRot0[0], camRot0[1], camRot0[2], - camRot1[0], camRot1[1], camRot1[2], - camRot2[0], camRot2[1], camRot2[2], - camTrans[0], camTrans[1], camTrans[2] - ); - */ - - - - //printf("GPU [%d, %d] truncateThreshold=%f, orig=[%f, %f, %f] \n", x, y, truncateThreshold, orig[0], orig[1], orig[2]); - - //printf("GPU tcurr=%f", tcurr); - - - //if (row >= 0 && row <= lastVolIndex-1) { - // printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); - // return; - //} - - /* - if ( point.x>-10 && point.x<10 && point.y>-10 && point.y<20 && point.z>-10 && point.z<10 ) - { - printf("[%d, %d] currRayPos=[%f, %f, %f] voxelSizeInv=%f point=[%d, %d, %d] \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, voxelSizeInv, point.x, point.y, point.z); - } - */ - - - - - - - - - - // coordinate-independent constants - /* - __global const float* cm = cam2volptr; - const float3 camRot0 = vload4(0, cm).xyz; - const float3 camRot1 = vload4(1, cm).xyz; - const float3 camRot2 = vload4(2, cm).xyz; - const float3 camTrans = (float3)(cm[3], cm[7], cm[11]); - - __global const float* vm = vol2camptr; - const float3 volRot0 = vload4(0, vm).xyz; - const float3 volRot1 = vload4(1, vm).xyz; - const float3 volRot2 = vload4(2, vm).xyz; - const float3 volTrans = (float3)(vm[3], vm[7], vm[11]); - - const float3 boxDown = boxDown4.xyz; - const float3 boxUp = boxUp4.xyz; - const int3 volDims = volDims4.xyz; - - const int3 volResolution = volResolution4.xyz; - - const float invVoxelSize = native_recip(voxelSize); - - // kernel itself - - float3 point = nan((uint)0); - float3 normal = nan((uint)0); - - float3 orig = camTrans; - - // get direction through pixel in volume space: - // 1. reproject (x, y) on projecting plane where z = 1.f - float3 planed = (float3)(((float2)(x, y) - cxy)*fixy, 1.f); - - // 2. rotate to volume space - planed = (float3)(dot(planed, camRot0), - dot(planed, camRot1), - dot(planed, camRot2)); - - // 3. normalize - float3 dir = fast_normalize(planed); - - // compute intersection of ray with all six bbox planes - float3 rayinv = native_recip(dir); - float3 tbottom = rayinv*(boxDown - orig); //incorrect value - float3 ttop = rayinv*(boxUp - orig); //incorrect value - - // re-order intersections to find smallest and largest on each axis - float3 minAx = min(ttop, tbottom); - float3 maxAx = max(ttop, tbottom); - - // near clipping plane - const float clip = 0.f; - float tmin = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip); - float tmax = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z)); - - // precautions against getting coordinates out of bounds - tmin = tmin + tstep; - tmax = tmax - tstep; - - //printf("GPU [%d, %d] tmin=%f tmax=%f minAx(%f, %f, %f) maxAx(%f, %f, %f) \n", - // x, y, tmin, tmax, minAx.x, minAx.y, minAx.z, maxAx.x, maxAx.y, maxAx.z); - - //printf("GPU [%d, %d] rayinv(%f, %f, %f) tbottom(%f, %f, %f) ttop(%f, %f, %f) \n", - // x, y, rayinv.x, rayinv.y, rayinv.z, tbottom.x, tbottom.y, tbottom.z, ttop.x, ttop.y, ttop.z); - - printf("GPU [%d, %d] orig(%f, %f, %f) boxDown(%f, %f, %f) boxUp(%f, %f, %f) \n", - x, y, orig.x, orig.y, orig.z, boxDown.x, boxDown.y, boxDown.z, boxUp.x, boxUp.y, boxUp.z); - - - if(tmin < tmax) - { - // interpolation optimized a little - orig *= invVoxelSize; - dir *= invVoxelSize; - - float3 rayStep = dir*tstep; - float3 next = (orig + dir*tmin); - - float3 currRayPos = orig + tmin * dir; - - printf("[%f, %f, %f] \n",currRayPos[0], currRayPos[1], currRayPos[2]); - - } - */ - - - - - } \ No newline at end of file From 57f861434a4aed197e28950b5cd1138bee3d6f8a Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 17 Dec 2020 14:28:53 +0300 Subject: [PATCH 081/216] fin dug --- modules/rgbd/src/hash_tsdf.cpp | 55 +++++++++++++++++++--------- modules/rgbd/src/opencl/hash_tsdf.cl | 28 ++++++++++++-- 2 files changed, 62 insertions(+), 21 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ad4fe084533..0632c82af8d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1493,7 +1493,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); - if(false) + if(true) { _points.create(frameSize, POINT_TYPE); @@ -1535,6 +1535,10 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Point3f orig = cam2volTrans; Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); + //if (x == 300 && y == 150) + // printf("CPU [%d, %d] orig=[%f, %f, %f] rayDirV=[%f, %f, %f] \n", x, y, orig.x, orig.y, orig.z, rayDirV.x, rayDirV.y, rayDirV.z); + + float tmin = 0; float tmax = volume.truncateThreshold; float tcurr = tmin; @@ -1547,8 +1551,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in float prevTsdf = volume.truncDist; Ptr currVolumeUnit; - //std::cout << "CPU " << tcurr << " " << tmax << std::endl; - //printf("CPU [%d, %d] truncateThreshold=%f, orig=[%f, %f, %f] \n", x, y, truncateThreshold, orig.x, orig.y, orig.z); + //if (x == 300 && y == 150) + // printf("CPU [%d, %d] tmin=%f tmax=%f tcurr=%f tprev=%f prevTsdf=%f \n", x, y, tmin, tmax, tcurr, tprev, prevTsdf); while (tcurr < tmax) { @@ -1562,10 +1566,12 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; - //if (x == 468 && y == 29) - // { - // std::cout << "CPU x,y=[" << x << ", " << y << "] currRayPos =" << currRayPos << " Idx=" << currVolumeUnitIdx << " row=" << idx << std::endl; - // } + if (x == 300 && y == 150) + { + printf("CPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currTsdf=%f currWeight=%d stepSize=%f \n", + x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], idx, currTsdf, currWeight, stepSize); + + } //! The subvolume exists in hashtable if (idx >= 0 && idx < _lastVolIndex) @@ -1585,9 +1591,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; - //if (x == 168 && y == 28) - // printf("CPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d \n", - // x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], idx, currVolUnitPos.x, currVolUnitPos.y, currVolUnitPos.z, volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); + //if (x == 300 && y == 150) + // printf("CPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], idx, currVolUnitPos.x, currVolUnitPos.y, currVolUnitPos.z, volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); stepSize = tstep; @@ -1599,9 +1604,15 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { + //if (y == 150) + // printf("CPU [%d, %d] tInterp=%f \n", x, y, tInterp); + Point3f pv = orig + tInterp * rayDirV; Point3f nv = volume._getNormalVoxel(pv); + //if (y == 150) + // printf("CPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f]\n", x, y, pv.x, pv.y, pv.z, nv.x, nv.y, nv.z); + if (!isNaN(nv)) { normal = vol2camRot * nv; @@ -1730,18 +1741,26 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in int resol = 1; size_t globalSize[2]; - //globalSize[0] = (size_t) resol; - //globalSize[1] = (size_t) resol; - globalSize[0] = (size_t) frameSize.width; - globalSize[1] = (size_t) frameSize.height; + globalSize[0] = (size_t) resol; + globalSize[1] = (size_t) resol; + //globalSize[0] = (size_t) frameSize.width; + //globalSize[1] = (size_t) frameSize.height; //globalSize[0] = (size_t) points.cols; //globalSize[1] = (size_t) points.rows; - //std::cout << "voxelSizeInv = " << voxelSizeInv << std::endl; - //std::cout << totalVolUnits << std::endl; - //printf("CPU volumeUnitSize=%f \n", volumeUnitSize); + //printf("CPU voxelSizeInv=%f volumeUnitSize=%f truncDist=%f \n", voxelSizeInv, volumeUnitSize, truncDist); + + //std::cout << "CPU cam2volRotGPU=" << cam2volRotGPU << std::endl; + //printf("CPU camRot0=[%f, %f, %f] \n camRot1=[%f, %f, %f] \n camRot2=[%f, %f, %f] \n volTrans=[%f, %f, %f] \n", + // cam2volRotGPU.val[0], cam2volRotGPU.val[1], cam2volRotGPU.val[2], cam2volRotGPU.val[4], cam2volRotGPU.val[5], cam2volRotGPU.val[6], + // cam2volRotGPU.val[8], cam2volRotGPU.val[9], cam2volRotGPU.val[10], cam2volRotGPU.val[3], cam2volRotGPU.val[7], cam2volRotGPU.val[11]); + + //std::cout << "CPU vol2camRotGPU=" << vol2camRotGPU << std::endl; + //printf("CPU camRot0=[%f, %f, %f] \n camRot1=[%f, %f, %f] \n camRot2=[%f, %f, %f] \n camTrans=[%f, %f, %f] \n", + // vol2camRotGPU.val[0], vol2camRotGPU.val[1], vol2camRotGPU.val[2], vol2camRotGPU.val[4], vol2camRotGPU.val[5], vol2camRotGPU.val[6], + // vol2camRotGPU.val[8], vol2camRotGPU.val[9], vol2camRotGPU.val[10], vol2camRotGPU.val[3], vol2camRotGPU.val[7], vol2camRotGPU.val[11]); + - //printf("CPU voxelSizeInv = %f", voxelSizeInv); if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 82d28d78489..04808b6d7a9 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -476,8 +476,12 @@ __kernel void raycast( int x = get_global_id(0); int y = get_global_id(1); + // tmp posution for 1 pixel //x+=468; y+=29; - x+=168; y+=28; + //x+=168; y+=28; + x+=300; y+=150; + + //printf("GPU voxelSizeInv=%f volumeUnitSize=%f truncDist=%f \n", voxelSizeInv, volumeUnitSize, truncDist); if(x >= frameSize.x || y >= frameSize.y) return; @@ -500,16 +504,27 @@ __kernel void raycast( dot(planed, camRot1), dot(planed, camRot2)); + //printf("GPU camRot0=[%f, %f, %f] \n camRot1=[%f, %f, %f] \n camRot2=[%f, %f, %f] \n camTrans=[%f, %f, %f] \n", + // camRot0[0], camRot0[1], camRot0[2], camRot1[0], camRot1[1], camRot1[2], + // camRot2[0], camRot2[1], camRot2[2], camTrans[0], camTrans[1], camTrans[2] ); + + //printf("GPU volRot0=[%f, %f, %f] \n volRot1=[%f, %f, %f] \n volRot2=[%f, %f, %f] \n volTrans=[%f, %f, %f] \n", + // volRot0[0], volRot0[1], volRot0[2], volRot1[0], volRot1[1], volRot1[2], + // volRot2[0], volRot2[1], volRot2[2], volTrans[0], volTrans[1], volTrans[2] ); + + float3 orig = (float3) (cam2volTransGPU[0], cam2volTransGPU[1], cam2volTransGPU[2]); float3 dir = fast_normalize(planed); + //printf("GPU [%d, %d] orig=[%f, %f, %f] dir=[%f, %f, %f] \n", x, y, orig[0], orig[1], orig[2], dir[0], dir[1], dir[2]); + float tmin = 0; float tmax = truncateThreshold; float tcurr = tmin; float tprev = tcurr; float prevTsdf = truncDist; - float stepSize = 0.5 * volumeUnitSize; + //printf("GPU [%d, %d] tmin=%f tmax=%f tcurr=%f tprev=%f prevTsdf=%f \n", x, y, tmin, tmax, tcurr, tprev, prevTsdf); while (tcurr < tmax) { @@ -533,6 +548,10 @@ __kernel void raycast( float stepSize = 0.5 * volumeUnitSize; int3 volUnitLocalIdx; + printf("GPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currTsdf=%f currWeight=%d stepSize=%f \n", + x, y, currRayPos[0], currRayPos[1], currRayPos[2], currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], row, currTsdf, currWeight, stepSize); + + if (row >= 0 && row < lastVolIndex) { @@ -570,7 +589,8 @@ __kernel void raycast( float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if ( !isnan(tInterp) && !isinf(tInterp) ) { - //printf("tInterp=%f \n", tInterp); + if (y == 150) + printf("GPU [%d, %d] tInterp=%f \n", x, y, tInterp); __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) (allVolumePtr + table_offset + (row) * 16*16*16); @@ -580,6 +600,8 @@ __kernel void raycast( float3 pv = orig + tInterp * dir; float3 nv = getNormalVoxel( pv, volumeptr, volResolution, volDims, neighbourCoords); + //if (y == 150) + // printf("GPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f] \n", x, y, pv[0], pv[1], pv[2], nv[0], nv[1], nv[2]); if(!any(isnan(nv))) { From e1fd7c87f032bb8847f4c86b4b5fc95207947656 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 17 Dec 2020 15:41:29 +0300 Subject: [PATCH 082/216] bug fix --- modules/rgbd/src/hash_tsdf.cpp | 19 +++++++------- modules/rgbd/src/opencl/hash_tsdf.cl | 39 ++++++++++++++-------------- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 0632c82af8d..2dc117a8a9f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1566,12 +1566,11 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; - if (x == 300 && y == 150) - { - printf("CPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currTsdf=%f currWeight=%d stepSize=%f \n", - x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], idx, currTsdf, currWeight, stepSize); - - } + //if (x == 300 && y == 150) + //{ + // printf("CPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currTsdf=%f currWeight=%d stepSize=%f \n", + // x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], idx, currTsdf, currWeight, stepSize); + //} //! The subvolume exists in hashtable if (idx >= 0 && idx < _lastVolIndex) @@ -1592,7 +1591,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in currWeight = currVoxel.weight; //if (x == 300 && y == 150) - // printf("CPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], idx, currVolUnitPos.x, currVolUnitPos.y, currVolUnitPos.z, volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); + // printf("CPU [%d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d \n", x, y, idx, currVolUnitPos.x, currVolUnitPos.y, currVolUnitPos.z, volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); stepSize = tstep; @@ -1604,14 +1603,14 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { - //if (y == 150) + //if (x == 300 && y == 150) // printf("CPU [%d, %d] tInterp=%f \n", x, y, tInterp); Point3f pv = orig + tInterp * rayDirV; Point3f nv = volume._getNormalVoxel(pv); - //if (y == 150) - // printf("CPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f]\n", x, y, pv.x, pv.y, pv.z, nv.x, nv.y, nv.z); + if (x == 300 && y == 150) + printf("CPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f]\n", x, y, pv.x, pv.y, pv.z, nv.x, nv.y, nv.z); if (!isNaN(nv)) { diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 04808b6d7a9..dcfb9542364 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -385,9 +385,7 @@ struct TsdfVoxel _at(int3 volumeIdx, int row, inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* volumePtr, int3 volResolution, int3 volDims, int8 neighbourCoords) { - if(any(p < 1) || any(p >= convert_float3(volResolution - 2))) - return nan((uint)0); - + float3 fip = floor(p); int3 ip = convert_int3(fip); float3 t = p - fip; @@ -420,6 +418,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* volumePt //gradientDeltaFactor is fixed at 1.0 of voxel size float3 n = vload3(0, an); float Norm = sqrt(n.x*n.x + n.y*n.y + n.z*n.z); + printf("[%f, %f, %f] \n", n[0], n[1], n[2]); return Norm < 0.0001f ? nan((uint)0) : n / Norm; //return fast_normalize(vload3(0, an)); } @@ -532,15 +531,15 @@ __kernel void raycast( // VolumeToVolumeUnitIdx() int3 currVolumeUnitIdx = (int3) ( - (int) (currRayPos.x / volumeUnitSize), - (int) (currRayPos.y / volumeUnitSize), - (int) (currRayPos.z / volumeUnitSize) ); + floor (currRayPos.x / volumeUnitSize), + floor (currRayPos.y / volumeUnitSize), + floor (currRayPos.z / volumeUnitSize) ); // VolumeToVolumeUnitIdx4() int4 point4 = (int4) ( - (int) (currRayPos.x / volumeUnitSize), - (int) (currRayPos.y / volumeUnitSize), - (int) (currRayPos.z / volumeUnitSize), 0); + floor (currRayPos.x / volumeUnitSize), + floor (currRayPos.y / volumeUnitSize), + floor (currRayPos.z / volumeUnitSize), 0); int row = findRow(hash_table, point4, list_size, bufferNums, hash_divisor); float currTsdf = prevTsdf; @@ -548,8 +547,8 @@ __kernel void raycast( float stepSize = 0.5 * volumeUnitSize; int3 volUnitLocalIdx; - printf("GPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currTsdf=%f currWeight=%d stepSize=%f \n", - x, y, currRayPos[0], currRayPos[1], currRayPos[2], currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], row, currTsdf, currWeight, stepSize); + //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currTsdf=%f currWeight=%d stepSize=%f \n", + // x, y, currRayPos[0], currRayPos[1], currRayPos[2], currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], row, currTsdf, currWeight, stepSize); if (row >= 0 && row < lastVolIndex) { @@ -565,9 +564,9 @@ __kernel void raycast( // VolumeToVoxelCoord() float3 pos = currRayPos - currVolUnitPos; volUnitLocalIdx = (int3) - (( (int) ( (float) (pos[0]) * voxelSizeInv) ), - ( (int) ( (float) (pos[1]) * voxelSizeInv) ), - ( (int) ( (float) (pos[2]) * voxelSizeInv) ) ); + (( floor ( (float) (pos[0]) * voxelSizeInv) ), + ( floor ( (float) (pos[1]) * voxelSizeInv) ), + ( floor ( (float) (pos[2]) * voxelSizeInv) ) ); struct TsdfVoxel currVoxel = _at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); @@ -575,11 +574,13 @@ __kernel void raycast( currWeight = currVoxel.weight; stepSize = tstep; + //printf("GPU [%d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d\n", + // x, y, row, currVolUnitPos[0], currVolUnitPos[1], currVolUnitPos[2], volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); + + //printf("GPU voxelSizeInv = %f", voxelSizeInv); //if (currTsdf!=1) // printf("GPU [%d, %d] currTsdf=%f currWeight=%d \n", x, y, currTsdf, currWeight); - //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d\n", - // x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row, currVolUnitPos[0], currVolUnitPos[1], currVolUnitPos[2], volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); } @@ -589,8 +590,8 @@ __kernel void raycast( float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if ( !isnan(tInterp) && !isinf(tInterp) ) { - if (y == 150) - printf("GPU [%d, %d] tInterp=%f \n", x, y, tInterp); + //if (y == 150) + // printf("GPU [%d, %d] tInterp=%f \n", x, y, tInterp); __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) (allVolumePtr + table_offset + (row) * 16*16*16); @@ -601,7 +602,7 @@ __kernel void raycast( float3 pv = orig + tInterp * dir; float3 nv = getNormalVoxel( pv, volumeptr, volResolution, volDims, neighbourCoords); //if (y == 150) - // printf("GPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f] \n", x, y, pv[0], pv[1], pv[2], nv[0], nv[1], nv[2]); + printf("GPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f] \n", x, y, pv[0], pv[1], pv[2], nv[0], nv[1], nv[2]); if(!any(isnan(nv))) { From 091fb861d64a570e180c6cca4465a3c665786a24 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 18 Dec 2020 16:47:02 +0300 Subject: [PATCH 083/216] getNorm works correctly --- modules/rgbd/src/hash_tsdf.cpp | 20 ++-- modules/rgbd/src/opencl/hash_tsdf.cl | 144 +++++++++++++++++++-------- 2 files changed, 117 insertions(+), 47 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 2dc117a8a9f..279c2edf171 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1348,7 +1348,7 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const Point3f ptVox = point * voxelSizeInv; Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); - + // A small hash table to reduce a number of find() calls bool queried[8]; VolumeIndex iterMap[8]; @@ -1493,7 +1493,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); - if(true) + if(false) { _points.create(frameSize, POINT_TYPE); @@ -1609,8 +1609,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Point3f pv = orig + tInterp * rayDirV; Point3f nv = volume._getNormalVoxel(pv); - if (x == 300 && y == 150) - printf("CPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f]\n", x, y, pv.x, pv.y, pv.z, nv.x, nv.y, nv.z); + //if (x == 300 && y == 150) + // printf("CPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f]\n", x, y, pv.x, pv.y, pv.z, nv.x, nv.y, nv.z); if (!isNaN(nv)) { @@ -1740,10 +1740,10 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in int resol = 1; size_t globalSize[2]; - globalSize[0] = (size_t) resol; - globalSize[1] = (size_t) resol; - //globalSize[0] = (size_t) frameSize.width; - //globalSize[1] = (size_t) frameSize.height; + //globalSize[0] = (size_t) resol; + //globalSize[1] = (size_t) resol; + globalSize[0] = (size_t) frameSize.width; + globalSize[1] = (size_t) frameSize.height; //globalSize[0] = (size_t) points.cols; //globalSize[1] = (size_t) points.rows; @@ -1763,6 +1763,10 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); + + + points.getMat(ACCESS_RW).copyTo(_points); + normals.getMat(ACCESS_RW).copyTo(_normals); } diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index dcfb9542364..cbaa6b9fb50 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -382,45 +382,112 @@ struct TsdfVoxel _at(int3 volumeIdx, int row, return volData[coordBase]; } -inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* volumePtr, - int3 volResolution, int3 volDims, int8 neighbourCoords) + +struct TsdfVoxel _atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row, int lastVolIndex, + int volumeUnitResolution, int4 volStrides, + __global const struct TsdfVoxel * allVolumePtr, int table_offset) + +{ + //! Out of bounds + if (row < 0 || row > lastVolIndex - 1) + { + struct TsdfVoxel dummy; + dummy.tsdf = floatToTsdf(1.0f); + dummy.weight = 0; + return dummy; + } + + int3 volUnitLocalIdx = volumeIdx - volumeUnitIdx * volumeUnitResolution; + __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) + (allVolumePtr + table_offset + (row) * 16*16*16); + int coordBase = + volUnitLocalIdx[0] * volStrides[0] + + volUnitLocalIdx[1] * volStrides[1] + + volUnitLocalIdx[2] * volStrides[2]; + return volData[coordBase]; +} + + +inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolumePtr, + int3 volResolution, int3 volDims, int8 neighbourCoords, + float voxelSizeInv, int lastVolIndex, + __global struct Volume_NODE * hash_table, + const int list_size, + const int bufferNums, + const int hash_divisor, + int4 volStrides, int table_offset) { - float3 fip = floor(p); - int3 ip = convert_int3(fip); - float3 t = p - fip; - - int3 cmul = volDims*ip; - int coordBase = cmul.x + cmul.y + cmul.z; - int nco[8]; - vstore8(neighbourCoords + coordBase, 0, nco); - - int arDims[3]; - vstore3(volDims, 0, arDims); - float an[3]; - for(int c = 0; c < 3; c++) + float3 normal = (float3) (0.0f, 0.0f, 0.0f); + float3 ptVox = p * voxelSizeInv; + int3 iptVox = (int3) ( floor (ptVox.x), floor (ptVox.y), floor (ptVox.z) ); + + bool queried[8]; + int iterMap[8]; + + for (int i = 0; i < 8; i++) + { + iterMap[i] = lastVolIndex - 1; + queried[i] = false; + } + + int3 offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3 + { 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7 + }; + + const int nVals = 6; + float vals[nVals]; + + for (int i = 0; i < nVals; i++) { - int dim = arDims[c]; + int3 pt = iptVox + offsets[i]; + + // VoxelToVolumeUnitIdx() + // TODO: add assertion - if (!(vuRes & (vuRes - 1))) + int3 volumeUnitIdx = (int3) ( + floor ( (float) pt[0] / volResolution[0]), + floor ( (float) pt[1] / volResolution[1]), + floor ( (float) pt[2] / volResolution[2]) ); + + int4 volumeUnitIdx4 = (int4) ( + floor ( (float) pt[0] / volResolution[0]), + floor ( (float) pt[1] / volResolution[1]), + floor ( (float) pt[2] / volResolution[2]), 0 ); + + + int dictIdx = (volumeUnitIdx[0] & 1) + + (volumeUnitIdx[1] & 1) * 2 + + (volumeUnitIdx[2] & 1) * 4; + + int it = iterMap[dictIdx]; + + if (!queried[dictIdx]) + { + it = findRow(hash_table, volumeUnitIdx4, list_size, bufferNums, hash_divisor); + if (it >= 0 || it < lastVolIndex) + { + iterMap[dictIdx] = it; + queried[dictIdx] = true; + } + } - float vaz[8]; - for(int i = 0; i < 8; i++) - vaz[i] = tsdfToFloat(volumePtr[nco[i] + dim].tsdf - - volumePtr[nco[i] - dim].tsdf); + //_at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); - float8 vz = vload8(0, vaz); + struct TsdfVoxel tmp = _atVolumeUnit(pt, volumeUnitIdx, it, lastVolIndex, volResolution[0], volStrides, allVolumePtr, table_offset) ; + vals[i] = tsdfToFloat( tmp.tsdf ); - float4 vy = mix(vz.s0246, vz.s1357, t.z); - float2 vx = mix(vy.s02, vy.s13, t.y); + } - an[c] = mix(vx.s0, vx.s1, t.x); + for (int c = 0; c < 3; c++) + { + normal[c] = vals[c * 2] - vals[c * 2 + 1]; } - //gradientDeltaFactor is fixed at 1.0 of voxel size - float3 n = vload3(0, an); - float Norm = sqrt(n.x*n.x + n.y*n.y + n.z*n.z); - printf("[%f, %f, %f] \n", n[0], n[1], n[2]); - return Norm < 0.0001f ? nan((uint)0) : n / Norm; - //return fast_normalize(vload3(0, an)); + float norm = + sqrt(normal.x*normal.x + + normal.y*normal.y + + normal.z*normal.z); + return norm < 0.0001f ? nan((uint)0) : normal / norm; } typedef float4 ptype; @@ -593,20 +660,23 @@ __kernel void raycast( //if (y == 150) // printf("GPU [%d, %d] tInterp=%f \n", x, y, tInterp); - __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) - (allVolumePtr + table_offset + (row) * 16*16*16); + //__global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) + // (allVolumePtr + table_offset + (row) * 16*16*16); int3 volResolution = (int3) (volResolution4[0], volResolution4[1], volResolution4[2]); int3 volDims = (int3) (volDims4[0], volDims4[1], volDims4[2]); float3 pv = orig + tInterp * dir; - float3 nv = getNormalVoxel( pv, volumeptr, volResolution, volDims, neighbourCoords); + float3 nv = getNormalVoxel( pv, allVolumePtr, volResolution, volDims, neighbourCoords, + voxelSizeInv, lastVolIndex, hash_table, + list_size, bufferNums, hash_divisor, + volStrides, table_offset); //if (y == 150) - printf("GPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f] \n", x, y, pv[0], pv[1], pv[2], nv[0], nv[1], nv[2]); + // printf("GPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f] \n", x, y, pv[0], pv[1], pv[2], nv[0], nv[1], nv[2]); if(!any(isnan(nv))) { - printf("lol \n"); + //printf("lol \n"); //convert pv and nv to camera space normal = (float3)(dot(nv, volRot0), dot(nv, volRot1), @@ -637,8 +707,4 @@ __kernel void raycast( vstore4((float4)(normal, 0), 0, nrm); - - - - } \ No newline at end of file From 69a5034b270e687eec9aa283babddb802b4f1504 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 21 Dec 2020 14:07:20 +0300 Subject: [PATCH 084/216] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 21 ++++++++++++++------- modules/rgbd/src/opencl/hash_tsdf.cl | 23 ++++++++++++++++------- 2 files changed, 30 insertions(+), 14 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 279c2edf171..90e957f298e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1660,8 +1660,14 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); - UMat points = _points.getUMat(); - UMat normals = _normals.getUMat(); + //Points points = _points.getMat(); + //Normals normals = _normals.getMat(); + + //Points& new_points(points); + //Normals& new_normals(normals); + + UMat Upoints = _points.getUMat(); + UMat Unormals = _normals.getUMat(); Intr::Reprojector r = intrinsics.makeReprojector(); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); @@ -1706,8 +1712,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in (int)_indexes.hash_divisor, (int)_lastVolIndex, //ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), - ocl::KernelArg::ReadWrite(points), - ocl::KernelArg::ReadWrite(normals), + ocl::KernelArg::ReadWrite(Upoints), + ocl::KernelArg::ReadWrite(Unormals), frameSize, ocl::KernelArg::ReadWrite(U_volUnitsData), @@ -1764,9 +1770,10 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); - - points.getMat(ACCESS_RW).copyTo(_points); - normals.getMat(ACCESS_RW).copyTo(_normals); + //std::cout << Upoints << std::endl; + std::cout << Unormals << std::endl; + //Upoints.getMat(ACCESS_RW).copyTo(new_points); + //Unormals.getMat(ACCESS_RW).copyTo(new_normals); } diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index cbaa6b9fb50..f706b0b0c96 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -545,7 +545,7 @@ __kernel void raycast( // tmp posution for 1 pixel //x+=468; y+=29; //x+=168; y+=28; - x+=300; y+=150; + //x+=300; y+=150; //printf("GPU voxelSizeInv=%f volumeUnitSize=%f truncDist=%f \n", voxelSizeInv, volumeUnitSize, truncDist); @@ -603,12 +603,12 @@ __kernel void raycast( floor (currRayPos.z / volumeUnitSize) ); // VolumeToVolumeUnitIdx4() - int4 point4 = (int4) ( + int4 currVolumeUnitIdx4 = (int4) ( floor (currRayPos.x / volumeUnitSize), floor (currRayPos.y / volumeUnitSize), floor (currRayPos.z / volumeUnitSize), 0); - int row = findRow(hash_table, point4, list_size, bufferNums, hash_divisor); + int row = findRow(hash_table, currVolumeUnitIdx4, list_size, bufferNums, hash_divisor); float currTsdf = prevTsdf; int currWeight = 0; float stepSize = 0.5 * volumeUnitSize; @@ -648,7 +648,7 @@ __kernel void raycast( //printf("GPU voxelSizeInv = %f", voxelSizeInv); //if (currTsdf!=1) // printf("GPU [%d, %d] currTsdf=%f currWeight=%d \n", x, y, currTsdf, currWeight); - //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); + //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx4[0], currVolumeUnitIdx4[1], currVolumeUnitIdx4[2], row); } @@ -672,11 +672,11 @@ __kernel void raycast( list_size, bufferNums, hash_divisor, volStrides, table_offset); //if (y == 150) - // printf("GPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f] \n", x, y, pv[0], pv[1], pv[2], nv[0], nv[1], nv[2]); + //printf("GPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f] \n", x, y, pv[0], pv[1], pv[2], nv[0], nv[1], nv[2]); if(!any(isnan(nv))) { - //printf("lol \n"); + //convert pv and nv to camera space normal = (float3)(dot(nv, volRot0), dot(nv, volRot1), @@ -686,13 +686,16 @@ __kernel void raycast( point = (float3)(dot(pv, volRot0), dot(pv, volRot1), dot(pv, volRot2)) + volTrans; + + //printf("GPU [%d, %d] normal=[%f, %f, %f] point=[%f, %f, %f] \n", + // x, y, normal[0], normal[1], normal[2], point[0], point[1], point[2]); } } } - //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, point4[0], point4[1], point4[2], row); + //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx4[0], currVolumeUnitIdx4[1], currVolumeUnitIdx4[2], row); //printf("[%d, %d] currRayPos=[%f, %f, %f] voxelSizeInv=%f point=[%d, %d, %d] \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, voxelSizeInv, point.x, point.y, point.z); //printf("lol [%d, %d] tcurr=%f tmax=%f tstep=%f [%d, %d, %d] \n", x, y, tcurr, tmax, tstep, point.x, point.y, point.z); @@ -700,9 +703,15 @@ __kernel void raycast( tcurr += stepSize; } + //if (!isnan(point[0])) + //printf("GPU [%d, %d] normal=[%f, %f, %f] point=[%f, %f, %f] \n", + // x, y, normal[0], normal[1], normal[2], point[0], point[1], point[2]); + __global float* pts = (__global float*)(points + points_offset + y*points_step + x*sizeof(ptype)); __global float* nrm = (__global float*)(normals + normals_offset + y*normals_step + x*sizeof(ptype)); + //__global float* pts = (__global float*)(points + points_offset + y*points_step + x); + //__global float* nrm = (__global float*)(normals + normals_offset + y*normals_step + x); vstore4((float4)(point, 0), 0, pts); vstore4((float4)(normal, 0), 0, nrm); From 5b0d59cd5c5a33ac1101ca4194528ce2fe0b00dd Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 22 Dec 2020 17:55:27 +0300 Subject: [PATCH 085/216] raycast - done --- modules/rgbd/src/hash_tsdf.cpp | 20 +++++++++++--------- modules/rgbd/src/opencl/hash_tsdf.cl | 11 +++++++---- modules/rgbd/test/test_tsdf.cpp | 2 +- 3 files changed, 19 insertions(+), 14 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 90e957f298e..832ee4da225 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1660,14 +1660,14 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); - //Points points = _points.getMat(); - //Normals normals = _normals.getMat(); + Points points = _points.getMat(); + Normals normals = _normals.getMat(); - //Points& new_points(points); - //Normals& new_normals(normals); + Points& new_points(points); + Normals& new_normals(normals); - UMat Upoints = _points.getUMat(); - UMat Unormals = _normals.getUMat(); + UMat Upoints = points.getUMat(ACCESS_RW); + UMat Unormals = normals.getUMat(ACCESS_RW); Intr::Reprojector r = intrinsics.makeReprojector(); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); @@ -1771,9 +1771,11 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in throw std::runtime_error("Failed to run kernel"); //std::cout << Upoints << std::endl; - std::cout << Unormals << std::endl; - //Upoints.getMat(ACCESS_RW).copyTo(new_points); - //Unormals.getMat(ACCESS_RW).copyTo(new_normals); + //std::cout << Unormals << std::endl; + Upoints.getMat(ACCESS_RW).copyTo(new_points); + Unormals.getMat(ACCESS_RW).copyTo(new_normals); + //std::cout << new_normals << std::endl; + } diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index f706b0b0c96..c8f784f04b0 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -554,6 +554,7 @@ __kernel void raycast( float3 point = nan((uint)0); float3 normal = nan((uint)0); + //float3 normal = (float3)(1, 1, 1); const float3 camRot0 = cam2volRotGPU.s012; const float3 camRot1 = cam2volRotGPU.s456; @@ -702,16 +703,18 @@ __kernel void raycast( tprev = tcurr; tcurr += stepSize; } + //if (!isnan(point[0])) //printf("GPU [%d, %d] normal=[%f, %f, %f] point=[%f, %f, %f] \n", // x, y, normal[0], normal[1], normal[2], point[0], point[1], point[2]); - __global float* pts = (__global float*)(points + points_offset + y*points_step + x*sizeof(ptype)); - __global float* nrm = (__global float*)(normals + normals_offset + y*normals_step + x*sizeof(ptype)); - //__global float* pts = (__global float*)(points + points_offset + y*points_step + x); - //__global float* nrm = (__global float*)(normals + normals_offset + y*normals_step + x); + //__global float* pts = (__global float*)(points + points_offset + y*points_step + x*sizeof(ptype)); + //__global float* nrm = (__global float*)(normals + normals_offset + y*normals_step + x*sizeof(ptype)); + //printf(" normals_step=%d points_step=%d sizeof=%d \n",normals_step, points_step, sizeof(ptype) ); + __global float* pts = (__global float*)(points + points_offset + y*points_step/sizeof(ptype) + x); + __global float* nrm = (__global float*)(normals + normals_offset + y*normals_step/sizeof(ptype) + x); vstore4((float4)(point, 0), 0, pts); vstore4((float4)(normal, 0), 0, nrm); diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 3c96f0ee868..49671a74931 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -274,7 +274,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im } // ---------------------------- -static const bool display = false; +static const bool display = true; static const bool parallelCheck = false; void normalsCheck(Mat normals) From fd5384d52ae3245688af7cc40a5bf688e128ef39 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 23 Dec 2020 10:31:44 +0300 Subject: [PATCH 086/216] minor fixes --- modules/rgbd/src/hash_tsdf.cpp | 490 +++++++-------------------- modules/rgbd/src/hash_tsdf.hpp | 16 +- modules/rgbd/src/opencl/hash_tsdf.cl | 74 ---- modules/rgbd/src/tsdf.cpp | 2 +- modules/rgbd/src/tsdf_functions.cpp | 2 +- modules/rgbd/test/test_tsdf.cpp | 2 +- 6 files changed, 130 insertions(+), 456 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 832ee4da225..ae153909c52 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -865,23 +865,21 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstM void HashTSDFVolumeGPU::reset() { CV_TRACE_FUNCTION(); - _lastVolIndex = 0; + lastVolIndex = 0; degree = 15; buff_lvl = pow(2, degree); - _volUnitsData = cv::Mat(buff_lvl, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); volUnitsData = cv::Mat(buff_lvl, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); //volUnitsData = cv::Mat(VOLUMES_SIZE, 1, rawType()); - indexes = cv::Mat(buff_lvl, 1, rawType()); poses = cv::Mat(buff_lvl, 1, rawType()); lastVisibleIndexes = cv::Mat(buff_lvl, 1, rawType()); - _indexes = VolumesTable(); + indexes = VolumesTable(); allVol2cam = cv::Mat(buff_lvl, 16, rawType()); allCam2vol = cv::Mat(buff_lvl, 16, rawType()); } -static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) +static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int lastVolIndex) { - for (int i = 0; i < _lastVolIndex+1; i++) + for (int i = 0; i < lastVolIndex +1; i++) { auto p = v.at(i, 0); if (p == tsdf_idx) @@ -894,7 +892,7 @@ static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int _lastVolIndex) inline int HashTSDFVolumeGPU::find_idx(cv::Mat v, Vec3i tsdf_idx) const { - for (int i = 0; i < _lastVolIndex; i++) + for (int i = 0; i < lastVolIndex; i++) { Vec3i p = v.at(i, 0); if (p == tsdf_idx) @@ -968,29 +966,28 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - int totalVolUnitsSize = _indexes.indexesGPU.size(); - Mat totalVolUnits(_indexes.indexesGPU, rawType()); + int totalVolUnitsSize = indexes.indexesGPU.size(); + Mat totalVolUnits(indexes.indexesGPU, rawType()); Mat _tmp; - _volUnitsData.copyTo(_tmp); + volUnitsData.copyTo(_tmp); UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); _tmp.release(); - _indexes.volumes.copyTo(_tmp); + indexes.volumes.copyTo(_tmp); UMat U_hashtable = _tmp.getUMat(ACCESS_RW); k.args(ocl::KernelArg::ReadOnly(depth), - //ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), ocl::KernelArg::PtrReadWrite(U_hashtable), - (int)_indexes.list_size, - (int)_indexes.bufferNums, - (int)_indexes.hash_divisor, + (int)indexes.list_size, + (int)indexes.bufferNums, + (int)indexes.hash_divisor, ocl::KernelArg::ReadWrite(totalVolUnits.getUMat(ACCESS_RW)), ocl::KernelArg::ReadWrite(U_volUnitsData), - ocl::KernelArg::PtrReadOnly(_pixNorms), + ocl::KernelArg::PtrReadOnly(pixNorms), ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), - _lastVolIndex, + lastVolIndex, voxelSize, volResGpu.val, volStrides.val, @@ -1011,8 +1008,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept throw std::runtime_error("Failed to run kernel"); // add updating of isActive for volUnits - U_volUnitsData.getMat(ACCESS_RW).copyTo(_volUnitsData); - U_hashtable.getMat(ACCESS_RW).copyTo(_indexes.volumes); + U_volUnitsData.getMat(ACCESS_RW).copyTo(volUnitsData); + U_hashtable.getMat(ACCESS_RW).copyTo(indexes.volumes); } void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) @@ -1073,26 +1070,23 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { Vec3i idx = _localAccessVolUnits.at(i, 0); - if (!_indexes.isExist(idx)) + if (!indexes.isExist(idx)) { - if (_lastVolIndex >= buff_lvl) + if (lastVolIndex >= buff_lvl) { degree++; buff_lvl = pow(2, degree); - indexes.resize(buff_lvl); - _volUnitsData.resize(buff_lvl); volUnitsData.resize(buff_lvl); poses.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); allVol2cam.resize(buff_lvl); allCam2vol.resize(buff_lvl); } - this->indexes.at(_lastVolIndex, 0) = idx; - _indexes.update(idx, _lastVolIndex); - _newIndices.at(vol_idx, 0) = _lastVolIndex; + indexes.update(idx, lastVolIndex); + _newIndices.at(vol_idx, 0) = lastVolIndex; newIndices.at(vol_idx, 0) = idx; vol_idx++; - _lastVolIndex++; + lastVolIndex++; } } mutex.unlock(); @@ -1105,13 +1099,13 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = 0; i < vol_idx; i++) { Vec3i tsdf_idx = newIndices.at(i, 0); - VolumeIndex idx = _indexes.find_Volume(tsdf_idx); + VolumeIndex idx = indexes.find_Volume(tsdf_idx); Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; poses.at(idx, 0) = subvolumePose; lastVisibleIndexes.at(idx, 0) = frameId; - _indexes.updateActive(tsdf_idx, 1); + indexes.updateActive(tsdf_idx, 1); Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); auto vol2camMatrix = vol2cam.matrix.val; @@ -1127,11 +1121,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma allCam2vol.at(idx, k) = cam2volMatrix[k]; } - _volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) - { - TsdfVoxel& v = reinterpret_cast(vv); - v.tsdf = floatToTsdf(0.0f); v.weight = 0; - }); volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) { TsdfVoxel& v = reinterpret_cast(vv); @@ -1140,7 +1129,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //! Get keys for all the allocated volume Units - std::vector _totalVolUnits = _indexes.indexes; + std::vector _totalVolUnits = indexes.indexes; //! Mark volumes in the camera frustum as active Range _inFrustumRange(0, (int)_totalVolUnits.size()); @@ -1152,23 +1141,23 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { Vec3i tsdf_idx = _totalVolUnits[i]; - VolumeIndex idx = _indexes.find_Volume(tsdf_idx); - if (idx < 0 || idx == _lastVolIndex - 1) return; + VolumeIndex idx = indexes.find_Volume(tsdf_idx); + if (idx < 0 || idx == lastVolIndex - 1) return; Point3f volumeUnitPos = volumeUnitIdxToVolume(poses.at(idx, 0)); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { - _indexes.updateActive(tsdf_idx, 0); + indexes.updateActive(tsdf_idx, 0); return; } Point2f cameraPoint = proj(volUnitInCamSpace); if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) { - assert(idx == _lastVolIndex - 1); + assert(idx == lastVolIndex - 1); lastVisibleIndexes.at(idx, 0) = frameId; - _indexes.update(tsdf_idx, 1, frameId); + indexes.update(tsdf_idx, 1, frameId); } } }; @@ -1181,41 +1170,11 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (!(frameParams == newParams)) { frameParams = newParams; - pixNorms = preCalculationPixNorm(depth, intrinsics); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - _pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); + pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); } //! Integrate the correct volumeUnits - /* - auto Integrate = [&](const Range& range) { - for (int i = range.start; i < range.end; i++) - { - Vec3i tsdf_idx = _totalVolUnits[i]; - VolumeIndex idx = _indexes.find_Volume(tsdf_idx); - if (idx < 0 || idx == _lastVolIndex - 1) return; - - bool _isActive = _indexes.getActive(tsdf_idx); - - if (_isActive) - { - //! The volume unit should already be added into the Volume from the allocator - Matx44f _pose = poses.at(idx, 0); - - integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, - Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, - depthFactor, cameraPose, intrinsics, pixNorms, _volUnitsData.row(idx)); - - //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); - //! Ensure all active volumeUnits are set to inactive for next integration - _indexes.updateActive(tsdf_idx, 0); - } - } - }; - */ - //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); - //Integrate(Range(0, (int)_totalVolUnits.size())); - integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); } @@ -1256,7 +1215,7 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeInd return dummy; } - const TsdfVoxel* volData = _volUnitsData.ptr(indx); + const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + @@ -1266,7 +1225,7 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeInd TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const { - if (indx < 0 || indx > _lastVolIndex - 1) + if (indx < 0 || indx > lastVolIndex - 1) { TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.f); @@ -1276,7 +1235,7 @@ TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& v Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; // expanding at(), removing bounds check - const TsdfVoxel* volData = _volUnitsData.ptr(indx); + const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = volUnitLocalIdx[0] * volStrides[0] + volUnitLocalIdx[1] * volStrides[1] + volUnitLocalIdx[2] * volStrides[2]; @@ -1290,12 +1249,10 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const // A small hash table to reduce a number of find() calls bool queried[8]; - //VolumeUnitIndexes::const_iterator iterMap[8]; VolumeIndex iterMap[8]; for (int i = 0; i < 8; i++) { - //iterMap[i] = volumeUnits.end(); - iterMap[i] = _lastVolIndex - 1; + iterMap[i] = lastVolIndex - 1; queried[i] = false; } @@ -1318,19 +1275,14 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const auto it = iterMap[dictIdx]; if (!queried[dictIdx]) { - //it = find_idx(indexes, volumeUnitIdx); - it = _indexes.find_Volume(volumeUnitIdx); - if (it >= 0 || it < _lastVolIndex) + it = indexes.find_Volume(volumeUnitIdx); + if (it >= 0 || it < lastVolIndex) { iterMap[dictIdx] = it; queried[dictIdx] = true; } - //it = volumeUnits.find(volumeUnitIdx); - //iterMap[dictIdx] = it; - //queried[dictIdx] = true; } - //vx[i] = atVolumeUnit(pt, volumeUnitIdx, it).tsdf; vx[i] = new_atVolumeUnit(pt, volumeUnitIdx, it).tsdf; } @@ -1342,7 +1294,7 @@ inline float HashTSDFVolumeGPU::interpolateVoxel(const cv::Point3f& point) const return interpolateVoxelPoint(point * voxelSizeInv); } -Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const +Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const { Vec3f normal = Vec3f(0, 0, 0); @@ -1355,7 +1307,7 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const for (int i = 0; i < 8; i++) { - iterMap[i] = _lastVolIndex - 1; + iterMap[i] = lastVolIndex - 1; queried[i] = false; } @@ -1390,9 +1342,8 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const if (!queried[dictIdx]) { - //it = find_idx(indexes, volumeUnitIdx); - it = _indexes.find_Volume(volumeUnitIdx); - if (it >= 0 || it < _lastVolIndex) + it = indexes.find_Volume(volumeUnitIdx); + if (it >= 0 || it < lastVolIndex) { iterMap[dictIdx] = it; queried[dictIdx] = true; @@ -1486,299 +1437,105 @@ Point3f HashTSDFVolumeGPU::_getNormalVoxel(const Point3f& point) const } - void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); - if(false) - { - - _points.create(frameSize, POINT_TYPE); - _normals.create(frameSize, POINT_TYPE); - - Points points = _points.getMat(); - Normals normals = _normals.getMat(); - - Points& new_points(points); - Normals& new_normals(normals); - - const HashTSDFVolumeGPU& 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; - - auto _HashRaycastInvoker = [&](const Range& range) - { - const Point3f cam2volTrans = cam2vol.translation(); - const Matx33f cam2volRot = cam2vol.rotation(); - const Matx33f vol2camRot = vol2cam.rotation(); - - const float blockSize = volume.volumeUnitSize; - - for (int y = range.start; y < range.end; y++) - { - ptype* _ptsRow = new_points[y]; - ptype* _nrmRow = new_normals[y]; - - for (int x = 0; x < new_points.cols; x++) - { - //! Initialize default value - Point3f point = nan3, normal = nan3; - - //! Ray origin and direction in the volume coordinate frame - Point3f orig = cam2volTrans; - Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); - - //if (x == 300 && y == 150) - // printf("CPU [%d, %d] orig=[%f, %f, %f] rayDirV=[%f, %f, %f] \n", x, y, orig.x, orig.y, orig.z, rayDirV.x, rayDirV.y, rayDirV.z); - - - 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()); - - float tprev = tcurr; - float prevTsdf = volume.truncDist; - Ptr currVolumeUnit; - - //if (x == 300 && y == 150) - // printf("CPU [%d, %d] tmin=%f tmax=%f tcurr=%f tprev=%f prevTsdf=%f \n", x, y, tmin, tmax, tcurr, tprev, prevTsdf); - - while (tcurr < tmax) - { - Point3f currRayPos = orig + tcurr * rayDirV; - cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - - //VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); - VolumeIndex idx = _indexes.find_Volume(currVolumeUnitIdx); - float currTsdf = prevTsdf; - int currWeight = 0; - float stepSize = 0.5f * blockSize; - cv::Vec3i volUnitLocalIdx; - - //if (x == 300 && y == 150) - //{ - // printf("CPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currTsdf=%f currWeight=%d stepSize=%f \n", - // x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], idx, currTsdf, currWeight, stepSize); - //} - - //! The subvolume exists in hashtable - if (idx >= 0 && idx < _lastVolIndex) - { - - - //std::cout << "CPU x,y=" << x << "," << y <<" currRayPos =" << currRayPos << " Idx=" << currVolumeUnitIdx <<" row=" << idx<< std::endl; - //return; - - cv::Point3f currVolUnitPos = - volume.volumeUnitIdxToVolume(currVolumeUnitIdx); - volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); - - - //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); - currTsdf = tsdfToFloat(currVoxel.tsdf); - currWeight = currVoxel.weight; - - //if (x == 300 && y == 150) - // printf("CPU [%d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d \n", x, y, idx, currVolUnitPos.x, currVolUnitPos.y, currVolUnitPos.z, volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); - - - stepSize = tstep; - } - - //! Surface crossing - if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) - { - float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); - if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) - { - //if (x == 300 && y == 150) - // printf("CPU [%d, %d] tInterp=%f \n", x, y, tInterp); - - Point3f pv = orig + tInterp * rayDirV; - Point3f nv = volume._getNormalVoxel(pv); - - //if (x == 300 && y == 150) - // printf("CPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f]\n", x, y, pv.x, pv.y, pv.z, nv.x, nv.y, nv.z); - - if (!isNaN(nv)) - { - normal = vol2camRot * nv; - point = vol2cam * pv; - } - } - break; - } - - prevVolumeUnitIdx = currVolumeUnitIdx; - prevTsdf = currTsdf; - tprev = tcurr; - tcurr += stepSize; - - } - - _ptsRow[x] = toPtype(point); - _nrmRow[x] = toPtype(normal); - - } - - } + String errorStr; + String name = "raycast"; + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); - }; + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); - //parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); - _HashRaycastInvoker(Range(0, new_points.rows)); - - } - - if (true) - { - String errorStr; - String name = "raycast"; - ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; - String options = "-cl-mad-enable"; - ocl::Kernel k; - k.create(name.c_str(), source, options, &errorStr); - - if (k.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); - - //int totalVolUnitsSize = _indexes.indexesGPU.size(); - Mat totalVolUnits(_indexes.indexesGPU, rawType()); + //int totalVolUnitsSize = _indexes.indexesGPU.size(); + Mat totalVolUnits(indexes.indexesGPU, rawType()); - _points.create(frameSize, CV_32FC4); - _normals.create(frameSize, CV_32FC4); + _points.create(frameSize, CV_32FC4); + _normals.create(frameSize, CV_32FC4); - Points points = _points.getMat(); - Normals normals = _normals.getMat(); + Points points = _points.getMat(); + Normals normals = _normals.getMat(); - Points& new_points(points); - Normals& new_normals(normals); + Points& new_points(points); + Normals& new_normals(normals); - UMat Upoints = points.getUMat(ACCESS_RW); - UMat Unormals = normals.getUMat(ACCESS_RW); + UMat Upoints = points.getUMat(ACCESS_RW); + UMat Unormals = normals.getUMat(ACCESS_RW); - Intr::Reprojector r = intrinsics.makeReprojector(); - Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); + Intr::Reprojector r = intrinsics.makeReprojector(); + Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); - Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, - volumeUnitSize - voxelSize, - volumeUnitSize - voxelSize); + Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, + volumeUnitSize - voxelSize, + volumeUnitSize - voxelSize); - float tstep = truncDist * raycastStepFactor; - Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); - - const HashTSDFVolumeGPU& volume(*this); - const Affine3f cam2vol(volume.pose.inv()* Affine3f(cameraPose)); - const Affine3f vol2cam(Affine3f(cameraPose.inv())* volume.pose); + float tstep = truncDist * raycastStepFactor; + Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); - const Point3f cam2volTrans = cam2vol.translation(); - const Matx33f cam2volRot = cam2vol.rotation(); - const Matx33f vol2camRot = vol2cam.rotation(); + const HashTSDFVolumeGPU& volume(*this); + const Affine3f cam2vol(volume.pose.inv()* Affine3f(cameraPose)); + const Affine3f vol2cam(Affine3f(cameraPose.inv())* volume.pose); - Vec4f cam2volTransGPU(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0); - Matx44f cam2volRotGPU = cam2vol.matrix; - Matx44f vol2camRotGPU = vol2cam.matrix; - - Mat _tmp; - _volUnitsData.copyTo(_tmp); - UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); - _tmp.release(); - - UMat volPoseGpu, invPoseGpu; - Mat(pose.matrix).copyTo(volPoseGpu); - Mat(pose.inv().matrix).copyTo(invPoseGpu); - - //std::cout << cam2volTrans << std::endl; - //std::cout << Vec4f(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0) << std::endl; - //std::cout << cam2volRot << std::endl; - //std::cout << cam2volRotGPU << std::endl; - - k.args( - ocl::KernelArg::PtrReadWrite(_indexes.volumes.getUMat(ACCESS_RW)), - (int)_indexes.list_size, - (int)_indexes.bufferNums, - (int)_indexes.hash_divisor, - (int)_lastVolIndex, - //ocl::KernelArg::PtrReadWrite(totalVolUnits.getUMat(ACCESS_RW)), - ocl::KernelArg::ReadWrite(Upoints), - ocl::KernelArg::ReadWrite(Unormals), - frameSize, - - ocl::KernelArg::ReadWrite(U_volUnitsData), - - - //ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), - //ocl::KernelArg::ReadOnly(allCam2vol.getUMat(ACCESS_READ)), - //ocl::KernelArg::PtrReadOnly(volPoseGpu), - //ocl::KernelArg::PtrReadOnly(invPoseGpu), - cam2volTransGPU, - cam2volRotGPU, - vol2camRotGPU, - float(volume.truncateThreshold), - finv.val, cxy.val, - boxMin.val, boxMax.val, - tstep, - voxelSize, - volResGpu.val, - volStrides.val, - neighbourCoords.val, - voxelSizeInv, - volumeUnitSize, - volume.truncDist, - volumeUnitResolution, - volStrides - - - //, int(123456789) - ); - - int resol = 1; - size_t globalSize[2]; - //globalSize[0] = (size_t) resol; - //globalSize[1] = (size_t) resol; - globalSize[0] = (size_t) frameSize.width; - globalSize[1] = (size_t) frameSize.height; - //globalSize[0] = (size_t) points.cols; - //globalSize[1] = (size_t) points.rows; - - //printf("CPU voxelSizeInv=%f volumeUnitSize=%f truncDist=%f \n", voxelSizeInv, volumeUnitSize, truncDist); - - //std::cout << "CPU cam2volRotGPU=" << cam2volRotGPU << std::endl; - //printf("CPU camRot0=[%f, %f, %f] \n camRot1=[%f, %f, %f] \n camRot2=[%f, %f, %f] \n volTrans=[%f, %f, %f] \n", - // cam2volRotGPU.val[0], cam2volRotGPU.val[1], cam2volRotGPU.val[2], cam2volRotGPU.val[4], cam2volRotGPU.val[5], cam2volRotGPU.val[6], - // cam2volRotGPU.val[8], cam2volRotGPU.val[9], cam2volRotGPU.val[10], cam2volRotGPU.val[3], cam2volRotGPU.val[7], cam2volRotGPU.val[11]); - - //std::cout << "CPU vol2camRotGPU=" << vol2camRotGPU << std::endl; - //printf("CPU camRot0=[%f, %f, %f] \n camRot1=[%f, %f, %f] \n camRot2=[%f, %f, %f] \n camTrans=[%f, %f, %f] \n", - // vol2camRotGPU.val[0], vol2camRotGPU.val[1], vol2camRotGPU.val[2], vol2camRotGPU.val[4], vol2camRotGPU.val[5], vol2camRotGPU.val[6], - // vol2camRotGPU.val[8], vol2camRotGPU.val[9], vol2camRotGPU.val[10], vol2camRotGPU.val[3], vol2camRotGPU.val[7], vol2camRotGPU.val[11]); - + const Point3f cam2volTrans = cam2vol.translation(); + const Matx33f cam2volRot = cam2vol.rotation(); + const Matx33f vol2camRot = vol2cam.rotation(); + Vec4f cam2volTransGPU(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0); + Matx44f cam2volRotGPU = cam2vol.matrix; + Matx44f vol2camRotGPU = vol2cam.matrix; - if (!k.run(2, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); + Mat _tmp; + volUnitsData.copyTo(_tmp); + UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); + _tmp.release(); - //std::cout << Upoints << std::endl; - //std::cout << Unormals << std::endl; - Upoints.getMat(ACCESS_RW).copyTo(new_points); - Unormals.getMat(ACCESS_RW).copyTo(new_normals); - //std::cout << new_normals << std::endl; + UMat volPoseGpu, invPoseGpu; + Mat(pose.matrix).copyTo(volPoseGpu); + Mat(pose.inv().matrix).copyTo(invPoseGpu); + + k.args( + ocl::KernelArg::PtrReadWrite(indexes.volumes.getUMat(ACCESS_RW)), + (int)indexes.list_size, + (int)indexes.bufferNums, + (int)indexes.hash_divisor, + (int)lastVolIndex, + ocl::KernelArg::ReadWrite(Upoints), + ocl::KernelArg::ReadWrite(Unormals), + frameSize, + ocl::KernelArg::ReadWrite(U_volUnitsData), + cam2volTransGPU, + cam2volRotGPU, + vol2camRotGPU, + float(volume.truncateThreshold), + finv.val, cxy.val, + boxMin.val, boxMax.val, + tstep, + voxelSize, + volResGpu.val, + volStrides.val, + neighbourCoords.val, + voxelSizeInv, + volumeUnitSize, + volume.truncDist, + volumeUnitResolution, + volStrides + ); - } + size_t globalSize[2]; + globalSize[0] = (size_t) frameSize.width; + globalSize[1] = (size_t) frameSize.height; + if (!k.run(2, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); + Upoints.getMat(ACCESS_RW).copyTo(new_points); + Unormals.getMat(ACCESS_RW).copyTo(new_normals); } void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const @@ -1788,10 +1545,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor if (_points.needed()) { std::vector> pVecs, nVecs; - - //std::vector _totalVolUnits; - std::vector _totalVolUnits = _indexes.indexes; - //for (int i = 0; i < indexes.size().height; i++){_totalVolUnits.push_back(indexes.at(i, 0));} + std::vector _totalVolUnits = indexes.indexes; Range _fetchRange(0, (int)_totalVolUnits.size()); @@ -1810,10 +1564,10 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor cv::Vec3i tsdf_idx = _totalVolUnits[i]; //VolumeIndex idx = find_idx(indexes, tsdf_idx); - VolumeIndex idx = _indexes.find_Volume(tsdf_idx); + VolumeIndex idx = indexes.find_Volume(tsdf_idx); Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); - if (idx >= 0 && idx < _lastVolIndex) + if (idx >= 0 && idx < lastVolIndex) { std::vector localPoints; std::vector localNormals; @@ -1831,7 +1585,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor localPoints.push_back(toPtype(point)); if (needNormals) { - Point3f normal = volume._getNormalVoxel(point); + Point3f normal = volume.getNormalVoxel(point); localNormals.push_back(toPtype(normal)); } } @@ -1887,7 +1641,7 @@ void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) c if (!isNaN(p)) { Point3f voxelPoint = invPose * p; - n = volume.pose.rotation() * volume._getNormalVoxel(voxelPoint); + n = volume.pose.rotation() * volume.getNormalVoxel(voxelPoint); } normals(position[0], position[1]) = toPtype(n); }; @@ -1900,7 +1654,7 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con { int numVisibleBlocks = 0; //! TODO: Iterate over map parallely? - for (int i = 0; i < _lastVolIndex; i++) + for (int i = 0; i < lastVolIndex; i++) { if (lastVisibleIndexes.at(i, 0) > (currFrameId - frameThreshold)) numVisibleBlocks++; diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index b25d52ccf7d..467d86b29e4 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -141,7 +141,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; - VolumeIndex getTotalVolumeUnits() const { return _lastVolIndex; } + VolumeIndex getTotalVolumeUnits() const { return lastVolIndex; } int getVisibleBlocks(int currFrameId, int frameThreshold) const; //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = @@ -152,7 +152,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume float interpolateVoxelPoint(const Point3f& point) const; float interpolateVoxel(const cv::Point3f& point) const; - Point3f _getNormalVoxel(const cv::Point3f& p) const; + Point3f getNormalVoxel(const cv::Point3f& p) const; //! Utility functions for coordinate transformations Vec3i volumeToVolumeUnitIdx(const Point3f& point) const; @@ -165,23 +165,17 @@ class HashTSDFVolumeGPU : public HashTSDFVolume public: Vec4i volStrides; Vec6f frameParams; - Mat pixNorms; int degree; int buff_lvl; - - VolumeIndex _lastVolIndex; - cv::Mat indexes; cv::Mat poses; cv::Mat lastVisibleIndexes; - cv::Mat _volUnitsData; - cv::Mat allVol2cam; cv::Mat allCam2vol; - cv::UMat _pixNorms; cv::Mat volUnitsData; - VolumesTable _indexes; + cv::UMat pixNorms; + VolumeIndex lastVolIndex; + VolumesTable indexes; Vec8i neighbourCoords; - }; #endif diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index c8f784f04b0..3ad8d38cdd2 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -471,8 +471,6 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum } } - //_at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); - struct TsdfVoxel tmp = _atVolumeUnit(pt, volumeUnitIdx, it, lastVolIndex, volResolution[0], volStrides, allVolumePtr, table_offset) ; vals[i] = tsdfToFloat( tmp.tsdf ); @@ -498,7 +496,6 @@ __kernel void raycast( const int bufferNums, const int hash_divisor, const int lastVolIndex, - //__global const int4 * totalVolUnits, __global float4 * points, int points_step, int points_offset, int points_rows, int points_cols, @@ -509,17 +506,6 @@ __kernel void raycast( __global struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, int table_rows, int table_cols, - - - //__global const float * allVol2cam, - // int val2cam_step, int val2cam_offset, - // int val2cam_rows, int val2cam_cols, - //__global const float * allCam2vol, - // int cam2vol_step, int cam2vol_offset, - // int cam2vol_rows, int cam2vol_cols, - //__global const float * vol2camptr, - //__global const float * cam2volptr, - float4 cam2volTransGPU, float16 cam2volRotGPU, float16 vol2camRotGPU, @@ -536,25 +522,16 @@ __kernel void raycast( float truncDist, int volumeUnitResolution, int4 volStrides - //, int test ) { int x = get_global_id(0); int y = get_global_id(1); - // tmp posution for 1 pixel - //x+=468; y+=29; - //x+=168; y+=28; - //x+=300; y+=150; - - //printf("GPU voxelSizeInv=%f volumeUnitSize=%f truncDist=%f \n", voxelSizeInv, volumeUnitSize, truncDist); - if(x >= frameSize.x || y >= frameSize.y) return; float3 point = nan((uint)0); float3 normal = nan((uint)0); - //float3 normal = (float3)(1, 1, 1); const float3 camRot0 = cam2volRotGPU.s012; const float3 camRot1 = cam2volRotGPU.s456; @@ -571,28 +548,15 @@ __kernel void raycast( dot(planed, camRot1), dot(planed, camRot2)); - //printf("GPU camRot0=[%f, %f, %f] \n camRot1=[%f, %f, %f] \n camRot2=[%f, %f, %f] \n camTrans=[%f, %f, %f] \n", - // camRot0[0], camRot0[1], camRot0[2], camRot1[0], camRot1[1], camRot1[2], - // camRot2[0], camRot2[1], camRot2[2], camTrans[0], camTrans[1], camTrans[2] ); - - //printf("GPU volRot0=[%f, %f, %f] \n volRot1=[%f, %f, %f] \n volRot2=[%f, %f, %f] \n volTrans=[%f, %f, %f] \n", - // volRot0[0], volRot0[1], volRot0[2], volRot1[0], volRot1[1], volRot1[2], - // volRot2[0], volRot2[1], volRot2[2], volTrans[0], volTrans[1], volTrans[2] ); - - float3 orig = (float3) (cam2volTransGPU[0], cam2volTransGPU[1], cam2volTransGPU[2]); float3 dir = fast_normalize(planed); - //printf("GPU [%d, %d] orig=[%f, %f, %f] dir=[%f, %f, %f] \n", x, y, orig[0], orig[1], orig[2], dir[0], dir[1], dir[2]); - float tmin = 0; float tmax = truncateThreshold; float tcurr = tmin; float tprev = tcurr; float prevTsdf = truncDist; - //printf("GPU [%d, %d] tmin=%f tmax=%f tcurr=%f tprev=%f prevTsdf=%f \n", x, y, tmin, tmax, tcurr, tprev, prevTsdf); - while (tcurr < tmax) { float3 currRayPos = orig + tcurr * dir; @@ -615,10 +579,6 @@ __kernel void raycast( float stepSize = 0.5 * volumeUnitSize; int3 volUnitLocalIdx; - //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] currVolumeUnitIdx=[%d, %d, %d] row=%d currTsdf=%f currWeight=%d stepSize=%f \n", - // x, y, currRayPos[0], currRayPos[1], currRayPos[2], currVolumeUnitIdx[0], currVolumeUnitIdx[1], currVolumeUnitIdx[2], row, currTsdf, currWeight, stepSize); - - if (row >= 0 && row < lastVolIndex) { @@ -641,15 +601,6 @@ __kernel void raycast( currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; - - //printf("GPU [%d, %d] row=%d currVolUnitPos=[%f, %f, %f] volUnitLocalIdx=[%d, %d, %d] currTsdf=%f currWeight=%d\n", - // x, y, row, currVolUnitPos[0], currVolUnitPos[1], currVolUnitPos[2], volUnitLocalIdx[0], volUnitLocalIdx[1], volUnitLocalIdx[2], currTsdf, currWeight); - - - //printf("GPU voxelSizeInv = %f", voxelSizeInv); - //if (currTsdf!=1) - // printf("GPU [%d, %d] currTsdf=%f currWeight=%d \n", x, y, currTsdf, currWeight); - //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx4[0], currVolumeUnitIdx4[1], currVolumeUnitIdx4[2], row); } @@ -658,12 +609,6 @@ __kernel void raycast( float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if ( !isnan(tInterp) && !isinf(tInterp) ) { - //if (y == 150) - // printf("GPU [%d, %d] tInterp=%f \n", x, y, tInterp); - - //__global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) - // (allVolumePtr + table_offset + (row) * 16*16*16); - int3 volResolution = (int3) (volResolution4[0], volResolution4[1], volResolution4[2]); int3 volDims = (int3) (volDims4[0], volDims4[1], volDims4[2]); @@ -672,8 +617,6 @@ __kernel void raycast( voxelSizeInv, lastVolIndex, hash_table, list_size, bufferNums, hash_divisor, volStrides, table_offset); - //if (y == 150) - //printf("GPU [%d, %d] pv=[%f, %f, %f] nv=[%f, %f, %f] \n", x, y, pv[0], pv[1], pv[2], nv[0], nv[1], nv[2]); if(!any(isnan(nv))) { @@ -687,32 +630,15 @@ __kernel void raycast( point = (float3)(dot(pv, volRot0), dot(pv, volRot1), dot(pv, volRot2)) + volTrans; - - //printf("GPU [%d, %d] normal=[%f, %f, %f] point=[%f, %f, %f] \n", - // x, y, normal[0], normal[1], normal[2], point[0], point[1], point[2]); } } } - - //printf("GPU [%d, %d] currRayPos=[%f, %f, %f] idx=[%d, %d, %d] row=%d \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, currVolumeUnitIdx4[0], currVolumeUnitIdx4[1], currVolumeUnitIdx4[2], row); - //printf("[%d, %d] currRayPos=[%f, %f, %f] voxelSizeInv=%f point=[%d, %d, %d] \n", x, y, currRayPos.x, currRayPos.y, currRayPos.z, voxelSizeInv, point.x, point.y, point.z); - //printf("lol [%d, %d] tcurr=%f tmax=%f tstep=%f [%d, %d, %d] \n", x, y, tcurr, tmax, tstep, point.x, point.y, point.z); - tprev = tcurr; tcurr += stepSize; } - - //if (!isnan(point[0])) - //printf("GPU [%d, %d] normal=[%f, %f, %f] point=[%f, %f, %f] \n", - // x, y, normal[0], normal[1], normal[2], point[0], point[1], point[2]); - - - //__global float* pts = (__global float*)(points + points_offset + y*points_step + x*sizeof(ptype)); - //__global float* nrm = (__global float*)(normals + normals_offset + y*normals_step + x*sizeof(ptype)); - //printf(" normals_step=%d points_step=%d sizeof=%d \n",normals_step, points_step, sizeof(ptype) ); __global float* pts = (__global float*)(points + points_offset + y*points_step/sizeof(ptype) + x); __global float* nrm = (__global float*)(normals + normals_offset + y*normals_step/sizeof(ptype) + x); vstore4((float4)(point, 0), 0, pts); diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index f7dab563aec..1e8704170f4 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -929,7 +929,7 @@ void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, size_t globalSize[2]; globalSize[0] = (size_t)volResolution.x; globalSize[1] = (size_t)volResolution.y; - std::cout << "volResolution = "<(i, 0); + const Volume_NODE& v = *volumes.ptr(i); if (v.idx == idx) return (v.isActive == 1); if (v.idx[0] == -2147483647) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 49671a74931..3c96f0ee868 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -274,7 +274,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im } // ---------------------------- -static const bool display = true; +static const bool display = false; static const bool parallelCheck = false; void normalsCheck(Mat normals) From 61e2d4bc44282a6a9bbaeebb3527e76b0f8ce654 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 23 Dec 2020 11:02:45 +0300 Subject: [PATCH 087/216] 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 ae153909c52..e1ce316908e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1300,7 +1300,7 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const Point3f ptVox = point * voxelSizeInv; Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); - + // A small hash table to reduce a number of find() calls bool queried[8]; VolumeIndex iterMap[8]; @@ -1442,7 +1442,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); - + String errorStr; String name = "raycast"; ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; @@ -1455,7 +1455,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in //int totalVolUnitsSize = _indexes.indexesGPU.size(); Mat totalVolUnits(indexes.indexesGPU, rawType()); - + _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); @@ -1470,7 +1470,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Intr::Reprojector r = intrinsics.makeReprojector(); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); - + Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, volumeUnitSize - voxelSize, volumeUnitSize - voxelSize); From 069e8828a7e2f01ab7b9dcbe62bd415d42e52f0f Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 23 Dec 2020 12:26:42 +0300 Subject: [PATCH 088/216] warning 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 e1ce316908e..d0bf8261758 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1114,8 +1114,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma allVol2cam.at(idx, k) = vol2camMatrix[k]; } - Affine3f cam2vol(Affine3f(subvolumePose.inv()) * Affine3f(cameraPose)); - auto cam2volMatrix = vol2cam.matrix.val; + Affine3f cam2volum(Affine3f(subvolumePose.inv()) * Affine3f(cameraPose)); + auto cam2volMatrix = cam2volum.matrix.val; for (int k = 0; k < 16; k++) { allCam2vol.at(idx, k) = cam2volMatrix[k]; @@ -1244,7 +1244,7 @@ TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& v float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const { - const Vec3i neighbourCoords[] = { {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, + const Vec3i local_neighbourCoords[] = { {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1} }; // A small hash table to reduce a number of find() calls @@ -1268,7 +1268,7 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const float vx[8]; for (int i = 0; i < 8; i++) { - Vec3i pt = iv + neighbourCoords[i]; + Vec3i pt = iv + local_neighbourCoords[i]; Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; @@ -1483,8 +1483,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in const Affine3f vol2cam(Affine3f(cameraPose.inv())* volume.pose); 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(); Vec4f cam2volTransGPU(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0); Matx44f cam2volRotGPU = cam2vol.matrix; From 3fc630f7aff510d482c4644ee5754bbed39873e5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 23 Dec 2020 16:38:04 +0300 Subject: [PATCH 089/216] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- modules/rgbd/src/tsdf_functions.cpp | 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 d0bf8261758..f6472645438 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -877,7 +877,7 @@ void HashTSDFVolumeGPU::reset() allCam2vol = cv::Mat(buff_lvl, 16, rawType()); } -static inline bool _find(cv::Mat v, Vec3i tsdf_idx, int lastVolIndex) +static inline bool find(cv::Mat v, Vec3i tsdf_idx, int lastVolIndex) { for (int i = 0; i < lastVolIndex +1; i++) { @@ -1055,7 +1055,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { const Vec3i tsdf_idx = Vec3i(i, j, k); - if (!_find(_localAccessVolUnits, tsdf_idx, loc_vol_idx)) + if (!find(_localAccessVolUnits, tsdf_idx, loc_vol_idx)) { _localAccessVolUnits.at(loc_vol_idx, 0) = tsdf_idx; loc_vol_idx++; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index c4906f87b52..82e21406345 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -573,7 +573,7 @@ int VolumesTable::find_Volume(Vec3i indx) const int i = hash * num * list_size; while (i != -1) { - Volume_NODE v = volumes.at(i, 0); + const Volume_NODE& v = *volumes.ptr(i); if (v.idx == idx) return v.row; //find nan cheking for int or Vec3i From 85e7decf55a451a56264da190de396f873ed419b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 24 Dec 2020 10:14:31 +0300 Subject: [PATCH 090/216] minor fix --- modules/rgbd/src/tsdf_functions.cpp | 75 +++++++++++++++-------------- 1 file changed, 40 insertions(+), 35 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 82e21406345..e23663863a1 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -393,12 +393,13 @@ VolumesTable::VolumesTable() this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType()); for (int i = 0; i < volumes.size().height; i++) { - Volume_NODE& v = volumes.at(i, 0); - v.idx = nan4; - v.row = -1; - v.nextVolumeRow = -1; - v.isActive = 0; - v.lastVisibleIndex = -1; + //Volume_NODE& v = volumes.at(i, 0); + Volume_NODE* v = volumes.ptr(i); + v->idx = nan4; + v->row = -1; + v->nextVolumeRow = -1; + v->isActive = 0; + v->lastVisibleIndex = -1; //v.tmp = i; } } @@ -413,20 +414,21 @@ void VolumesTable::update(Vec3i indx) while (i != -1) { - Volume_NODE& v = volumes.at(i, 0); - if (v.idx == idx) + //Volume_NODE& v = volumes.at(i, 0); + Volume_NODE* v = volumes.ptr(i); + if (v->idx == idx) return; //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx[0] == -2147483647) + if (v->idx[0] == -2147483647) { - v.idx = idx; - v.nextVolumeRow = getNextVolume(hash, num, i, start); + v->idx = idx; + v->nextVolumeRow = getNextVolume(hash, num, i, start); indexes.push_back(indx); indexesGPU.push_back(idx); return; } - i = v.nextVolumeRow; + i = v->nextVolumeRow; } } @@ -440,24 +442,25 @@ void VolumesTable::update(Vec3i indx, int row) while (i != -1) { - Volume_NODE& v = volumes.at(i, 0); - if (v.idx == idx) + //Volume_NODE& v = volumes.at(i, 0); + Volume_NODE* v = volumes.ptr(i); + if (v->idx == idx) { - v.row = row; + v->row = row; return; } //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx[0] == -2147483647) + if (v->idx[0] == -2147483647) { - v.idx = idx; - v.row = row; - v.nextVolumeRow = getNextVolume(hash, num, i, start); + v->idx = idx; + v->row = row; + v->nextVolumeRow = getNextVolume(hash, num, i, start); indexes.push_back(indx); indexesGPU.push_back(idx); return; } - i = v.nextVolumeRow; + i = v->nextVolumeRow; } } @@ -471,26 +474,27 @@ void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) while (i != -1) { - Volume_NODE& v = volumes.at(i, 0); - if (v.idx == idx) + //Volume_NODE& v = volumes.at(i, 0); + Volume_NODE* v = volumes.ptr(i); + if (v->idx == idx) { - v.isActive = isActive; - v.lastVisibleIndex = lastVisibleIndex; + v->isActive = isActive; + v->lastVisibleIndex = lastVisibleIndex; return; } //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx[0] == -2147483647) + if (v->idx[0] == -2147483647) { - v.idx = idx; - v.nextVolumeRow = getNextVolume(hash, num, i, start); - v.isActive = isActive; - v.lastVisibleIndex = lastVisibleIndex; + v->idx = idx; + v->nextVolumeRow = getNextVolume(hash, num, i, start); + v->isActive = isActive; + v->lastVisibleIndex = lastVisibleIndex; indexes.push_back(indx); indexesGPU.push_back(idx); return; } - i = v.nextVolumeRow; + i = v->nextVolumeRow; } } @@ -504,19 +508,20 @@ void VolumesTable::updateActive(Vec3i indx, int isActive) while (i != -1) { - Volume_NODE& v = volumes.at(i, 0); - if (v.idx == idx) + //Volume_NODE& v = volumes.at(i, 0); + Volume_NODE* v = volumes.ptr(i); + if (v->idx == idx) { - v.isActive = isActive; + v->isActive = isActive; return; } //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx[0] == -2147483647) + if (v->idx[0] == -2147483647) { return; } - i = v.nextVolumeRow; + i = v->nextVolumeRow; } } From 9a243a4e3e2aae97a6d2cc691a7dde20622f5b94 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 24 Dec 2020 11:56:54 +0300 Subject: [PATCH 091/216] warning fix --- modules/rgbd/src/hash_tsdf.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index f6472645438..a9362982e9a 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -867,7 +867,7 @@ void HashTSDFVolumeGPU::reset() CV_TRACE_FUNCTION(); lastVolIndex = 0; degree = 15; - buff_lvl = pow(2, degree); + buff_lvl = (int) pow(2, degree); volUnitsData = cv::Mat(buff_lvl, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); //volUnitsData = cv::Mat(VOLUMES_SIZE, 1, rawType()); poses = cv::Mat(buff_lvl, 1, rawType()); @@ -966,8 +966,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - int totalVolUnitsSize = indexes.indexesGPU.size(); - Mat totalVolUnits(indexes.indexesGPU, rawType()); + int totalVolUnitsSize = (int) indexes.indexesGPU.size(); + Mat totalVolUnits(indexes.indexesGPU, (bool) rawType()); Mat _tmp; volUnitsData.copyTo(_tmp); @@ -1075,7 +1075,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (lastVolIndex >= buff_lvl) { degree++; - buff_lvl = pow(2, degree); + buff_lvl = (int) pow(2, degree); volUnitsData.resize(buff_lvl); poses.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); @@ -1454,7 +1454,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in throw std::runtime_error("Failed to create kernel: " + errorStr); //int totalVolUnitsSize = _indexes.indexesGPU.size(); - Mat totalVolUnits(indexes.indexesGPU, rawType()); + Mat totalVolUnits(indexes.indexesGPU, (bool) rawType()); _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); From ca67f0ace139d0e70938df582890fa185a374793 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 24 Dec 2020 12:46:43 +0300 Subject: [PATCH 092/216] warning fix 1 --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a9362982e9a..334fae1e44b 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -967,7 +967,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); int totalVolUnitsSize = (int) indexes.indexesGPU.size(); - Mat totalVolUnits(indexes.indexesGPU, (bool) rawType()); + Mat totalVolUnits = cv::Mat(indexes.indexesGPU, rawType()); Mat _tmp; volUnitsData.copyTo(_tmp); @@ -1454,7 +1454,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in throw std::runtime_error("Failed to create kernel: " + errorStr); //int totalVolUnitsSize = _indexes.indexesGPU.size(); - Mat totalVolUnits(indexes.indexesGPU, (bool) rawType()); + Mat totalVolUnits = cv::Mat(indexes.indexesGPU, rawType()); _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); From 693bedce90848e96f1b4c39d10bb17b71932ce9d Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 24 Dec 2020 16:15:16 +0300 Subject: [PATCH 093/216] win warning fix --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 334fae1e44b..40d6eb66e46 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -967,7 +967,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); int totalVolUnitsSize = (int) indexes.indexesGPU.size(); - Mat totalVolUnits = cv::Mat(indexes.indexesGPU, rawType()); + Mat totalVolUnits = cv::Mat(indexes.indexesGPU, true); Mat _tmp; volUnitsData.copyTo(_tmp); @@ -1454,7 +1454,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in throw std::runtime_error("Failed to create kernel: " + errorStr); //int totalVolUnitsSize = _indexes.indexesGPU.size(); - Mat totalVolUnits = cv::Mat(indexes.indexesGPU, rawType()); + Mat totalVolUnits = cv::Mat(indexes.indexesGPU, true); _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); From b3d0567410ec3d335f6c8c81fe2532127ca90689 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 24 Dec 2020 16:26:46 +0300 Subject: [PATCH 094/216] mac fix --- modules/rgbd/src/opencl/hash_tsdf.cl | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 3ad8d38cdd2..473b40e7060 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -4,10 +4,7 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory -//#define NAN_NUM -2147483647; - typedef __INT8_TYPE__ int8_t; -typedef __UINT32_TYPE__ uint32_t; typedef __INT32_TYPE__ int32_t; typedef int8_t TsdfType; @@ -51,18 +48,18 @@ __kernel void preCalculationPixNorm (__global float * pixNorms, pixNorms[idx] = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); } -uint calc_hash(int4 x) +static uint calc_hash(int4 x) { - uint32_t seed = 0; + unsigned int seed = 0; //uint GOLDEN_RATIO = 0x9e3779b9; - uint32_t GOLDEN_RATIO = 0x9e3779b9; + unsigned int GOLDEN_RATIO = 0x9e3779b9; seed ^= x[0] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); seed ^= x[1] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); return seed; } -int findRow(__global struct Volume_NODE * hash_table, int4 indx, +static int findRow(__global struct Volume_NODE * hash_table, int4 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; @@ -85,7 +82,7 @@ int findRow(__global struct Volume_NODE * hash_table, int4 indx, return -2; } -int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, +static int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; @@ -108,7 +105,7 @@ int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, return 0; } -void updateIsActive(__global struct Volume_NODE * hash_table, int4 indx, int isActive, +static void updateIsActive(__global struct Volume_NODE * hash_table, int4 indx, int isActive, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; @@ -131,7 +128,7 @@ void updateIsActive(__global struct Volume_NODE * hash_table, int4 indx, int isA } -void integrateVolumeUnit( +static void integrateVolumeUnit( int x, int y, __global const char * depthptr, int depth_step, int depth_offset, @@ -358,7 +355,7 @@ __kernel void integrateAllVolumeUnits( } -struct TsdfVoxel _at(int3 volumeIdx, int row, +static struct TsdfVoxel _at(int3 volumeIdx, int row, int volumeUnitResolution, int4 volStrides, __global struct TsdfVoxel * allVolumePtr, int table_offset) @@ -383,7 +380,7 @@ struct TsdfVoxel _at(int3 volumeIdx, int row, } -struct TsdfVoxel _atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row, int lastVolIndex, +static struct TsdfVoxel _atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row, int lastVolIndex, int volumeUnitResolution, int4 volStrides, __global const struct TsdfVoxel * allVolumePtr, int table_offset) From 5388f75151ce36249b5ec80de672f5734f05a2e8 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 25 Dec 2020 11:59:35 +0300 Subject: [PATCH 095/216] win debug fix --- modules/rgbd/src/hash_tsdf.cpp | 55 ++++++++++++++--------------- modules/rgbd/src/tsdf_functions.cpp | 5 --- 2 files changed, 27 insertions(+), 33 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 40d6eb66e46..1ca5e98fbe1 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -869,20 +869,19 @@ void HashTSDFVolumeGPU::reset() degree = 15; buff_lvl = (int) pow(2, degree); volUnitsData = cv::Mat(buff_lvl, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); - //volUnitsData = cv::Mat(VOLUMES_SIZE, 1, rawType()); poses = cv::Mat(buff_lvl, 1, rawType()); - lastVisibleIndexes = cv::Mat(buff_lvl, 1, rawType()); + lastVisibleIndexes = cv::Mat(buff_lvl, 1, CV_32S); indexes = VolumesTable(); - allVol2cam = cv::Mat(buff_lvl, 16, rawType()); - allCam2vol = cv::Mat(buff_lvl, 16, rawType()); + allVol2cam = cv::Mat(buff_lvl, 16, CV_32F); + allCam2vol = cv::Mat(buff_lvl, 16, CV_32F); } static inline bool find(cv::Mat v, Vec3i tsdf_idx, int lastVolIndex) { for (int i = 0; i < lastVolIndex +1; i++) { - auto p = v.at(i, 0); - if (p == tsdf_idx) + Vec3i* p = v.ptr(i); + if (*p == tsdf_idx) { return true; } @@ -894,8 +893,8 @@ inline int HashTSDFVolumeGPU::find_idx(cv::Mat v, Vec3i tsdf_idx) const { for (int i = 0; i < lastVolIndex; i++) { - Vec3i p = v.at(i, 0); - if (p == tsdf_idx) + Vec3i* p = v.ptr(i); + if (*p == tsdf_idx) { return i; } @@ -910,9 +909,9 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f Mat _pixNorm(1, depth_rows * depth_cols, CV_32F); for (int i = 0; i < depth_cols; i++) - x.at(0, i) = (i - cxy[0]) / fxy[0]; + *x.ptr(0, i) = (i - cxy[0]) / fxy[0]; for (int i = 0; i < depth_rows; i++) - y.at(0, i) = (i - cxy[1]) / fxy[1]; + *y.ptr(0, i) = (i - cxy[1]) / fxy[1]; cv::String errorStr; cv::String name = "preCalculationPixNorm"; @@ -1025,15 +1024,15 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma const Intr::Reprojector reproj(intrinsics.makeReprojector()); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Point3f truncPt(truncDist, truncDist, truncDist); - _VolumeUnitIndexSet _newIndices = cv::Mat(VOLUMES_SIZE, 1, rawType()); - cv::Mat newIndices = cv::Mat(VOLUMES_SIZE, 1, rawType()); - Mutex mutex; + _VolumeUnitIndexSet _newIndices = cv::Mat(VOLUMES_SIZE, 1, CV_32S); + cv::Mat newIndices = cv::Mat(VOLUMES_SIZE, 1, CV_32SC3); + //Mutex mutex; Range allocateRange(0, depth.rows); int loc_vol_idx = 0; int vol_idx = 0; auto AllocateVolumeUnitsInvoker = [&](const Range& range) { - _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(VOLUMES_SIZE, 1, rawType()); + _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(VOLUMES_SIZE, 1, CV_32SC3); for (int y = range.start; y < range.end; y += depthStride) { @@ -1057,7 +1056,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (!find(_localAccessVolUnits, tsdf_idx, loc_vol_idx)) { - _localAccessVolUnits.at(loc_vol_idx, 0) = tsdf_idx; + *_localAccessVolUnits.ptr(loc_vol_idx) = tsdf_idx; loc_vol_idx++; } @@ -1065,10 +1064,10 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } - mutex.lock(); + //mutex.lock(); for (int i = 0; i < loc_vol_idx; i++) { - Vec3i idx = _localAccessVolUnits.at(i, 0); + Vec3i idx = *_localAccessVolUnits.ptr(i); if (!indexes.isExist(idx)) { @@ -1083,13 +1082,13 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma allCam2vol.resize(buff_lvl); } indexes.update(idx, lastVolIndex); - _newIndices.at(vol_idx, 0) = lastVolIndex; - newIndices.at(vol_idx, 0) = idx; + *_newIndices.ptr(vol_idx) = lastVolIndex; + *newIndices.ptr(vol_idx) = idx; vol_idx++; lastVolIndex++; } } - mutex.unlock(); + //mutex.unlock(); }; //parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); @@ -1098,27 +1097,27 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! Perform the allocation for (int i = 0; i < vol_idx; i++) { - Vec3i tsdf_idx = newIndices.at(i, 0); + Vec3i tsdf_idx = *newIndices.ptr(i); VolumeIndex idx = indexes.find_Volume(tsdf_idx); Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; - poses.at(idx, 0) = subvolumePose; - lastVisibleIndexes.at(idx, 0) = frameId; + *poses.ptr(idx, 0) = subvolumePose; + *lastVisibleIndexes.ptr(idx, 0) = frameId; indexes.updateActive(tsdf_idx, 1); Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); auto vol2camMatrix = vol2cam.matrix.val; for (int k = 0; k < 16; k++) { - allVol2cam.at(idx, k) = vol2camMatrix[k]; + *allVol2cam.ptr(idx, k) = vol2camMatrix[k]; } Affine3f cam2volum(Affine3f(subvolumePose.inv()) * Affine3f(cameraPose)); auto cam2volMatrix = cam2volum.matrix.val; for (int k = 0; k < 16; k++) { - allCam2vol.at(idx, k) = cam2volMatrix[k]; + *allCam2vol.ptr(idx, k) = cam2volMatrix[k]; } volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) @@ -1144,7 +1143,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma VolumeIndex idx = indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == lastVolIndex - 1) return; - Point3f volumeUnitPos = volumeUnitIdxToVolume(poses.at(idx, 0)); + Point3f volumeUnitPos = volumeUnitIdxToVolume(*poses.ptr(idx)); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) @@ -1156,7 +1155,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) { assert(idx == lastVolIndex - 1); - lastVisibleIndexes.at(idx, 0) = frameId; + *lastVisibleIndexes.ptr(idx, 0) = frameId; indexes.update(tsdf_idx, 1, frameId); } } @@ -1656,7 +1655,7 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con //! TODO: Iterate over map parallely? for (int i = 0; i < lastVolIndex; i++) { - if (lastVisibleIndexes.at(i, 0) > (currFrameId - frameThreshold)) + if (*lastVisibleIndexes.ptr(i) > (currFrameId - frameThreshold)) numVisibleBlocks++; } return numVisibleBlocks; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index e23663863a1..244ab632020 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -393,7 +393,6 @@ VolumesTable::VolumesTable() this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType()); for (int i = 0; i < volumes.size().height; i++) { - //Volume_NODE& v = volumes.at(i, 0); Volume_NODE* v = volumes.ptr(i); v->idx = nan4; v->row = -1; @@ -414,7 +413,6 @@ void VolumesTable::update(Vec3i indx) while (i != -1) { - //Volume_NODE& v = volumes.at(i, 0); Volume_NODE* v = volumes.ptr(i); if (v->idx == idx) return; @@ -442,7 +440,6 @@ void VolumesTable::update(Vec3i indx, int row) while (i != -1) { - //Volume_NODE& v = volumes.at(i, 0); Volume_NODE* v = volumes.ptr(i); if (v->idx == idx) { @@ -474,7 +471,6 @@ void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) while (i != -1) { - //Volume_NODE& v = volumes.at(i, 0); Volume_NODE* v = volumes.ptr(i); if (v->idx == idx) { @@ -508,7 +504,6 @@ void VolumesTable::updateActive(Vec3i indx, int isActive) while (i != -1) { - //Volume_NODE& v = volumes.at(i, 0); Volume_NODE* v = volumes.ptr(i); if (v->idx == idx) { From d01c7cef60066e66ce8eb2342f84c0d9de402139 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 25 Dec 2020 13:03:24 +0300 Subject: [PATCH 096/216] mac cl warning fix --- modules/rgbd/src/hash_tsdf.cpp | 7 +++++++ modules/rgbd/src/opencl/hash_tsdf.cl | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 1ca5e98fbe1..ea8ebeb0017 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1009,6 +1009,9 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept // add updating of isActive for volUnits U_volUnitsData.getMat(ACCESS_RW).copyTo(volUnitsData); U_hashtable.getMat(ACCESS_RW).copyTo(indexes.volumes); + + U_volUnitsData.release(); + U_hashtable.release(); } void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) @@ -1535,6 +1538,10 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Upoints.getMat(ACCESS_RW).copyTo(new_points); Unormals.getMat(ACCESS_RW).copyTo(new_normals); + + Upoints.release(); + Unormals.release(); + U_volUnitsData.release(); } void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 473b40e7060..09efb1411e0 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -533,7 +533,7 @@ __kernel void raycast( const float3 camRot0 = cam2volRotGPU.s012; const float3 camRot1 = cam2volRotGPU.s456; const float3 camRot2 = cam2volRotGPU.s89a; - const float3 camTrans = cam2volRotGPU.s37b; + //const float3 camTrans = cam2volRotGPU.s37b; const float3 volRot0 = vol2camRotGPU.s012; const float3 volRot1 = vol2camRotGPU.s456; From b296279fc71454e349161efc5f193cf60252d24b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 28 Dec 2020 10:33:32 +0300 Subject: [PATCH 097/216] allCam2vol fix --- modules/rgbd/src/hash_tsdf.cpp | 9 --------- modules/rgbd/src/hash_tsdf.hpp | 1 - 2 files changed, 10 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ea8ebeb0017..bc01f605cb3 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -873,7 +873,6 @@ void HashTSDFVolumeGPU::reset() lastVisibleIndexes = cv::Mat(buff_lvl, 1, CV_32S); indexes = VolumesTable(); allVol2cam = cv::Mat(buff_lvl, 16, CV_32F); - allCam2vol = cv::Mat(buff_lvl, 16, CV_32F); } static inline bool find(cv::Mat v, Vec3i tsdf_idx, int lastVolIndex) @@ -1082,7 +1081,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma poses.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); allVol2cam.resize(buff_lvl); - allCam2vol.resize(buff_lvl); } indexes.update(idx, lastVolIndex); *_newIndices.ptr(vol_idx) = lastVolIndex; @@ -1116,13 +1114,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma *allVol2cam.ptr(idx, k) = vol2camMatrix[k]; } - Affine3f cam2volum(Affine3f(subvolumePose.inv()) * Affine3f(cameraPose)); - auto cam2volMatrix = cam2volum.matrix.val; - for (int k = 0; k < 16; k++) - { - *allCam2vol.ptr(idx, k) = cam2volMatrix[k]; - } - volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) { TsdfVoxel& v = reinterpret_cast(vv); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 467d86b29e4..49a6588e66b 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -170,7 +170,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::Mat poses; cv::Mat lastVisibleIndexes; cv::Mat allVol2cam; - cv::Mat allCam2vol; cv::Mat volUnitsData; cv::UMat pixNorms; VolumeIndex lastVolIndex; From db1a876cff2255be512113d1c34184531521f3c3 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 28 Dec 2020 11:10:14 +0300 Subject: [PATCH 098/216] rename num with bufferNum --- modules/rgbd/src/tsdf_functions.cpp | 40 ++++++++++++++--------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 244ab632020..8d118ea7428 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -407,8 +407,8 @@ void VolumesTable::update(Vec3i indx) { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int num = 1; - int start = hash * num * list_size; + int bufferNum = 1; + int start = hash * bufferNum * list_size; int i = start; while (i != -1) @@ -421,7 +421,7 @@ void VolumesTable::update(Vec3i indx) if (v->idx[0] == -2147483647) { v->idx = idx; - v->nextVolumeRow = getNextVolume(hash, num, i, start); + v->nextVolumeRow = getNextVolume(hash, bufferNum, i, start); indexes.push_back(indx); indexesGPU.push_back(idx); return; @@ -434,8 +434,8 @@ void VolumesTable::update(Vec3i indx, int row) { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int num = 1; - int start = hash * num * list_size; + int bufferNum = 1; + int start = hash * bufferNum * list_size; int i = start; while (i != -1) @@ -452,7 +452,7 @@ void VolumesTable::update(Vec3i indx, int row) { v->idx = idx; v->row = row; - v->nextVolumeRow = getNextVolume(hash, num, i, start); + v->nextVolumeRow = getNextVolume(hash, bufferNum, i, start); indexes.push_back(indx); indexesGPU.push_back(idx); return; @@ -465,8 +465,8 @@ void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int num = 1; - int start = hash * num * list_size; + int bufferNum = 1; + int start = hash * bufferNum * list_size; int i = start; while (i != -1) @@ -483,7 +483,7 @@ void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) if (v->idx[0] == -2147483647) { v->idx = idx; - v->nextVolumeRow = getNextVolume(hash, num, i, start); + v->nextVolumeRow = getNextVolume(hash, bufferNum, i, start); v->isActive = isActive; v->lastVisibleIndex = lastVisibleIndex; indexes.push_back(indx); @@ -498,8 +498,8 @@ void VolumesTable::updateActive(Vec3i indx, int isActive) { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int num = 1; - int start = hash * num * list_size; + int bufferNum = 1; + int start = hash * bufferNum * list_size; int i = start; while (i != -1) @@ -524,8 +524,8 @@ bool VolumesTable::getActive(Vec3i indx) const { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int num = 1; - int i = hash * num * list_size; + int bufferNum = 1; + int i = hash * bufferNum * list_size; while (i != -1) { const Volume_NODE& v = *volumes.ptr(i); @@ -538,20 +538,20 @@ bool VolumesTable::getActive(Vec3i indx) const return false; } -int VolumesTable::getNextVolume(int hash, int& num, int i, int start) +int VolumesTable::getNextVolume(int hash, int& bufferNum, int i, int start) { if (i != start && i % list_size == 0) { - if (num < bufferNums) + if (bufferNum < bufferNums) { - num++; + bufferNum++; } else { this->expand(); - num++; + bufferNum++; } - return hash * num * list_size; + return hash * bufferNum * list_size; } else { @@ -569,8 +569,8 @@ int VolumesTable::find_Volume(Vec3i indx) const { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int num = 1; - int i = hash * num * list_size; + int bufferNum = 1; + int i = hash * bufferNum * list_size; while (i != -1) { const Volume_NODE& v = *volumes.ptr(i); From 85dcd2b4cb51b768207d26f814b6865a2bcb3fdb Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 28 Dec 2020 11:36:43 +0300 Subject: [PATCH 099/216] remove nums hardcode --- modules/rgbd/src/opencl/hash_tsdf.cl | 8 +++++--- modules/rgbd/src/tsdf_functions.cpp | 12 ++++++------ modules/rgbd/src/tsdf_functions.hpp | 11 ++++++++--- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 09efb1411e0..4ad742d8b9a 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -4,6 +4,8 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +#define NAN_ELEMENT -2147483647 + typedef __INT8_TYPE__ int8_t; typedef __INT32_TYPE__ int32_t; @@ -66,7 +68,7 @@ static int findRow(__global struct Volume_NODE * hash_table, int4 indx, int num = 1; int i = hash * num * list_size; - int NAN_NUM = -2147483647; + int NAN_NUM = NAN_ELEMENT; while (i != NAN_NUM) { struct Volume_NODE v = hash_table[i]; @@ -88,7 +90,7 @@ static int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, int hash = calc_hash(indx) % hash_divisor; int num = 1; int i = hash * num * list_size; - int NAN_NUM = -2147483647; + int NAN_NUM = NAN_ELEMENT; while (i != NAN_NUM) { @@ -111,7 +113,7 @@ static void updateIsActive(__global struct Volume_NODE * hash_table, int4 indx, int hash = calc_hash(indx) % hash_divisor; int num = 1; int i = hash * num * list_size; - int NAN_NUM = -2147483647; + int NAN_NUM = NAN_ELEMENT; while (i != NAN_NUM) { __global struct Volume_NODE * v = (hash_table + i); diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 8d118ea7428..8b5ee5ae698 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -418,7 +418,7 @@ void VolumesTable::update(Vec3i indx) return; //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v->idx[0] == -2147483647) + if (v->idx[0] == NAN_ELEMENT) { v->idx = idx; v->nextVolumeRow = getNextVolume(hash, bufferNum, i, start); @@ -448,7 +448,7 @@ void VolumesTable::update(Vec3i indx, int row) } //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v->idx[0] == -2147483647) + if (v->idx[0] == NAN_ELEMENT) { v->idx = idx; v->row = row; @@ -480,7 +480,7 @@ void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) } //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v->idx[0] == -2147483647) + if (v->idx[0] == NAN_ELEMENT) { v->idx = idx; v->nextVolumeRow = getNextVolume(hash, bufferNum, i, start); @@ -512,7 +512,7 @@ void VolumesTable::updateActive(Vec3i indx, int isActive) } //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v->idx[0] == -2147483647) + if (v->idx[0] == NAN_ELEMENT) { return; } @@ -531,7 +531,7 @@ bool VolumesTable::getActive(Vec3i indx) const const Volume_NODE& v = *volumes.ptr(i); if (v.idx == idx) return (v.isActive == 1); - if (v.idx[0] == -2147483647) + if (v.idx[0] == NAN_ELEMENT) return false; i = v.nextVolumeRow; } @@ -578,7 +578,7 @@ int VolumesTable::find_Volume(Vec3i indx) const return v.row; //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) - if (v.idx[0] == -2147483647) + if (v.idx[0] == NAN_ELEMENT) return -2; i = v.nextVolumeRow; } diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index ce72069d4a4..1fe06f8c3ba 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -10,6 +10,8 @@ #include #include "tsdf.hpp" +#define NAN_ELEMENT -2147483647 + namespace cv { namespace kinfu @@ -56,12 +58,15 @@ struct Volume_NODE size_t calc_hash(Vec4i x); +const int _hash_divisor = 32768; +const int _list_size = 4; + class VolumesTable { public: - int hash_divisor = 32768; - int list_size = 4; - int bufferNums = 1; + int hash_divisor = _hash_divisor; + int list_size = _list_size; + int bufferNums = 1; cv::Mat volumes; std::vector indexes; From b5e86e85ba4a7d42a69f1719df84959d51e3bf79 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 28 Dec 2020 11:42:26 +0300 Subject: [PATCH 100/216] minor fix --- modules/rgbd/src/tsdf_functions.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 1fe06f8c3ba..21e8e75dd0b 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -48,7 +48,7 @@ void integrateVolumeUnit( struct Volume_NODE { - Vec4i idx = Vec4i(-2147483647); + Vec4i idx = Vec4i(NAN_ELEMENT); int32_t row = -1; int32_t nextVolumeRow = -1; int32_t isActive = 0; @@ -71,7 +71,7 @@ class VolumesTable cv::Mat volumes; std::vector indexes; std::vector indexesGPU; - cv::Vec4i nan4 = cv::Vec4i(-2147483647); + cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT); VolumesTable(); ~VolumesTable() {}; From 0c62905ca8459a742bed222ef00f84088f7b8cc1 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 28 Dec 2020 12:31:26 +0300 Subject: [PATCH 101/216] change logic of calculating hash --- modules/rgbd/src/opencl/hash_tsdf.cl | 12 +++++------ modules/rgbd/src/tsdf_functions.cpp | 30 ++++++++++++++-------------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 4ad742d8b9a..6a18fc1d01c 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -66,8 +66,8 @@ static int findRow(__global struct Volume_NODE * hash_table, int4 indx, { int hash = calc_hash(indx) % hash_divisor; - int num = 1; - int i = hash * num * list_size; + int bufferNum = 0; + int i = (bufferNum * list_size * hash_divisor) + (hash * list_size); int NAN_NUM = NAN_ELEMENT; while (i != NAN_NUM) { @@ -88,8 +88,8 @@ static int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; - int num = 1; - int i = hash * num * list_size; + int bufferNum = 0; + int i = (bufferNum * list_size * hash_divisor) + (hash * list_size); int NAN_NUM = NAN_ELEMENT; while (i != NAN_NUM) @@ -111,8 +111,8 @@ static void updateIsActive(__global struct Volume_NODE * hash_table, int4 indx, int list_size, int bufferNums, int hash_divisor) { int hash = calc_hash(indx) % hash_divisor; - int num = 1; - int i = hash * num * list_size; + int bufferNum = 0; + int i = (bufferNum * list_size * hash_divisor) + (hash * list_size); int NAN_NUM = NAN_ELEMENT; while (i != NAN_NUM) { diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 8b5ee5ae698..639fa4398e1 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -407,8 +407,8 @@ void VolumesTable::update(Vec3i indx) { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int bufferNum = 1; - int start = hash * bufferNum * list_size; + int bufferNum = 0; + int start = (bufferNum * list_size * hash_divisor) + (hash * list_size); int i = start; while (i != -1) @@ -434,8 +434,8 @@ void VolumesTable::update(Vec3i indx, int row) { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int bufferNum = 1; - int start = hash * bufferNum * list_size; + int bufferNum = 0; + int start = (bufferNum * list_size * hash_divisor) + (hash * list_size); int i = start; while (i != -1) @@ -465,8 +465,8 @@ void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int bufferNum = 1; - int start = hash * bufferNum * list_size; + int bufferNum = 0; + int start = (bufferNum * list_size * hash_divisor) + (hash * list_size); int i = start; while (i != -1) @@ -498,8 +498,8 @@ void VolumesTable::updateActive(Vec3i indx, int isActive) { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int bufferNum = 1; - int start = hash * bufferNum * list_size; + int bufferNum = 0; + int start = (bufferNum * list_size * hash_divisor) + (hash * list_size); int i = start; while (i != -1) @@ -524,8 +524,8 @@ bool VolumesTable::getActive(Vec3i indx) const { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int bufferNum = 1; - int i = hash * bufferNum * list_size; + int bufferNum = 0; + int i = (bufferNum * list_size * hash_divisor) + (hash * list_size); while (i != -1) { const Volume_NODE& v = *volumes.ptr(i); @@ -542,7 +542,7 @@ int VolumesTable::getNextVolume(int hash, int& bufferNum, int i, int start) { if (i != start && i % list_size == 0) { - if (bufferNum < bufferNums) + if (bufferNum < bufferNums-1) { bufferNum++; } @@ -551,7 +551,7 @@ int VolumesTable::getNextVolume(int hash, int& bufferNum, int i, int start) this->expand(); bufferNum++; } - return hash * bufferNum * list_size; + return (bufferNum * list_size * hash_divisor) + (hash * list_size); } else { @@ -561,7 +561,7 @@ int VolumesTable::getNextVolume(int hash, int& bufferNum, int i, int start) void VolumesTable::expand() { - this->volumes.resize(hash_divisor * (bufferNums + 1)); + this->volumes.resize(hash_divisor * (bufferNums)); this->bufferNums++; } @@ -569,8 +569,8 @@ int VolumesTable::find_Volume(Vec3i indx) const { Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); - int bufferNum = 1; - int i = hash * bufferNum * list_size; + int bufferNum = 0; + int i = (bufferNum * list_size * hash_divisor) + (hash * list_size); while (i != -1) { const Volume_NODE& v = *volumes.ptr(i); From 16c08402591e878e834ce36ef788050a26e21051 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 28 Dec 2020 12:40:48 +0300 Subject: [PATCH 102/216] replace float16 by vload16 --- modules/rgbd/src/opencl/hash_tsdf.cl | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 6a18fc1d01c..853e74a7678 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -329,11 +329,7 @@ __kernel void integrateAllVolumeUnits( __global const float * p_vol2camMatrix = (__global const float *) (allVol2camMatrix + val2cam_offset + (row) * 16); - const float16 vol2camMatrix = (float16) ( - p_vol2camMatrix[0], p_vol2camMatrix[1], p_vol2camMatrix[2], p_vol2camMatrix[3], - p_vol2camMatrix[4], p_vol2camMatrix[5], p_vol2camMatrix[6], p_vol2camMatrix[7], - p_vol2camMatrix[8], p_vol2camMatrix[9], p_vol2camMatrix[10], p_vol2camMatrix[11], - p_vol2camMatrix[12], p_vol2camMatrix[13], p_vol2camMatrix[14], p_vol2camMatrix[15]); + const float16 vol2camMatrix = vload16(0, p_vol2camMatrix); integrateVolumeUnit( i, j, From 50358d4c2aec20c87cdbedc2f85335cab371f12f Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 28 Dec 2020 12:57:33 +0300 Subject: [PATCH 103/216] replace true with false in zFirstMemOrder --- modules/rgbd/src/hash_tsdf.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 49a6588e66b..7db76d0cfed 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -124,9 +124,9 @@ class HashTSDFVolumeGPU : public HashTSDFVolume { public: HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); + float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = false); - HashTSDFVolumeGPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); + HashTSDFVolumeGPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = false); void reset() override; From ef2f417deaea56245d8e5b5ca370a3b575600845 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 28 Dec 2020 17:31:24 +0300 Subject: [PATCH 104/216] poses fix --- 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 bc01f605cb3..f60e051474b 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1137,9 +1137,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma VolumeIndex idx = indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == lastVolIndex - 1) return; - Point3f volumeUnitPos = volumeUnitIdxToVolume(*poses.ptr(idx)); + Point3f volumeUnitPos = volumeUnitIdxToVolume(tsdf_idx); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; - if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { indexes.updateActive(tsdf_idx, 0); From 3cf1e29b4a43d689d0a9be3b13eb90db542ab57c Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 29 Dec 2020 13:18:03 +0300 Subject: [PATCH 105/216] add new tests --- modules/rgbd/test/test_tsdf.cpp | 72 +++++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 3c96f0ee868..cf94ac74731 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -473,6 +473,8 @@ void valid_points_test(bool isHashTSDF) ASSERT_LT(0.5 - percentValidity, 0.3); } +#ifndef HAVE_OPENCL + TEST(TSDF, raycast_normals) { normal_test(false, true, false, false); @@ -513,4 +515,74 @@ TEST(HashTSDF, valid_points) valid_points_test(true); } +#else + +TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); } +TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } +TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } +TEST(TSDF_GPU, valid_points) { valid_points_test(false); } + +TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } +TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } +TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } +TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } + +TEST(TSDF_CPU, raycast_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(false, true, false, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(TSDF_CPU, fetch_points_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(false, false, true, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(TSDF_CPU, fetch_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(false, false, false, true); + cv::ocl::setUseOpenCL(true); +} + +TEST(TSDF_CPU, valid_points) +{ + cv::ocl::setUseOpenCL(false); + valid_points_test(false); + cv::ocl::setUseOpenCL(true); +} + +TEST(HashTSDF_CPU, raycast_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(true, true, false, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(HashTSDF_CPU, fetch_points_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(true, false, true, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(HashTSDF_CPU, fetch_normals) +{ + cv::ocl::setUseOpenCL(false); + normal_test(true, false, false, true); + cv::ocl::setUseOpenCL(true); +} + +TEST(HashTSDF_CPU, valid_points) +{ + cv::ocl::setUseOpenCL(false); + valid_points_test(true); + cv::ocl::setUseOpenCL(true); +} + +#endif + }} // namespace From 4cb3a38237008ffffe2ef6e7ccb793dc0b6c6d18 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 29 Dec 2020 15:03:48 +0300 Subject: [PATCH 106/216] debug print --- modules/rgbd/src/opencl/hash_tsdf.cl | 2 ++ 1 file changed, 2 insertions(+) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 853e74a7678..1ad5330fecc 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -315,6 +315,8 @@ __kernel void integrateAllVolumeUnits( int j = get_global_id(1); int k = get_global_id(2); + printf("x=%d, x=%d, vu=%d \n", i, j, k); + int4 v = totalVolUnits[k]; int row = findRow(hash_table, v, list_size, bufferNums, hash_divisor); if (row < 0 || row > lastVolIndex-1) From 6a4c76d997403d3d8a6f988838bbd8132b12c686 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 29 Dec 2020 16:21:03 +0300 Subject: [PATCH 107/216] debug print 1 --- modules/rgbd/test/test_tsdf.cpp | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index cf94ac74731..47b57336c42 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -522,10 +522,22 @@ TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } TEST(TSDF_GPU, valid_points) { valid_points_test(false); } -TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } -TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } -TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } -TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } +TEST(HashTSDF_GPU, raycast_normals) { + cv::ocl::Context context; + cv::ocl::Device(context.device(1)); + normal_test(true, true, false, false); } +TEST(HashTSDF_GPU, fetch_points_normals) { + cv::ocl::Context context; + cv::ocl::Device(context.device(1)); + normal_test(true, false, true, false); } +TEST(HashTSDF_GPU, fetch_normals) { + cv::ocl::Context context; + cv::ocl::Device(context.device(1)); + normal_test(true, false, false, true); } +TEST(HashTSDF_GPU, valid_points) { + cv::ocl::Context context; + cv::ocl::Device(context.device(1)); + valid_points_test(true); } TEST(TSDF_CPU, raycast_normals) { From 69582fb77abb52b0a8f14d4cf35a25e131096a35 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 29 Dec 2020 20:16:37 +0300 Subject: [PATCH 108/216] replace [] with .s --- modules/rgbd/src/opencl/hash_tsdf.cl | 84 ++++++++++++++-------------- 1 file changed, 42 insertions(+), 42 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 1ad5330fecc..4ea89277e0b 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -55,9 +55,9 @@ static uint calc_hash(int4 x) unsigned int seed = 0; //uint GOLDEN_RATIO = 0x9e3779b9; unsigned int GOLDEN_RATIO = 0x9e3779b9; - seed ^= x[0] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed ^= x[1] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x.s0 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x.s1 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x.s2 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); return seed; } @@ -72,9 +72,9 @@ static int findRow(__global struct Volume_NODE * hash_table, int4 indx, while (i != NAN_NUM) { struct Volume_NODE v = hash_table[i]; - if (v.idx[0] == indx[0] && - v.idx[1] == indx[1] && - v.idx[2] == indx[2]) + if (v.idx.s0 == indx.s0 && + v.idx.s1 == indx.s1 && + v.idx.s2 == indx.s2) return v.row; if (v.idx.x == NAN_NUM) return -2; @@ -96,11 +96,11 @@ static int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, { struct Volume_NODE v = hash_table[i]; - if (v.idx[0] == indx[0] && - v.idx[1] == indx[1] && - v.idx[2] == indx[2]) + if (v.idx.s0 == indx.s0 && + v.idx.s1 == indx.s1 && + v.idx.s2 == indx.s2) return v.isActive; - if (v.idx[0] == NAN_NUM) + if (v.idx.s0 == NAN_NUM) return 0; i = v.nextVolumeRow; } @@ -118,11 +118,11 @@ static void updateIsActive(__global struct Volume_NODE * hash_table, int4 indx, { __global struct Volume_NODE * v = (hash_table + i); - if (v->idx[0] == indx[0] && - v->idx[1] == indx[1] && - v->idx[2] == indx[2]) + if (v->idx.s0 == indx.s0 && + v->idx.s1 == indx.s1 && + v->idx.s2 == indx.s2) v->isActive = isActive; - if (v->idx[0] == NAN_NUM) + if (v->idx.s0 == NAN_NUM) return; i = v->nextVolumeRow; } @@ -361,9 +361,9 @@ static struct TsdfVoxel _at(int3 volumeIdx, int row, { //! Out of bounds - if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || - (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || - (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) + if ((volumeIdx.s0 >= volumeUnitResolution || volumeIdx.s0 < 0) || + (volumeIdx.s1 >= volumeUnitResolution || volumeIdx.s1 < 0) || + (volumeIdx.s2 >= volumeUnitResolution || volumeIdx.s2 < 0)) { struct TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -373,9 +373,9 @@ static struct TsdfVoxel _at(int3 volumeIdx, int row, __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) (allVolumePtr + table_offset + (row) * 16*16*16); int coordBase = - volumeIdx[0] * volStrides[0] + - volumeIdx[1] * volStrides[1] + - volumeIdx[2] * volStrides[2]; + volumeIdx.s0 * volStrides.s0 + + volumeIdx.s1 * volStrides.s1 + + volumeIdx.s2 * volStrides.s2; return volData[coordBase]; } @@ -398,9 +398,9 @@ static struct TsdfVoxel _atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int ro __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) (allVolumePtr + table_offset + (row) * 16*16*16); int coordBase = - volUnitLocalIdx[0] * volStrides[0] + - volUnitLocalIdx[1] * volStrides[1] + - volUnitLocalIdx[2] * volStrides[2]; + volUnitLocalIdx.s0 * volStrides.s0 + + volUnitLocalIdx.s1 * volStrides.s1 + + volUnitLocalIdx.s2 * volStrides.s2; return volData[coordBase]; } @@ -442,19 +442,19 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum // VoxelToVolumeUnitIdx() // TODO: add assertion - if (!(vuRes & (vuRes - 1))) int3 volumeUnitIdx = (int3) ( - floor ( (float) pt[0] / volResolution[0]), - floor ( (float) pt[1] / volResolution[1]), - floor ( (float) pt[2] / volResolution[2]) ); + floor ( (float) pt.s0 / volResolution.s0), + floor ( (float) pt.s1 / volResolution.s1), + floor ( (float) pt.s2 / volResolution.s2) ); int4 volumeUnitIdx4 = (int4) ( - floor ( (float) pt[0] / volResolution[0]), - floor ( (float) pt[1] / volResolution[1]), - floor ( (float) pt[2] / volResolution[2]), 0 ); + floor ( (float) pt.s0 / volResolution.s0), + floor ( (float) pt.s1 / volResolution.s1), + floor ( (float) pt.s2 / volResolution.s2), 0 ); - int dictIdx = (volumeUnitIdx[0] & 1) - + (volumeUnitIdx[1] & 1) * 2 - + (volumeUnitIdx[2] & 1) * 4; + int dictIdx = (volumeUnitIdx.s0 & 1) + + (volumeUnitIdx.s1 & 1) * 2 + + (volumeUnitIdx.s2 & 1) * 4; int it = iterMap[dictIdx]; @@ -468,7 +468,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum } } - struct TsdfVoxel tmp = _atVolumeUnit(pt, volumeUnitIdx, it, lastVolIndex, volResolution[0], volStrides, allVolumePtr, table_offset) ; + struct TsdfVoxel tmp = _atVolumeUnit(pt, volumeUnitIdx, it, lastVolIndex, volResolution.s0, volStrides, allVolumePtr, table_offset) ; vals[i] = tsdfToFloat( tmp.tsdf ); } @@ -545,7 +545,7 @@ __kernel void raycast( dot(planed, camRot1), dot(planed, camRot2)); - float3 orig = (float3) (cam2volTransGPU[0], cam2volTransGPU[1], cam2volTransGPU[2]); + float3 orig = (float3) (cam2volTransGPU.s0, cam2volTransGPU.s1, cam2volTransGPU.s2); float3 dir = fast_normalize(planed); float tmin = 0; @@ -582,16 +582,16 @@ __kernel void raycast( //TsdfVoxel currVoxel // VolumeUnitIdxToVolume() float3 currVolUnitPos = (float3) - (( (float) (currVolumeUnitIdx[0]) * volumeUnitSize), - ( (float) (currVolumeUnitIdx[1]) * volumeUnitSize), - ( (float) (currVolumeUnitIdx[2]) * volumeUnitSize) ); + (( (float) (currVolumeUnitIdx.s0) * volumeUnitSize), + ( (float) (currVolumeUnitIdx.s1) * volumeUnitSize), + ( (float) (currVolumeUnitIdx.s2) * volumeUnitSize) ); // VolumeToVoxelCoord() float3 pos = currRayPos - currVolUnitPos; volUnitLocalIdx = (int3) - (( floor ( (float) (pos[0]) * voxelSizeInv) ), - ( floor ( (float) (pos[1]) * voxelSizeInv) ), - ( floor ( (float) (pos[2]) * voxelSizeInv) ) ); + (( floor ( (float) (pos.s0) * voxelSizeInv) ), + ( floor ( (float) (pos.s1) * voxelSizeInv) ), + ( floor ( (float) (pos.s2) * voxelSizeInv) ) ); struct TsdfVoxel currVoxel = _at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); @@ -606,8 +606,8 @@ __kernel void raycast( float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if ( !isnan(tInterp) && !isinf(tInterp) ) { - int3 volResolution = (int3) (volResolution4[0], volResolution4[1], volResolution4[2]); - int3 volDims = (int3) (volDims4[0], volDims4[1], volDims4[2]); + int3 volResolution = (int3) (volResolution4.s0, volResolution4.s1, volResolution4.s2); + int3 volDims = (int3) (volDims4.s0, volDims4.s1, volDims4.s2); float3 pv = orig + tInterp * dir; float3 nv = getNormalVoxel( pv, allVolumePtr, volResolution, volDims, neighbourCoords, From 12db52c24f19b2b5922857c88e35d77a90eac816 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 29 Dec 2020 20:23:24 +0300 Subject: [PATCH 109/216] minor fix --- modules/rgbd/src/opencl/hash_tsdf.cl | 41 ++++++++++++++-------------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 4ea89277e0b..4021ca2d7c8 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -57,7 +57,7 @@ static uint calc_hash(int4 x) unsigned int GOLDEN_RATIO = 0x9e3779b9; seed ^= x.s0 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); seed ^= x.s1 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed ^= x.s2 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); return seed; } @@ -74,7 +74,7 @@ static int findRow(__global struct Volume_NODE * hash_table, int4 indx, struct Volume_NODE v = hash_table[i]; if (v.idx.s0 == indx.s0 && v.idx.s1 == indx.s1 && - v.idx.s2 == indx.s2) + v.idx[2] == indx[2]) return v.row; if (v.idx.x == NAN_NUM) return -2; @@ -98,7 +98,7 @@ static int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, if (v.idx.s0 == indx.s0 && v.idx.s1 == indx.s1 && - v.idx.s2 == indx.s2) + v.idx[2] == indx[2]) return v.isActive; if (v.idx.s0 == NAN_NUM) return 0; @@ -120,7 +120,7 @@ static void updateIsActive(__global struct Volume_NODE * hash_table, int4 indx, if (v->idx.s0 == indx.s0 && v->idx.s1 == indx.s1 && - v->idx.s2 == indx.s2) + v->idx[2] == indx[2]) v->isActive = isActive; if (v->idx.s0 == NAN_NUM) return; @@ -363,7 +363,7 @@ static struct TsdfVoxel _at(int3 volumeIdx, int row, //! Out of bounds if ((volumeIdx.s0 >= volumeUnitResolution || volumeIdx.s0 < 0) || (volumeIdx.s1 >= volumeUnitResolution || volumeIdx.s1 < 0) || - (volumeIdx.s2 >= volumeUnitResolution || volumeIdx.s2 < 0)) + (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) { struct TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -375,7 +375,7 @@ static struct TsdfVoxel _at(int3 volumeIdx, int row, (allVolumePtr + table_offset + (row) * 16*16*16); int coordBase = volumeIdx.s0 * volStrides.s0 + volumeIdx.s1 * volStrides.s1 + - volumeIdx.s2 * volStrides.s2; + volumeIdx[2] * volStrides[2]; return volData[coordBase]; } @@ -400,7 +400,7 @@ static struct TsdfVoxel _atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int ro int coordBase = volUnitLocalIdx.s0 * volStrides.s0 + volUnitLocalIdx.s1 * volStrides.s1 + - volUnitLocalIdx.s2 * volStrides.s2; + volUnitLocalIdx[2] * volStrides[2]; return volData[coordBase]; } @@ -433,7 +433,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum }; const int nVals = 6; - float vals[nVals]; + float vals[6]; for (int i = 0; i < nVals; i++) { @@ -444,17 +444,17 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum int3 volumeUnitIdx = (int3) ( floor ( (float) pt.s0 / volResolution.s0), floor ( (float) pt.s1 / volResolution.s1), - floor ( (float) pt.s2 / volResolution.s2) ); + floor ( (float) pt[2] / volResolution[2]) ); int4 volumeUnitIdx4 = (int4) ( floor ( (float) pt.s0 / volResolution.s0), floor ( (float) pt.s1 / volResolution.s1), - floor ( (float) pt.s2 / volResolution.s2), 0 ); + floor ( (float) pt[2] / volResolution[2]), 0 ); int dictIdx = (volumeUnitIdx.s0 & 1) + (volumeUnitIdx.s1 & 1) * 2 - + (volumeUnitIdx.s2 & 1) * 4; + + (volumeUnitIdx[2] & 1) * 4; int it = iterMap[dictIdx]; @@ -473,10 +473,11 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum } - for (int c = 0; c < 3; c++) - { - normal[c] = vals[c * 2] - vals[c * 2 + 1]; - } + //for (int c = 0; c < 3; c++){normal[c] = vals[c * 2] - vals[c * 2 + 1];} + + normal.s0 = vals[0 * 2] - vals[0 * 2 + 1]; + normal.s1 = vals[1 * 2] - vals[1 * 2 + 1]; + normal.s2 = vals[2 * 2] - vals[2 * 2 + 1]; float norm = sqrt(normal.x*normal.x @@ -545,7 +546,7 @@ __kernel void raycast( dot(planed, camRot1), dot(planed, camRot2)); - float3 orig = (float3) (cam2volTransGPU.s0, cam2volTransGPU.s1, cam2volTransGPU.s2); + float3 orig = (float3) (cam2volTransGPU.s0, cam2volTransGPU.s1, cam2volTransGPU[2]); float3 dir = fast_normalize(planed); float tmin = 0; @@ -584,14 +585,14 @@ __kernel void raycast( float3 currVolUnitPos = (float3) (( (float) (currVolumeUnitIdx.s0) * volumeUnitSize), ( (float) (currVolumeUnitIdx.s1) * volumeUnitSize), - ( (float) (currVolumeUnitIdx.s2) * volumeUnitSize) ); + ( (float) (currVolumeUnitIdx[2]) * volumeUnitSize) ); // VolumeToVoxelCoord() float3 pos = currRayPos - currVolUnitPos; volUnitLocalIdx = (int3) (( floor ( (float) (pos.s0) * voxelSizeInv) ), ( floor ( (float) (pos.s1) * voxelSizeInv) ), - ( floor ( (float) (pos.s2) * voxelSizeInv) ) ); + ( floor ( (float) (pos[2]) * voxelSizeInv) ) ); struct TsdfVoxel currVoxel = _at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); @@ -606,8 +607,8 @@ __kernel void raycast( float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if ( !isnan(tInterp) && !isinf(tInterp) ) { - int3 volResolution = (int3) (volResolution4.s0, volResolution4.s1, volResolution4.s2); - int3 volDims = (int3) (volDims4.s0, volDims4.s1, volDims4.s2); + int3 volResolution = (int3) (volResolution4.s0, volResolution4.s1, volResolution4[2]); + int3 volDims = (int3) (volDims4.s0, volDims4.s1, volDims4[2]); float3 pv = orig + tInterp * dir; float3 nv = getNormalVoxel( pv, allVolumePtr, volResolution, volDims, neighbourCoords, From b0177e23dbd0894ff175bf81c05cc3db7ada5020 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 29 Dec 2020 20:25:43 +0300 Subject: [PATCH 110/216] minor fix 1 --- modules/rgbd/src/opencl/hash_tsdf.cl | 30 ++++++++++++++-------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 4021ca2d7c8..5f7c49e4c6a 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -57,7 +57,7 @@ static uint calc_hash(int4 x) unsigned int GOLDEN_RATIO = 0x9e3779b9; seed ^= x.s0 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); seed ^= x.s1 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= x.s2 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); return seed; } @@ -74,7 +74,7 @@ static int findRow(__global struct Volume_NODE * hash_table, int4 indx, struct Volume_NODE v = hash_table[i]; if (v.idx.s0 == indx.s0 && v.idx.s1 == indx.s1 && - v.idx[2] == indx[2]) + v.idx.s2 == indx.s2) return v.row; if (v.idx.x == NAN_NUM) return -2; @@ -98,7 +98,7 @@ static int getIsActive(__global struct Volume_NODE * hash_table, int4 indx, if (v.idx.s0 == indx.s0 && v.idx.s1 == indx.s1 && - v.idx[2] == indx[2]) + v.idx.s2 == indx.s2) return v.isActive; if (v.idx.s0 == NAN_NUM) return 0; @@ -120,7 +120,7 @@ static void updateIsActive(__global struct Volume_NODE * hash_table, int4 indx, if (v->idx.s0 == indx.s0 && v->idx.s1 == indx.s1 && - v->idx[2] == indx[2]) + v->idx.s2 == indx.s2) v->isActive = isActive; if (v->idx.s0 == NAN_NUM) return; @@ -363,7 +363,7 @@ static struct TsdfVoxel _at(int3 volumeIdx, int row, //! Out of bounds if ((volumeIdx.s0 >= volumeUnitResolution || volumeIdx.s0 < 0) || (volumeIdx.s1 >= volumeUnitResolution || volumeIdx.s1 < 0) || - (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) + (volumeIdx.s2 >= volumeUnitResolution || volumeIdx.s2 < 0)) { struct TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -375,7 +375,7 @@ static struct TsdfVoxel _at(int3 volumeIdx, int row, (allVolumePtr + table_offset + (row) * 16*16*16); int coordBase = volumeIdx.s0 * volStrides.s0 + volumeIdx.s1 * volStrides.s1 + - volumeIdx[2] * volStrides[2]; + volumeIdx.s2 * volStrides.s2; return volData[coordBase]; } @@ -400,7 +400,7 @@ static struct TsdfVoxel _atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int ro int coordBase = volUnitLocalIdx.s0 * volStrides.s0 + volUnitLocalIdx.s1 * volStrides.s1 + - volUnitLocalIdx[2] * volStrides[2]; + volUnitLocalIdx.s2 * volStrides.s2; return volData[coordBase]; } @@ -444,17 +444,17 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum int3 volumeUnitIdx = (int3) ( floor ( (float) pt.s0 / volResolution.s0), floor ( (float) pt.s1 / volResolution.s1), - floor ( (float) pt[2] / volResolution[2]) ); + floor ( (float) pt.s2 / volResolution.s2) ); int4 volumeUnitIdx4 = (int4) ( floor ( (float) pt.s0 / volResolution.s0), floor ( (float) pt.s1 / volResolution.s1), - floor ( (float) pt[2] / volResolution[2]), 0 ); + floor ( (float) pt.s2 / volResolution.s2), 0 ); int dictIdx = (volumeUnitIdx.s0 & 1) + (volumeUnitIdx.s1 & 1) * 2 - + (volumeUnitIdx[2] & 1) * 4; + + (volumeUnitIdx.s2 & 1) * 4; int it = iterMap[dictIdx]; @@ -546,7 +546,7 @@ __kernel void raycast( dot(planed, camRot1), dot(planed, camRot2)); - float3 orig = (float3) (cam2volTransGPU.s0, cam2volTransGPU.s1, cam2volTransGPU[2]); + float3 orig = (float3) (cam2volTransGPU.s0, cam2volTransGPU.s1, cam2volTransGPU.s2); float3 dir = fast_normalize(planed); float tmin = 0; @@ -585,14 +585,14 @@ __kernel void raycast( float3 currVolUnitPos = (float3) (( (float) (currVolumeUnitIdx.s0) * volumeUnitSize), ( (float) (currVolumeUnitIdx.s1) * volumeUnitSize), - ( (float) (currVolumeUnitIdx[2]) * volumeUnitSize) ); + ( (float) (currVolumeUnitIdx.s2) * volumeUnitSize) ); // VolumeToVoxelCoord() float3 pos = currRayPos - currVolUnitPos; volUnitLocalIdx = (int3) (( floor ( (float) (pos.s0) * voxelSizeInv) ), ( floor ( (float) (pos.s1) * voxelSizeInv) ), - ( floor ( (float) (pos[2]) * voxelSizeInv) ) ); + ( floor ( (float) (pos.s2) * voxelSizeInv) ) ); struct TsdfVoxel currVoxel = _at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); @@ -607,8 +607,8 @@ __kernel void raycast( float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if ( !isnan(tInterp) && !isinf(tInterp) ) { - int3 volResolution = (int3) (volResolution4.s0, volResolution4.s1, volResolution4[2]); - int3 volDims = (int3) (volDims4.s0, volDims4.s1, volDims4[2]); + int3 volResolution = (int3) (volResolution4.s0, volResolution4.s1, volResolution4.s2); + int3 volDims = (int3) (volDims4.s0, volDims4.s1, volDims4.s2); float3 pv = orig + tInterp * dir; float3 nv = getNormalVoxel( pv, allVolumePtr, volResolution, volDims, neighbourCoords, From 3b051634d00114c9835f0e95b3a274e2b91094a7 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 30 Dec 2020 18:32:30 +0300 Subject: [PATCH 111/216] remove print --- modules/rgbd/src/opencl/hash_tsdf.cl | 2 -- 1 file changed, 2 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 5f7c49e4c6a..e7e48c7c964 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -315,8 +315,6 @@ __kernel void integrateAllVolumeUnits( int j = get_global_id(1); int k = get_global_id(2); - printf("x=%d, x=%d, vu=%d \n", i, j, k); - int4 v = totalVolUnits[k]; int row = findRow(hash_table, v, list_size, bufferNums, hash_divisor); if (row < 0 || row > lastVolIndex-1) From 9f1e8fb9bdc5622689e60b9e728f38a01f117e2d Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 31 Dec 2020 13:08:28 +0300 Subject: [PATCH 112/216] hashtsdf work fix --- modules/rgbd/src/hash_tsdf.cpp | 25 +++++++++++++++---------- modules/rgbd/src/hash_tsdf.hpp | 5 ++++- modules/rgbd/src/large_kinfu.cpp | 6 +++--- modules/rgbd/src/submap.hpp | 12 ++++++------ modules/rgbd/src/volume.cpp | 4 +--- modules/rgbd/test/test_tsdf.cpp | 2 +- 6 files changed, 30 insertions(+), 24 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index f60e051474b..49d1c82c381 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -72,6 +72,9 @@ void HashTSDFVolumeCPU::reset() CV_TRACE_FUNCTION(); lastVolIndex = 0; volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + frameParams = Vec6f(); + pixNorms = Mat(); + volumeUnits = VolumeUnitIndexes(); } void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) @@ -1135,7 +1138,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Vec3i tsdf_idx = _totalVolUnits[i]; VolumeIndex idx = indexes.find_Volume(tsdf_idx); - if (idx < 0 || idx == lastVolIndex - 1) return; + if (idx < 0 || idx == lastVolIndex) return; Point3f volumeUnitPos = volumeUnitIdxToVolume(tsdf_idx); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; @@ -1147,7 +1150,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Point2f cameraPoint = proj(volUnitInCamSpace); if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) { - assert(idx == lastVolIndex - 1); + assert(idx != lastVolIndex); *lastVisibleIndexes.ptr(idx, 0) = frameId; indexes.update(tsdf_idx, 1, frameId); } @@ -1217,7 +1220,7 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeInd TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const { - if (indx < 0 || indx > lastVolIndex - 1) + if (indx < 0 || indx > lastVolIndex) { TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.f); @@ -1244,7 +1247,7 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const VolumeIndex iterMap[8]; for (int i = 0; i < 8; i++) { - iterMap[i] = lastVolIndex - 1; + iterMap[i] = lastVolIndex; queried[i] = false; } @@ -1299,7 +1302,7 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const for (int i = 0; i < 8; i++) { - iterMap[i] = lastVolIndex - 1; + iterMap[i] = lastVolIndex; queried[i] = false; } @@ -1661,20 +1664,22 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con #endif //template -Ptr makeHashTSDFVolume(const VolumeParams& _volumeParams) +Ptr makeHashTSDFVolume(const VolumeParams& _params) { -#ifdef HAVE_OPENCL +#ifdef _HAVE_OPENCL if (ocl::useOpenCL()) - return makePtr(_volumeParams); + return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + _params.depthTruncThreshold, _params.unitResolution); #endif - return makePtr(_volumeParams); + return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + _params.depthTruncThreshold, _params.unitResolution); } //template Ptr makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float truncateThreshold, int volumeUnitResolution) { -#ifdef HAVE_OPENCL +#ifdef _HAVE_OPENCL if (ocl::useOpenCL()) return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, volumeUnitResolution); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 7db76d0cfed..1d5d2e2fde5 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -26,6 +26,9 @@ class HashTSDFVolume : public Volume virtual ~HashTSDFVolume() = default; + virtual int getVisibleBlocks(int currFrameId, int frameThreshold) const = 0; + virtual size_t getTotalVolumeUnits() const = 0; + public: int maxWeight; float truncDist; @@ -141,7 +144,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; - VolumeIndex getTotalVolumeUnits() const { return lastVolIndex; } + size_t getTotalVolumeUnits() const { return size_t(lastVolIndex); } int getVisibleBlocks(int currFrameId, int frameThreshold) const; //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index dedeabb82db..16006713c53 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -316,21 +316,21 @@ template void LargeKinfuImpl::getCloud(OutputArray p, OutputArray n) const { auto currSubmap = submapMgr->getCurrentSubmap(); - currSubmap->volume.fetchPointsNormals(p, n); + currSubmap->volume->fetchPointsNormals(p, n); } template void LargeKinfuImpl::getPoints(OutputArray points) const { auto currSubmap = submapMgr->getCurrentSubmap(); - currSubmap->volume.fetchPointsNormals(points, noArray()); + currSubmap->volume->fetchPointsNormals(points, noArray()); } template void LargeKinfuImpl::getNormals(InputArray points, OutputArray normals) const { auto currSubmap = submapMgr->getCurrentSubmap(); - currSubmap->volume.fetchNormals(points, normals); + currSubmap->volume->fetchNormals(points, normals); } // importing class diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 65a2bed6229..b40023b6a56 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -42,7 +42,7 @@ class Submap Submap(int _id, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(), int _startFrameId = 0) - : id(_id), pose(_pose), cameraPose(Affine3f::Identity()), startFrameId(_startFrameId), volume(volumeParams) + : id(_id), pose(_pose), cameraPose(Affine3f::Identity()), startFrameId(_startFrameId), volume(makeHashTSDFVolume(volumeParams)) { std::cout << "Created volume\n"; } @@ -53,10 +53,10 @@ class Submap OutputArray points, OutputArray normals); virtual void updatePyrPointsNormals(const int pyramidLevels); - virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); }; + virtual int getTotalAllocatedBlocks() const { return int(volume->getTotalVolumeUnits()); }; virtual int getVisibleBlocks(int currFrameId) const { - return volume.getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD); + return volume->getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD); } float calcVisibilityRatio(int currFrameId) const @@ -91,7 +91,7 @@ class Submap //! TODO: Add support for GPU arrays (UMat) std::vector pyrPoints; std::vector pyrNormals; - HashTSDFVolumeCPU volume; + std::shared_ptr volume; }; template @@ -100,14 +100,14 @@ void Submap::integrate(InputArray _depth, float depthFactor, const cv:: const int currFrameId) { CV_Assert(currFrameId >= startFrameId); - volume.integrate(_depth, depthFactor, cameraPose.matrix, intrinsics, currFrameId); + volume->integrate(_depth, depthFactor, cameraPose.matrix, intrinsics, currFrameId); } template void Submap::raycast(const cv::Affine3f& _cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, OutputArray points, OutputArray normals) { - volume.raycast(_cameraPose.matrix, intrinsics, frameSize, points, normals); + volume->raycast(_cameraPose.matrix, intrinsics, frameSize, points, normals); } template diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index 70e77fd7086..73a26966d22 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -80,12 +80,10 @@ Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, Point3i _presolution = _resolution; if (_volumeType == VolumeType::TSDF) { - return makeTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, - _presolution); + return makeTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _presolution); } else if (_volumeType == VolumeType::HASHTSDF) { - //return makeHashTSDFVolume( return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); } CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 47b57336c42..6753d999ebd 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -274,7 +274,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im } // ---------------------------- -static const bool display = false; +static const bool display = true; static const bool parallelCheck = false; void normalsCheck(Mat normals) From 6c6405d2e6d3ba0dc7c274dba77af84c9c7a3070 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 31 Dec 2020 13:39:39 +0300 Subject: [PATCH 113/216] hashtable fix --- modules/rgbd/src/hash_tsdf.cpp | 4 +- modules/rgbd/src/tsdf_functions.cpp | 125 ++++++++++------------------ modules/rgbd/src/tsdf_functions.hpp | 2 +- 3 files changed, 49 insertions(+), 82 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 49d1c82c381..554f1ef5a3d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1666,7 +1666,7 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con //template Ptr makeHashTSDFVolume(const VolumeParams& _params) { -#ifdef _HAVE_OPENCL +#ifdef HAVE_OPENCL if (ocl::useOpenCL()) return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, _params.depthTruncThreshold, _params.unitResolution); @@ -1679,7 +1679,7 @@ Ptr makeHashTSDFVolume(const VolumeParams& _params) Ptr makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float truncateThreshold, int volumeUnitResolution) { -#ifdef _HAVE_OPENCL +#ifdef HAVE_OPENCL if (ocl::useOpenCL()) return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, volumeUnitResolution); diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 639fa4398e1..5c8af8843f0 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -403,8 +403,14 @@ VolumesTable::VolumesTable() } } -void VolumesTable::update(Vec3i indx) +inline void VolumesTable::updateVolumeUnit(int mode, Vec3i indx, int row=-1, int isActive=0, int lastVisibleIndex=-1) { +// modes: +// 0 - Vec3i indx +// 1 - Vec3i indx, int row +// 2 - Vec3i indx, int isActive, int lastVisibleIndex +// 3 - Vec3i indx, int isActive + Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); int bufferNum = 0; @@ -414,110 +420,71 @@ void VolumesTable::update(Vec3i indx) while (i != -1) { Volume_NODE* v = volumes.ptr(i); - if (v->idx == idx) - return; + //find nan cheking for int or Vec3i //if (isNaN(Point3i(v.idx))) if (v->idx[0] == NAN_ELEMENT) { v->idx = idx; + v->row = row; + v->isActive = isActive; + v->lastVisibleIndex = lastVisibleIndex; v->nextVolumeRow = getNextVolume(hash, bufferNum, i, start); indexes.push_back(indx); indexesGPU.push_back(idx); return; } - i = v->nextVolumeRow; - } -} - -void VolumesTable::update(Vec3i indx, int row) -{ - Vec4i idx(indx[0], indx[1], indx[2], 0); - int hash = int(calc_hash(idx) % hash_divisor); - int bufferNum = 0; - int start = (bufferNum * list_size * hash_divisor) + (hash * list_size); - int i = start; - while (i != -1) - { - Volume_NODE* v = volumes.ptr(i); - if (v->idx == idx) + if (mode == 1) { - v->row = row; - return; + if (v->idx == idx) + { + v->row = row; + return; + } } - //find nan cheking for int or Vec3i - //if (isNaN(Point3i(v.idx))) - if (v->idx[0] == NAN_ELEMENT) + else if (mode == 2) { - v->idx = idx; - v->row = row; - v->nextVolumeRow = getNextVolume(hash, bufferNum, i, start); - indexes.push_back(indx); - indexesGPU.push_back(idx); - return; - } - i = v->nextVolumeRow; - } -} - -void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) -{ - Vec4i idx(indx[0], indx[1], indx[2], 0); - int hash = int(calc_hash(idx) % hash_divisor); - int bufferNum = 0; - int start = (bufferNum * list_size * hash_divisor) + (hash * list_size); - int i = start; + if (v->idx == idx) + { + v->isActive = isActive; + v->lastVisibleIndex = lastVisibleIndex; + return; + } - while (i != -1) - { - Volume_NODE* v = volumes.ptr(i); - if (v->idx == idx) - { - v->isActive = isActive; - v->lastVisibleIndex = lastVisibleIndex; - return; } - //find nan cheking for int or Vec3i - //if (isNaN(Point3i(v.idx))) - if (v->idx[0] == NAN_ELEMENT) + else if (mode == 3) { - v->idx = idx; - v->nextVolumeRow = getNextVolume(hash, bufferNum, i, start); v->isActive = isActive; - v->lastVisibleIndex = lastVisibleIndex; - indexes.push_back(indx); - indexesGPU.push_back(idx); return; } + i = v->nextVolumeRow; } } -void VolumesTable::updateActive(Vec3i indx, int isActive) +void VolumesTable::update(Vec3i indx) { - Vec4i idx(indx[0], indx[1], indx[2], 0); - int hash = int(calc_hash(idx) % hash_divisor); - int bufferNum = 0; - int start = (bufferNum * list_size * hash_divisor) + (hash * list_size); - int i = start; + int mode = 0; + this->updateVolumeUnit(mode, indx); +} - while (i != -1) - { - Volume_NODE* v = volumes.ptr(i); - if (v->idx == idx) - { - v->isActive = isActive; - return; - } - //find nan cheking for int or Vec3i - //if (isNaN(Point3i(v.idx))) - if (v->idx[0] == NAN_ELEMENT) - { - return; - } - i = v->nextVolumeRow; - } +void VolumesTable::update(Vec3i indx, int row) +{ + int mode = 1; + this->updateVolumeUnit(mode, indx); +} + +void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) +{ + int mode = 2; + this->updateVolumeUnit(mode, indx); +} + +void VolumesTable::updateActive(Vec3i indx, int isActive) +{ + int mode = 3; + this->updateVolumeUnit(mode, indx); } bool VolumesTable::getActive(Vec3i indx) const diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 21e8e75dd0b..29677932674 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -53,7 +53,6 @@ struct Volume_NODE int32_t nextVolumeRow = -1; int32_t isActive = 0; int32_t lastVisibleIndex = -1; - //int32_t tmp; }; size_t calc_hash(Vec4i x); @@ -80,6 +79,7 @@ class VolumesTable void update(Vec3i indx, int row); void update(Vec3i indx, int isActive, int lastVisibleIndex); void updateActive(Vec3i indx, int isActive); + void updateVolumeUnit(int mode, Vec3i indx, int row, int isActive, int lastVisibleIndex); void expand(); bool getActive(Vec3i indx) const; int getNextVolume(int hash, int& num, int i, int start); From 4ca4dd8dad07fb959f5b478fe9d6047106e94d2b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 12 Jan 2021 13:59:31 +0300 Subject: [PATCH 114/216] resrite hashtasble update --- modules/rgbd/src/hash_tsdf.cpp | 8 +-- modules/rgbd/src/tsdf_functions.cpp | 79 +++++++++++++++++++---------- modules/rgbd/src/tsdf_functions.hpp | 14 +++-- 3 files changed, 65 insertions(+), 36 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 554f1ef5a3d..bdcf85453a3 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1085,7 +1085,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma lastVisibleIndexes.resize(buff_lvl); allVol2cam.resize(buff_lvl); } - indexes.update(idx, lastVolIndex); + indexes.updateRow(idx, lastVolIndex); *_newIndices.ptr(vol_idx) = lastVolIndex; *newIndices.ptr(vol_idx) = idx; vol_idx++; @@ -1108,7 +1108,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma *poses.ptr(idx, 0) = subvolumePose; *lastVisibleIndexes.ptr(idx, 0) = frameId; - indexes.updateActive(tsdf_idx, 1); + indexes.updateIsActive(tsdf_idx, 1); Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); auto vol2camMatrix = vol2cam.matrix.val; @@ -1144,7 +1144,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { - indexes.updateActive(tsdf_idx, 0); + indexes.updateIsActive(tsdf_idx, 0); return; } Point2f cameraPoint = proj(volUnitInCamSpace); @@ -1152,7 +1152,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { assert(idx != lastVolIndex); *lastVisibleIndexes.ptr(idx, 0) = frameId; - indexes.update(tsdf_idx, 1, frameId); + indexes.updateActivity(tsdf_idx, 1, frameId); } } }; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 5c8af8843f0..23c76927bf7 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -403,13 +403,13 @@ VolumesTable::VolumesTable() } } -inline void VolumesTable::updateVolumeUnit(int mode, Vec3i indx, int row=-1, int isActive=0, int lastVisibleIndex=-1) +inline void VolumesTable::updateVolumeUnit(int mode, Vec3i indx, int row = -1, int isActive = 0, int lastVisibleIndex = -1) { -// modes: -// 0 - Vec3i indx -// 1 - Vec3i indx, int row -// 2 - Vec3i indx, int isActive, int lastVisibleIndex -// 3 - Vec3i indx, int isActive + // mode 0 updateIndx + // mode 1 updateRow + // mode 2 updateIsActive + // mode 3 updateLastVolumeIndx + // mode 4 updateActivity Vec4i idx(indx[0], indx[1], indx[2], 0); int hash = int(calc_hash(idx) % hash_divisor); @@ -420,9 +420,7 @@ inline void VolumesTable::updateVolumeUnit(int mode, Vec3i indx, int row=-1, int while (i != -1) { Volume_NODE* v = volumes.ptr(i); - - //find nan cheking for int or Vec3i - //if (isNaN(Point3i(v.idx))) + if (v->idx[0] == NAN_ELEMENT) { v->idx = idx; @@ -435,7 +433,15 @@ inline void VolumesTable::updateVolumeUnit(int mode, Vec3i indx, int row=-1, int return; } - if (mode == 1) + if (mode == 0) // updateIndx + { + if (v->idx == idx) + { + return; + } + } + + if (mode == 1) // updateRow { if (v->idx == idx) { @@ -443,48 +449,67 @@ inline void VolumesTable::updateVolumeUnit(int mode, Vec3i indx, int row=-1, int return; } } - else if (mode == 2) + + if (mode == 2) // updateIsActive { if (v->idx == idx) { v->isActive = isActive; - v->lastVisibleIndex = lastVisibleIndex; return; } + } + if (mode == 3) // updateLastVolumeIndx + { + if (v->idx == idx) + { + v->lastVisibleIndex = lastVisibleIndex; + return; + } } - else if (mode == 3) + + if (mode == 4) // updateActivity { - v->isActive = isActive; - return; + if (v->idx == idx) + { + v->isActive = isActive; + v->lastVisibleIndex = lastVisibleIndex; + return; + } } i = v->nextVolumeRow; } } -void VolumesTable::update(Vec3i indx) +void VolumesTable::updateIndx(Vec3i indx) +{ + const int mode = 0; + updateVolumeUnit(mode, indx); +} + +void VolumesTable::updateRow(Vec3i indx, int row) { - int mode = 0; - this->updateVolumeUnit(mode, indx); + const int mode = 1; + updateVolumeUnit(mode, indx, row); } -void VolumesTable::update(Vec3i indx, int row) +void VolumesTable::updateIsActive(Vec3i indx, int isActive) { - int mode = 1; - this->updateVolumeUnit(mode, indx); + const int mode = 2; + updateVolumeUnit(mode, indx, free_row, isActive); } -void VolumesTable::update(Vec3i indx, int isActive, int lastVisibleIndex) +void VolumesTable::updateLastVolumeIndx(Vec3i indx, int lastVisibleIndex) { - int mode = 2; - this->updateVolumeUnit(mode, indx); + const int mode = 3; + updateVolumeUnit(mode, indx, free_row, free_isActive, lastVisibleIndex); } -void VolumesTable::updateActive(Vec3i indx, int isActive) +void VolumesTable::updateActivity(Vec3i indx, int isActive, int lastVisibleIndex) { - int mode = 3; - this->updateVolumeUnit(mode, indx); + const int mode = 4; + updateVolumeUnit(mode, indx, free_row, isActive, lastVisibleIndex); } bool VolumesTable::getActive(Vec3i indx) const diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 29677932674..b3fe6e38e6c 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -67,6 +67,9 @@ class VolumesTable int list_size = _list_size; int bufferNums = 1; + int32_t free_row = -1; + int32_t free_isActive = 0; + cv::Mat volumes; std::vector indexes; std::vector indexesGPU; @@ -74,12 +77,13 @@ class VolumesTable VolumesTable(); ~VolumesTable() {}; - - void update(Vec3i indx); - void update(Vec3i indx, int row); - void update(Vec3i indx, int isActive, int lastVisibleIndex); - void updateActive(Vec3i indx, int isActive); void updateVolumeUnit(int mode, Vec3i indx, int row, int isActive, int lastVisibleIndex); + void updateIndx(Vec3i indx); + void updateRow(Vec3i indx, int row); + void updateIsActive(Vec3i indx, int isActive); + void updateLastVolumeIndx(Vec3i indx, int lastVisibleIndex); + void updateActivity(Vec3i indx, int isActive, int lastVisibleIndex); + void expand(); bool getActive(Vec3i indx) const; int getNextVolume(int hash, int& num, int i, int start); From 781bedd1e63032dd1b60a32169d60158aa338993 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 12 Jan 2021 14:06:13 +0300 Subject: [PATCH 115/216] test_tsdf fix --- modules/rgbd/test/test_tsdf.cpp | 30 +++++++++--------------------- 1 file changed, 9 insertions(+), 21 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 6753d999ebd..70b7893edcf 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -274,7 +274,7 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im } // ---------------------------- -static const bool display = true; +static const bool display = false; static const bool parallelCheck = false; void normalsCheck(Mat normals) @@ -358,7 +358,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo points = _points.getMat(af); renderPointsNormals(points, normals, image, _params->lightPose); imshow("render", image); - waitKey(20000); + waitKey(2000); } if (isRaycast) @@ -384,7 +384,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo points = _newPoints.getMat(af); renderPointsNormals(points, normals, image, _params->lightPose); imshow("render", image); - waitKey(20000); + waitKey(2000); } } @@ -448,7 +448,7 @@ void valid_points_test(bool isHashTSDF) imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); renderPointsNormals(points, normals, image, _params->lightPose); imshow("render", image); - waitKey(20000); + waitKey(2000); } volume->raycast(poses[17].matrix, _params->intr, _params->frameSize, _newPoints, _newNormals); @@ -463,7 +463,7 @@ void valid_points_test(bool isHashTSDF) imshow("depth", depth * (1.f / _params->depthFactor / 4.f)); renderPointsNormals(points, normals, image, _params->lightPose); imshow("render", image); - waitKey(20000); + waitKey(2000); } float percentValidity; @@ -522,22 +522,10 @@ TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } TEST(TSDF_GPU, valid_points) { valid_points_test(false); } -TEST(HashTSDF_GPU, raycast_normals) { - cv::ocl::Context context; - cv::ocl::Device(context.device(1)); - normal_test(true, true, false, false); } -TEST(HashTSDF_GPU, fetch_points_normals) { - cv::ocl::Context context; - cv::ocl::Device(context.device(1)); - normal_test(true, false, true, false); } -TEST(HashTSDF_GPU, fetch_normals) { - cv::ocl::Context context; - cv::ocl::Device(context.device(1)); - normal_test(true, false, false, true); } -TEST(HashTSDF_GPU, valid_points) { - cv::ocl::Context context; - cv::ocl::Device(context.device(1)); - valid_points_test(true); } +TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } +TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } +TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } +TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } TEST(TSDF_CPU, raycast_normals) { From 9abbfd2b0eaa6191d30ddc244727acf08e63730c Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 12 Jan 2021 15:49:52 +0300 Subject: [PATCH 116/216] override warning fix --- modules/rgbd/src/hash_tsdf.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 1d5d2e2fde5..d151b3a6f0f 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -84,8 +84,8 @@ class HashTSDFVolumeCPU : public HashTSDFVolume void fetchPointsNormals(OutputArray points, OutputArray normals) const override; void reset() override; - size_t getTotalVolumeUnits() const { return volumeUnits.size(); } - int getVisibleBlocks(int currFrameId, int frameThreshold) const; + size_t getTotalVolumeUnits() const override { return volumeUnits.size(); } + int getVisibleBlocks(int currFrameId, int frameThreshold) const override; //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) TsdfVoxel at(const Vec3i& volumeIdx) const; @@ -144,8 +144,8 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; - size_t getTotalVolumeUnits() const { return size_t(lastVolIndex); } - int getVisibleBlocks(int currFrameId, int frameThreshold) const; + size_t getTotalVolumeUnits() const override { return size_t(lastVolIndex); } + int getVisibleBlocks(int currFrameId, int frameThreshold) const override; //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) From 3166fbb57371726670ac15defb2eafcae11195ee Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 12 Jan 2021 19:31:36 +0300 Subject: [PATCH 117/216] docs fix --- 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 b3fe6e38e6c..d8998e8c8c6 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -83,7 +83,7 @@ class VolumesTable void updateIsActive(Vec3i indx, int isActive); void updateLastVolumeIndx(Vec3i indx, int lastVisibleIndex); void updateActivity(Vec3i indx, int isActive, int lastVisibleIndex); - + void expand(); bool getActive(Vec3i indx) const; int getNextVolume(int hash, int& num, int i, int start); From 5cfc78a573690417142dc97ee848919bdc2cf002 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 13 Jan 2021 12:13:46 +0300 Subject: [PATCH 118/216] mutex fix --- modules/rgbd/src/hash_tsdf.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index bdcf85453a3..b43767aa95e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1031,14 +1031,13 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma const Point3f truncPt(truncDist, truncDist, truncDist); _VolumeUnitIndexSet _newIndices = cv::Mat(VOLUMES_SIZE, 1, CV_32S); cv::Mat newIndices = cv::Mat(VOLUMES_SIZE, 1, CV_32SC3); - //Mutex mutex; + Mutex mutex; Range allocateRange(0, depth.rows); - int loc_vol_idx = 0; int vol_idx = 0; auto AllocateVolumeUnitsInvoker = [&](const Range& range) { _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(VOLUMES_SIZE, 1, CV_32SC3); - + int loc_vol_idx = 0; for (int y = range.start; y < range.end; y += depthStride) { const depthType* depthRow = depth[y]; @@ -1069,7 +1068,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } - //mutex.lock(); + mutex.lock(); for (int i = 0; i < loc_vol_idx; i++) { Vec3i idx = *_localAccessVolUnits.ptr(i); @@ -1092,11 +1091,11 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma lastVolIndex++; } } - //mutex.unlock(); + mutex.unlock(); }; - //parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); - AllocateVolumeUnitsInvoker(allocateRange); + parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); + //AllocateVolumeUnitsInvoker(allocateRange); //! Perform the allocation for (int i = 0; i < vol_idx; i++) From 2209af8df30d30f99d154bcb9edf8e3b94fbd181 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 13 Jan 2021 15:38:58 +0300 Subject: [PATCH 119/216] setUseOptimizes replace false with true --- modules/rgbd/samples/large_kinfu_demo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp index 40d14ca8cae..b1d638ecb0b 100644 --- a/modules/rgbd/samples/large_kinfu_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -128,7 +128,7 @@ int main(int argc, char** argv) ds->updateParams(*params); // Disabled until there is no OpenCL accelerated HashTSDF is available - cv::setUseOptimized(false); + cv::setUseOptimized(true); if (!idle) largeKinfu = LargeKinfu::create(params); From 0a064b90de6665869d74093888c36ba5e7180575 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 14 Jan 2021 17:25:15 +0300 Subject: [PATCH 120/216] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b43767aa95e..c1ac8764d5d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -906,8 +906,8 @@ inline int HashTSDFVolumeGPU::find_idx(cv::Mat v, Vec3i tsdf_idx) const static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f fxy, Vec2f cxy) { - Mat x(1, depth_cols, CV_32F); - Mat y(1, depth_rows, CV_32F); + Mat x(1, depth_cols, CV_32FC1); + Mat y(1, depth_rows, CV_32FC1); Mat _pixNorm(1, depth_rows * depth_cols, CV_32F); for (int i = 0; i < depth_cols; i++) @@ -1102,6 +1102,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { Vec3i tsdf_idx = *newIndices.ptr(i); VolumeIndex idx = indexes.find_Volume(tsdf_idx); + if (idx < 0) + continue; Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; From ba8169aa696fbea4ea3afc780612d02358877183 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 18 Jan 2021 14:54:28 +0300 Subject: [PATCH 121/216] merge with master --- 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 c1ac8764d5d..b14bb1322d7 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -112,7 +112,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = lower_bound[0]; i <= upper_bound[0]; i++) for (int j = lower_bound[1]; j <= upper_bound[1]; j++) - for (int k = lower_bound[2]; k <= lower_bound[2]; k++) + for (int k = lower_bound[2]; k <= upper_bound[2]; k++) { const Vec3i tsdf_idx = Vec3i(i, j, k); if (!localAccessVolUnits.count(tsdf_idx)) From 2468b139231c9d854a80feb6fc5609d338dcba45 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 19 Jan 2021 13:55:12 +0300 Subject: [PATCH 122/216] all minor fixes --- 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 b14bb1322d7..6c6ba6bae77 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -18,7 +18,7 @@ #include "opencl_kernels_rgbd.hpp" #define USE_INTERPOLATION_IN_GETNORMAL 1 -#define VOLUMES_SIZE 1024 +#define VOLUMES_SIZE 8192 namespace cv { @@ -880,7 +880,7 @@ void HashTSDFVolumeGPU::reset() static inline bool find(cv::Mat v, Vec3i tsdf_idx, int lastVolIndex) { - for (int i = 0; i < lastVolIndex +1; i++) + for (int i = 0; i < lastVolIndex; i++) { Vec3i* p = v.ptr(i); if (*p == tsdf_idx) @@ -1054,7 +1054,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma for (int i = lower_bound[0]; i <= upper_bound[0]; i++) for (int j = lower_bound[1]; j <= upper_bound[1]; j++) - for (int k = lower_bound[2]; k <= lower_bound[2]; k++) + for (int k = lower_bound[2]; k <= upper_bound[2]; k++) { const Vec3i tsdf_idx = Vec3i(i, j, k); @@ -1109,7 +1109,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma *poses.ptr(idx, 0) = subvolumePose; *lastVisibleIndexes.ptr(idx, 0) = frameId; - indexes.updateIsActive(tsdf_idx, 1); + indexes.updateActivity(tsdf_idx, 1, frameId); Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); auto vol2camMatrix = vol2cam.matrix.val; @@ -1139,7 +1139,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Vec3i tsdf_idx = _totalVolUnits[i]; VolumeIndex idx = indexes.find_Volume(tsdf_idx); - if (idx < 0 || idx == lastVolIndex) return; + if (idx < 0 || idx > lastVolIndex - 1) return; Point3f volumeUnitPos = volumeUnitIdxToVolume(tsdf_idx); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; @@ -1151,7 +1151,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Point2f cameraPoint = proj(volUnitInCamSpace); if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) { - assert(idx != lastVolIndex); + assert(idx >= 0 || idx < lastVolIndex); *lastVisibleIndexes.ptr(idx, 0) = frameId; indexes.updateActivity(tsdf_idx, 1, frameId); } From c076b89c9ffa9503543a7af84b09c388d3e4e7d2 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 19 Jan 2021 14:50:42 +0300 Subject: [PATCH 123/216] add CPU code for debug --- modules/rgbd/src/hash_tsdf.cpp | 292 ++++++++++++++++++++-------- modules/rgbd/src/hash_tsdf.hpp | 1 + modules/rgbd/src/tsdf_functions.cpp | 4 +- 3 files changed, 210 insertions(+), 87 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 6c6ba6bae77..9aa7e047cdd 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1168,10 +1168,34 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma frameParams = newParams; Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); + pixNorms_tmp = preCalculationPixNorm(depth, intrinsics); } //! Integrate the correct volumeUnits - integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); + auto Integrate = [&](const Range& range) { + for (int i = range.start; i < range.end; i++) + { + Vec3i tsdf_idx = _totalVolUnits[i]; + VolumeIndex idx = indexes.find_Volume(tsdf_idx); + if (idx < 0 || idx == lastVolIndex - 1) return; + bool _isActive = indexes.getActive(tsdf_idx); + if (_isActive) + { + //! The volume unit should already be added into the Volume from the allocator + Matx44f _pose = *poses.ptr(idx, 0); + integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, + Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, + depthFactor, cameraPose, intrinsics, pixNorms_tmp, volUnitsData.row(idx)); + //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); + //! Ensure all active volumeUnits are set to inactive for next integration + } + } + }; + //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); + Integrate(Range(0, (int)_totalVolUnits.size())); + + //! Integrate the correct volumeUnits + //integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); } @@ -1436,106 +1460,204 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals) const { - CV_TRACE_FUNCTION(); - CV_Assert(frameSize.area() > 0); - - String errorStr; - String name = "raycast"; - ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; - String options = "-cl-mad-enable"; - ocl::Kernel k; - k.create(name.c_str(), source, options, &errorStr); - - if (k.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); - - //int totalVolUnitsSize = _indexes.indexesGPU.size(); - Mat totalVolUnits = cv::Mat(indexes.indexesGPU, true); - - _points.create(frameSize, CV_32FC4); - _normals.create(frameSize, CV_32FC4); - Points points = _points.getMat(); - Normals normals = _normals.getMat(); + if (true) + { + _points.create(frameSize, POINT_TYPE); + _normals.create(frameSize, POINT_TYPE); + Points points = _points.getMat(); + Normals normals = _normals.getMat(); + Points& new_points(points); + Normals& new_normals(normals); + const HashTSDFVolumeGPU& 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; + auto _HashRaycastInvoker = [&](const Range& range) + { + const Point3f cam2volTrans = cam2vol.translation(); + const Matx33f cam2volRot = cam2vol.rotation(); + const Matx33f vol2camRot = vol2cam.rotation(); + const float blockSize = volume.volumeUnitSize; + for (int y = range.start; y < range.end; y++) + { + ptype* _ptsRow = new_points[y]; + ptype* _nrmRow = new_normals[y]; + for (int x = 0; x < new_points.cols; x++) + { + //! Initialize default value + Point3f point = nan3, normal = nan3; + //! Ray origin and direction in the volume coordinate frame + Point3f orig = cam2volTrans; + Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); + 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()); + float tprev = tcurr; + float prevTsdf = volume.truncDist; + Ptr currVolumeUnit; + while (tcurr < tmax) + { + Point3f currRayPos = orig + tcurr * rayDirV; + cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); + //VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); + VolumeIndex idx = indexes.find_Volume(currVolumeUnitIdx); + float currTsdf = prevTsdf; + int currWeight = 0; + float stepSize = 0.5f * blockSize; + cv::Vec3i volUnitLocalIdx; + //! The subvolume exists in hashtable + if (idx >= 0 && idx < lastVolIndex) + { - Points& new_points(points); - Normals& new_normals(normals); + cv::Point3f currVolUnitPos = + volume.volumeUnitIdxToVolume(currVolumeUnitIdx); + volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); + //! TODO: Figure out voxel interpolation + TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); + currTsdf = tsdfToFloat(currVoxel.tsdf); + currWeight = currVoxel.weight; + stepSize = tstep; + } + //! Surface crossing + if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) + { + float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); + if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) + { + Point3f pv = orig + tInterp * rayDirV; + Point3f nv = volume.getNormalVoxel(pv); + if (!isNaN(nv)) + { + normal = vol2camRot * nv; + point = vol2cam * pv; + } + } + break; + } + prevVolumeUnitIdx = currVolumeUnitIdx; + prevTsdf = currTsdf; + tprev = tcurr; + tcurr += stepSize; + } + _ptsRow[x] = toPtype(point); + _nrmRow[x] = toPtype(normal); + } + } + }; + //parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); + _HashRaycastInvoker(Range(0, new_points.rows)); - UMat Upoints = points.getUMat(ACCESS_RW); - UMat Unormals = normals.getUMat(ACCESS_RW); + } - Intr::Reprojector r = intrinsics.makeReprojector(); - Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); + if (false) + { + CV_TRACE_FUNCTION(); + CV_Assert(frameSize.area() > 0); - Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, - volumeUnitSize - voxelSize, - volumeUnitSize - voxelSize); + String errorStr; + String name = "raycast"; + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); - float tstep = truncDist * raycastStepFactor; - Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); - const HashTSDFVolumeGPU& volume(*this); - const Affine3f cam2vol(volume.pose.inv()* Affine3f(cameraPose)); - const Affine3f vol2cam(Affine3f(cameraPose.inv())* volume.pose); + //int totalVolUnitsSize = _indexes.indexesGPU.size(); + Mat totalVolUnits = cv::Mat(indexes.indexesGPU, true); - const Point3f cam2volTrans = cam2vol.translation(); - //const Matx33f cam2volRot = cam2vol.rotation(); - //const Matx33f vol2camRot = vol2cam.rotation(); + _points.create(frameSize, CV_32FC4); + _normals.create(frameSize, CV_32FC4); - Vec4f cam2volTransGPU(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0); - Matx44f cam2volRotGPU = cam2vol.matrix; - Matx44f vol2camRotGPU = vol2cam.matrix; + Points points = _points.getMat(); + Normals normals = _normals.getMat(); - Mat _tmp; - volUnitsData.copyTo(_tmp); - UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); - _tmp.release(); + Points& new_points(points); + Normals& new_normals(normals); - UMat volPoseGpu, invPoseGpu; - Mat(pose.matrix).copyTo(volPoseGpu); - Mat(pose.inv().matrix).copyTo(invPoseGpu); + UMat Upoints = points.getUMat(ACCESS_RW); + UMat Unormals = normals.getUMat(ACCESS_RW); - k.args( - ocl::KernelArg::PtrReadWrite(indexes.volumes.getUMat(ACCESS_RW)), - (int)indexes.list_size, - (int)indexes.bufferNums, - (int)indexes.hash_divisor, - (int)lastVolIndex, - ocl::KernelArg::ReadWrite(Upoints), - ocl::KernelArg::ReadWrite(Unormals), - frameSize, - ocl::KernelArg::ReadWrite(U_volUnitsData), - cam2volTransGPU, - cam2volRotGPU, - vol2camRotGPU, - float(volume.truncateThreshold), - finv.val, cxy.val, - boxMin.val, boxMax.val, - tstep, - voxelSize, - volResGpu.val, - volStrides.val, - neighbourCoords.val, - voxelSizeInv, - volumeUnitSize, - volume.truncDist, - volumeUnitResolution, - volStrides - ); + Intr::Reprojector r = intrinsics.makeReprojector(); + Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); - size_t globalSize[2]; - globalSize[0] = (size_t) frameSize.width; - globalSize[1] = (size_t) frameSize.height; + Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, + volumeUnitSize - voxelSize, + volumeUnitSize - voxelSize); - if (!k.run(2, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); + float tstep = truncDist * raycastStepFactor; + Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); - Upoints.getMat(ACCESS_RW).copyTo(new_points); - Unormals.getMat(ACCESS_RW).copyTo(new_normals); + const HashTSDFVolumeGPU& volume(*this); + const Affine3f cam2vol(volume.pose.inv() * Affine3f(cameraPose)); + const Affine3f vol2cam(Affine3f(cameraPose.inv()) * volume.pose); - Upoints.release(); - Unormals.release(); - U_volUnitsData.release(); + const Point3f cam2volTrans = cam2vol.translation(); + //const Matx33f cam2volRot = cam2vol.rotation(); + //const Matx33f vol2camRot = vol2cam.rotation(); + + Vec4f cam2volTransGPU(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0); + Matx44f cam2volRotGPU = cam2vol.matrix; + Matx44f vol2camRotGPU = vol2cam.matrix; + + Mat _tmp; + volUnitsData.copyTo(_tmp); + UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); + _tmp.release(); + + UMat volPoseGpu, invPoseGpu; + Mat(pose.matrix).copyTo(volPoseGpu); + Mat(pose.inv().matrix).copyTo(invPoseGpu); + + k.args( + ocl::KernelArg::PtrReadWrite(indexes.volumes.getUMat(ACCESS_RW)), + (int)indexes.list_size, + (int)indexes.bufferNums, + (int)indexes.hash_divisor, + (int)lastVolIndex, + ocl::KernelArg::ReadWrite(Upoints), + ocl::KernelArg::ReadWrite(Unormals), + frameSize, + ocl::KernelArg::ReadWrite(U_volUnitsData), + cam2volTransGPU, + cam2volRotGPU, + vol2camRotGPU, + float(volume.truncateThreshold), + finv.val, cxy.val, + boxMin.val, boxMax.val, + tstep, + voxelSize, + volResGpu.val, + volStrides.val, + neighbourCoords.val, + voxelSizeInv, + volumeUnitSize, + volume.truncDist, + volumeUnitResolution, + volStrides + ); + + size_t globalSize[2]; + globalSize[0] = (size_t)frameSize.width; + globalSize[1] = (size_t)frameSize.height; + + if (!k.run(2, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); + + Upoints.getMat(ACCESS_RW).copyTo(new_points); + Unormals.getMat(ACCESS_RW).copyTo(new_normals); + + Upoints.release(); + Unormals.release(); + U_volUnitsData.release(); + } } void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index d151b3a6f0f..e9ee6e3a525 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -175,6 +175,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::Mat allVol2cam; cv::Mat volUnitsData; cv::UMat pixNorms; + cv::Mat pixNorms_tmp; VolumeIndex lastVolIndex; VolumesTable indexes; Vec8i neighbourCoords; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 23c76927bf7..309e5896696 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -253,8 +253,8 @@ void integrateVolumeUnit( 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)); + //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: From 2b8cf75270222cd3ed65fffe2ae31cd3e689f4ab Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 19 Jan 2021 15:03:25 +0300 Subject: [PATCH 124/216] settings set --- modules/rgbd/src/hash_tsdf.cpp | 7 +++---- modules/rgbd/src/hash_tsdf.hpp | 1 - 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 9aa7e047cdd..ec6d754141d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1168,7 +1168,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma frameParams = newParams; Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); - pixNorms_tmp = preCalculationPixNorm(depth, intrinsics); } //! Integrate the correct volumeUnits @@ -1185,17 +1184,17 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Matx44f _pose = *poses.ptr(idx, 0); integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, - depthFactor, cameraPose, intrinsics, pixNorms_tmp, volUnitsData.row(idx)); + depthFactor, cameraPose, intrinsics, pixNorms, volUnitsData.row(idx)); //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); //! Ensure all active volumeUnits are set to inactive for next integration } } }; //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); - Integrate(Range(0, (int)_totalVolUnits.size())); + //Integrate(Range(0, (int)_totalVolUnits.size())); //! Integrate the correct volumeUnits - //integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); + integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); } diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index e9ee6e3a525..d151b3a6f0f 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -175,7 +175,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::Mat allVol2cam; cv::Mat volUnitsData; cv::UMat pixNorms; - cv::Mat pixNorms_tmp; VolumeIndex lastVolIndex; VolumesTable indexes; Vec8i neighbourCoords; From 0c38e8ee4c3c6bb742c8d3d167eec57f1ac81439 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 19 Jan 2021 15:25:30 +0300 Subject: [PATCH 125/216] move code from hpp to cpp --- modules/rgbd/src/hash_tsdf.cpp | 141 ++++++++++++++++++++++++++++++++ modules/rgbd/src/hash_tsdf.hpp | 143 --------------------------------- 2 files changed, 141 insertions(+), 143 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ec6d754141d..3f3e11e2961 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -38,6 +38,87 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca truncDist = std::max(_truncDist, 4.0f * voxelSize); } +//! Spatial hashing +struct tsdf_hash +{ + size_t operator()(const Vec3i& x) const noexcept + { + size_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + for (uint16_t i = 0; i < 3; i++) + { + seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + } + return seed; + } +}; + +typedef int VolumeIndex; +struct VolumeUnit +{ + cv::Vec3i coord; + VolumeIndex index; + cv::Matx44f pose; + int lastVisibleIndex = 0; + bool isActive; +}; + +typedef std::unordered_set VolumeUnitIndexSet; +typedef std::unordered_map VolumeUnitIndexes; + +class HashTSDFVolumeCPU : public HashTSDFVolume +{ +public: + // dimension in voxels, size in meters + HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); + + HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); + + void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, + const int frameId = 0) override; + void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, + OutputArray normals) const override; + + void fetchNormals(InputArray points, OutputArray _normals) const override; + void fetchPointsNormals(OutputArray points, OutputArray normals) const override; + + void reset() override; + size_t getTotalVolumeUnits() const override { return volumeUnits.size(); } + int getVisibleBlocks(int currFrameId, int frameThreshold) const override; + + //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) + TsdfVoxel at(const Vec3i& volumeIdx) const; + + //! 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; + + TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const; + + + float interpolateVoxelPoint(const Point3f& point) const; + float interpolateVoxel(const cv::Point3f& point) const; + Point3f getNormalVoxel(const cv::Point3f& p) const; + + //! Utility functions for coordinate transformations + Vec3i volumeToVolumeUnitIdx(const Point3f& point) const; + Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const; + + Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; + Vec3i volumeToVoxelCoord(const Point3f& point) const; + +public: + Vec4i volStrides; + Vec6f frameParams; + Mat pixNorms; + VolumeUnitIndexes volumeUnits; + cv::Mat volUnitsData; + VolumeIndex lastVolIndex; +}; + + HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, @@ -826,6 +907,66 @@ int HashTSDFVolumeCPU::getVisibleBlocks(int currFrameId, int frameThreshold) con #ifdef HAVE_OPENCL +typedef cv::Mat _VolumeUnitIndexSet; +typedef cv::Mat _VolumeUnitIndexes; + +class HashTSDFVolumeGPU : public HashTSDFVolume +{ +public: + HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = false); + + HashTSDFVolumeGPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = false); + + void reset() override; + + //void integrateVolumeUnitGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx); + void integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics); + + void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, + const int frameId = 0) override; + void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, + OutputArray normals) const override; + + void fetchNormals(InputArray points, OutputArray _normals) const override; + void fetchPointsNormals(OutputArray points, OutputArray normals) const override; + + size_t getTotalVolumeUnits() const override { return size_t(lastVolIndex); } + int getVisibleBlocks(int currFrameId, int frameThreshold) const override; + + //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = + //! 1m) + virtual TsdfVoxel new_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const; + TsdfVoxel new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const; + + + float interpolateVoxelPoint(const Point3f& point) const; + float interpolateVoxel(const cv::Point3f& point) const; + Point3f getNormalVoxel(const cv::Point3f& p) const; + + //! Utility functions for coordinate transformations + Vec3i volumeToVolumeUnitIdx(const Point3f& point) const; + Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const; + + Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; + Vec3i volumeToVoxelCoord(const Point3f& point) const; + int find_idx(cv::Mat v, Vec3i tsdf_idx) const; + +public: + Vec4i volStrides; + Vec6f frameParams; + int degree; + int buff_lvl; + cv::Mat poses; + cv::Mat lastVisibleIndexes; + cv::Mat allVol2cam; + cv::Mat volUnitsData; + cv::UMat pixNorms; + VolumeIndex lastVolIndex; + VolumesTable indexes; + Vec8i neighbourCoords; +}; + HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index d151b3a6f0f..a6420266140 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -38,149 +38,6 @@ class HashTSDFVolume : public Volume bool zFirstMemOrder; }; -//! Spatial hashing -struct tsdf_hash -{ - size_t operator()(const Vec3i& x) const noexcept - { - size_t seed = 0; - constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; - for (uint16_t i = 0; i < 3; i++) - { - seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - } - return seed; - } -}; - -typedef int VolumeIndex; -struct VolumeUnit -{ - cv::Vec3i coord; - VolumeIndex index; - cv::Matx44f pose; - int lastVisibleIndex = 0; - bool isActive; -}; - -typedef std::unordered_set VolumeUnitIndexSet; -typedef std::unordered_map VolumeUnitIndexes; - -class HashTSDFVolumeCPU : public HashTSDFVolume -{ - public: - // dimension in voxels, size in meters - HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); - - HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); - - void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, - const int frameId = 0) override; - void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, - OutputArray normals) const override; - - void fetchNormals(InputArray points, OutputArray _normals) const override; - void fetchPointsNormals(OutputArray points, OutputArray normals) const override; - - void reset() override; - size_t getTotalVolumeUnits() const override { return volumeUnits.size(); } - int getVisibleBlocks(int currFrameId, int frameThreshold) const override; - - //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) - TsdfVoxel at(const Vec3i& volumeIdx) const; - - //! 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; - - TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const; - - - float interpolateVoxelPoint(const Point3f& point) const; - float interpolateVoxel(const cv::Point3f& point) const; - Point3f getNormalVoxel(const cv::Point3f& p) const; - - //! Utility functions for coordinate transformations - Vec3i volumeToVolumeUnitIdx(const Point3f& point) const; - Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const; - - Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; - Vec3i volumeToVoxelCoord(const Point3f& point) const; - - public: - Vec4i volStrides; - Vec6f frameParams; - Mat pixNorms; - VolumeUnitIndexes volumeUnits; - cv::Mat volUnitsData; - VolumeIndex lastVolIndex; -}; - -#ifdef HAVE_OPENCL - -typedef cv::Mat _VolumeUnitIndexSet; -typedef cv::Mat _VolumeUnitIndexes; - -class HashTSDFVolumeGPU : public HashTSDFVolume -{ -public: - HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = false); - - HashTSDFVolumeGPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = false); - - void reset() override; - - //void integrateVolumeUnitGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx); - void integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics); - - void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, - const int frameId = 0) override; - void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, - OutputArray normals) const override; - - void fetchNormals(InputArray points, OutputArray _normals) const override; - void fetchPointsNormals(OutputArray points, OutputArray normals) const override; - - size_t getTotalVolumeUnits() const override { return size_t(lastVolIndex); } - int getVisibleBlocks(int currFrameId, int frameThreshold) const override; - - //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = - //! 1m) - virtual TsdfVoxel new_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const; - TsdfVoxel new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const; - - - float interpolateVoxelPoint(const Point3f& point) const; - float interpolateVoxel(const cv::Point3f& point) const; - Point3f getNormalVoxel(const cv::Point3f& p) const; - - //! Utility functions for coordinate transformations - Vec3i volumeToVolumeUnitIdx(const Point3f& point) const; - Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const; - - Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; - Vec3i volumeToVoxelCoord(const Point3f& point) const; - int find_idx(cv::Mat v, Vec3i tsdf_idx) const; - -public: - Vec4i volStrides; - Vec6f frameParams; - int degree; - int buff_lvl; - cv::Mat poses; - cv::Mat lastVisibleIndexes; - cv::Mat allVol2cam; - cv::Mat volUnitsData; - cv::UMat pixNorms; - VolumeIndex lastVolIndex; - VolumesTable indexes; - Vec8i neighbourCoords; -}; -#endif - //template Ptr makeHashTSDFVolume(const VolumeParams& _volumeParams); //template From deb94c590f13117b2546a1aef8feccbb9391a8cd Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 19 Jan 2021 17:26:36 +0300 Subject: [PATCH 126/216] minor opencl fix --- modules/rgbd/src/opencl/hash_tsdf.cl | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index e7e48c7c964..d65e3d2c075 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -631,6 +631,7 @@ __kernel void raycast( } } + prevTsdf = currTsdf; tprev = tcurr; tcurr += stepSize; } From 3ea8a7232d273ab90424cb0141978169bce4445b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 19 Jan 2021 17:39:14 +0300 Subject: [PATCH 127/216] fid bug --- modules/rgbd/src/opencl/hash_tsdf.cl | 47 ++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index d65e3d2c075..dba1bfe7f3e 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -402,6 +402,18 @@ static struct TsdfVoxel _atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int ro return volData[coordBase]; } +inline float interpolate(float tx, float ty, float tz, float vx[8]) +{ + float v00 = vx[0] + tz * (vx[1] - vx[0]); + float v01 = vx[2] + tz * (vx[3] - vx[2]); + float v10 = vx[4] + tz * (vx[5] - vx[4]); + float v11 = vx[6] + tz * (vx[7] - vx[6]); + + float v0 = v00 + ty * (v01 - v00); + float v1 = v10 + ty * (v11 - v10); + + return v0 + tx * (v1 - v0); +} inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolumePtr, int3 volResolution, int3 volDims, int8 neighbourCoords, @@ -477,6 +489,41 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum normal.s1 = vals[1 * 2] - vals[1 * 2 + 1]; normal.s2 = vals[2 * 2] - vals[2 * 2 + 1]; +// <========================================================> + + float cxv[8], cyv[8], czv[8]; + + // How these numbers were obtained: + // 1. Take the basic interpolation sequence: + // 000, 001, 010, 011, 100, 101, 110, 111 + // where each digit corresponds to shift by x, y, z axis respectively. + // 2. Add +1 for next or -1 for prev to each coordinate to corresponding axis + // 3. Search corresponding values in offsets + const int idxxp[8] = { 8, 9, 10, 11, 0, 1, 2, 3 }; + const int idxxn[8] = { 4, 5, 6, 7, 12, 13, 14, 15 }; + const int idxyp[8] = { 16, 17, 0, 1, 18, 19, 4, 5 }; + const int idxyn[8] = { 2, 3, 20, 21, 6, 7, 22, 23 }; + const int idxzp[8] = { 24, 0, 25, 2, 26, 4, 27, 6 }; + const int idxzn[8] = { 1, 28, 3, 29, 5, 30, 7, 31 }; + + for (int i = 0; i < 8; i++) + { + cxv[i] = vals[idxxn[i]] - vals[idxxp[i]]; + cyv[i] = vals[idxyn[i]] - vals[idxyp[i]]; + czv[i] = vals[idxzn[i]] - vals[idxzp[i]]; + } + + float tx = ptVox.x - iptVox[0]; + float ty = ptVox.y - iptVox[1]; + float tz = ptVox.z - iptVox[2]; + + normal.s0 = interpolate(tx, ty, tz, cxv); + normal.s1 = interpolate(tx, ty, tz, cyv); + normal.s2 = interpolate(tx, ty, tz, czv); + printf("%f, %f, %f" ,normal.s0, normal.s1, normal.s2); + +// <========================================================> + float norm = sqrt(normal.x*normal.x + normal.y*normal.y From 42494d3e9576a1a47b215ec5dcb8b70827754783 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 19 Jan 2021 17:53:46 +0300 Subject: [PATCH 128/216] coment fix --- modules/rgbd/src/opencl/hash_tsdf.cl | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index dba1bfe7f3e..f99714c07fa 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -490,7 +490,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum normal.s2 = vals[2 * 2] - vals[2 * 2 + 1]; // <========================================================> - +/* float cxv[8], cyv[8], czv[8]; // How these numbers were obtained: @@ -513,15 +513,17 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum czv[i] = vals[idxzn[i]] - vals[idxzp[i]]; } - float tx = ptVox.x - iptVox[0]; - float ty = ptVox.y - iptVox[1]; - float tz = ptVox.z - iptVox[2]; + float tx = ptVox.x - iptVox.x; + float ty = ptVox.y - iptVox.y; + float tz = ptVox.z - iptVox.z; normal.s0 = interpolate(tx, ty, tz, cxv); normal.s1 = interpolate(tx, ty, tz, cyv); normal.s2 = interpolate(tx, ty, tz, czv); - printf("%f, %f, %f" ,normal.s0, normal.s1, normal.s2); - + + if(!any(isnan(normal))) + printf("%f, %f, %f" ,normal.s0, normal.s1, normal.s2); +*/ // <========================================================> float norm = From 164993beaafce8807602d5410363dffd4703a3bb Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 20 Jan 2021 17:15:49 +0300 Subject: [PATCH 129/216] minor fix --- modules/rgbd/src/hash_tsdf.cpp | 4 +++- modules/rgbd/src/opencl/hash_tsdf.cl | 9 ++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 3f3e11e2961..8267123ddb8 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1017,6 +1017,8 @@ void HashTSDFVolumeGPU::reset() lastVisibleIndexes = cv::Mat(buff_lvl, 1, CV_32S); indexes = VolumesTable(); allVol2cam = cv::Mat(buff_lvl, 16, CV_32F); + frameParams = Vec6f(); + pixNorms = UMat(); } static inline bool find(cv::Mat v, Vec3i tsdf_idx, int lastVolIndex) @@ -1471,7 +1473,7 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const queried[i] = false; } -#if !USE_INTERPOLATION_IN_GETNORMAL +#if USE_INTERPOLATION_IN_GETNORMAL const Vec3i offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3 { 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7 }; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index f99714c07fa..a3044ac514e 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -200,8 +200,7 @@ static void integrateVolumeUnit( else { // z loop shouldn't be performed - //startZ = endZ = 0; - return; + startZ = endZ = 0; } } @@ -434,7 +433,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum for (int i = 0; i < 8; i++) { - iterMap[i] = lastVolIndex - 1; + iterMap[i] = lastVolIndex; queried[i] = false; } @@ -483,8 +482,6 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum } - //for (int c = 0; c < 3; c++){normal[c] = vals[c * 2] - vals[c * 2 + 1];} - normal.s0 = vals[0 * 2] - vals[0 * 2 + 1]; normal.s1 = vals[1 * 2] - vals[1 * 2 + 1]; normal.s2 = vals[2 * 2] - vals[2 * 2 + 1]; @@ -526,6 +523,8 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum */ // <========================================================> + //normal.x*=(-1); normal.y*=(-1); normal.z*=(-1); + float norm = sqrt(normal.x*normal.x + normal.y*normal.y From 265b7420ffa16ad4f188c8d6ee1a0099d3e6a3e3 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 20 Jan 2021 17:19:23 +0300 Subject: [PATCH 130/216] bug 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 8267123ddb8..ff43430152e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1473,7 +1473,7 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const queried[i] = false; } -#if USE_INTERPOLATION_IN_GETNORMAL +#if !USE_INTERPOLATION_IN_GETNORMAL const Vec3i offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3 { 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7 }; From e469b2fd905ae4cc81b9410f64ab0b444f0630e0 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 21 Jan 2021 22:00:39 +0300 Subject: [PATCH 131/216] integrate fixed --- modules/rgbd/src/hash_tsdf.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ff43430152e..d3c269ba62f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1321,20 +1321,21 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma VolumeIndex idx = indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == lastVolIndex - 1) return; bool _isActive = indexes.getActive(tsdf_idx); + if (_isActive) { - //! The volume unit should already be added into the Volume from the allocator Matx44f _pose = *poses.ptr(idx, 0); - integrateVolumeUnit(truncDist, voxelSize, maxWeight, _pose, - Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, depth, - depthFactor, cameraPose, intrinsics, pixNorms, volUnitsData.row(idx)); - //integrateVolumeUnitGPU(depth, depthFactor, _pose, intrinsics, idx); - //! Ensure all active volumeUnits are set to inactive for next integration + const cv::Affine3f vol2cam(Affine3f(cameraPose.inv())* Affine3f(_pose)); + auto vol2camMatrix = vol2cam.matrix.val; + for (int k = 0; k < 16; k++) + { + *allVol2cam.ptr(idx, k) = vol2camMatrix[k]; + } } } }; //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); - //Integrate(Range(0, (int)_totalVolUnits.size())); + Integrate(Range(0, (int)_totalVolUnits.size())); //! Integrate the correct volumeUnits integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); From 1da1bb4d7f16339e4da99a11eb5c25febd547c2a Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 21 Jan 2021 22:25:14 +0300 Subject: [PATCH 132/216] speed up --- modules/rgbd/src/hash_tsdf.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d3c269ba62f..a2708160854 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1314,7 +1314,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } //! Integrate the correct volumeUnits - auto Integrate = [&](const Range& range) { + auto preCalculateAllVol3Cam = [&](const Range& range) { for (int i = range.start; i < range.end; i++) { Vec3i tsdf_idx = _totalVolUnits[i]; @@ -1334,8 +1334,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } }; - //parallel_for_(Range(0, (int)_totalVolUnits.size()), Integrate ); - Integrate(Range(0, (int)_totalVolUnits.size())); + parallel_for_(Range(0, (int)_totalVolUnits.size()), preCalculateAllVol3Cam ); //! Integrate the correct volumeUnits integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); From e8da147bdc82a245ac6f4e5c93df8c270c25ddb6 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 21 Jan 2021 22:26:53 +0300 Subject: [PATCH 133/216] remove extra code --- modules/rgbd/src/hash_tsdf.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a2708160854..2765c808aee 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1254,13 +1254,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma *lastVisibleIndexes.ptr(idx, 0) = frameId; indexes.updateActivity(tsdf_idx, 1, frameId); - Affine3f vol2cam(Affine3f(cameraPose.inv()) * Affine3f(subvolumePose)); - auto vol2camMatrix = vol2cam.matrix.val; - for (int k = 0; k < 16; k++) - { - *allVol2cam.ptr(idx, k) = vol2camMatrix[k]; - } - volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) { TsdfVoxel& v = reinterpret_cast(vv); From dfcddd20e1de18f8a180e99cdd6b3b836d1fef68 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 22 Jan 2021 12:53:26 +0300 Subject: [PATCH 134/216] raycast fix, but still not work --- modules/rgbd/src/hash_tsdf.cpp | 25 ++++++------------------- modules/rgbd/src/opencl/hash_tsdf.cl | 14 ++++++-------- 2 files changed, 12 insertions(+), 27 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 2765c808aee..d19d59fe364 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1596,7 +1596,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in OutputArray _points, OutputArray _normals) const { - if (true) + if (false) { _points.create(frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE); @@ -1690,7 +1690,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in } - if (false) + if (true) { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); @@ -1711,14 +1711,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); - Points points = _points.getMat(); - Normals normals = _normals.getMat(); - - Points& new_points(points); - Normals& new_normals(normals); - - UMat Upoints = points.getUMat(ACCESS_RW); - UMat Unormals = normals.getUMat(ACCESS_RW); + UMat points = _points.getUMat(); + UMat normals = _normals.getUMat(); Intr::Reprojector r = intrinsics.makeReprojector(); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); @@ -1757,8 +1751,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in (int)indexes.bufferNums, (int)indexes.hash_divisor, (int)lastVolIndex, - ocl::KernelArg::ReadWrite(Upoints), - ocl::KernelArg::ReadWrite(Unormals), + ocl::KernelArg::WriteOnlyNoSize(points), + ocl::KernelArg::WriteOnlyNoSize(normals), frameSize, ocl::KernelArg::ReadWrite(U_volUnitsData), cam2volTransGPU, @@ -1785,13 +1779,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); - - Upoints.getMat(ACCESS_RW).copyTo(new_points); - Unormals.getMat(ACCESS_RW).copyTo(new_normals); - - Upoints.release(); - Unormals.release(); - U_volUnitsData.release(); } } diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index a3044ac514e..09fa24d3723 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -540,12 +540,10 @@ __kernel void raycast( const int bufferNums, const int hash_divisor, const int lastVolIndex, - __global float4 * points, - int points_step, int points_offset, - int points_rows, int points_cols, - __global float4 * normals, - int normals_step, int normals_offset, - int normals_rows, int normals_cols, + __global char * pointsptr, + int points_step, int points_offset, + __global char * normalsptr, + int normals_step, int normals_offset, const int2 frameSize, __global struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, @@ -684,8 +682,8 @@ __kernel void raycast( tcurr += stepSize; } - __global float* pts = (__global float*)(points + points_offset + y*points_step/sizeof(ptype) + x); - __global float* nrm = (__global float*)(normals + normals_offset + y*normals_step/sizeof(ptype) + x); + __global float* pts = (__global float*)(pointsptr + points_offset + y*points_step + x*sizeof(ptype)); + __global float* nrm = (__global float*)(normalsptr + normals_offset + y*normals_step + x*sizeof(ptype)); vstore4((float4)(point, 0), 0, pts); vstore4((float4)(normal, 0), 0, nrm); From 5cb7271353fd373cfbd49d86299169756d32ea62 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 22 Jan 2021 14:02:36 +0300 Subject: [PATCH 135/216] raycast fix --- modules/rgbd/src/opencl/hash_tsdf.cl | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 09fa24d3723..2f78ced04dc 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -668,7 +668,6 @@ __kernel void raycast( dot(nv, volRot1), dot(nv, volRot2)); // interpolation optimized a little - pv *= voxelSize; point = (float3)(dot(pv, volRot0), dot(pv, volRot1), dot(pv, volRot2)) + volTrans; @@ -682,7 +681,7 @@ __kernel void raycast( tcurr += stepSize; } - __global float* pts = (__global float*)(pointsptr + points_offset + y*points_step + x*sizeof(ptype)); + __global float* pts = (__global float*)(pointsptr + points_offset + y*points_step + x*sizeof(ptype)); __global float* nrm = (__global float*)(normalsptr + normals_offset + y*normals_step + x*sizeof(ptype)); vstore4((float4)(point, 0), 0, pts); vstore4((float4)(normal, 0), 0, nrm); From 1eeefac81559c1a2496dbf2e94de924fd5435d9a Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 25 Jan 2021 03:26:41 +0300 Subject: [PATCH 136/216] HT refactoring: constants --- modules/rgbd/src/tsdf_functions.hpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index d8998e8c8c6..45d654f4cbc 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -10,8 +10,6 @@ #include #include "tsdf.hpp" -#define NAN_ELEMENT -2147483647 - namespace cv { namespace kinfu @@ -45,6 +43,7 @@ void integrateVolumeUnit( InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); +const int NAN_ELEMENT = -2147483647; struct Volume_NODE { @@ -63,17 +62,17 @@ const int _list_size = 4; class VolumesTable { public: - int hash_divisor = _hash_divisor; - int list_size = _list_size; - int bufferNums = 1; + const int hash_divisor = _hash_divisor; + const int list_size = _list_size; + const int32_t free_row = -1; + const int32_t free_isActive = 0; - int32_t free_row = -1; - int32_t free_isActive = 0; + const cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT); + int bufferNums; cv::Mat volumes; std::vector indexes; std::vector indexesGPU; - cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT); VolumesTable(); ~VolumesTable() {}; From da6005d0069a3254d3043c336788939316b197dc Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 25 Jan 2021 03:30:41 +0300 Subject: [PATCH 137/216] HT refactoring: calc_hash() --- modules/rgbd/src/tsdf_functions.cpp | 17 ++--------------- modules/rgbd/src/tsdf_functions.hpp | 16 +++++++++++++--- 2 files changed, 15 insertions(+), 18 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 309e5896696..3a03eb3fd76 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -373,22 +373,9 @@ void integrateVolumeUnit( } -size_t calc_hash(Vec4i x) -{ - uint32_t seed = 0; - constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; - //uint32_t GOLDEN_RATIO = 0x9e3779b9; - //seed ^= x[0] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - //seed ^= x[1] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - //seed ^= x[2] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - for (int i = 0; i < 3; i++) - { - seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - } - return seed; -} +/// Custom Volume Hash Table -VolumesTable::VolumesTable() +VolumesTable::VolumesTable() : bufferNums(1), indexes(), indexesGPU() { this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType()); for (int i = 0; i < volumes.size().height; i++) diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 45d654f4cbc..78f27829d12 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -54,8 +54,6 @@ struct Volume_NODE int32_t lastVisibleIndex = -1; }; -size_t calc_hash(Vec4i x); - const int _hash_divisor = 32768; const int _list_size = 4; @@ -70,9 +68,10 @@ class VolumesTable const cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT); int bufferNums; - cv::Mat volumes; std::vector indexes; std::vector indexesGPU; + cv::Mat volumes; + VolumesTable(); ~VolumesTable() {}; @@ -88,6 +87,17 @@ class VolumesTable int getNextVolume(int hash, int& num, int i, int start); int find_Volume(Vec3i indx) const; bool isExist(Vec3i indx); + + inline size_t calc_hash(Vec3i x) const + { + uint32_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + for (int i = 0; i < 3; i++) + { + seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + } + return seed; + } }; From 298701fb0f4348ecf52d9a3e7f186ef8fc30f942 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 25 Jan 2021 03:33:10 +0300 Subject: [PATCH 138/216] HT refactoring: methods set shortened --- modules/rgbd/src/tsdf_functions.cpp | 194 ++++++++-------------------- modules/rgbd/src/tsdf_functions.hpp | 22 ++-- 2 files changed, 61 insertions(+), 155 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 3a03eb3fd76..86079327375 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -390,186 +390,94 @@ VolumesTable::VolumesTable() : bufferNums(1), indexes(), indexesGPU() } } -inline void VolumesTable::updateVolumeUnit(int mode, Vec3i indx, int row = -1, int isActive = 0, int lastVisibleIndex = -1) +const VolumesTable& VolumesTable::operator=(const VolumesTable& vt) { - // mode 0 updateIndx - // mode 1 updateRow - // mode 2 updateIsActive - // mode 3 updateLastVolumeIndx - // mode 4 updateActivity + this->volumes = vt.volumes; + this->bufferNums = vt.bufferNums; + this->indexes = vt.indexes; + this->indexesGPU = vt.indexesGPU; + return *this; +} - Vec4i idx(indx[0], indx[1], indx[2], 0); - int hash = int(calc_hash(idx) % hash_divisor); +Volume_NODE* VolumesTable::insert(Vec3i idx, int row, bool isActive, int lastVisibleIndex) +{ int bufferNum = 0; - int start = (bufferNum * list_size * hash_divisor) + (hash * list_size); + int hash = int(calc_hash(idx) % hash_divisor); + int start = getPos(idx, bufferNum); int i = start; - while (i != -1) + while (i >= 0) { Volume_NODE* v = volumes.ptr(i); if (v->idx[0] == NAN_ELEMENT) { - v->idx = idx; - v->row = row; - v->isActive = isActive; - v->lastVisibleIndex = lastVisibleIndex; - v->nextVolumeRow = getNextVolume(hash, bufferNum, i, start); - indexes.push_back(indx); - indexesGPU.push_back(idx); - return; - } + Vec4i idx4(idx[0], idx[1], idx[2], 0); - if (mode == 0) // updateIndx - { - if (v->idx == idx) + if (i != start && i % list_size == 0) { - return; + if (bufferNum >= bufferNums - 1) + { + volumes.resize(hash_divisor * bufferNums); + bufferNums++; + } + bufferNum++; + v->nextVolumeRow = (bufferNum * hash_divisor + hash) * list_size; } - } - - if (mode == 1) // updateRow - { - if (v->idx == idx) + else { - v->row = row; - return; + v->nextVolumeRow = i + 1; } - } - if (mode == 2) // updateIsActive - { - if (v->idx == idx) - { - v->isActive = isActive; - return; - } - } + v->idx = idx4; + v->row = row; + v->isActive = isActive; + v->lastVisibleIndex = lastVisibleIndex; - if (mode == 3) // updateLastVolumeIndx - { - if (v->idx == idx) - { - v->lastVisibleIndex = lastVisibleIndex; - return; - } - } - - if (mode == 4) // updateActivity - { - if (v->idx == idx) - { - v->isActive = isActive; - v->lastVisibleIndex = lastVisibleIndex; - return; - } + indexes.push_back(idx); + indexesGPU.push_back(idx4); + return v; } i = v->nextVolumeRow; } } -void VolumesTable::updateIndx(Vec3i indx) -{ - const int mode = 0; - updateVolumeUnit(mode, indx); -} - -void VolumesTable::updateRow(Vec3i indx, int row) +const Volume_NODE* VolumesTable::find(Vec3i idx) const { - const int mode = 1; - updateVolumeUnit(mode, indx, row); -} - -void VolumesTable::updateIsActive(Vec3i indx, int isActive) -{ - const int mode = 2; - updateVolumeUnit(mode, indx, free_row, isActive); -} - -void VolumesTable::updateLastVolumeIndx(Vec3i indx, int lastVisibleIndex) -{ - const int mode = 3; - updateVolumeUnit(mode, indx, free_row, free_isActive, lastVisibleIndex); -} - -void VolumesTable::updateActivity(Vec3i indx, int isActive, int lastVisibleIndex) -{ - const int mode = 4; - updateVolumeUnit(mode, indx, free_row, isActive, lastVisibleIndex); -} - -bool VolumesTable::getActive(Vec3i indx) const -{ - Vec4i idx(indx[0], indx[1], indx[2], 0); - int hash = int(calc_hash(idx) % hash_divisor); int bufferNum = 0; - int i = (bufferNum * list_size * hash_divisor) + (hash * list_size); - while (i != -1) - { - const Volume_NODE& v = *volumes.ptr(i); - if (v.idx == idx) - return (v.isActive == 1); - if (v.idx[0] == NAN_ELEMENT) - return false; - i = v.nextVolumeRow; - } - return false; -} + int i = getPos(idx, bufferNum); -int VolumesTable::getNextVolume(int hash, int& bufferNum, int i, int start) -{ - if (i != start && i % list_size == 0) + while (i >= 0) { - if (bufferNum < bufferNums-1) - { - bufferNum++; - } + const Volume_NODE* v = volumes.ptr(i); + + if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0)) + return v; else - { - this->expand(); - bufferNum++; - } - return (bufferNum * list_size * hash_divisor) + (hash * list_size); + i = v->nextVolumeRow; } - else - { - return i+1; - } -} -void VolumesTable::expand() -{ - this->volumes.resize(hash_divisor * (bufferNums)); - this->bufferNums++; + return nullptr; } -int VolumesTable::find_Volume(Vec3i indx) const +Volume_NODE* VolumesTable::find(Vec3i idx) { - Vec4i idx(indx[0], indx[1], indx[2], 0); - int hash = int(calc_hash(idx) % hash_divisor); int bufferNum = 0; - int i = (bufferNum * list_size * hash_divisor) + (hash * list_size); - while (i != -1) + int i = getPos(idx, bufferNum); + + while (i >= 0) { - const Volume_NODE& v = *volumes.ptr(i); - if (v.idx == idx) - return v.row; - //find nan cheking for int or Vec3i - //if (isNaN(Point3i(v.idx))) - if (v.idx[0] == NAN_ELEMENT) - return -2; - i = v.nextVolumeRow; + Volume_NODE* v = volumes.ptr(i); + + if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0)) + return v; + else + i = v->nextVolumeRow; } - return -2; -} -bool VolumesTable::isExist(Vec3i indx) -{ - if (this->find_Volume(indx) == -2) - return false; - return true; -} + return nullptr; +} } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 78f27829d12..733ae5ecbae 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -73,20 +73,18 @@ class VolumesTable cv::Mat volumes; VolumesTable(); + const VolumesTable& operator=(const VolumesTable&); ~VolumesTable() {}; - void updateVolumeUnit(int mode, Vec3i indx, int row, int isActive, int lastVisibleIndex); - void updateIndx(Vec3i indx); - void updateRow(Vec3i indx, int row); - void updateIsActive(Vec3i indx, int isActive); - void updateLastVolumeIndx(Vec3i indx, int lastVisibleIndex); - void updateActivity(Vec3i indx, int isActive, int lastVisibleIndex); - - void expand(); - bool getActive(Vec3i indx) const; - int getNextVolume(int hash, int& num, int i, int start); - int find_Volume(Vec3i indx) const; - bool isExist(Vec3i indx); + Volume_NODE* insert(Vec3i idx, int row, bool isActive = false, int lastVisibleIndex = -1); + const Volume_NODE* find(Vec3i idx) const; + Volume_NODE* find(Vec3i idx); + + inline int getPos(Vec3i idx, int bufferNum) const + { + int hash = int(calc_hash(idx) % hash_divisor); + return (bufferNum * hash_divisor + hash) * list_size; + } inline size_t calc_hash(Vec3i x) const { From d23514ef3d31198950e3002fd5d932922f7fe31b Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 25 Jan 2021 03:37:40 +0300 Subject: [PATCH 139/216] no typedef VolumeIndex --- modules/rgbd/src/hash_tsdf.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d19d59fe364..6037f35dc40 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -53,11 +53,10 @@ struct tsdf_hash } }; -typedef int VolumeIndex; struct VolumeUnit { cv::Vec3i coord; - VolumeIndex index; + int index; cv::Matx44f pose; int lastVisibleIndex = 0; bool isActive; @@ -93,7 +92,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, int indx) const; TsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const; @@ -115,7 +114,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume Mat pixNorms; VolumeUnitIndexes volumeUnits; cv::Mat volUnitsData; - VolumeIndex lastVolIndex; + int lastVolIndex; }; @@ -228,7 +227,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 > int(volUnitsData.size().height)) { volUnitsData.resize((lastVolIndex - 1) * 2); } @@ -936,8 +935,8 @@ class HashTSDFVolumeGPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) - virtual TsdfVoxel new_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const; - TsdfVoxel new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const; + virtual TsdfVoxel new_at(const cv::Vec3i& volumeIdx, int indx) const; + TsdfVoxel new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, int indx) const; float interpolateVoxelPoint(const Point3f& point) const; @@ -962,7 +961,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::Mat allVol2cam; cv::Mat volUnitsData; cv::UMat pixNorms; - VolumeIndex lastVolIndex; + int lastVolIndex; VolumesTable indexes; Vec8i neighbourCoords; }; @@ -1404,7 +1403,7 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const // A small hash table to reduce a number of find() calls bool queried[8]; - VolumeIndex iterMap[8]; + int iterMap[8]; for (int i = 0; i < 8; i++) { iterMap[i] = lastVolIndex; @@ -1458,7 +1457,7 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const // A small hash table to reduce a number of find() calls bool queried[8]; - VolumeIndex iterMap[8]; + int iterMap[8]; for (int i = 0; i < 8; i++) { From 18471c972afd50bf740637ac2379171391350377 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 25 Jan 2021 03:38:08 +0300 Subject: [PATCH 140/216] fix a bug in CPU version --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 6037f35dc40..38b13756c33 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -259,14 +259,14 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma Vec3i tsdf_idx = totalVolUnits[i]; VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx); if (it == volumeUnits.end()) - return; + continue; Point3f volumeUnitPos = volumeUnitIdxToVolume(it->first); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { it->second.isActive = false; - return; + continue; } Point2f cameraPoint = proj(volUnitInCamSpace); if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) From b177f7ee67ba0a63a2556b88167ae86180e5d226 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 25 Jan 2021 03:41:36 +0300 Subject: [PATCH 141/216] VolumeIndex --- 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 38b13756c33..cdc68044d3e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -334,7 +334,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, int indx) const { //! Out of bounds if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || From af1f2a2905cb9d6401594c413cc0b34ace3913ee Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 25 Jan 2021 03:41:53 +0300 Subject: [PATCH 142/216] identation --- 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 cdc68044d3e..45a4a7bc539 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1074,9 +1074,9 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f UMat yy = y.getUMat(af); kk.args(ocl::KernelArg::PtrReadWrite(pixNorm), - ocl::KernelArg::PtrReadOnly(xx), - ocl::KernelArg::PtrReadOnly(yy), - depth_cols); + ocl::KernelArg::PtrReadOnly(xx), + ocl::KernelArg::PtrReadOnly(yy), + depth_cols); size_t globalSize[2]; globalSize[0] = depth_rows; From 9e1a6c933afa2e7b2eb1da0c8bb132a2f15f51f3 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 25 Jan 2021 04:10:22 +0300 Subject: [PATCH 143/216] HashTSDF refactoring: a lot of things --- modules/rgbd/src/hash_tsdf.cpp | 209 +++++++++++++++++++++------------ 1 file changed, 132 insertions(+), 77 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 45a4a7bc539..65c362e6e2b 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -922,6 +922,8 @@ class HashTSDFVolumeGPU : public HashTSDFVolume //void integrateVolumeUnitGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx); void integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics); + std::vector allocateVolumeUnits(Depth depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); + void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, @@ -1064,7 +1066,6 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f ocl::Kernel kk; kk.create(name.c_str(), source, options, &errorStr); - if (kk.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); @@ -1158,12 +1159,13 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept U_hashtable.release(); } -void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) -{ - CV_TRACE_FUNCTION(); - CV_Assert(_depth.type() == DEPTH_TYPE); - Depth depth = _depth.getMat(); +std::vector HashTSDFVolumeGPU::allocateVolumeUnits(Depth depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) +{ + const int newIndicesCapacity = VOLUMES_SIZE; + const int localCapacity = VOLUMES_SIZE; + std::vector nodePtrs; + nodePtrs.reserve(newIndicesCapacity); //! Compute volumes to be allocated const int depthStride = int(log2(volumeUnitResolution)); @@ -1171,15 +1173,10 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma const Intr::Reprojector reproj(intrinsics.makeReprojector()); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Point3f truncPt(truncDist, truncDist, truncDist); - _VolumeUnitIndexSet _newIndices = cv::Mat(VOLUMES_SIZE, 1, CV_32S); - cv::Mat newIndices = cv::Mat(VOLUMES_SIZE, 1, CV_32SC3); Mutex mutex; Range allocateRange(0, depth.rows); - int vol_idx = 0; - auto AllocateVolumeUnitsInvoker = [&](const Range& range) { - _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(VOLUMES_SIZE, 1, CV_32SC3); - int loc_vol_idx = 0; + auto fillLocalAcessVolUnits = [&](const Range& range, _VolumeUnitIndexSet& _localAccessVolUnits, int& loc_vol_idx) { for (int y = range.start; y < range.end; y += depthStride) { const depthType* depthRow = depth[y]; @@ -1204,32 +1201,32 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { *_localAccessVolUnits.ptr(loc_vol_idx) = tsdf_idx; loc_vol_idx++; - } + if (loc_vol_idx >= localCapacity) + { + return; + } + } } } } + }; + + auto AllocateVolumeUnitsInvoker = [&](const Range& range) { + _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(localCapacity, 1, CV_32SC3); + int loc_vol_idx = 0; + + fillLocalAcessVolUnits(range, _localAccessVolUnits, loc_vol_idx); mutex.lock(); for (int i = 0; i < loc_vol_idx; i++) { Vec3i idx = *_localAccessVolUnits.ptr(i); - if (!indexes.isExist(idx)) + if (!indexes.find(idx)) { - if (lastVolIndex >= buff_lvl) - { - degree++; - buff_lvl = (int) pow(2, degree); - volUnitsData.resize(buff_lvl); - poses.resize(buff_lvl); - lastVisibleIndexes.resize(buff_lvl); - allVol2cam.resize(buff_lvl); - } - indexes.updateRow(idx, lastVolIndex); - *_newIndices.ptr(vol_idx) = lastVolIndex; - *newIndices.ptr(vol_idx) = idx; - vol_idx++; + Volume_NODE* node = indexes.insert(idx, lastVolIndex); + nodePtrs.push_back(node); lastVolIndex++; } } @@ -1237,35 +1234,66 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma }; parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); - //AllocateVolumeUnitsInvoker(allocateRange); + + return nodePtrs; +} + + +void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + Depth depth = _depth.getMat(); + + // Save pointers to avoid searches when allocating + std::vector nodePtrs = allocateVolumeUnits(depth, depthFactor, cameraPose, intrinsics); //! Perform the allocation - for (int i = 0; i < vol_idx; i++) + + // Grow buffers + if (lastVolIndex >= buff_lvl) + { + degree = (int)(log2(lastVolIndex) + 1); // clz() would be better + buff_lvl = (int)pow(2, degree); + volUnitsData.resize(buff_lvl); + poses.resize(buff_lvl); + lastVisibleIndexes.resize(buff_lvl); + allVol2cam.resize(buff_lvl); + } + + // Place data for new volume units + for (size_t i = 0; i < nodePtrs.size(); i++) { - Vec3i tsdf_idx = *newIndices.ptr(i); - VolumeIndex idx = indexes.find_Volume(tsdf_idx); - if (idx < 0) - continue; + Volume_NODE* node = nodePtrs[i]; + int row = node->row; + + Vec4i idx4 = node->idx; + Vec3i tsdf_idx(idx4[0], idx4[1], idx4[2]); Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; - *poses.ptr(idx, 0) = subvolumePose; - *lastVisibleIndexes.ptr(idx, 0) = frameId; - indexes.updateActivity(tsdf_idx, 1, frameId); + *poses.ptr(row, 0) = subvolumePose; + *lastVisibleIndexes.ptr(row, 0) = frameId; + node->isActive = true; + node->lastVisibleIndex = frameId; - volUnitsData.row(idx).forEach([](VecTsdfVoxel& vv, const int*) - { - TsdfVoxel& v = reinterpret_cast(vv); - v.tsdf = floatToTsdf(0.0f); v.weight = 0; - }); + volUnitsData.row(row).forEach([](VecTsdfVoxel& vv, const int*) + { + TsdfVoxel& v = reinterpret_cast(vv); + v.tsdf = floatToTsdf(0.0f); v.weight = 0; + }); } + //TODO: make isActive an array (as well as lastVisibleIndices) + //TODO: to iterate over them + //! Get keys for all the allocated volume Units std::vector _totalVolUnits = indexes.indexes; //! Mark volumes in the camera frustum as active Range _inFrustumRange(0, (int)_totalVolUnits.size()); - auto markActivities = [&](const Range& range) { + auto markActive = [&](const Range& range) { const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); const Intr::Projector proj(intrinsics.makeProjector()); @@ -1273,27 +1301,39 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { Vec3i tsdf_idx = _totalVolUnits[i]; - VolumeIndex idx = indexes.find_Volume(tsdf_idx); - if (idx < 0 || idx > lastVolIndex - 1) return; + int row = -1; + Volume_NODE* node = indexes.find(tsdf_idx); + if (node) + { + row = node->row; + } + else + { + CV_Error(Error::StsInternal, "Internal error: node not found"); + } + if (row < 0) + { + CV_Error(Error::StsInternal, "Internal error: row < 0"); + } Point3f volumeUnitPos = volumeUnitIdxToVolume(tsdf_idx); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { - indexes.updateIsActive(tsdf_idx, 0); - return; + node->isActive = false; + continue; } Point2f cameraPoint = proj(volUnitInCamSpace); if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) { - assert(idx >= 0 || idx < lastVolIndex); - *lastVisibleIndexes.ptr(idx, 0) = frameId; - indexes.updateActivity(tsdf_idx, 1, frameId); + assert(row >= 0 || row < lastVolIndex); + *lastVisibleIndexes.ptr(row, 0) = frameId; + node->isActive = true; + node->lastVisibleIndex = frameId; } } }; - parallel_for_(_inFrustumRange, markActivities); - //markActivities(_inFrustumRange); + parallel_for_(_inFrustumRange, markActive); Vec6f newParams((float)depth.rows, (float)depth.cols, intrinsics.fx, intrinsics.fy, @@ -1305,23 +1345,45 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); } + //TODO: the same about iteration + //! Integrate the correct volumeUnits auto preCalculateAllVol3Cam = [&](const Range& range) { for (int i = range.start; i < range.end; i++) { Vec3i tsdf_idx = _totalVolUnits[i]; - VolumeIndex idx = indexes.find_Volume(tsdf_idx); + + Volume_NODE* node = indexes.find(tsdf_idx); + int row = -1; + if (node) + { + row = node->row; + } + else + { + CV_Error(Error::StsInternal, "Internal error: node not found"); + } + if (row < 0) + { + CV_Error(Error::StsInternal, "Internal error: row < 0"); + } + + bool _isActive = node->isActive; + + /* + int idx = indexes.find_Volume(tsdf_idx); if (idx < 0 || idx == lastVolIndex - 1) return; bool _isActive = indexes.getActive(tsdf_idx); + */ if (_isActive) { - Matx44f _pose = *poses.ptr(idx, 0); + Matx44f _pose = *poses.ptr(row, 0); const cv::Affine3f vol2cam(Affine3f(cameraPose.inv())* Affine3f(_pose)); auto vol2camMatrix = vol2cam.matrix.val; for (int k = 0; k < 16; k++) { - *allVol2cam.ptr(idx, k) = vol2camMatrix[k]; + *allVol2cam.ptr(row, k) = vol2camMatrix[k]; } } } @@ -1356,7 +1418,7 @@ cv::Vec3i HashTSDFVolumeGPU::volumeToVoxelCoord(const cv::Point3f& point) const cvFloor(point.z * voxelSizeInv)); } -inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeIndex indx) const +inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, int indx) const { //! Out of bounds if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || @@ -1377,7 +1439,7 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, VolumeInd return volData[coordBase]; } -TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeIndex indx) const +TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, int indx) const { if (indx < 0 || indx > lastVolIndex) { @@ -1429,12 +1491,10 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const auto it = iterMap[dictIdx]; if (!queried[dictIdx]) { - it = indexes.find_Volume(volumeUnitIdx); - if (it >= 0 || it < lastVolIndex) - { - iterMap[dictIdx] = it; - queried[dictIdx] = true; - } + const Volume_NODE* node = indexes.find(volumeUnitIdx); + it = node ? node->row : -1; + iterMap[dictIdx] = it; + queried[dictIdx] = true; } vx[i] = new_atVolumeUnit(pt, volumeUnitIdx, it).tsdf; @@ -1496,12 +1556,10 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const if (!queried[dictIdx]) { - it = indexes.find_Volume(volumeUnitIdx); - if (it >= 0 || it < lastVolIndex) - { - iterMap[dictIdx] = it; - queried[dictIdx] = true; - } + const Volume_NODE* node = indexes.find(volumeUnitIdx); + it = node ? node->row : -1; + iterMap[dictIdx] = it; + queried[dictIdx] = true; } vals[i] = tsdfToFloat(new_atVolumeUnit(pt, volumeUnitIdx, it).tsdf); @@ -1639,21 +1697,19 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in { Point3f currRayPos = orig + tcurr * rayDirV; cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - //VolumeIndex idx = find_idx(indexes, currVolumeUnitIdx); - VolumeIndex idx = indexes.find_Volume(currVolumeUnitIdx); + const Volume_NODE* node = indexes.find(currVolumeUnitIdx); float currTsdf = prevTsdf; int currWeight = 0; float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; //! The subvolume exists in hashtable - if (idx >= 0 && idx < lastVolIndex) + if (node) { - cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = new_at(volUnitLocalIdx, idx); + TsdfVoxel currVoxel = new_at(volUnitLocalIdx, node->row); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -1806,11 +1862,10 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { cv::Vec3i tsdf_idx = _totalVolUnits[i]; - //VolumeIndex idx = find_idx(indexes, tsdf_idx); - VolumeIndex idx = indexes.find_Volume(tsdf_idx); + const Volume_NODE* node = indexes.find(tsdf_idx); Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); - if (idx >= 0 && idx < lastVolIndex) + if (node) { std::vector localPoints; std::vector localNormals; @@ -1819,7 +1874,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = new_at(voxelIdx, idx); + TsdfVoxel voxel = new_at(voxelIdx, node->row); if (voxel.tsdf != -128 && voxel.weight != 0) { From a1fddd2a1f956580746f19f3a45e6632de65567a Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 26 Jan 2021 02:29:33 +0300 Subject: [PATCH 144/216] less TODOs --- modules/rgbd/src/tsdf.hpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 009c4a8466c..e429c31fc68 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -17,9 +17,6 @@ namespace cv { namespace kinfu { -// TODO: Optimization possible: -// * TsdfType can be FP16 -// * WeightType can be uint16 typedef int8_t TsdfType; typedef uchar WeightType; @@ -107,8 +104,7 @@ class TSDFVolumeGPU : public TSDFVolume UMat pixNorms; // See zFirstMemOrder arg of parent class constructor // for the array layout info - // Array elem is CV_32FC2, read as (float, int) - // TODO: optimization possible to (fp16, int16), see Voxel definition + // Array elem is CV_8UC2, read as (int8, uint8) UMat volume; }; #endif From e6a69fbe84e3085ead3d1041bea7ce942c864aeb Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 26 Jan 2021 03:52:07 +0300 Subject: [PATCH 145/216] lastVisibleIndex dropped from HT --- modules/rgbd/src/hash_tsdf.cpp | 4 ---- modules/rgbd/src/opencl/hash_tsdf.cl | 2 +- modules/rgbd/src/tsdf_functions.cpp | 7 +++---- modules/rgbd/src/tsdf_functions.hpp | 4 ++-- 4 files changed, 6 insertions(+), 11 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 6d2fba61131..d655d352df5 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1282,11 +1282,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma *poses.ptr(row, 0) = subvolumePose; *lastVisibleIndexes.ptr(row, 0) = frameId; - *isActiveFlags.ptr(row, 0) = 1; - node->lastVisibleIndex = frameId; - volUnitsData.row(row).forEach([](VecTsdfVoxel& vv, const int*) { TsdfVoxel& v = reinterpret_cast(vv); @@ -1338,7 +1335,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma assert(row >= 0 || row < lastVolIndex); *lastVisibleIndexes.ptr(row, 0) = frameId; *isActiveFlags.ptr(row, 0) = 1; - node->lastVisibleIndex = frameId; } } }; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index ba1fb4a765e..7e9ae84c92d 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -24,7 +24,7 @@ struct Volume_NODE int32_t row; int32_t nextVolumeRow; int32_t dummy; - int32_t lastVisibleIndex; + int32_t dummy2; }; static inline TsdfType floatToTsdf(float num) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index eb9353f4019..3135460da63 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -384,8 +384,6 @@ VolumesTable::VolumesTable() : bufferNums(1), indexes(), indexesGPU() v->idx = nan4; v->row = -1; v->nextVolumeRow = -1; - v->lastVisibleIndex = -1; - //v.tmp = i; } } @@ -398,8 +396,10 @@ const VolumesTable& VolumesTable::operator=(const VolumesTable& vt) return *this; } -Volume_NODE* VolumesTable::insert(Vec3i idx, int row, int lastVisibleIndex) +Volume_NODE* VolumesTable::insert(Vec3i idx, int row) { + CV_Assert(row >= 0); + int bufferNum = 0; int hash = int(calc_hash(idx) % hash_divisor); int start = getPos(idx, bufferNum); @@ -430,7 +430,6 @@ Volume_NODE* VolumesTable::insert(Vec3i idx, int row, int lastVisibleIndex) v->idx = idx4; v->row = row; - v->lastVisibleIndex = lastVisibleIndex; indexes.push_back(idx); indexesGPU.push_back(idx4); diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 53071ce7c48..08aefafedee 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -51,7 +51,7 @@ struct Volume_NODE int32_t row = -1; int32_t nextVolumeRow = -1; int32_t dummy = 0; - int32_t lastVisibleIndex = -1; + int32_t dummy2 = 0; }; const int _hash_divisor = 32768; @@ -76,7 +76,7 @@ class VolumesTable const VolumesTable& operator=(const VolumesTable&); ~VolumesTable() {}; - Volume_NODE* insert(Vec3i idx, int row, int lastVisibleIndex = -1); + Volume_NODE* insert(Vec3i idx, int row); const Volume_NODE* find(Vec3i idx) const; Volume_NODE* find(Vec3i idx); From 8652e747075f621a77ff91c1f1ce5e00efc58a1f Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 26 Jan 2021 03:55:12 +0300 Subject: [PATCH 146/216] HT refactor: find -> findRow --- modules/rgbd/src/hash_tsdf.cpp | 60 ++++++++--------------------- modules/rgbd/src/tsdf_functions.cpp | 25 ++---------- modules/rgbd/src/tsdf_functions.hpp | 4 +- 3 files changed, 22 insertions(+), 67 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d655d352df5..1ac955d7223 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1227,7 +1227,8 @@ std::vector HashTSDFVolumeGPU::allocateVolumeUnits(Depth depth, fl { Vec3i idx = *_localAccessVolUnits.ptr(i); - if (!indexes.find(idx)) + // if not found + if (indexes.findRow(idx) < 0) { Volume_NODE* node = indexes.insert(idx, lastVolIndex); nodePtrs.push_back(node); @@ -1263,9 +1264,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma volUnitsData.resize(buff_lvl); poses.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); - isActiveFlags.resize(buff_lvl); - allVol2cam.resize(buff_lvl); } @@ -1307,20 +1306,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { Vec3i tsdf_idx = _totalVolUnits[i]; - int row = -1; - Volume_NODE* node = indexes.find(tsdf_idx); - if (node) - { - row = node->row; - } - else - { - CV_Error(Error::StsInternal, "Internal error: node not found"); - } - if (row < 0) - { - CV_Error(Error::StsInternal, "Internal error: row < 0"); - } + int row = indexes.findRow(tsdf_idx); + CV_Assert(row >= 0); Point3f volumeUnitPos = volumeUnitIdxToVolume(tsdf_idx); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; @@ -1332,7 +1319,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Point2f cameraPoint = proj(volUnitInCamSpace); if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) { - assert(row >= 0 || row < lastVolIndex); + CV_Assert(row >= 0 || row < lastVolIndex); *lastVisibleIndexes.ptr(row, 0) = frameId; *isActiveFlags.ptr(row, 0) = 1; } @@ -1358,20 +1345,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { Vec3i tsdf_idx = _totalVolUnits[i]; - Volume_NODE* node = indexes.find(tsdf_idx); - int row = -1; - if (node) - { - row = node->row; - } - else - { - CV_Error(Error::StsInternal, "Internal error: node not found"); - } - if (row < 0) - { - CV_Error(Error::StsInternal, "Internal error: row < 0"); - } + int row = indexes.findRow(tsdf_idx); + CV_Assert(row >= 0); bool _isActive = bool(*isActiveFlags.ptr(row, 0)); @@ -1439,7 +1414,7 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, int indx) TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, int indx) const { - if (indx < 0 || indx > lastVolIndex) + if (indx < 0) { TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.f); @@ -1489,8 +1464,7 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const auto it = iterMap[dictIdx]; if (!queried[dictIdx]) { - const Volume_NODE* node = indexes.find(volumeUnitIdx); - it = node ? node->row : -1; + it = indexes.findRow(volumeUnitIdx); iterMap[dictIdx] = it; queried[dictIdx] = true; } @@ -1554,8 +1528,7 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const if (!queried[dictIdx]) { - const Volume_NODE* node = indexes.find(volumeUnitIdx); - it = node ? node->row : -1; + it = indexes.findRow(volumeUnitIdx); iterMap[dictIdx] = it; queried[dictIdx] = true; } @@ -1695,19 +1668,20 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in { Point3f currRayPos = orig + tcurr * rayDirV; cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - const Volume_NODE* node = indexes.find(currVolumeUnitIdx); + + int row = indexes.findRow(currVolumeUnitIdx); float currTsdf = prevTsdf; int currWeight = 0; float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; //! The subvolume exists in hashtable - if (node) + if (row >= 0) { cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = new_at(volUnitLocalIdx, node->row); + TsdfVoxel currVoxel = new_at(volUnitLocalIdx, row); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; stepSize = tstep; @@ -1860,10 +1834,10 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { cv::Vec3i tsdf_idx = _totalVolUnits[i]; - const Volume_NODE* node = indexes.find(tsdf_idx); + int row = indexes.findRow(tsdf_idx); Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); - if (node) + if (row >= 0) { std::vector localPoints; std::vector localNormals; @@ -1872,7 +1846,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = new_at(voxelIdx, node->row); + TsdfVoxel voxel = new_at(voxelIdx, row); if (voxel.tsdf != -128 && voxel.weight != 0) { diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 3135460da63..69f0cc46a67 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -440,40 +440,23 @@ Volume_NODE* VolumesTable::insert(Vec3i idx, int row) } } -const Volume_NODE* VolumesTable::find(Vec3i idx) const -{ - int bufferNum = 0; - int i = getPos(idx, bufferNum); - - while (i >= 0) - { - const Volume_NODE* v = volumes.ptr(i); - - if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0)) - return v; - else - i = v->nextVolumeRow; - } - return nullptr; -} - -Volume_NODE* VolumesTable::find(Vec3i idx) +int VolumesTable::findRow(Vec3i idx) const { int bufferNum = 0; int i = getPos(idx, bufferNum); while (i >= 0) { - Volume_NODE* v = volumes.ptr(i); + const Volume_NODE* v = volumes.ptr(i); if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0)) - return v; + return v->row; else i = v->nextVolumeRow; } - return nullptr; + return -1; } } // namespace kinfu diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 08aefafedee..dec21c932a9 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -77,8 +77,7 @@ class VolumesTable ~VolumesTable() {}; Volume_NODE* insert(Vec3i idx, int row); - const Volume_NODE* find(Vec3i idx) const; - Volume_NODE* find(Vec3i idx); + int findRow(Vec3i idx) const; inline int getPos(Vec3i idx, int bufferNum) const { @@ -99,7 +98,6 @@ class VolumesTable }; - } // namespace kinfu } // namespace cv #endif From 6b8ef0acb3289e48b89e4e03a5f015c0f45c7c42 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 26 Jan 2021 03:56:48 +0300 Subject: [PATCH 147/216] interpolate & getNormal small hash: no queried --- modules/rgbd/src/hash_tsdf.cpp | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 1ac955d7223..353ae06e6e2 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1437,12 +1437,13 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1} }; // A small hash table to reduce a number of find() calls - bool queried[8]; + // -2 and lower means not queried yet + // -1 means not found + // 0+ means found int iterMap[8]; for (int i = 0; i < 8; i++) { - iterMap[i] = lastVolIndex; - queried[i] = false; + iterMap[i] = -2; } int ix = cvFloor(point.x); @@ -1462,11 +1463,10 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; auto it = iterMap[dictIdx]; - if (!queried[dictIdx]) + if (it < -1) { it = indexes.findRow(volumeUnitIdx); iterMap[dictIdx] = it; - queried[dictIdx] = true; } vx[i] = new_atVolumeUnit(pt, volumeUnitIdx, it).tsdf; @@ -1488,13 +1488,13 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); // A small hash table to reduce a number of find() calls - bool queried[8]; + // -2 and lower means not queried yet + // -1 means not found + // 0+ means found int iterMap[8]; - for (int i = 0; i < 8; i++) { - iterMap[i] = lastVolIndex; - queried[i] = false; + iterMap[i] = -2; } #if !USE_INTERPOLATION_IN_GETNORMAL @@ -1525,12 +1525,10 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; auto it = iterMap[dictIdx]; - - if (!queried[dictIdx]) + if (it < -1) { it = indexes.findRow(volumeUnitIdx); iterMap[dictIdx] = it; - queried[dictIdx] = true; } vals[i] = tsdfToFloat(new_atVolumeUnit(pt, volumeUnitIdx, it).tsdf); From 012c137f05a6e0cebf9b9c1749f2f48c40c8e765 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 26 Jan 2021 05:19:56 +0300 Subject: [PATCH 148/216] totalVolUnits, indexes -> volumeUnitIndices, better iteration --- modules/rgbd/src/hash_tsdf.cpp | 149 ++++++++++----------------- modules/rgbd/src/opencl/hash_tsdf.cl | 23 +---- modules/rgbd/src/tsdf_functions.cpp | 6 +- modules/rgbd/src/tsdf_functions.hpp | 2 - 4 files changed, 63 insertions(+), 117 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 353ae06e6e2..3dc372ff608 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -792,17 +792,13 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor bool needNormals(_normals.needed()); Mutex mutex; - auto HashFetchPointsNormalsInvoker = [&](const Range& range) { - - std::vector points, normals; for (int i = range.start; i < range.end; i++) { cv::Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(tsdf_idx); Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); if (it != volume.volumeUnits.end()) @@ -837,8 +833,6 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor parallel_for_(fetchRange, HashFetchPointsNormalsInvoker, nstripes); - - std::vector points, normals; for (size_t i = 0; i < pVecs.size(); i++) { @@ -958,13 +952,15 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Vec6f frameParams; int degree; int buff_lvl; + + // per-volume-unit data + cv::Mat volumeUnitIndices; cv::Mat poses; cv::Mat lastVisibleIndexes; - cv::Mat isActiveFlags; - cv::Mat allVol2cam; cv::Mat volUnitsData; + cv::UMat pixNorms; int lastVolIndex; VolumesTable indexes; @@ -1016,14 +1012,16 @@ void HashTSDFVolumeGPU::reset() lastVolIndex = 0; degree = 15; buff_lvl = (int) pow(2, degree); + volUnitsData = cv::Mat(buff_lvl, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); poses = cv::Mat(buff_lvl, 1, rawType()); lastVisibleIndexes = cv::Mat(buff_lvl, 1, CV_32S); - isActiveFlags = cv::Mat(buff_lvl, 1, CV_8U); + allVol2cam = cv::Mat(buff_lvl, 16, CV_32F); + volumeUnitIndices = cv::Mat(buff_lvl, 1, CV_32SC4); indexes = VolumesTable(); - allVol2cam = cv::Mat(buff_lvl, 16, CV_32F); + frameParams = Vec6f(); pixNorms = UMat(); } @@ -1116,51 +1114,38 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - int totalVolUnitsSize = (int) indexes.indexesGPU.size(); - Mat totalVolUnits = cv::Mat(indexes.indexesGPU, true); - Mat _tmp; volUnitsData.copyTo(_tmp); UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); _tmp.release(); - indexes.volumes.copyTo(_tmp); - UMat U_hashtable = _tmp.getUMat(ACCESS_RW); - k.args(ocl::KernelArg::ReadOnly(depth), - ocl::KernelArg::PtrReadWrite(U_hashtable), - (int)indexes.list_size, - (int)indexes.bufferNums, - (int)indexes.hash_divisor, - ocl::KernelArg::ReadWrite(totalVolUnits.getUMat(ACCESS_RW)), - ocl::KernelArg::ReadWrite(U_volUnitsData), - ocl::KernelArg::PtrReadOnly(pixNorms), - ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), - ocl::KernelArg::ReadOnly(isActiveFlags.getUMat(ACCESS_READ)), - lastVolIndex, - voxelSize, - volResGpu.val, - volStrides.val, - fxy.val, - cxy.val, - dfac, - truncDist, - int(maxWeight) + ocl::KernelArg::ReadOnly(volumeUnitIndices.getUMat(ACCESS_READ)), + ocl::KernelArg::ReadWrite(U_volUnitsData), + ocl::KernelArg::PtrReadOnly(pixNorms), + ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), + ocl::KernelArg::ReadOnly(isActiveFlags.getUMat(ACCESS_READ)), + voxelSize, + volResGpu.val, + volStrides.val, + fxy.val, + cxy.val, + dfac, + truncDist, + int(maxWeight) ); int resol = volumeUnitResolution; size_t globalSize[3]; globalSize[0] = (size_t)resol; // volumeUnitResolution globalSize[1] = (size_t)resol; // volumeUnitResolution - globalSize[2] = (size_t)totalVolUnitsSize; // num of voxels + globalSize[2] = (size_t)lastVolIndex; // num of volume units if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); U_volUnitsData.getMat(ACCESS_RW).copyTo(volUnitsData); - U_volUnitsData.release(); - U_hashtable.release(); } @@ -1264,6 +1249,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma volUnitsData.resize(buff_lvl); poses.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); + volumeUnitIndices.resize(buff_lvl); isActiveFlags.resize(buff_lvl); allVol2cam.resize(buff_lvl); } @@ -1282,6 +1268,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma *poses.ptr(row, 0) = subvolumePose; *lastVisibleIndexes.ptr(row, 0) = frameId; *isActiveFlags.ptr(row, 0) = 1; + *volumeUnitIndices.ptr(row, 0) = idx4; volUnitsData.row(row).forEach([](VecTsdfVoxel& vv, const int*) { @@ -1290,26 +1277,18 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma }); } - //TODO: make isActive an array (as well as lastVisibleIndices) - //TODO: to iterate over them - - //! Get keys for all the allocated volume Units - std::vector _totalVolUnits = indexes.indexes; - //! Mark volumes in the camera frustum as active - Range _inFrustumRange(0, (int)_totalVolUnits.size()); + Range _inFrustumRange(0, lastVolIndex); auto markActive = [&](const Range& range) { const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); const Intr::Projector proj(intrinsics.makeProjector()); - for (int i = range.start; i < range.end; ++i) + for (int row = range.start; row < range.end; ++row) { - Vec3i tsdf_idx = _totalVolUnits[i]; + Vec4i idx4 = *volumeUnitIndices.ptr(row, 0); + Vec3i idx(idx4[0], idx4[1], idx4[2]); - int row = indexes.findRow(tsdf_idx); - CV_Assert(row >= 0); - - Point3f volumeUnitPos = volumeUnitIdxToVolume(tsdf_idx); + Point3f volumeUnitPos = volumeUnitIdxToVolume(idx); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { @@ -1337,16 +1316,12 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); } - //TODO: the same about iteration - //! Integrate the correct volumeUnits auto preCalculateAllVol3Cam = [&](const Range& range) { - for (int i = range.start; i < range.end; i++) + for (int row = range.start; row < range.end; row++) { - Vec3i tsdf_idx = _totalVolUnits[i]; - - int row = indexes.findRow(tsdf_idx); - CV_Assert(row >= 0); + Vec4i idx4 = *volumeUnitIndices.ptr(row, 0); + Vec3i idx(idx4[0], idx4[1], idx4[2]); bool _isActive = bool(*isActiveFlags.ptr(row, 0)); @@ -1362,7 +1337,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma } } }; - parallel_for_(Range(0, (int)_totalVolUnits.size()), preCalculateAllVol3Cam ); + parallel_for_(Range(0, lastVolIndex), preCalculateAllVol3Cam ); //! Integrate the correct volumeUnits integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); @@ -1730,9 +1705,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); - //int totalVolUnitsSize = _indexes.indexesGPU.size(); - Mat totalVolUnits = cv::Mat(indexes.indexesGPU, true); - _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); @@ -1814,9 +1786,8 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor if (_points.needed()) { std::vector> pVecs, nVecs; - std::vector _totalVolUnits = indexes.indexes; - Range _fetchRange(0, (int)_totalVolUnits.size()); + Range _fetchRange(0, lastVolIndex); const int nstripes = -1; @@ -1824,51 +1795,45 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor bool needNormals(_normals.needed()); Mutex mutex; - auto _HashFetchPointsNormalsInvoker = [&](const Range& range) { std::vector points, normals; - for (int i = range.start; i < range.end; i++) + for (int row = range.start; row < range.end; row++) { - cv::Vec3i tsdf_idx = _totalVolUnits[i]; + cv::Vec4i idx4 = *volumeUnitIndices.ptr(row, 0); + cv::Vec3i idx(idx4[0], idx4[1], idx4[2]); - int row = indexes.findRow(tsdf_idx); - Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); + Point3f base_point = volume.volumeUnitIdxToVolume(idx); - if (row >= 0) - { - 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++) + 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 = new_at(voxelIdx, row); + + if (voxel.tsdf != -128 && voxel.weight != 0) { - cv::Vec3i voxelIdx(x, y, z); - TsdfVoxel voxel = new_at(voxelIdx, row); + Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); - if (voxel.tsdf != -128 && voxel.weight != 0) + localPoints.push_back(toPtype(point)); + if (needNormals) { - Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); - - localPoints.push_back(toPtype(point)); - if (needNormals) - { - Point3f normal = volume.getNormalVoxel(point); - localNormals.push_back(toPtype(normal)); - } + 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); } }; parallel_for_(_fetchRange, _HashFetchPointsNormalsInvoker, nstripes); - //_HashFetchPointsNormalsInvoker(_fetchRange); - std::vector points, normals; for (size_t i = 0; i < pVecs.size(); i++) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 7e9ae84c92d..432503e9907 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -240,13 +240,9 @@ __kernel void integrateAllVolumeUnits( __global const char * depthptr, int depth_step, int depth_offset, int depth_rows, int depth_cols, - __global struct Volume_NODE * hash_table, - const int list_size, - const int bufferNums, - const int hash_divisor, - __global const int4 * totalVolUnits, - int totalVolUnits_step, int totalVolUnits_offset, - int totalVolUnits_rows, int totalVolUnits_cols, + __global const int4 * volumeUnitIndices, + int volumeUnitIndices_step, int volumeUnitIndices_offset, + int volumeUnitIndices_rows, int volumeUnitIndices_cols, __global struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, int table_rows, int table_cols, @@ -254,12 +250,9 @@ __kernel void integrateAllVolumeUnits( __global const float * allVol2camMatrix, int val2cam_step, int val2cam_offset, int val2cam_rows, int val2cam_cols, - __global const uchar* isActiveFlagsPtr, int isActiveFlagsStep, int isActiveFlagsOffset, int isActiveFlagsRows, int isActiveFlagsCols, - - const int lastVolIndex, const float voxelSize, const int4 volResolution4, const int4 volDims4, @@ -272,13 +265,9 @@ __kernel void integrateAllVolumeUnits( { int i = get_global_id(0); int j = get_global_id(1); - int k = get_global_id(2); + int row = get_global_id(2); + int4 idx = volumeUnitIndices[row]; - int4 v = totalVolUnits[k]; - int row = findRow(hash_table, v, list_size, bufferNums, hash_divisor); - if (row < 0 || row > lastVolIndex-1) - return; - int isActive = (__global const uchar*)(isActiveFlagsPtr + isActiveFlagsOffset + (row)); if (isActive) @@ -307,9 +296,7 @@ __kernel void integrateAllVolumeUnits( truncDist, maxWeight ); - //updateIsActive(hash_table, v, 0, list_size, bufferNums, hash_divisor); } - } static struct TsdfVoxel _at(int3 volumeIdx, int row, diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 69f0cc46a67..a101817bd98 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -375,7 +375,7 @@ void integrateVolumeUnit( /// Custom Volume Hash Table -VolumesTable::VolumesTable() : bufferNums(1), indexes(), indexesGPU() +VolumesTable::VolumesTable() : bufferNums(1) { this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType()); for (int i = 0; i < volumes.size().height; i++) @@ -391,8 +391,6 @@ const VolumesTable& VolumesTable::operator=(const VolumesTable& vt) { this->volumes = vt.volumes; this->bufferNums = vt.bufferNums; - this->indexes = vt.indexes; - this->indexesGPU = vt.indexesGPU; return *this; } @@ -431,8 +429,6 @@ Volume_NODE* VolumesTable::insert(Vec3i idx, int row) v->idx = idx4; v->row = row; - indexes.push_back(idx); - indexesGPU.push_back(idx4); return v; } diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index dec21c932a9..62d79d54005 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -68,8 +68,6 @@ class VolumesTable const cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT); int bufferNums; - std::vector indexes; - std::vector indexesGPU; cv::Mat volumes; VolumesTable(); From eb828c3d17a2ba6fd6edc4d384097b3bbc2787b5 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 26 Jan 2021 22:40:53 +0300 Subject: [PATCH 149/216] raycast artifacts fixed --- modules/rgbd/src/opencl/hash_tsdf.cl | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 432503e9907..b0abb5b23d2 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -380,7 +380,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum for (int i = 0; i < 8; i++) { - iterMap[i] = lastVolIndex; + iterMap[i] = -1; queried[i] = false; } @@ -417,11 +417,8 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum if (!queried[dictIdx]) { it = findRow(hash_table, volumeUnitIdx4, list_size, bufferNums, hash_divisor); - if (it >= 0 || it < lastVolIndex) - { - iterMap[dictIdx] = it; - queried[dictIdx] = true; - } + iterMap[dictIdx] = it; + queried[dictIdx] = true; } struct TsdfVoxel tmp = _atVolumeUnit(pt, volumeUnitIdx, it, lastVolIndex, volResolution.s0, volStrides, allVolumePtr, table_offset) ; @@ -592,7 +589,6 @@ __kernel void raycast( stepSize = tstep; } - if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) { float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); @@ -609,7 +605,6 @@ __kernel void raycast( if(!any(isnan(nv))) { - //convert pv and nv to camera space normal = (float3)(dot(nv, volRot0), dot(nv, volRot1), @@ -619,9 +614,8 @@ __kernel void raycast( dot(pv, volRot1), dot(pv, volRot2)) + volTrans; } - } - + break; } prevTsdf = currTsdf; tprev = tcurr; From 1114c02a6a5edebd7353a5f9c375333f1eba8567 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 26 Jan 2021 22:51:24 +0300 Subject: [PATCH 150/216] interpolation in getNormal is on --- modules/rgbd/src/opencl/hash_tsdf.cl | 40 +++++++++++++++++----------- 1 file changed, 25 insertions(+), 15 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index b0abb5b23d2..eb828f4a44d 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -5,6 +5,7 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory #define NAN_ELEMENT -2147483647 +#define USE_INTERPOLATION_IN_GETNORMAL 1 typedef __INT8_TYPE__ int8_t; typedef __INT32_TYPE__ int32_t; @@ -384,12 +385,26 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum queried[i] = false; } +#if !USE_INTERPOLATION_IN_GETNORMAL int3 offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3 { 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7 }; const int nVals = 6; float vals[6]; +#else + int3 offsets[]={{ 0, 0, 0}, { 0, 0, 1}, { 0, 1, 0}, { 0, 1, 1}, // 0-3 + { 1, 0, 0}, { 1, 0, 1}, { 1, 1, 0}, { 1, 1, 1}, // 4-7 + {-1, 0, 0}, {-1, 0, 1}, {-1, 1, 0}, {-1, 1, 1}, // 8-11 + { 2, 0, 0}, { 2, 0, 1}, { 2, 1, 0}, { 2, 1, 1}, // 12-15 + { 0, -1, 0}, { 0, -1, 1}, { 1, -1, 0}, { 1, -1, 1}, // 16-19 + { 0, 2, 0}, { 0, 2, 1}, { 1, 2, 0}, { 1, 2, 1}, // 20-23 + { 0, 0, -1}, { 0, 1, -1}, { 1, 0, -1}, { 1, 1, -1}, // 24-27 + { 0, 0, 2}, { 0, 1, 2}, { 1, 0, 2}, { 1, 1, 2}, // 28-31 + }; + const int nVals = 32; + float vals[32]; +#endif for (int i = 0; i < nVals; i++) { @@ -423,15 +438,14 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum struct TsdfVoxel tmp = _atVolumeUnit(pt, volumeUnitIdx, it, lastVolIndex, volResolution.s0, volStrides, allVolumePtr, table_offset) ; vals[i] = tsdfToFloat( tmp.tsdf ); - } +#if !USE_INTERPOLATION_IN_GETNORMAL normal.s0 = vals[0 * 2] - vals[0 * 2 + 1]; normal.s1 = vals[1 * 2] - vals[1 * 2 + 1]; normal.s2 = vals[2 * 2] - vals[2 * 2 + 1]; +#else -// <========================================================> -/* float cxv[8], cyv[8], czv[8]; // How these numbers were obtained: @@ -462,17 +476,14 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum normal.s1 = interpolate(tx, ty, tz, cyv); normal.s2 = interpolate(tx, ty, tz, czv); - if(!any(isnan(normal))) - printf("%f, %f, %f" ,normal.s0, normal.s1, normal.s2); -*/ -// <========================================================> + //if(!any(isnan(normal))) + // printf("%f, %f, %f" ,normal.s0, normal.s1, normal.s2); - //normal.x*=(-1); normal.y*=(-1); normal.z*=(-1); +#endif - float norm = - sqrt(normal.x*normal.x - + normal.y*normal.y - + normal.z*normal.z); + float norm = sqrt(normal.x*normal.x + + normal.y*normal.y + + normal.z*normal.z); return norm < 0.0001f ? nan((uint)0) : normal / norm; } @@ -565,9 +576,8 @@ __kernel void raycast( float stepSize = 0.5 * volumeUnitSize; int3 volUnitLocalIdx; - if (row >= 0 && row < lastVolIndex) { - - + if (row >= 0 && row < lastVolIndex) + { //TsdfVoxel currVoxel // VolumeUnitIdxToVolume() float3 currVolUnitPos = (float3) From ce92dd66f2b71248d650f156a52a7918aeb86513 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 01:42:53 +0300 Subject: [PATCH 151/216] minors --- modules/rgbd/src/hash_tsdf.cpp | 12 ++++++------ modules/rgbd/src/opencl/hash_tsdf.cl | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 3dc372ff608..f9f75b8e3e9 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -411,8 +411,8 @@ static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) else { return Vec3i(cvFloor(float(pt[0]) / vuRes), - cvFloor(float(pt[1]) / vuRes), - cvFloor(float(pt[2]) / vuRes)); + cvFloor(float(pt[1]) / vuRes), + cvFloor(float(pt[2]) / vuRes)); } } @@ -1409,7 +1409,7 @@ TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& v float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const { const Vec3i local_neighbourCoords[] = { {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, - {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1} }; + {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1} }; // A small hash table to reduce a number of find() calls // -2 and lower means not queried yet @@ -1594,7 +1594,7 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray _points, OutputArray _normals) const + OutputArray _points, OutputArray _normals) const { if (false) @@ -1685,8 +1685,8 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in } } }; - //parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); - _HashRaycastInvoker(Range(0, new_points.rows)); + parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); + //_HashRaycastInvoker(Range(0, new_points.rows)); } diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index eb828f4a44d..a3a4a3f84bb 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -592,7 +592,7 @@ __kernel void raycast( ( floor ( (float) (pos.s1) * voxelSizeInv) ), ( floor ( (float) (pos.s2) * voxelSizeInv) ) ); - struct TsdfVoxel currVoxel = _at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); + struct TsdfVoxel currVoxel = at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; From 4bec743366812bfda8f7d2e5e9f313a04465393d Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 01:45:28 +0300 Subject: [PATCH 152/216] findRow() refactored --- modules/rgbd/src/opencl/hash_tsdf.cl | 37 +++++++++------------------- 1 file changed, 11 insertions(+), 26 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index a3a4a3f84bb..399d4a3908c 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -51,10 +51,9 @@ __kernel void preCalculationPixNorm (__global float * pixNorms, pixNorms[idx] = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); } -static uint calc_hash(int4 x) +static uint calc_hash(int3 x) { unsigned int seed = 0; - //uint GOLDEN_RATIO = 0x9e3779b9; unsigned int GOLDEN_RATIO = 0x9e3779b9; seed ^= x.s0 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); seed ^= x.s1 + GOLDEN_RATIO + (seed << 6) + (seed >> 2); @@ -62,27 +61,23 @@ static uint calc_hash(int4 x) return seed; } -static int findRow(__global struct Volume_NODE * hash_table, int4 indx, + +static int findRow(__global struct Volume_NODE * hash_table, int3 indx, int list_size, int bufferNums, int hash_divisor) { - int hash = calc_hash(indx) % hash_divisor; - int bufferNum = 0; + int hash = calc_hash(indx) % hash_divisor; int i = (bufferNum * hash_divisor + hash) * list_size; - int NAN_NUM = NAN_ELEMENT; - while (i != NAN_NUM) + while (i >= 0) { struct Volume_NODE v = hash_table[i]; - if (v.idx.s0 == indx.s0 && - v.idx.s1 == indx.s1 && - v.idx.s2 == indx.s2) + if (all(v.idx.s012 == indx.s012)) return v.row; - if (v.idx.x == NAN_NUM) - return -2; - i = v.nextVolumeRow; + else + i = v.nextVolumeRow; } - return -2; + return -1; } @@ -417,10 +412,6 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum floor ( (float) pt.s1 / volResolution.s1), floor ( (float) pt.s2 / volResolution.s2) ); - int4 volumeUnitIdx4 = (int4) ( - floor ( (float) pt.s0 / volResolution.s0), - floor ( (float) pt.s1 / volResolution.s1), - floor ( (float) pt.s2 / volResolution.s2), 0 ); int dictIdx = (volumeUnitIdx.s0 & 1) @@ -431,7 +422,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum if (!queried[dictIdx]) { - it = findRow(hash_table, volumeUnitIdx4, list_size, bufferNums, hash_divisor); + it = findRow(hash_table, volumeUnitIdx, list_size, bufferNums, hash_divisor); iterMap[dictIdx] = it; queried[dictIdx] = true; } @@ -564,13 +555,7 @@ __kernel void raycast( floor (currRayPos.y / volumeUnitSize), floor (currRayPos.z / volumeUnitSize) ); - // VolumeToVolumeUnitIdx4() - int4 currVolumeUnitIdx4 = (int4) ( - floor (currRayPos.x / volumeUnitSize), - floor (currRayPos.y / volumeUnitSize), - floor (currRayPos.z / volumeUnitSize), 0); - - int row = findRow(hash_table, currVolumeUnitIdx4, list_size, bufferNums, hash_divisor); + int row = findRow(hash_table, currVolumeUnitIdx, list_size, bufferNums, hash_divisor); float currTsdf = prevTsdf; int currWeight = 0; float stepSize = 0.5 * volumeUnitSize; From 9dee84b43a2a25d5a9d7f2e4c67108b3186b4229 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 01:58:28 +0300 Subject: [PATCH 153/216] volStrides, lastVolIndex, volDims, neighbourCoords --- modules/rgbd/src/opencl/hash_tsdf.cl | 61 +++++++++++++--------------- 1 file changed, 29 insertions(+), 32 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 399d4a3908c..06d2eb2675b 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -295,15 +295,14 @@ __kernel void integrateAllVolumeUnits( } } -static struct TsdfVoxel _at(int3 volumeIdx, int row, - int volumeUnitResolution, int4 volStrides, - __global struct TsdfVoxel * allVolumePtr, int table_offset) +static struct TsdfVoxel at(int3 volumeIdx, int row, int volumeUnitResolution, + int3 volStrides, __global struct TsdfVoxel * allVolumePtr, int table_offset) { //! Out of bounds - if ((volumeIdx.s0 >= volumeUnitResolution || volumeIdx.s0 < 0) || - (volumeIdx.s1 >= volumeUnitResolution || volumeIdx.s1 < 0) || - (volumeIdx.s2 >= volumeUnitResolution || volumeIdx.s2 < 0)) + if (any(volumeIdx >= volumeUnitResolution) || + any(volumeIdx < 0)) + { struct TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -312,21 +311,20 @@ static struct TsdfVoxel _at(int3 volumeIdx, int row, } __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) - (allVolumePtr + table_offset + (row) * 16*16*16); - int coordBase = volumeIdx.s0 * volStrides.s0 + - volumeIdx.s1 * volStrides.s1 + - volumeIdx.s2 * volStrides.s2; + (allVolumePtr + table_offset + row * 16*16*16); + int3 ismul = volumeIdx * volStrides; + int coordBase = ismul.x + ismul.y + ismul.z; return volData[coordBase]; } -static struct TsdfVoxel _atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row, int lastVolIndex, - int volumeUnitResolution, int4 volStrides, - __global const struct TsdfVoxel * allVolumePtr, int table_offset) +static struct TsdfVoxel atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row, + int volumeUnitResolution, int3 volStrides, + __global const struct TsdfVoxel * allVolumePtr, int table_offset) { //! Out of bounds - if (row < 0 || row > lastVolIndex - 1) + if (row < 0) { struct TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -336,11 +334,9 @@ static struct TsdfVoxel _atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int ro int3 volUnitLocalIdx = volumeIdx - volumeUnitIdx * volumeUnitResolution; __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) - (allVolumePtr + table_offset + (row) * 16*16*16); - int coordBase = - volUnitLocalIdx.s0 * volStrides.s0 + - volUnitLocalIdx.s1 * volStrides.s1 + - volUnitLocalIdx.s2 * volStrides.s2; + (allVolumePtr + table_offset + row * 16*16*16); + int3 ismul = volUnitLocalIdx * volStrides; + int coordBase = ismul.x + ismul.y + ismul.z; return volData[coordBase]; } @@ -358,18 +354,18 @@ inline float interpolate(float tx, float ty, float tz, float vx[8]) } inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolumePtr, - int3 volResolution, int3 volDims, int8 neighbourCoords, - float voxelSizeInv, int lastVolIndex, + int3 volResolution, + float voxelSizeInv, __global struct Volume_NODE * hash_table, - const int list_size, - const int bufferNums, + const int list_size, + const int bufferNums, const int hash_divisor, - int4 volStrides, int table_offset) + int3 volStrides, int table_offset) { float3 normal = (float3) (0.0f, 0.0f, 0.0f); float3 ptVox = p * voxelSizeInv; - int3 iptVox = (int3) ( floor (ptVox.x), floor (ptVox.y), floor (ptVox.z) ); + int3 iptVox = convert_int3(floor(ptVox)); bool queried[8]; int iterMap[8]; @@ -427,7 +423,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum queried[dictIdx] = true; } - struct TsdfVoxel tmp = _atVolumeUnit(pt, volumeUnitIdx, it, lastVolIndex, volResolution.s0, volStrides, allVolumePtr, table_offset) ; + struct TsdfVoxel tmp = atVolumeUnit(pt, volumeUnitIdx, it, volResolution.s0, volStrides, allVolumePtr, table_offset); vals[i] = tsdfToFloat( tmp.tsdf ); } @@ -509,7 +505,7 @@ __kernel void raycast( float volumeUnitSize, float truncDist, int volumeUnitResolution, - int4 volStrides + int4 volStrides4 ) { int x = get_global_id(0); @@ -545,6 +541,8 @@ __kernel void raycast( float tprev = tcurr; float prevTsdf = truncDist; + int3 volStrides = volStrides4.xyz; + while (tcurr < tmax) { float3 currRayPos = orig + tcurr * dir; @@ -561,7 +559,7 @@ __kernel void raycast( float stepSize = 0.5 * volumeUnitSize; int3 volUnitLocalIdx; - if (row >= 0 && row < lastVolIndex) + if (row >= 0) { //TsdfVoxel currVoxel // VolumeUnitIdxToVolume() @@ -590,11 +588,10 @@ __kernel void raycast( if ( !isnan(tInterp) && !isinf(tInterp) ) { int3 volResolution = (int3) (volResolution4.s0, volResolution4.s1, volResolution4.s2); - int3 volDims = (int3) (volDims4.s0, volDims4.s1, volDims4.s2); - + float3 pv = orig + tInterp * dir; - float3 nv = getNormalVoxel( pv, allVolumePtr, volResolution, volDims, neighbourCoords, - voxelSizeInv, lastVolIndex, hash_table, + float3 nv = getNormalVoxel( pv, allVolumePtr, volResolution, + voxelSizeInv, hash_table, list_size, bufferNums, hash_divisor, volStrides, table_offset); From 3bc21c26c2691332ee93dc0413d7b018436e581f Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 01:59:01 +0300 Subject: [PATCH 154/216] normals, vectorization --- modules/rgbd/src/opencl/hash_tsdf.cl | 29 ++++++++++------------------ 1 file changed, 10 insertions(+), 19 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 06d2eb2675b..0f46d86a6e6 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -367,13 +367,14 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum float3 ptVox = p * voxelSizeInv; int3 iptVox = convert_int3(floor(ptVox)); - bool queried[8]; + // A small hash table to reduce a number of findRow() calls + // -2 and lower means not queried yet + // -1 means not found + // 0+ means found int iterMap[8]; - for (int i = 0; i < 8; i++) { - iterMap[i] = -1; - queried[i] = false; + iterMap[i] = -2; } #if !USE_INTERPOLATION_IN_GETNORMAL @@ -403,24 +404,16 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum // VoxelToVolumeUnitIdx() // TODO: add assertion - if (!(vuRes & (vuRes - 1))) - int3 volumeUnitIdx = (int3) ( - floor ( (float) pt.s0 / volResolution.s0), - floor ( (float) pt.s1 / volResolution.s1), - floor ( (float) pt.s2 / volResolution.s2) ); - + int3 volumeUnitIdx = convert_int3(floor(convert_float3(pt.s012) / convert_float3(volResolution.s012))); - - int dictIdx = (volumeUnitIdx.s0 & 1) - + (volumeUnitIdx.s1 & 1) * 2 - + (volumeUnitIdx.s2 & 1) * 4; + int3 vand = (volumeUnitIdx & 1); + int dictIdx = vand.s0 + vand.s1 * 2 + vand.s2 * 4; int it = iterMap[dictIdx]; - - if (!queried[dictIdx]) + if (it < -1) { it = findRow(hash_table, volumeUnitIdx, list_size, bufferNums, hash_divisor); iterMap[dictIdx] = it; - queried[dictIdx] = true; } struct TsdfVoxel tmp = atVolumeUnit(pt, volumeUnitIdx, it, volResolution.s0, volStrides, allVolumePtr, table_offset); @@ -468,9 +461,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum #endif - float norm = sqrt(normal.x*normal.x + - normal.y*normal.y + - normal.z*normal.z); + float norm = sqrt(dot(normal, normal)); return norm < 0.0001f ? nan((uint)0) : normal / norm; } From b3cbc9c09729311bb6aaf3aef190d655d5e42ddb Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 04:22:20 +0300 Subject: [PATCH 155/216] interpolation in getNormalVoxel() --- modules/rgbd/src/opencl/hash_tsdf.cl | 54 ++++++++++++++-------------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 0f46d86a6e6..f58e51c7122 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -340,17 +340,11 @@ static struct TsdfVoxel atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row return volData[coordBase]; } -inline float interpolate(float tx, float ty, float tz, float vx[8]) +inline float interpolate(float3 t, float8 vz) { - float v00 = vx[0] + tz * (vx[1] - vx[0]); - float v01 = vx[2] + tz * (vx[3] - vx[2]); - float v10 = vx[4] + tz * (vx[5] - vx[4]); - float v11 = vx[6] + tz * (vx[7] - vx[6]); - - float v0 = v00 + ty * (v01 - v00); - float v1 = v10 + ty * (v11 - v10); - - return v0 + tx * (v1 - v0); + float4 vy = mix(vz.s0246, vz.s1357, t.z); + float2 vx = mix(vy.s02, vy.s13, t.y); + return mix(vx.s0, vx.s1, t.x); } inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolumePtr, @@ -421,9 +415,11 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum } #if !USE_INTERPOLATION_IN_GETNORMAL - normal.s0 = vals[0 * 2] - vals[0 * 2 + 1]; - normal.s1 = vals[1 * 2] - vals[1 * 2 + 1]; - normal.s2 = vals[2 * 2] - vals[2 * 2 + 1]; + float3 pv, nv; + + pv = (float3)(vals[0*2 ], vals[1*2 ], vals[2*2 ]); + nv = (float3)(vals[0*2+1], vals[1*2+1], vals[2*2+1]); + normal = pv - nv; #else float cxv[8], cyv[8], czv[8]; @@ -441,24 +437,28 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum const int idxzp[8] = { 24, 0, 25, 2, 26, 4, 27, 6 }; const int idxzn[8] = { 1, 28, 3, 29, 5, 30, 7, 31 }; + float vcxp[8], vcxn[8]; + float vcyp[8], vcyn[8]; + float vczp[8], vczn[8]; + for (int i = 0; i < 8; i++) { - cxv[i] = vals[idxxn[i]] - vals[idxxp[i]]; - cyv[i] = vals[idxyn[i]] - vals[idxyp[i]]; - czv[i] = vals[idxzn[i]] - vals[idxzp[i]]; + vcxp[i] = vals[idxxp[i]]; vcxn[i] = vals[idxxn[i]]; + vcyp[i] = vals[idxyp[i]]; vcyn[i] = vals[idxyn[i]]; + vczp[i] = vals[idxzp[i]]; vczn[i] = vals[idxzn[i]]; } - float tx = ptVox.x - iptVox.x; - float ty = ptVox.y - iptVox.y; - float tz = ptVox.z - iptVox.z; - - normal.s0 = interpolate(tx, ty, tz, cxv); - normal.s1 = interpolate(tx, ty, tz, cyv); - normal.s2 = interpolate(tx, ty, tz, czv); - - //if(!any(isnan(normal))) - // printf("%f, %f, %f" ,normal.s0, normal.s1, normal.s2); - + float8 cxp = vload8(0, vcxp), cxn = vload8(0, vcxn); + float8 cyp = vload8(0, vcyp), cyn = vload8(0, vcyn); + float8 czp = vload8(0, vczp), czn = vload8(0, vczn); + float8 cx = cxn - cxp; + float8 cy = cyn - cyp; + float8 cz = czn - czp; + + float3 tv = ptVox - convert_float3(iptVox); + normal.x = interpolate(tv, cx); + normal.y = interpolate(tv, cy); + normal.z = interpolate(tv, cz); #endif float norm = sqrt(dot(normal, normal)); From f9d9a53a0cbbe3203994433eec35e26e6392a450 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 04:24:36 +0300 Subject: [PATCH 156/216] no 16*16*16, use volumeUnitResolution instead --- modules/rgbd/src/opencl/hash_tsdf.cl | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index f58e51c7122..ca7fb6eb848 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -268,8 +268,9 @@ __kernel void integrateAllVolumeUnits( if (isActive) { + int volCubed = volUnitResolution * volUnitResolution * volUnitResolution; __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) - (allVolumePtr + table_offset + (row) * 16*16*16); + (allVolumePtr + table_offset + (row) * volCubed); __global const float * p_vol2camMatrix = (__global const float *) (allVol2camMatrix + val2cam_offset + (row) * 16); @@ -310,8 +311,9 @@ static struct TsdfVoxel at(int3 volumeIdx, int row, int volumeUnitResolution, return dummy; } + int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) - (allVolumePtr + table_offset + row * 16*16*16); + (allVolumePtr + table_offset + row * volCubed); int3 ismul = volumeIdx * volStrides; int coordBase = ismul.x + ismul.y + ismul.z; return volData[coordBase]; @@ -333,8 +335,9 @@ static struct TsdfVoxel atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row } int3 volUnitLocalIdx = volumeIdx - volumeUnitIdx * volumeUnitResolution; + int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) - (allVolumePtr + table_offset + row * 16*16*16); + (allVolumePtr + table_offset + row * volCubed); int3 ismul = volUnitLocalIdx * volStrides; int coordBase = ismul.x + ismul.y + ismul.z; return volData[coordBase]; From f000fd1ee2eae33a3e4aa8f22a8935c46cb2ef43 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 04:26:57 +0300 Subject: [PATCH 157/216] resolution, stride... --- modules/rgbd/src/hash_tsdf.cpp | 11 +-------- modules/rgbd/src/opencl/hash_tsdf.cl | 35 ++++++++++++++-------------- 2 files changed, 18 insertions(+), 28 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index f9f75b8e3e9..d0cc3f1e1a6 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -354,7 +354,6 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, int indx) co } inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const - { Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), @@ -433,8 +432,6 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum return volData[coordBase]; } - - #if USE_INTRINSICS inline float interpolate(float tx, float ty, float tz, float vx[8]) { @@ -709,7 +706,6 @@ void HashTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), std::numeric_limits::min()); - float tprev = tcurr; float prevTsdf = volume.truncDist; Ptr currVolumeUnit; @@ -1111,7 +1107,6 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept throw std::runtime_error("Failed to create kernel: " + errorStr); float dfac = 1.f / depthFactor; - Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); Mat _tmp; @@ -1126,7 +1121,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), ocl::KernelArg::ReadOnly(isActiveFlags.getUMat(ACCESS_READ)), voxelSize, - volResGpu.val, + volumeUnitResolution, volStrides.val, fxy.val, cxy.val, @@ -1719,7 +1714,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in volumeUnitSize - voxelSize); float tstep = truncDist * raycastStepFactor; - Vec4i volResGpu(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution); const HashTSDFVolumeGPU& volume(*this); const Affine3f cam2vol(volume.pose.inv() * Affine3f(cameraPose)); @@ -1760,9 +1754,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in boxMin.val, boxMax.val, tstep, voxelSize, - volResGpu.val, - volStrides.val, - neighbourCoords.val, voxelSizeInv, volumeUnitSize, volume.truncDist, diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index ca7fb6eb848..81771182371 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -91,7 +91,7 @@ static void integrateVolumeUnit( const float16 vol2camMatrix, const float voxelSize, const int4 volResolution4, - const int4 volDims4, + const int4 volStrides4, const float2 fxy, const float2 cxy, const float dfac, @@ -105,7 +105,7 @@ static void integrateVolumeUnit( return; // coord-independent constants - const int3 volDims = volDims4.xyz; + const int3 volStrides = volStrides4.xyz; const float2 limits = (float2)(depth_cols-1, depth_rows-1); const float4 vol2cam0 = vol2camMatrix.s0123; @@ -125,7 +125,7 @@ static void integrateVolumeUnit( // zStep == vol2cam*(float3(x, y, 1)*voxelSize) - basePt; float3 zStep = ((float3)(vol2cam0.z, vol2cam1.z, vol2cam2.z))*voxelSize; - int volYidx = x*volDims.x + y*volDims.y; + int volYidx = x*volStrides.x + y*volStrides.y; int startZ, endZ; if(fabs(zStep.z) > 1e-5) @@ -215,7 +215,7 @@ static void integrateVolumeUnit( if(sdf >= -truncDist) { float tsdf = fmin(1.0f, sdf * truncDistInv); - int volIdx = volYidx + z*volDims.z; + int volIdx = volYidx + z*volStrides.z; struct TsdfVoxel voxel = volumeptr[volIdx]; float value = tsdfToFloat(voxel.tsdf); @@ -250,8 +250,8 @@ __kernel void integrateAllVolumeUnits( int isActiveFlagsStep, int isActiveFlagsOffset, int isActiveFlagsRows, int isActiveFlagsCols, const float voxelSize, - const int4 volResolution4, - const int4 volDims4, + const int volUnitResolution, + const int4 volStrides4, const float2 fxy, const float2 cxy, const float dfac, @@ -264,6 +264,11 @@ __kernel void integrateAllVolumeUnits( int row = get_global_id(2); int4 idx = volumeUnitIndices[row]; + const int4 volResolution4 = (int4)(volUnitResolution, + volUnitResolution, + volUnitResolution, + volUnitResolution); + int isActive = (__global const uchar*)(isActiveFlagsPtr + isActiveFlagsOffset + (row)); if (isActive) @@ -286,7 +291,7 @@ __kernel void integrateAllVolumeUnits( vol2camMatrix, voxelSize, volResolution4, - volDims4, + volStrides4, fxy, cxy, dfac, @@ -351,7 +356,7 @@ inline float interpolate(float3 t, float8 vz) } inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolumePtr, - int3 volResolution, + int volumeUnitResolution, float voxelSizeInv, __global struct Volume_NODE * hash_table, const int list_size, @@ -359,7 +364,6 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum const int hash_divisor, int3 volStrides, int table_offset) { - float3 normal = (float3) (0.0f, 0.0f, 0.0f); float3 ptVox = p * voxelSizeInv; int3 iptVox = convert_int3(floor(ptVox)); @@ -401,7 +405,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum // VoxelToVolumeUnitIdx() // TODO: add assertion - if (!(vuRes & (vuRes - 1))) - int3 volumeUnitIdx = convert_int3(floor(convert_float3(pt.s012) / convert_float3(volResolution.s012))); + int3 volumeUnitIdx = convert_int3(floor(convert_float3(pt.s012) / (float)(volumeUnitResolution))); int3 vand = (volumeUnitIdx & 1); int dictIdx = vand.s0 + vand.s1 * 2 + vand.s2 * 4; @@ -413,7 +417,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum iterMap[dictIdx] = it; } - struct TsdfVoxel tmp = atVolumeUnit(pt, volumeUnitIdx, it, volResolution.s0, volStrides, allVolumePtr, table_offset); + struct TsdfVoxel tmp = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnitResolution, volStrides, allVolumePtr, table_offset); vals[i] = tsdfToFloat( tmp.tsdf ); } @@ -492,10 +496,7 @@ __kernel void raycast( const float4 boxDown4, const float4 boxUp4, const float tstep, const float voxelSize, - const int4 volResolution4, - const int4 volDims4, - const int8 neighbourCoords, - float voxelSizeInv, + const float voxelSizeInv, float volumeUnitSize, float truncDist, int volumeUnitResolution, @@ -581,10 +582,8 @@ __kernel void raycast( float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if ( !isnan(tInterp) && !isinf(tInterp) ) { - int3 volResolution = (int3) (volResolution4.s0, volResolution4.s1, volResolution4.s2); - float3 pv = orig + tInterp * dir; - float3 nv = getNormalVoxel( pv, allVolumePtr, volResolution, + float3 nv = getNormalVoxel( pv, allVolumePtr, volumeUnitResolution, voxelSizeInv, hash_table, list_size, bufferNums, hash_divisor, volStrides, table_offset); From a7491cd5062dfca93a04fd4fec98af00ddce257c Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 04:27:35 +0300 Subject: [PATCH 158/216] raycast: cur idx, etc. --- modules/rgbd/src/opencl/hash_tsdf.cl | 23 ++++------------------- 1 file changed, 4 insertions(+), 19 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 81771182371..832c8325d84 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -543,11 +543,9 @@ __kernel void raycast( float3 currRayPos = orig + tcurr * dir; // VolumeToVolumeUnitIdx() - int3 currVolumeUnitIdx = (int3) ( - floor (currRayPos.x / volumeUnitSize), - floor (currRayPos.y / volumeUnitSize), - floor (currRayPos.z / volumeUnitSize) ); - + float3 currVolUnitIdxF = floor(currRayPos / volumeUnitSize); + int3 currVolumeUnitIdx = convert_int3(currVolUnitIdxF); + int row = findRow(hash_table, currVolumeUnitIdx, list_size, bufferNums, hash_divisor); float currTsdf = prevTsdf; int currWeight = 0; @@ -556,20 +554,7 @@ __kernel void raycast( if (row >= 0) { - //TsdfVoxel currVoxel - // VolumeUnitIdxToVolume() - float3 currVolUnitPos = (float3) - (( (float) (currVolumeUnitIdx.s0) * volumeUnitSize), - ( (float) (currVolumeUnitIdx.s1) * volumeUnitSize), - ( (float) (currVolumeUnitIdx.s2) * volumeUnitSize) ); - - // VolumeToVoxelCoord() - float3 pos = currRayPos - currVolUnitPos; - volUnitLocalIdx = (int3) - (( floor ( (float) (pos.s0) * voxelSizeInv) ), - ( floor ( (float) (pos.s1) * voxelSizeInv) ), - ( floor ( (float) (pos.s2) * voxelSizeInv) ) ); - + volUnitLocalIdx = convert_int3(currRayPos*voxelSizeInv - currVolUnitIdxF*(float)volumeUnitResolution); struct TsdfVoxel currVoxel = at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); currTsdf = tsdfToFloat(currVoxel.tsdf); From 061793d9d554f8deff9fa32c1f0410a298d21694 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 04:33:50 +0300 Subject: [PATCH 159/216] CPU debugging code removed --- modules/rgbd/src/hash_tsdf.cpp | 231 ++++++++++----------------------- 1 file changed, 66 insertions(+), 165 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d0cc3f1e1a6..0b4b329362d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1591,183 +1591,84 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals) const { + CV_TRACE_FUNCTION(); + CV_Assert(frameSize.area() > 0); - if (false) - { - _points.create(frameSize, POINT_TYPE); - _normals.create(frameSize, POINT_TYPE); - Points points = _points.getMat(); - Normals normals = _normals.getMat(); - Points& new_points(points); - Normals& new_normals(normals); - const HashTSDFVolumeGPU& 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; - auto _HashRaycastInvoker = [&](const Range& range) - { - const Point3f cam2volTrans = cam2vol.translation(); - const Matx33f cam2volRot = cam2vol.rotation(); - const Matx33f vol2camRot = vol2cam.rotation(); - const float blockSize = volume.volumeUnitSize; - for (int y = range.start; y < range.end; y++) - { - ptype* _ptsRow = new_points[y]; - ptype* _nrmRow = new_normals[y]; - for (int x = 0; x < new_points.cols; x++) - { - //! Initialize default value - Point3f point = nan3, normal = nan3; - //! Ray origin and direction in the volume coordinate frame - Point3f orig = cam2volTrans; - Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); - 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()); - float tprev = tcurr; - float prevTsdf = volume.truncDist; - Ptr currVolumeUnit; - while (tcurr < tmax) - { - Point3f currRayPos = orig + tcurr * rayDirV; - cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - - int row = indexes.findRow(currVolumeUnitIdx); - float currTsdf = prevTsdf; - int currWeight = 0; - float stepSize = 0.5f * blockSize; - cv::Vec3i volUnitLocalIdx; - //! The subvolume exists in hashtable - if (row >= 0) - { - cv::Point3f currVolUnitPos = - volume.volumeUnitIdxToVolume(currVolumeUnitIdx); - volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); - //! TODO: Figure out voxel interpolation - TsdfVoxel currVoxel = new_at(volUnitLocalIdx, row); - currTsdf = tsdfToFloat(currVoxel.tsdf); - currWeight = currVoxel.weight; - stepSize = tstep; - } - //! Surface crossing - if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) - { - float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); - if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) - { - Point3f pv = orig + tInterp * rayDirV; - Point3f nv = volume.getNormalVoxel(pv); - if (!isNaN(nv)) - { - normal = vol2camRot * nv; - point = vol2cam * pv; - } - } - break; - } - prevVolumeUnitIdx = currVolumeUnitIdx; - prevTsdf = currTsdf; - tprev = tcurr; - tcurr += stepSize; - } - _ptsRow[x] = toPtype(point); - _nrmRow[x] = toPtype(normal); - } - } - }; - parallel_for_(Range(0, new_points.rows), _HashRaycastInvoker, nstripes); - //_HashRaycastInvoker(Range(0, new_points.rows)); + String errorStr; + String name = "raycast"; + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); - } + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); - if (true) - { - CV_TRACE_FUNCTION(); - CV_Assert(frameSize.area() > 0); + _points.create(frameSize, CV_32FC4); + _normals.create(frameSize, CV_32FC4); - String errorStr; - String name = "raycast"; - ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; - String options = "-cl-mad-enable"; - ocl::Kernel k; - k.create(name.c_str(), source, options, &errorStr); + UMat points = _points.getUMat(); + UMat normals = _normals.getUMat(); - if (k.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); + Intr::Reprojector r = intrinsics.makeReprojector(); + Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); + + Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, + volumeUnitSize - voxelSize, + volumeUnitSize - voxelSize); - _points.create(frameSize, CV_32FC4); - _normals.create(frameSize, CV_32FC4); + float tstep = truncDist * raycastStepFactor; - UMat points = _points.getUMat(); - UMat normals = _normals.getUMat(); + const HashTSDFVolumeGPU& volume(*this); + const Affine3f cam2vol(volume.pose.inv() * Affine3f(cameraPose)); + const Affine3f vol2cam(Affine3f(cameraPose.inv()) * volume.pose); - Intr::Reprojector r = intrinsics.makeReprojector(); - Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); + const Point3f cam2volTrans = cam2vol.translation(); - Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, - volumeUnitSize - voxelSize, - volumeUnitSize - voxelSize); + Vec4f cam2volTransGPU(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0); + Matx44f cam2volRotGPU = cam2vol.matrix; + Matx44f vol2camRotGPU = vol2cam.matrix; - float tstep = truncDist * raycastStepFactor; + Mat _tmp; + volUnitsData.copyTo(_tmp); + UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); + _tmp.release(); - const HashTSDFVolumeGPU& volume(*this); - const Affine3f cam2vol(volume.pose.inv() * Affine3f(cameraPose)); - const Affine3f vol2cam(Affine3f(cameraPose.inv()) * volume.pose); + UMat volPoseGpu, invPoseGpu; + Mat(pose.matrix).copyTo(volPoseGpu); + Mat(pose.inv().matrix).copyTo(invPoseGpu); + + k.args( + ocl::KernelArg::PtrReadWrite(indexes.volumes.getUMat(ACCESS_RW)), + (int)indexes.list_size, + (int)indexes.bufferNums, + (int)indexes.hash_divisor, + (int)lastVolIndex, + ocl::KernelArg::WriteOnlyNoSize(points), + ocl::KernelArg::WriteOnlyNoSize(normals), + frameSize, + ocl::KernelArg::ReadWrite(U_volUnitsData), + cam2volTransGPU, + cam2volRotGPU, + vol2camRotGPU, + float(volume.truncateThreshold), + finv.val, cxy.val, + boxMin.val, boxMax.val, + tstep, + voxelSize, + voxelSizeInv, + volumeUnitSize, + volume.truncDist, + volumeUnitResolution, + volStrides + ); - const Point3f cam2volTrans = cam2vol.translation(); - //const Matx33f cam2volRot = cam2vol.rotation(); - //const Matx33f vol2camRot = vol2cam.rotation(); - - Vec4f cam2volTransGPU(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0); - Matx44f cam2volRotGPU = cam2vol.matrix; - Matx44f vol2camRotGPU = vol2cam.matrix; - - Mat _tmp; - volUnitsData.copyTo(_tmp); - UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); - _tmp.release(); - - UMat volPoseGpu, invPoseGpu; - Mat(pose.matrix).copyTo(volPoseGpu); - Mat(pose.inv().matrix).copyTo(invPoseGpu); - - k.args( - ocl::KernelArg::PtrReadWrite(indexes.volumes.getUMat(ACCESS_RW)), - (int)indexes.list_size, - (int)indexes.bufferNums, - (int)indexes.hash_divisor, - (int)lastVolIndex, - ocl::KernelArg::WriteOnlyNoSize(points), - ocl::KernelArg::WriteOnlyNoSize(normals), - frameSize, - ocl::KernelArg::ReadWrite(U_volUnitsData), - cam2volTransGPU, - cam2volRotGPU, - vol2camRotGPU, - float(volume.truncateThreshold), - finv.val, cxy.val, - boxMin.val, boxMax.val, - tstep, - voxelSize, - voxelSizeInv, - volumeUnitSize, - volume.truncDist, - volumeUnitResolution, - volStrides - ); - - size_t globalSize[2]; - globalSize[0] = (size_t)frameSize.width; - globalSize[1] = (size_t)frameSize.height; - - if (!k.run(2, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); - } + size_t globalSize[2]; + globalSize[0] = (size_t)frameSize.width; + globalSize[1] = (size_t)frameSize.height; + + if (!k.run(2, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); } void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const From b2845972f293d607023e1190a1bb8415e9ea6b38 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 05:19:37 +0300 Subject: [PATCH 160/216] volUnitsData moved to OpenCL memory --- modules/rgbd/src/hash_tsdf.cpp | 57 +++++++++++++++++++++------------- 1 file changed, 35 insertions(+), 22 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 0b4b329362d..d00873f67dc 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -919,12 +919,15 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; + void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; size_t getTotalVolumeUnits() const override { return size_t(lastVolIndex); } int getVisibleBlocks(int currFrameId, int frameThreshold) const override; + + //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) virtual TsdfVoxel new_at(const cv::Vec3i& volumeIdx, int indx) const; @@ -955,7 +958,9 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::Mat lastVisibleIndexes; cv::Mat isActiveFlags; cv::Mat allVol2cam; - cv::Mat volUnitsData; + //TODO: make it UMat + cv::UMat volUnitsData; + cv::Mat volUnitsDataCopy; cv::UMat pixNorms; int lastVolIndex; @@ -1009,7 +1014,10 @@ void HashTSDFVolumeGPU::reset() degree = 15; buff_lvl = (int) pow(2, degree); - volUnitsData = cv::Mat(buff_lvl, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; + volUnitsDataCopy = cv::Mat(buff_lvl, volCubed, rawType()); + volUnitsData = cv::UMat(buff_lvl, volCubed, CV_8UC2); + poses = cv::Mat(buff_lvl, 1, rawType()); lastVisibleIndexes = cv::Mat(buff_lvl, 1, CV_32S); isActiveFlags = cv::Mat(buff_lvl, 1, CV_8U); @@ -1052,7 +1060,7 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f { Mat x(1, depth_cols, CV_32FC1); Mat y(1, depth_rows, CV_32FC1); - Mat _pixNorm(1, depth_rows * depth_cols, CV_32F); + UMat pixNorm(1, depth_rows * depth_cols, CV_32F); for (int i = 0; i < depth_cols; i++) *x.ptr(0, i) = (i - cxy[0]) / fxy[0]; @@ -1070,11 +1078,10 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f throw std::runtime_error("Failed to create kernel: " + errorStr); AccessFlag af = ACCESS_READ; - UMat pixNorm = _pixNorm.getUMat(af); UMat xx = x.getUMat(af); UMat yy = y.getUMat(af); - kk.args(ocl::KernelArg::PtrReadWrite(pixNorm), + kk.args(ocl::KernelArg::PtrWriteOnly(pixNorm), ocl::KernelArg::PtrReadOnly(xx), ocl::KernelArg::PtrReadOnly(yy), depth_cols); @@ -1109,14 +1116,9 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept float dfac = 1.f / depthFactor; Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - Mat _tmp; - volUnitsData.copyTo(_tmp); - UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); - _tmp.release(); - k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::ReadOnly(volumeUnitIndices.getUMat(ACCESS_READ)), - ocl::KernelArg::ReadWrite(U_volUnitsData), + ocl::KernelArg::ReadWrite(volUnitsData), ocl::KernelArg::PtrReadOnly(pixNorms), ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), ocl::KernelArg::ReadOnly(isActiveFlags.getUMat(ACCESS_READ)), @@ -1138,9 +1140,6 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); - - U_volUnitsData.getMat(ACCESS_RW).copyTo(volUnitsData); - U_volUnitsData.release(); } @@ -1232,6 +1231,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Depth depth = _depth.getMat(); // Save pointers to avoid searches when allocating + //TODO: return range of rows instead std::vector nodePtrs = allocateVolumeUnits(depth, depthFactor, cameraPose, intrinsics); //! Perform the allocation @@ -1241,7 +1241,13 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { degree = (int)(log2(lastVolIndex) + 1); // clz() would be better buff_lvl = (int)pow(2, degree); - volUnitsData.resize(buff_lvl); + + volUnitsDataCopy.resize(buff_lvl); + int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; + UMat newData(buff_lvl, volCubed, CV_8UC2); + volUnitsData.copyTo(newData(Range(0, volUnitsData.rows), Range::all())); + volUnitsData = newData; + poses.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); volumeUnitIndices.resize(buff_lvl); @@ -1265,11 +1271,11 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma *isActiveFlags.ptr(row, 0) = 1; *volumeUnitIndices.ptr(row, 0) = idx4; - volUnitsData.row(row).forEach([](VecTsdfVoxel& vv, const int*) - { - TsdfVoxel& v = reinterpret_cast(vv); - v.tsdf = floatToTsdf(0.0f); v.weight = 0; - }); + //TODO: init a whole row range instead + TsdfVoxel emptyVoxel; + emptyVoxel.tsdf = floatToTsdf(0.0f); + emptyVoxel.weight = 0; + volUnitsData.row(row) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight)); } //! Mark volumes in the camera frustum as active @@ -1311,6 +1317,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); } + //TODO: maybe to merge this into integrateAllVolumeUnits //! Integrate the correct volumeUnits auto preCalculateAllVol3Cam = [&](const Range& range) { for (int row = range.start; row < range.end; row++) @@ -1374,7 +1381,7 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, int indx) return dummy; } - const TsdfVoxel* volData = volUnitsData.ptr(indx); + const TsdfVoxel* volData = volUnitsDataCopy.ptr(indx); int coordBase = volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + @@ -1394,7 +1401,7 @@ TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& v Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; // expanding at(), removing bounds check - const TsdfVoxel* volData = volUnitsData.ptr(indx); + const TsdfVoxel* volData = volUnitsDataCopy.ptr(indx); int coordBase = volUnitLocalIdx[0] * volStrides[0] + volUnitLocalIdx[1] * volStrides[1] + volUnitLocalIdx[2] * volStrides[2]; @@ -1675,6 +1682,9 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { CV_TRACE_FUNCTION(); + //TODO: remove it when it works w/o CPU code + volUnitsData.copyTo(volUnitsDataCopy); + if (_points.needed()) { std::vector> pVecs, nVecs; @@ -1751,6 +1761,9 @@ void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) c { CV_TRACE_FUNCTION(); + //TODO: remove it when it works w/o CPU code + volUnitsData.copyTo(volUnitsDataCopy); + if (_normals.needed()) { Points points = _points.getMat(); From 2b5efd0df7c0a4d82b923659cd559834913cb8a1 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 05:37:01 +0300 Subject: [PATCH 161/216] raycast: no copy --- modules/rgbd/src/hash_tsdf.cpp | 9 ++------- modules/rgbd/src/opencl/hash_tsdf.cl | 4 ++-- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d00873f67dc..301ad1ff80f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1636,17 +1636,12 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Matx44f cam2volRotGPU = cam2vol.matrix; Matx44f vol2camRotGPU = vol2cam.matrix; - Mat _tmp; - volUnitsData.copyTo(_tmp); - UMat U_volUnitsData = _tmp.getUMat(ACCESS_RW); - _tmp.release(); - UMat volPoseGpu, invPoseGpu; Mat(pose.matrix).copyTo(volPoseGpu); Mat(pose.inv().matrix).copyTo(invPoseGpu); k.args( - ocl::KernelArg::PtrReadWrite(indexes.volumes.getUMat(ACCESS_RW)), + ocl::KernelArg::PtrReadOnly(indexes.volumes.getUMat(ACCESS_RW)), (int)indexes.list_size, (int)indexes.bufferNums, (int)indexes.hash_divisor, @@ -1654,7 +1649,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in ocl::KernelArg::WriteOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), frameSize, - ocl::KernelArg::ReadWrite(U_volUnitsData), + ocl::KernelArg::ReadOnly(volUnitsData), cam2volTransGPU, cam2volRotGPU, vol2camRotGPU, diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 832c8325d84..0a22e86be52 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -475,7 +475,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum typedef float4 ptype; __kernel void raycast( - __global struct Volume_NODE * hash_table, + __global const struct Volume_NODE * hash_table, const int list_size, const int bufferNums, const int hash_divisor, @@ -485,7 +485,7 @@ __kernel void raycast( __global char * normalsptr, int normals_step, int normals_offset, const int2 frameSize, - __global struct TsdfVoxel * allVolumePtr, + __global const struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, int table_rows, int table_cols, float4 cam2volTransGPU, From 7e0df4e5de90da2fa0c7b11e27cc61ff776010e7 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 27 Jan 2021 05:38:49 +0300 Subject: [PATCH 162/216] allocate accelerated a bit --- modules/rgbd/src/hash_tsdf.cpp | 77 +++++++++++++++++++++++----------- 1 file changed, 53 insertions(+), 24 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 301ad1ff80f..a341f03a13e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -964,6 +964,8 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::UMat pixNorms; int lastVolIndex; + + //TODO: move indexes.volumes to GPU VolumesTable indexes; Vec8i neighbourCoords; }; @@ -1157,13 +1159,14 @@ std::vector HashTSDFVolumeGPU::allocateVolumeUnits(Depth depth, fl const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Point3f truncPt(truncDist, truncDist, truncDist); Mutex mutex; - Range allocateRange(0, depth.rows); + + // ----------------------- - auto fillLocalAcessVolUnits = [&](const Range& range, _VolumeUnitIndexSet& _localAccessVolUnits, int& loc_vol_idx) { - for (int y = range.start; y < range.end; y += depthStride) + auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, _VolumeUnitIndexSet& _localAccessVolUnits, int& loc_vol_idx) { + for (int y = yrange.start; y < yrange.end; y += depthStride) { const depthType* depthRow = depth[y]; - for (int x = 0; x < depth.cols; x += depthStride) + for (int x = xrange.start; x < xrange.end; x += depthStride) { depthType z = depthRow[x] * invDepthFactor; if (z <= 0 || z > this->truncateThreshold) @@ -1180,14 +1183,20 @@ std::vector HashTSDFVolumeGPU::allocateVolumeUnits(Depth depth, fl { const Vec3i tsdf_idx = Vec3i(i, j, k); - if (!find(_localAccessVolUnits, tsdf_idx, loc_vol_idx)) + if (indexes.findRow(tsdf_idx) < 0) { - *_localAccessVolUnits.ptr(loc_vol_idx) = tsdf_idx; - loc_vol_idx++; - - if (loc_vol_idx >= localCapacity) + if (!find(_localAccessVolUnits, tsdf_idx, loc_vol_idx)) { - return; + *_localAccessVolUnits.ptr(loc_vol_idx) = tsdf_idx; + loc_vol_idx++; + + if (loc_vol_idx >= localCapacity) + { + //DEBUG + std::cout << "allocate: local capacity exhausted" << std::endl; + + return; + } } } } @@ -1195,29 +1204,49 @@ std::vector HashTSDFVolumeGPU::allocateVolumeUnits(Depth depth, fl } }; - auto AllocateVolumeUnitsInvoker = [&](const Range& range) { - _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(localCapacity, 1, CV_32SC3); - int loc_vol_idx = 0; + Rect dim(0, 0, depth.cols, depth.rows); + Size gsz(64, 64); + Size gg(divUp(dim.width, gsz.width), divUp(dim.height, gsz.height)); - fillLocalAcessVolUnits(range, _localAccessVolUnits, loc_vol_idx); + auto allocateLambda = [&](const Range& r) { - mutex.lock(); - for (int i = 0; i < loc_vol_idx; i++) + for (int yg = r.start; yg < r.end; yg++) + { + for (int xg = 0; xg < gg.width; xg++) { - Vec3i idx = *_localAccessVolUnits.ptr(i); + Rect gr(xg * gsz.width, yg * gsz.height, (xg + 1) * gsz.width, (yg + 1) * gsz.height); + gr = gr & dim; + Range xr(gr.tl().x, gr.br().x), yr(gr.tl().y, gr.br().y); + + _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(localCapacity, 1, CV_32SC3); + int loc_vol_idx = 0; + + fillLocalAcessVolUnits(xr, yr, _localAccessVolUnits, loc_vol_idx); - // if not found - if (indexes.findRow(idx) < 0) + mutex.lock(); + for (int i = 0; i < loc_vol_idx; i++) { - Volume_NODE* node = indexes.insert(idx, lastVolIndex); - nodePtrs.push_back(node); - lastVolIndex++; + Vec3i idx = *_localAccessVolUnits.ptr(i); + + // if not found + if (indexes.findRow(idx) < 0) + { + Volume_NODE* node = indexes.insert(idx, lastVolIndex); + nodePtrs.push_back(node); + lastVolIndex++; + } } + mutex.unlock(); } - mutex.unlock(); + } + }; - parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); + parallel_for_(Range(0, gg.height), allocateLambda); + //allocateLambda(Range(0, gg.height)); + + + // --------------------- return nodePtrs; } From 4cd30eff91c5093aeef70c81422dc3fbfb5f0c32 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 28 Jan 2021 04:14:23 +0300 Subject: [PATCH 163/216] hash_tsdf.cl: pointers fixed --- modules/rgbd/src/opencl/hash_tsdf.cl | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 0a22e86be52..5ac891cc76a 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -62,7 +62,7 @@ static uint calc_hash(int3 x) } -static int findRow(__global struct Volume_NODE * hash_table, int3 indx, +static int findRow(__global const struct Volume_NODE * hash_table, int3 indx, int list_size, int bufferNums, int hash_divisor) { int bufferNum = 0; @@ -269,7 +269,7 @@ __kernel void integrateAllVolumeUnits( volUnitResolution, volUnitResolution); - int isActive = (__global const uchar*)(isActiveFlagsPtr + isActiveFlagsOffset + (row)); + int isActive = *(__global const uchar*)(isActiveFlagsPtr + isActiveFlagsOffset + row); if (isActive) { @@ -301,8 +301,9 @@ __kernel void integrateAllVolumeUnits( } } + static struct TsdfVoxel at(int3 volumeIdx, int row, int volumeUnitResolution, - int3 volStrides, __global struct TsdfVoxel * allVolumePtr, int table_offset) + int3 volStrides, __global const struct TsdfVoxel * allVolumePtr, int table_offset) { //! Out of bounds From 6ed14d77fbf32726e6ee325ff5a06861ffaeea64 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 28 Jan 2021 04:18:32 +0300 Subject: [PATCH 164/216] allVol2cam, poses moved to kernel; rowRange for volUnitsData init --- modules/rgbd/src/hash_tsdf.cpp | 60 ++++++++-------------------- modules/rgbd/src/opencl/hash_tsdf.cl | 34 +++++++++++----- 2 files changed, 41 insertions(+), 53 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a341f03a13e..7f22573bdf3 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -909,8 +909,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void reset() override; - //void integrateVolumeUnitGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, VolumeIndex idx); - void integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics); + void integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); std::vector allocateVolumeUnits(Depth depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); @@ -951,15 +950,14 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Vec6f frameParams; int degree; int buff_lvl; - + // per-volume-unit data cv::Mat volumeUnitIndices; - cv::Mat poses; cv::Mat lastVisibleIndexes; cv::Mat isActiveFlags; - cv::Mat allVol2cam; - //TODO: make it UMat + cv::UMat volUnitsData; + //TODO: remove it when there's no CPU parts cv::Mat volUnitsDataCopy; cv::UMat pixNorms; @@ -1007,6 +1005,7 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstM : HashTSDFVolume(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) { + //TODO: move reset() contents here, reset() just clears the data } // zero volume, leave rest params the same void HashTSDFVolumeGPU::reset() @@ -1020,10 +1019,8 @@ void HashTSDFVolumeGPU::reset() volUnitsDataCopy = cv::Mat(buff_lvl, volCubed, rawType()); volUnitsData = cv::UMat(buff_lvl, volCubed, CV_8UC2); - poses = cv::Mat(buff_lvl, 1, rawType()); lastVisibleIndexes = cv::Mat(buff_lvl, 1, CV_32S); isActiveFlags = cv::Mat(buff_lvl, 1, CV_8U); - allVol2cam = cv::Mat(buff_lvl, 16, CV_32F); volumeUnitIndices = cv::Mat(buff_lvl, 1, CV_32SC4); indexes = VolumesTable(); @@ -1098,7 +1095,8 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f return pixNorm; } -void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Intr& intrinsics) + +void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { CV_TRACE_FUNCTION(); CV_Assert(!_depth.empty()); @@ -1117,13 +1115,16 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept float dfac = 1.f / depthFactor; Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); + Matx44f vol2camMatrix = (Affine3f(cameraPose).inv() * pose).matrix; + Matx44f camInvMatrix = Affine3f(cameraPose).inv().matrix; k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::ReadOnly(volumeUnitIndices.getUMat(ACCESS_READ)), ocl::KernelArg::ReadWrite(volUnitsData), ocl::KernelArg::PtrReadOnly(pixNorms), - ocl::KernelArg::ReadOnly(allVol2cam.getUMat(ACCESS_READ)), ocl::KernelArg::ReadOnly(isActiveFlags.getUMat(ACCESS_READ)), + vol2camMatrix, + camInvMatrix, voxelSize, volumeUnitResolution, volStrides.val, @@ -1261,6 +1262,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma // Save pointers to avoid searches when allocating //TODO: return range of rows instead + int oldLastVolIndex = lastVolIndex; std::vector nodePtrs = allocateVolumeUnits(depth, depthFactor, cameraPose, intrinsics); //! Perform the allocation @@ -1277,11 +1279,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma volUnitsData.copyTo(newData(Range(0, volUnitsData.rows), Range::all())); volUnitsData = newData; - poses.resize(buff_lvl); lastVisibleIndexes.resize(buff_lvl); volumeUnitIndices.resize(buff_lvl); isActiveFlags.resize(buff_lvl); - allVol2cam.resize(buff_lvl); } // Place data for new volume units @@ -1293,18 +1293,16 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Vec4i idx4 = node->idx; Vec3i tsdf_idx(idx4[0], idx4[1], idx4[2]); - Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(tsdf_idx)).matrix; - - *poses.ptr(row, 0) = subvolumePose; *lastVisibleIndexes.ptr(row, 0) = frameId; *isActiveFlags.ptr(row, 0) = 1; *volumeUnitIndices.ptr(row, 0) = idx4; - - //TODO: init a whole row range instead + } + if (oldLastVolIndex > lastVolIndex) + { TsdfVoxel emptyVoxel; emptyVoxel.tsdf = floatToTsdf(0.0f); emptyVoxel.weight = 0; - volUnitsData.row(row) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight)); + volUnitsData.rowRange(oldLastVolIndex, lastVolIndex) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight)); } //! Mark volumes in the camera frustum as active @@ -1346,32 +1344,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); } - //TODO: maybe to merge this into integrateAllVolumeUnits - //! Integrate the correct volumeUnits - auto preCalculateAllVol3Cam = [&](const Range& range) { - for (int row = range.start; row < range.end; row++) - { - Vec4i idx4 = *volumeUnitIndices.ptr(row, 0); - Vec3i idx(idx4[0], idx4[1], idx4[2]); - - bool _isActive = bool(*isActiveFlags.ptr(row, 0)); - - if (_isActive) - { - Matx44f _pose = *poses.ptr(row, 0); - const cv::Affine3f vol2cam(Affine3f(cameraPose.inv())* Affine3f(_pose)); - auto vol2camMatrix = vol2cam.matrix.val; - for (int k = 0; k < 16; k++) - { - *allVol2cam.ptr(row, k) = vol2camMatrix[k]; - } - } - } - }; - parallel_for_(Range(0, lastVolIndex), preCalculateAllVol3Cam ); - //! Integrate the correct volumeUnits - integrateAllVolumeUnitsGPU(depth, depthFactor, intrinsics); + integrateAllVolumeUnitsGPU(depth, depthFactor, cameraPose, intrinsics); } cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 5ac891cc76a..6d23be8c1af 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -232,23 +232,30 @@ static void integrateVolumeUnit( } + __kernel void integrateAllVolumeUnits( + // depth __global const char * depthptr, int depth_step, int depth_offset, int depth_rows, int depth_cols, + // volumeUnitIndices __global const int4 * volumeUnitIndices, int volumeUnitIndices_step, int volumeUnitIndices_offset, int volumeUnitIndices_rows, int volumeUnitIndices_cols, + // volUnitsData __global struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, int table_rows, int table_cols, + // Ptr(pixNorms) __global const float * pixNorms, - __global const float * allVol2camMatrix, - int val2cam_step, int val2cam_offset, - int val2cam_rows, int val2cam_cols, + // isActiveFlags __global const uchar* isActiveFlagsPtr, int isActiveFlagsStep, int isActiveFlagsOffset, int isActiveFlagsRows, int isActiveFlagsCols, + // cam matrices: + const float16 vol2cam, + const float16 camInv, + // scalars: const float voxelSize, const int volUnitResolution, const int4 volStrides4, @@ -262,7 +269,7 @@ __kernel void integrateAllVolumeUnits( int i = get_global_id(0); int j = get_global_id(1); int row = get_global_id(2); - int4 idx = volumeUnitIndices[row]; + int3 idx = volumeUnitIndices[row].xyz; const int4 volResolution4 = (int4)(volUnitResolution, volUnitResolution, @@ -275,11 +282,18 @@ __kernel void integrateAllVolumeUnits( { int volCubed = volUnitResolution * volUnitResolution * volUnitResolution; __global struct TsdfVoxel * volumeptr = (__global struct TsdfVoxel*) - (allVolumePtr + table_offset + (row) * volCubed); - __global const float * p_vol2camMatrix = (__global const float *) - (allVol2camMatrix + val2cam_offset + (row) * 16); - - const float16 vol2camMatrix = vload16(0, p_vol2camMatrix); + (allVolumePtr + table_offset + row * volCubed); + + // volUnit2cam = world2cam * volUnit2world = + // camPoseInv * volUnitPose = camPoseInv * (volPose + idx * volUnitSize) = + // camPoseInv * (volPose + idx * volUnitResolution * voxelSize) = + // camPoseInv * (volPose + mulIdx) = camPoseInv * volPose + camPoseInv * mulIdx = + // vol2cam + camPoseInv * mulIdx + float3 mulIdx = convert_float3(idx * volUnitResolution) * voxelSize; + float16 volUnit2cam = vol2cam; + volUnit2cam.s37b += (float3)(dot(mulIdx, camInv.s012), + dot(mulIdx, camInv.s456), + dot(mulIdx, camInv.s89a)); integrateVolumeUnit( i, j, @@ -288,7 +302,7 @@ __kernel void integrateAllVolumeUnits( depth_rows, depth_cols, volumeptr, pixNorms, - vol2camMatrix, + volUnit2cam, voxelSize, volResolution4, volStrides4, From 4deeacfcaae69b6905d4830dbccc776809034a7d Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 28 Jan 2021 04:32:17 +0300 Subject: [PATCH 165/216] TSDF voxel: constructor; row range fixed --- modules/rgbd/src/hash_tsdf.cpp | 37 ++++++++-------------------------- modules/rgbd/src/tsdf.cpp | 5 +---- modules/rgbd/src/tsdf.hpp | 3 +++ 3 files changed, 12 insertions(+), 33 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 7f22573bdf3..a5a071faf1e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -341,10 +341,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, int indx) co (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.0f); - dummy.weight = 0; - return dummy; + return TsdfVoxel(floatToTsdf(1.f), 0); } const TsdfVoxel* volData = volUnitsData.ptr(indx); @@ -363,10 +360,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const if (it == volumeUnits.end()) { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.f); - dummy.weight = 0; - return dummy; + return TsdfVoxel(floatToTsdf(1.f), 0); } cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, @@ -386,10 +380,7 @@ TsdfVoxel HashTSDFVolumeCPU::at(const Point3f& point) const if (it == volumeUnits.end()) { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.f); - dummy.weight = 0; - return dummy; + return TsdfVoxel(floatToTsdf(1.f), 0); } cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); @@ -419,10 +410,7 @@ TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volum { if (it == volumeUnits.end()) { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.f); - dummy.weight = 0; - return dummy; + return TsdfVoxel(floatToTsdf(1.f), 0); } Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; @@ -714,7 +702,6 @@ void HashTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Point3f currRayPos = orig + tcurr * rayDirV; cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); float currTsdf = prevTsdf; @@ -1297,11 +1284,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma *isActiveFlags.ptr(row, 0) = 1; *volumeUnitIndices.ptr(row, 0) = idx4; } - if (oldLastVolIndex > lastVolIndex) + if (oldLastVolIndex < lastVolIndex) { - TsdfVoxel emptyVoxel; - emptyVoxel.tsdf = floatToTsdf(0.0f); - emptyVoxel.weight = 0; + TsdfVoxel emptyVoxel(floatToTsdf(0.0f), 0); volUnitsData.rowRange(oldLastVolIndex, lastVolIndex) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight)); } @@ -1378,10 +1363,7 @@ inline TsdfVoxel HashTSDFVolumeGPU::new_at(const cv::Vec3i& volumeIdx, int indx) (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.0f); - dummy.weight = 0; - return dummy; + return TsdfVoxel(floatToTsdf(1.0f), 0); } const TsdfVoxel* volData = volUnitsDataCopy.ptr(indx); @@ -1396,10 +1378,7 @@ TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& v { if (indx < 0) { - TsdfVoxel dummy; - dummy.tsdf = floatToTsdf(1.f); - dummy.weight = 0; - return dummy; + return TsdfVoxel(floatToTsdf(1.f), 0); } Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 1e8704170f4..c689bd7997b 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -102,10 +102,7 @@ TsdfVoxel TSDFVolumeCPU::at(const Vec3i& volumeIdx) const (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; + return TsdfVoxel(floatToTsdf(1.f), 0); } const TsdfVoxel* volData = volume.ptr(); diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index e429c31fc68..67361aa59d9 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -23,6 +23,9 @@ typedef uchar WeightType; struct TsdfVoxel { + TsdfVoxel(TsdfType _tsdf, WeightType _weight) : + tsdf(_tsdf), weight(_weight) + { } TsdfType tsdf; WeightType weight; }; From bb1b64ad0e4f5e118520ea636e9fc63adc0181cf Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 28 Jan 2021 14:51:58 +0300 Subject: [PATCH 166/216] rowRange --- modules/rgbd/src/hash_tsdf.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index a5a071faf1e..ccaf66b77b7 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1248,7 +1248,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Depth depth = _depth.getMat(); // Save pointers to avoid searches when allocating - //TODO: return range of rows instead int oldLastVolIndex = lastVolIndex; std::vector nodePtrs = allocateVolumeUnits(depth, depthFactor, cameraPose, intrinsics); @@ -1280,14 +1279,22 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Vec4i idx4 = node->idx; Vec3i tsdf_idx(idx4[0], idx4[1], idx4[2]); + *lastVisibleIndexes.ptr(row, 0) = frameId; *isActiveFlags.ptr(row, 0) = 1; + + //TODO: replace .ptr<...> everywhere by .at<...> *volumeUnitIndices.ptr(row, 0) = idx4; } if (oldLastVolIndex < lastVolIndex) { + Range r(oldLastVolIndex, lastVolIndex); + + //lastVisibleIndexes.rowRange(r) = frameId; + //isActiveFlags.rowRange(r) = 1; + TsdfVoxel emptyVoxel(floatToTsdf(0.0f), 0); - volUnitsData.rowRange(oldLastVolIndex, lastVolIndex) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight)); + volUnitsData.rowRange(r) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight)); } //! Mark volumes in the camera frustum as active From 488a66a0ea745166d58f26b3ee21973011ec876d Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 28 Jan 2021 15:43:44 +0300 Subject: [PATCH 167/216] pixNorms fix + minors --- modules/rgbd/src/hash_tsdf.cpp | 9 ++++--- modules/rgbd/src/opencl/hash_tsdf.cl | 35 +++++++++++++++++----------- 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ccaf66b77b7..e1294ff8fe5 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1046,7 +1046,7 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f { Mat x(1, depth_cols, CV_32FC1); Mat y(1, depth_rows, CV_32FC1); - UMat pixNorm(1, depth_rows * depth_cols, CV_32F); + UMat pixNorm(depth_rows, depth_cols, CV_32F); for (int i = 0; i < depth_cols; i++) *x.ptr(0, i) = (i - cxy[0]) / fxy[0]; @@ -1067,10 +1067,9 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f UMat xx = x.getUMat(af); UMat yy = y.getUMat(af); - kk.args(ocl::KernelArg::PtrWriteOnly(pixNorm), + kk.args(ocl::KernelArg::WriteOnly(pixNorm), ocl::KernelArg::PtrReadOnly(xx), - ocl::KernelArg::PtrReadOnly(yy), - depth_cols); + ocl::KernelArg::PtrReadOnly(yy)); size_t globalSize[2]; globalSize[0] = depth_rows; @@ -1108,7 +1107,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::ReadOnly(volumeUnitIndices.getUMat(ACCESS_READ)), ocl::KernelArg::ReadWrite(volUnitsData), - ocl::KernelArg::PtrReadOnly(pixNorms), + ocl::KernelArg::ReadOnly(pixNorms), ocl::KernelArg::ReadOnly(isActiveFlags.getUMat(ACCESS_READ)), vol2camMatrix, camInvMatrix, diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 6d23be8c1af..579f4eb3e82 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -40,15 +40,18 @@ static inline float tsdfToFloat(TsdfType num) return ( (float) num ) / (-128); } -__kernel void preCalculationPixNorm (__global float * pixNorms, +__kernel void preCalculationPixNorm (__global char * pixNormsPtr, + int pixNormsStep, int pixNormsOffset, + int pixNormsRows, int pixNormsCols, const __global float * xx, - const __global float * yy, - int width) + const __global float * yy) { int i = get_global_id(0); int j = get_global_id(1); - int idx = i*width + j; - pixNorms[idx] = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); + if (i < pixNormsRows && j < pixNormsCols) + { + *(__global float*)(pixNormsPtr + pixNormsOffset + i*pixNormsStep + j*sizeof(float)) = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); + } } static uint calc_hash(int3 x) @@ -87,7 +90,9 @@ static void integrateVolumeUnit( int depth_step, int depth_offset, int depth_rows, int depth_cols, __global struct TsdfVoxel * volumeptr, - const __global float * pixNorms, + const __global char * pixNormsPtr, + int pixNormsStep, int pixNormsOffset, + int pixNormsRows, int pixNormsCols, const float16 vol2camMatrix, const float voxelSize, const int4 volResolution4, @@ -128,7 +133,7 @@ static void integrateVolumeUnit( int volYidx = x*volStrides.x + y*volStrides.y; int startZ, endZ; - if(fabs(zStep.z) > 1e-5) + if(fabs(zStep.z) > 1e-5f) { int baseZ = convert_int(-basePt.z / zStep.z); if(zStep.z > 0) @@ -204,8 +209,8 @@ static void integrateVolumeUnit( if(v == 0) continue; - int idx = projected.y * depth_rows + projected.x; - float pixNorm = pixNorms[idx]; + int2 projInt = convert_int2(projected); + float pixNorm = *(__global const float*)(pixNormsPtr + pixNormsOffset + projInt.y*pixNormsStep + projInt.x*sizeof(float)); //float pixNorm = length(camPixVec); // difference between distances of point and of surface to camera @@ -246,8 +251,10 @@ __kernel void integrateAllVolumeUnits( __global struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, int table_rows, int table_cols, - // Ptr(pixNorms) - __global const float * pixNorms, + // pixNorms + const __global char * pixNormsPtr, + int pixNormsStep, int pixNormsOffset, + int pixNormsRows, int pixNormsCols, // isActiveFlags __global const uchar* isActiveFlagsPtr, int isActiveFlagsStep, int isActiveFlagsOffset, @@ -301,7 +308,9 @@ __kernel void integrateAllVolumeUnits( depth_step, depth_offset, depth_rows, depth_cols, volumeptr, - pixNorms, + pixNormsPtr, + pixNormsStep, pixNormsOffset, + pixNormsRows, pixNormsCols, volUnit2cam, voxelSize, volResolution4, @@ -373,7 +382,7 @@ inline float interpolate(float3 t, float8 vz) inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolumePtr, int volumeUnitResolution, float voxelSizeInv, - __global struct Volume_NODE * hash_table, + const __global struct Volume_NODE * hash_table, const int list_size, const int bufferNums, const int hash_divisor, From ce76aef028e1b64afb1653f0023cf201801d1683 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 28 Jan 2021 23:37:00 +0300 Subject: [PATCH 168/216] depth is now UMat --- modules/rgbd/src/hash_tsdf.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index e1294ff8fe5..2f98fccb7a6 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -896,9 +896,9 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void reset() override; - void integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); + void integrateAllVolumeUnitsGPU(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); - std::vector allocateVolumeUnits(Depth depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); + std::vector allocateVolumeUnits(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; @@ -1082,12 +1082,10 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f } -void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) +void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { CV_TRACE_FUNCTION(); - CV_Assert(!_depth.empty()); - - UMat depth = _depth.getUMat(); + CV_Assert(!depth.empty()); String errorStr; String name = "integrateAllVolumeUnits"; @@ -1132,13 +1130,15 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(InputArray _depth, float dept } -std::vector HashTSDFVolumeGPU::allocateVolumeUnits(Depth depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) +std::vector HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { const int newIndicesCapacity = VOLUMES_SIZE; const int localCapacity = VOLUMES_SIZE; std::vector nodePtrs; nodePtrs.reserve(newIndicesCapacity); + Depth depth = _depth.getMat(ACCESS_READ); + //! Compute volumes to be allocated const int depthStride = int(log2(volumeUnitResolution)); const float invDepthFactor = 1.f / depthFactor; @@ -1244,7 +1244,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); - Depth depth = _depth.getMat(); + UMat depth = _depth.getUMat(); // Save pointers to avoid searches when allocating int oldLastVolIndex = lastVolIndex; From 0ace4c2e2d48b57689977d1fcc044b1fd6f62adf Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 28 Jan 2021 23:41:29 +0300 Subject: [PATCH 169/216] markActive is done on GPU, lastVisibleIndices and isActiveFlags moved to GPU --- modules/rgbd/src/hash_tsdf.cpp | 111 +++++++++++++++++---------- modules/rgbd/src/opencl/hash_tsdf.cl | 58 +++++++++++++- 2 files changed, 126 insertions(+), 43 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 2f98fccb7a6..8c7ba709233 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -900,6 +900,8 @@ class HashTSDFVolumeGPU : public HashTSDFVolume std::vector allocateVolumeUnits(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); + void markActive(const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId); + void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, @@ -940,8 +942,10 @@ class HashTSDFVolumeGPU : public HashTSDFVolume // per-volume-unit data cv::Mat volumeUnitIndices; - cv::Mat lastVisibleIndexes; - cv::Mat isActiveFlags; + + cv::UMat lastVisibleIndices; + + cv::UMat isActiveFlags; cv::UMat volUnitsData; //TODO: remove it when there's no CPU parts @@ -952,6 +956,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume //TODO: move indexes.volumes to GPU VolumesTable indexes; + Vec8i neighbourCoords; }; @@ -1004,10 +1009,13 @@ void HashTSDFVolumeGPU::reset() int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; volUnitsDataCopy = cv::Mat(buff_lvl, volCubed, rawType()); + volUnitsData = cv::UMat(buff_lvl, volCubed, CV_8UC2); - lastVisibleIndexes = cv::Mat(buff_lvl, 1, CV_32S); - isActiveFlags = cv::Mat(buff_lvl, 1, CV_8U); + lastVisibleIndices = cv::UMat(buff_lvl, 1, CV_32S); + + isActiveFlags = cv::UMat(buff_lvl, 1, CV_8U); + volumeUnitIndices = cv::Mat(buff_lvl, 1, CV_32SC4); indexes = VolumesTable(); @@ -1106,7 +1114,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept ocl::KernelArg::ReadOnly(volumeUnitIndices.getUMat(ACCESS_READ)), ocl::KernelArg::ReadWrite(volUnitsData), ocl::KernelArg::ReadOnly(pixNorms), - ocl::KernelArg::ReadOnly(isActiveFlags.getUMat(ACCESS_READ)), + ocl::KernelArg::ReadOnly(isActiveFlags), vol2camMatrix, camInvMatrix, voxelSize, @@ -1239,6 +1247,44 @@ std::vector HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _de } +void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId) +{ + //! Mark volumes in the camera frustum as active + String errorStr; + String name = "markActive"; + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); + + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); + + UMat volumeUnitIndicesGPU = volumeUnitIndices.getUMat(ACCESS_READ); + const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + const Intr::Projector proj(intrinsics.makeProjector()); + Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); + + k.args( + ocl::KernelArg::ReadOnly(volumeUnitIndicesGPU), + ocl::KernelArg::WriteOnly(isActiveFlags), + ocl::KernelArg::WriteOnly(lastVisibleIndices), + vol2cam.matrix, + fxy, + cxy, + frameSz, + volumeUnitSize, + lastVolIndex, + truncateThreshold, + frameId + ); + + size_t globalSize[1] = { (size_t)lastVolIndex }; + if (!k.run(1, globalSize, nullptr, true)) + throw std::runtime_error("Failed to run kernel"); +} + + void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); @@ -1256,17 +1302,26 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (lastVolIndex >= buff_lvl) { degree = (int)(log2(lastVolIndex) + 1); // clz() would be better + int oldBuffSize = buff_lvl; buff_lvl = (int)pow(2, degree); volUnitsDataCopy.resize(buff_lvl); + + Range oldr(0, oldBuffSize); int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; UMat newData(buff_lvl, volCubed, CV_8UC2); - volUnitsData.copyTo(newData(Range(0, volUnitsData.rows), Range::all())); + volUnitsData.copyTo(newData.rowRange(oldr)); volUnitsData = newData; - lastVisibleIndexes.resize(buff_lvl); + UMat newLastVisibleIndices(buff_lvl, 1, CV_32S); + lastVisibleIndices.copyTo(newLastVisibleIndices.rowRange(oldr)); + lastVisibleIndices = newLastVisibleIndices; + + UMat newIsActiveFlags(buff_lvl, 1, CV_8U); + isActiveFlags.copyTo(newIsActiveFlags.rowRange(oldr)); + isActiveFlags = newIsActiveFlags; + volumeUnitIndices.resize(buff_lvl); - isActiveFlags.resize(buff_lvl); } // Place data for new volume units @@ -1278,10 +1333,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma Vec4i idx4 = node->idx; Vec3i tsdf_idx(idx4[0], idx4[1], idx4[2]); - - *lastVisibleIndexes.ptr(row, 0) = frameId; - *isActiveFlags.ptr(row, 0) = 1; - //TODO: replace .ptr<...> everywhere by .at<...> *volumeUnitIndices.ptr(row, 0) = idx4; } @@ -1289,41 +1340,15 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma { Range r(oldLastVolIndex, lastVolIndex); - //lastVisibleIndexes.rowRange(r) = frameId; - //isActiveFlags.rowRange(r) = 1; + lastVisibleIndices.rowRange(r) = frameId; + isActiveFlags.rowRange(r) = 1; TsdfVoxel emptyVoxel(floatToTsdf(0.0f), 0); volUnitsData.rowRange(r) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight)); } //! Mark volumes in the camera frustum as active - Range _inFrustumRange(0, lastVolIndex); - auto markActive = [&](const Range& range) { - const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); - const Intr::Projector proj(intrinsics.makeProjector()); - - for (int row = range.start; row < range.end; ++row) - { - Vec4i idx4 = *volumeUnitIndices.ptr(row, 0); - Vec3i idx(idx4[0], idx4[1], idx4[2]); - - Point3f volumeUnitPos = volumeUnitIdxToVolume(idx); - Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; - if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) - { - *isActiveFlags.ptr(row, 0) = 0; - continue; - } - Point2f cameraPoint = proj(volUnitInCamSpace); - if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) - { - CV_Assert(row >= 0 || row < lastVolIndex); - *lastVisibleIndexes.ptr(row, 0) = frameId; - *isActiveFlags.ptr(row, 0) = 1; - } - } - }; - parallel_for_(_inFrustumRange, markActive); + markActive(cameraPose, intrinsics, depth.size(), frameId); Vec6f newParams((float)depth.rows, (float)depth.cols, intrinsics.fx, intrinsics.fy, @@ -1773,11 +1798,13 @@ void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) c int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) const { + Mat cpuIndices = lastVisibleIndices.getMat(ACCESS_READ); + int numVisibleBlocks = 0; //! TODO: Iterate over map parallely? for (int i = 0; i < lastVolIndex; i++) { - if (*lastVisibleIndexes.ptr(i) > (currFrameId - frameThreshold)) + if (*cpuIndices.ptr(i) > (currFrameId - frameThreshold)) numVisibleBlocks++; } return numVisibleBlocks; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 579f4eb3e82..e4052664abe 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -620,4 +620,60 @@ __kernel void raycast( __global float* nrm = (__global float*)(normalsptr + normals_offset + y*normals_step + x*sizeof(ptype)); vstore4((float4)(point, 0), 0, pts); vstore4((float4)(normal, 0), 0, nrm); -} \ No newline at end of file +} + + +__kernel void markActive ( + const __global char* volumeUnitIndicesPtr, + int volumeUnitIndicesStep, int volumeUnitIndicesOffset, + int volumeUnitIndicesRows, int volumeUnitIndicesCols, + + __global char* isActiveFlagsPtr, + int isActiveFlagsStep, int isActiveFlagsOffset, + int isActiveFlagsRows, int isActiveFlagsCols, + + __global char* lastVisibleIndicesPtr, + int lastVisibleIndicesStep, int lastVisibleIndicesOffset, + int lastVisibleIndicesRows, int lastVisibleIndicesCols, + + const float16 vol2cam, + const float2 fxy, + const float2 cxy, + const int2 frameSz, + const float volumeUnitSize, + const int lastVolIndex, + const float truncateThreshold, + const int frameId + ) +{ + int row = get_global_id(0); + + if (row < lastVolIndex) + { + int3 idx = (*(__global const int4*)(volumeUnitIndicesPtr + volumeUnitIndicesOffset + + row * volumeUnitIndicesStep)).xyz; + + float3 volumeUnitPos = convert_float3(idx) * volumeUnitSize; + + float3 volUnitInCamSpace = (float3) (dot(volumeUnitPos, vol2cam.s012), + dot(volumeUnitPos, vol2cam.s456), + dot(volumeUnitPos, vol2cam.s89a)) + vol2cam.s37b; + + if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) + { + *(isActiveFlagsPtr + isActiveFlagsOffset + row * isActiveFlagsStep) = 0; + return; + } + + float2 cameraPoint; + float invz = 1.f / volUnitInCamSpace.z; + cameraPoint.x = fxy.x*(volUnitInCamSpace.x*invz) + cxy.x; + cameraPoint.y = fxy.y*(volUnitInCamSpace.y*invz) + cxy.y; + + if (all(cameraPoint >= 0) && all(cameraPoint < convert_float2(frameSz))) + { + *(__global int*)(lastVisibleIndicesPtr + lastVisibleIndicesOffset + row * lastVisibleIndicesStep) = frameId; + *(isActiveFlagsPtr + isActiveFlagsOffset + row * isActiveFlagsStep) = 1; + } + } +} From 109b675e5197ce61798f5fd0132c114e9f748a34 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Fri, 29 Jan 2021 04:13:58 +0300 Subject: [PATCH 170/216] CL: minor --- modules/rgbd/src/opencl/hash_tsdf.cl | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index e4052664abe..1689ed60f31 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -667,8 +667,7 @@ __kernel void markActive ( float2 cameraPoint; float invz = 1.f / volUnitInCamSpace.z; - cameraPoint.x = fxy.x*(volUnitInCamSpace.x*invz) + cxy.x; - cameraPoint.y = fxy.y*(volUnitInCamSpace.y*invz) + cxy.y; + cameraPoint = fxy * volUnitInCamSpace.xy * invz + cxy; if (all(cameraPoint >= 0) && all(cameraPoint < convert_float2(frameSz))) { From e7bef78d015bdcadcface005f69273ee21d73172 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sun, 31 Jan 2021 05:17:03 +0300 Subject: [PATCH 171/216] volumeUnitIndices -> GPU --- modules/rgbd/src/hash_tsdf.cpp | 72 ++++++++++++++++------------------ 1 file changed, 34 insertions(+), 38 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 8c7ba709233..632487af3f4 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -898,7 +898,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void integrateAllVolumeUnitsGPU(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); - std::vector allocateVolumeUnits(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); + UMat allocateVolumeUnits(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); void markActive(const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId); @@ -941,7 +941,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume int buff_lvl; // per-volume-unit data - cv::Mat volumeUnitIndices; + cv::UMat volumeUnitIndices; cv::UMat lastVisibleIndices; @@ -995,7 +995,7 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, flo HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstMemOrder) : HashTSDFVolume(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, - _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) + _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) { //TODO: move reset() contents here, reset() just clears the data } @@ -1016,7 +1016,7 @@ void HashTSDFVolumeGPU::reset() isActiveFlags = cv::UMat(buff_lvl, 1, CV_8U); - volumeUnitIndices = cv::Mat(buff_lvl, 1, CV_32SC4); + volumeUnitIndices = cv::UMat(buff_lvl, 1, CV_32SC4); indexes = VolumesTable(); @@ -1111,7 +1111,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept Matx44f camInvMatrix = Affine3f(cameraPose).inv().matrix; k.args(ocl::KernelArg::ReadOnly(depth), - ocl::KernelArg::ReadOnly(volumeUnitIndices.getUMat(ACCESS_READ)), + ocl::KernelArg::ReadOnly(volumeUnitIndices), ocl::KernelArg::ReadWrite(volUnitsData), ocl::KernelArg::ReadOnly(pixNorms), ocl::KernelArg::ReadOnly(isActiveFlags), @@ -1138,12 +1138,12 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept } -std::vector HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) +UMat HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { const int newIndicesCapacity = VOLUMES_SIZE; const int localCapacity = VOLUMES_SIZE; - std::vector nodePtrs; - nodePtrs.reserve(newIndicesCapacity); + std::vector nodeIndices; + nodeIndices.reserve(newIndicesCapacity); Depth depth = _depth.getMat(ACCESS_READ); @@ -1243,7 +1243,9 @@ std::vector HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _de // --------------------- - return nodePtrs; + UMat uni(nodeIndices.size(), 1, CV_32SC4); + Mat(nodeIndices).copyTo(uni); + return uni; } @@ -1260,13 +1262,12 @@ void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrin if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); - UMat volumeUnitIndicesGPU = volumeUnitIndices.getUMat(ACCESS_READ); const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); const Intr::Projector proj(intrinsics.makeProjector()); Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); k.args( - ocl::KernelArg::ReadOnly(volumeUnitIndicesGPU), + ocl::KernelArg::ReadOnly(volumeUnitIndices), ocl::KernelArg::WriteOnly(isActiveFlags), ocl::KernelArg::WriteOnly(lastVisibleIndices), vol2cam.matrix, @@ -1292,9 +1293,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma CV_Assert(_depth.type() == DEPTH_TYPE); UMat depth = _depth.getUMat(); - // Save pointers to avoid searches when allocating + // Save length to fill new data in ranges int oldLastVolIndex = lastVolIndex; - std::vector nodePtrs = allocateVolumeUnits(depth, depthFactor, cameraPose, intrinsics); + UMat nodeIndices = allocateVolumeUnits(depth, depthFactor, cameraPose, intrinsics); //! Perform the allocation @@ -1321,25 +1322,16 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma isActiveFlags.copyTo(newIsActiveFlags.rowRange(oldr)); isActiveFlags = newIsActiveFlags; - volumeUnitIndices.resize(buff_lvl); + UMat newVolumeUnitIndices(buff_lvl, 1, CV_32SC4); + volumeUnitIndices.copyTo(newVolumeUnitIndices.rowRange(oldr)); + volumeUnitIndices = newVolumeUnitIndices; } - // Place data for new volume units - for (size_t i = 0; i < nodePtrs.size(); i++) + // Fill data for new volume units + Range r(oldLastVolIndex, lastVolIndex); + if (r.start < r.end) { - Volume_NODE* node = nodePtrs[i]; - int row = node->row; - - Vec4i idx4 = node->idx; - Vec3i tsdf_idx(idx4[0], idx4[1], idx4[2]); - - //TODO: replace .ptr<...> everywhere by .at<...> - *volumeUnitIndices.ptr(row, 0) = idx4; - } - if (oldLastVolIndex < lastVolIndex) - { - Range r(oldLastVolIndex, lastVolIndex); - + nodeIndices.copyTo(volumeUnitIndices.rowRange(r)); lastVisibleIndices.rowRange(r) = frameId; isActiveFlags.rowRange(r) = 1; @@ -1351,8 +1343,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma markActive(cameraPose, intrinsics, depth.size(), frameId); Vec6f newParams((float)depth.rows, (float)depth.cols, - intrinsics.fx, intrinsics.fy, - intrinsics.cx, intrinsics.cy); + intrinsics.fx, intrinsics.fy, + intrinsics.cx, intrinsics.cy); if (!(frameParams == newParams)) { frameParams = newParams; @@ -1364,6 +1356,8 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma integrateAllVolumeUnitsGPU(depth, depthFactor, cameraPose, intrinsics); } +//TODO: replace .ptr<...> everywhere by .at<...> + cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const { return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), @@ -1690,11 +1684,13 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { CV_TRACE_FUNCTION(); - //TODO: remove it when it works w/o CPU code - volUnitsData.copyTo(volUnitsDataCopy); - if (_points.needed()) { + //TODO: remove it when it works w/o CPU code + volUnitsData.copyTo(volUnitsDataCopy); + Mat volumeUnitIndicesCopy(buff_lvl, 1, CV_32SC4); + volumeUnitIndices.copyTo(volumeUnitIndicesCopy); + std::vector> pVecs, nVecs; Range _fetchRange(0, lastVolIndex); @@ -1710,7 +1706,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor std::vector points, normals; for (int row = range.start; row < range.end; row++) { - cv::Vec4i idx4 = *volumeUnitIndices.ptr(row, 0); + cv::Vec4i idx4 = *volumeUnitIndicesCopy.ptr(row, 0); cv::Vec3i idx(idx4[0], idx4[1], idx4[2]); Point3f base_point = volume.volumeUnitIdxToVolume(idx); @@ -1769,11 +1765,11 @@ void HashTSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) c { CV_TRACE_FUNCTION(); - //TODO: remove it when it works w/o CPU code - volUnitsData.copyTo(volUnitsDataCopy); - if (_normals.needed()) { + //TODO: remove it when it works w/o CPU code + volUnitsData.copyTo(volUnitsDataCopy); + Points points = _points.getMat(); CV_Assert(points.type() == POINT_TYPE); _normals.createSameSize(_points, _points.type()); From 8c7d77b53bc164dec521b695c985193e7846fd8e Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 4 Feb 2021 03:09:09 +0300 Subject: [PATCH 172/216] ToyHashMap instead of HT, volumeUnitIndices removed, lastVolIndex removed, + a lot of debugging code --- modules/rgbd/src/hash_tsdf.cpp | 549 ++++++++++++++++++++++----- modules/rgbd/src/opencl/hash_tsdf.cl | 70 +++- 2 files changed, 506 insertions(+), 113 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 632487af3f4..4975f7976dc 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -883,8 +883,115 @@ int HashTSDFVolumeCPU::getVisibleBlocks(int currFrameId, int frameThreshold) con #ifdef HAVE_OPENCL -typedef cv::Mat _VolumeUnitIndexSet; -typedef cv::Mat _VolumeUnitIndexes; + +//TODO: hash set, not hash map +class ToyHashMap +{ +public: + static const int hashDivisor = 32768; + static const int startCapacity = 1024; // 32768*4; + + std::vector hashes; + // 0-3 for key, 4th for internal use + // don't keep keep value + std::vector data; + int capacity; + int last; + + ToyHashMap() + { + hashes.resize(hashDivisor); + for (int i = 0; i < hashDivisor; i++) + hashes[i] = -1; + capacity = startCapacity; + + data.resize(capacity); + for (int i = 0; i < capacity; i++) + data[i] = { 0, 0, 0, -1 }; + + last = 0; + } + + ~ToyHashMap() { } + + inline size_t calc_hash(Vec3i x) const + { + uint32_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + for (int i = 0; i < 3; i++) + { + seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + } + return seed; + } + + // should work on existing elements too + int insert(Vec3i idx) + { + if (last < capacity) + { + int hash = int(calc_hash(idx) % hashDivisor); + int place = hashes[hash]; + if (place >= 0) + { + int oldPlace = place; + while (place >= 0) + { + if (data[place][0] == idx[0] && + data[place][1] == idx[1] && + data[place][2] == idx[2]) + return 2; + else + { + oldPlace = place; + place = data[place][3]; + //std::cout << "place=" << place << std::endl; + } + } + + // found, create here + data[oldPlace][3] = last; + } + else + { + // insert at last + hashes[hash] = last; + } + + data[last][0] = idx[0]; + data[last][1] = idx[1]; + data[last][2] = idx[2]; + data[last][3] = -1; + last++; + + return 1; + } + else + return 0; + } + + int find(Vec3i idx) const + { + int hash = int(calc_hash(idx) % hashDivisor); + int place = hashes[hash]; + // search a place + while (place >= 0) + { + if (data[place][0] == idx[0] && + data[place][1] == idx[1] && + data[place][2] == idx[2]) + break; + else + { + place = data[place][3]; + } + } + + return place; + } +}; + + class HashTSDFVolumeGPU : public HashTSDFVolume { @@ -898,7 +1005,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void integrateAllVolumeUnitsGPU(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); - UMat allocateVolumeUnits(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); + void allocateVolumeUnits(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics); void markActive(const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId); @@ -911,7 +1018,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; - size_t getTotalVolumeUnits() const override { return size_t(lastVolIndex); } + size_t getTotalVolumeUnits() const override { return size_t(hashMap.last); } int getVisibleBlocks(int currFrameId, int frameThreshold) const override; @@ -932,7 +1039,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; Vec3i volumeToVoxelCoord(const Point3f& point) const; - int find_idx(cv::Mat v, Vec3i tsdf_idx) const; public: Vec4i volStrides; @@ -941,8 +1047,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume int buff_lvl; // per-volume-unit data - cv::UMat volumeUnitIndices; - cv::UMat lastVisibleIndices; cv::UMat isActiveFlags; @@ -952,10 +1056,12 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::Mat volUnitsDataCopy; cv::UMat pixNorms; - int lastVolIndex; //TODO: move indexes.volumes to GPU - VolumesTable indexes; + //VolumesTable indexes; + ToyHashMap hashMap; + + Vec8i neighbourCoords; }; @@ -1003,7 +1109,6 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstM void HashTSDFVolumeGPU::reset() { CV_TRACE_FUNCTION(); - lastVolIndex = 0; degree = 15; buff_lvl = (int) pow(2, degree); @@ -1016,39 +1121,13 @@ void HashTSDFVolumeGPU::reset() isActiveFlags = cv::UMat(buff_lvl, 1, CV_8U); - volumeUnitIndices = cv::UMat(buff_lvl, 1, CV_32SC4); - - indexes = VolumesTable(); + //indexes = VolumesTable(); + hashMap = ToyHashMap(); frameParams = Vec6f(); pixNorms = UMat(); } -static inline bool find(cv::Mat v, Vec3i tsdf_idx, int lastVolIndex) -{ - for (int i = 0; i < lastVolIndex; i++) - { - Vec3i* p = v.ptr(i); - if (*p == tsdf_idx) - { - return true; - } - } - return false; -} - -inline int HashTSDFVolumeGPU::find_idx(cv::Mat v, Vec3i tsdf_idx) const -{ - for (int i = 0; i < lastVolIndex; i++) - { - Vec3i* p = v.ptr(i); - if (*p == tsdf_idx) - { - return i; - } - } - return -1; -} static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f fxy, Vec2f cxy) { @@ -1110,8 +1189,16 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept Matx44f vol2camMatrix = (Affine3f(cameraPose).inv() * pose).matrix; Matx44f camInvMatrix = Affine3f(cameraPose).inv().matrix; + UMat hashesGpu(hashMap.hashDivisor, 1, CV_32S); + Mat(hashMap.hashes, false).copyTo(hashesGpu); + + UMat hashDataGpu(hashMap.capacity, 1, CV_32SC4); + Mat(hashMap.data, false).copyTo(hashDataGpu); + k.args(ocl::KernelArg::ReadOnly(depth), - ocl::KernelArg::ReadOnly(volumeUnitIndices), + ocl::KernelArg::PtrReadOnly(hashesGpu), + ocl::KernelArg::PtrReadOnly(hashDataGpu), + (int)hashMap.hashDivisor, ocl::KernelArg::ReadWrite(volUnitsData), ocl::KernelArg::ReadOnly(pixNorms), ocl::KernelArg::ReadOnly(isActiveFlags), @@ -1131,19 +1218,18 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept size_t globalSize[3]; globalSize[0] = (size_t)resol; // volumeUnitResolution globalSize[1] = (size_t)resol; // volumeUnitResolution - globalSize[2] = (size_t)lastVolIndex; // num of volume units + globalSize[2] = (size_t)hashMap.last; // num of volume units if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } -UMat HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) +void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { const int newIndicesCapacity = VOLUMES_SIZE; - const int localCapacity = VOLUMES_SIZE; - std::vector nodeIndices; - nodeIndices.reserve(newIndicesCapacity); + constexpr size_t pixCapacity = 16; + typedef std::array LocalVolUnits; Depth depth = _depth.getMat(ACCESS_READ); @@ -1155,9 +1241,89 @@ UMat HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto const Point3f truncPt(truncDist, truncDist, truncDist); Mutex mutex; + // for new indices + ToyHashMap thm; + + /* ----------------------- + + String errorStr; + String name = "toy_alloc"; + + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); + + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); + + ToyHashMap ths; + UMat newSetHashes(ths.hashDivisor, 1, CV_32S); + Mat(ths.hashes, false).copyTo(newSetHashes); + int dt = CV_MAKETYPE(CV_32S, 8); + UMat newSetData(ths.capacity, 1, dt); + Mat(ths.data, false).copyTo(newSetData); + UMat newSetLast(1, 1, CV_32S); + newSetLast = ths.last; + + k.args(ocl::KernelArg::PtrReadWrite(newSetHashes), + ocl::KernelArg::PtrReadWrite(newSetData), + ocl::KernelArg::PtrReadWrite(newSetLast), + ths.capacity, + ths.hashDivisor, + + + + ); + + size_t globalSize[2]; + globalSize[0] = divUp(depth.cols, depthStride); + globalSize[1] = divUp(depth.rows, depthStride); + + if (!k.run(2, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); + + newSetHashes.copyTo(Mat(ths.hashes, false)); + newSetData.copyTo(Mat(ths.data, false)); + + //TODO HUGE: add to globalMap instead + + ------------------------------------------- */ + /* + __global int* newSetHashes, + __global int8* newSetData, + __global int* newSetLast, + const int newSetCapacity, + const int newSetHashDivisor, + + __global const int* globalMapHashes, + __global const int8* globalMapData, + const int globalMapHashDivisor, + + __global const char * depthPtr, + int depthStep, int depthOffset, + int depthRows, int depthCols, + + const int depthStride, + const float invDepthFactor, + const float truncateThreshold, + + const float2 fixy, const float2 cxy, + + const float16 cam2vol, + const float volumeUnitSizeInv, + const float truncDist, + + __global volatile int* hsMutex, // set to 0 initially + __global int* full // set to 0 initially + + */ + + // ----------------------- - auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, _VolumeUnitIndexSet& _localAccessVolUnits, int& loc_vol_idx) { + auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, ToyHashMap& ghm/*LocalVolUnits& localAccessVolUnits, int& locVolIdx*/) + { for (int y = yrange.start; y < yrange.end; y += depthStride) { const depthType* depthRow = depth[y]; @@ -1171,39 +1337,62 @@ UMat HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto //! Find accessed TSDF volume unit for valid 3D vertex Vec3i lower_bound = this->volumeToVolumeUnitIdx(volPoint - truncPt); Vec3i upper_bound = this->volumeToVolumeUnitIdx(volPoint + truncPt); - + + int pixLocalCounter = 0; + LocalVolUnits pixLocalVolUnits; for (int i = lower_bound[0]; i <= upper_bound[0]; i++) for (int j = lower_bound[1]; j <= upper_bound[1]; j++) for (int k = lower_bound[2]; k <= upper_bound[2]; k++) { const Vec3i tsdf_idx = Vec3i(i, j, k); - if (indexes.findRow(tsdf_idx) < 0) + //if (indexes.findRow(tsdf_idx) < 0) + if (hashMap.find(tsdf_idx) < 0) { - if (!find(_localAccessVolUnits, tsdf_idx, loc_vol_idx)) + bool found = false; + for (int i = 0; i < pixLocalCounter; i++) { - *_localAccessVolUnits.ptr(loc_vol_idx) = tsdf_idx; - loc_vol_idx++; - - if (loc_vol_idx >= localCapacity) + if (pixLocalVolUnits[i] == tsdf_idx) + { + found = true; break; + } + } + if (!found) + { + pixLocalVolUnits[pixLocalCounter++] = tsdf_idx; + if (pixLocalCounter >= pixCapacity) { //DEBUG - std::cout << "allocate: local capacity exhausted" << std::endl; - + std::cout << "allocate: pix capacity exhausted" << std::endl; return; } } } } + + // lock localAccessVolUnits somehow + for (int i = 0; i < pixLocalCounter; i++) + { + Vec3i idx = pixLocalVolUnits[i]; + if (!ghm.insert(idx)) + { + //DEBUG + std::cout << "allocate: local capacity exhausted" << std::endl; + //return; + } + } + // unlock } } }; Rect dim(0, 0, depth.cols, depth.rows); - Size gsz(64, 64); + Size gsz(32, 32); Size gg(divUp(dim.width, gsz.width), divUp(dim.height, gsz.height)); - auto allocateLambda = [&](const Range& r) { + bool needReallocation = false; + auto allocateLambda = [&](const Range& r) + { for (int yg = r.start; yg < r.end; yg++) { @@ -1213,39 +1402,190 @@ UMat HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto gr = gr & dim; Range xr(gr.tl().x, gr.br().x), yr(gr.tl().y, gr.br().y); - _VolumeUnitIndexSet _localAccessVolUnits = cv::Mat(localCapacity, 1, CV_32SC3); + /* + LocalVolUnits localAccessVolUnits; int loc_vol_idx = 0; + */ + + ToyHashMap ghm; - fillLocalAcessVolUnits(xr, yr, _localAccessVolUnits, loc_vol_idx); + fillLocalAcessVolUnits(xr, yr, ghm /*localAccessVolUnits, loc_vol_idx*/); - mutex.lock(); - for (int i = 0; i < loc_vol_idx; i++) + if (ghm.last) { - Vec3i idx = *_localAccessVolUnits.ptr(i); + std::lock_guard al(mutex); - // if not found - if (indexes.findRow(idx) < 0) + //mutex.lock(); + for (int i = 0; i < ghm.last; i++) { - Volume_NODE* node = indexes.insert(idx, lastVolIndex); - nodePtrs.push_back(node); - lastVolIndex++; + Vec4i node = ghm.data[i]; + Vec3i idx(node[0], node[1], node[2]); + + //TODO: 1. add to separate hash map instead, then merge on GPU side + + int result = thm.insert(idx); + if (!result) + { + needReallocation = true; + //DEBUG + std::cout << "new indices: need reallocation, exiting" << std::endl; + + return; + } + + /* + // if not found + //if (indexes.findRow(idx) < 0) + if (hashMap.find(idx) < 0) + { + //bool extend = indexes.insert(idx, lastVolIndex); + int result = hashMap.insert(idx, lastVolIndex); + //TODO: replace 1 by enum + if (result == 1) + { + Vec4i idx4(idx[0], idx[1], idx[2], 0); + nodeIndices.push_back(idx4); + lastVolIndex++; + } + else if (result == 0) + { + needReallocation = true; + //DEBUG + std::cout << "need reallocation, exiting" << std::endl; + + return; + } + } + */ + + } + //mutex.unlock(); + + } + + /* + if (loc_vol_idx > 0) + { + //DEBUG + //std::cout << "loc_vol_idx: " << loc_vol_idx << std::endl; + + mutex.lock(); + for (int i = 0; i < loc_vol_idx; i++) + { + Vec3i idx = localAccessVolUnits[i]; + + // if not found + if (indexes.findRow(idx) < 0) + { + Volume_NODE* node = indexes.insert(idx, lastVolIndex); + nodePtrs.push_back(node); + lastVolIndex++; + } } + mutex.unlock(); } - mutex.unlock(); + */ } } }; - parallel_for_(Range(0, gg.height), allocateLambda); - //allocateLambda(Range(0, gg.height)); + do + { + if (needReallocation) + { + /* + std::cout << "reallocation!! from: " << hashMap.capacity << " to x2: " << hashMap.capacity * 2 << std::endl; + hashMap.capacity *= 2; + hashMap.data.resize(hashMap.capacity); + */ + std::cout << "reallocation group!! from: " << thm.capacity << " to x2: " << thm.capacity * 2 << std::endl; + thm.capacity *= 2; + thm.data.resize(thm.capacity); + + needReallocation = false; + } + + parallel_for_(Range(0, gg.height), allocateLambda); + //allocateLambda(Range(0, gg.height)); + } while (needReallocation); - // --------------------- + auto pushToGlobal = [](const ToyHashMap thm, ToyHashMap& globalHashMap, + bool& needReallocation, Mutex& mutex) + { + for (int i = 0; i < thm.last; i++) + { + Vec4i node = thm.data[i]; + Vec3i idx(node[0], node[1], node[2]); + + std::lock_guard al(mutex); + + int result = globalHashMap.insert(idx); + if (result == 0) + { + needReallocation = true; + //DEBUG + std::cout << "need reallocation, exiting" << std::endl; + return; + } + } + }; + /* + String errorStr; + String name = "push_to_global"; + + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); + + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); + + ToyHashMap ths; + UMat newSetHashes(ths.hashDivisor, 1, CV_32S); + Mat(ths.hashes, false).copyTo(newSetHashes); + int dt = CV_MAKETYPE(CV_32S, 8); + UMat newSetData(ths.capacity, 1, dt); + Mat(ths.data, false).copyTo(newSetData); + UMat newSetLast(1, 1, CV_32S); + newSetLast = ths.last; + + k.args(ocl::KernelArg::PtrReadWrite(newSetHashes), + ocl::KernelArg::PtrReadWrite(newSetData), + ocl::KernelArg::PtrReadWrite(newSetLast), + ths.capacity, + ths.hashDivisor, + + ); + + size_t globalSize[1]; + globalSize[0] = thm.last; + + if (!k.run(2, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); + + newSetHashes.copyTo(Mat(ths.hashes, false)); + newSetData.copyTo(Mat(ths.data, false)); + */ + + needReallocation = false; + do + { + if (needReallocation) + { + std::cout << "reallocation global!! from: " << hashMap.capacity << " to x2: " << hashMap.capacity * 2 << std::endl; + hashMap.capacity *= 2; + hashMap.data.resize(hashMap.capacity); + + needReallocation = false; + } - UMat uni(nodeIndices.size(), 1, CV_32SC4); - Mat(nodeIndices).copyTo(uni); - return uni; + pushToGlobal(thm, hashMap, needReallocation, mutex); + } while (needReallocation); + + // --------------------- } @@ -1266,8 +1606,16 @@ void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrin const Intr::Projector proj(intrinsics.makeProjector()); Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); + UMat hashesGpu(hashMap.hashDivisor, 1, CV_32S); + Mat(hashMap.hashes, false).copyTo(hashesGpu); + + UMat hashDataGpu(hashMap.capacity, 1, CV_32SC4); + Mat(hashMap.data, false).copyTo(hashDataGpu); + k.args( - ocl::KernelArg::ReadOnly(volumeUnitIndices), + ocl::KernelArg::PtrReadOnly(hashesGpu), + ocl::KernelArg::PtrReadOnly(hashDataGpu), + (int)hashMap.hashDivisor, ocl::KernelArg::WriteOnly(isActiveFlags), ocl::KernelArg::WriteOnly(lastVisibleIndices), vol2cam.matrix, @@ -1275,12 +1623,12 @@ void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrin cxy, frameSz, volumeUnitSize, - lastVolIndex, + hashMap.last, truncateThreshold, frameId ); - size_t globalSize[1] = { (size_t)lastVolIndex }; + size_t globalSize[1] = { (size_t)hashMap.last }; if (!k.run(1, globalSize, nullptr, true)) throw std::runtime_error("Failed to run kernel"); } @@ -1294,15 +1642,15 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma UMat depth = _depth.getUMat(); // Save length to fill new data in ranges - int oldLastVolIndex = lastVolIndex; - UMat nodeIndices = allocateVolumeUnits(depth, depthFactor, cameraPose, intrinsics); - + int sizeBefore = hashMap.last; + allocateVolumeUnits(depth, depthFactor, cameraPose, intrinsics); + int sizeAfter = hashMap.last; //! Perform the allocation // Grow buffers - if (lastVolIndex >= buff_lvl) + if (sizeAfter >= buff_lvl) { - degree = (int)(log2(lastVolIndex) + 1); // clz() would be better + degree = (int)(log2(sizeAfter) + 1); // clz() would be better int oldBuffSize = buff_lvl; buff_lvl = (int)pow(2, degree); @@ -1321,17 +1669,12 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma UMat newIsActiveFlags(buff_lvl, 1, CV_8U); isActiveFlags.copyTo(newIsActiveFlags.rowRange(oldr)); isActiveFlags = newIsActiveFlags; - - UMat newVolumeUnitIndices(buff_lvl, 1, CV_32SC4); - volumeUnitIndices.copyTo(newVolumeUnitIndices.rowRange(oldr)); - volumeUnitIndices = newVolumeUnitIndices; } // Fill data for new volume units - Range r(oldLastVolIndex, lastVolIndex); + Range r(sizeBefore, sizeAfter); if (r.start < r.end) { - nodeIndices.copyTo(volumeUnitIndices.rowRange(r)); lastVisibleIndices.rowRange(r) = frameId; isActiveFlags.rowRange(r) = 1; @@ -1449,7 +1792,8 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const auto it = iterMap[dictIdx]; if (it < -1) { - it = indexes.findRow(volumeUnitIdx); + //it = indexes.findRow(volumeUnitIdx); + it = hashMap.find(volumeUnitIdx); iterMap[dictIdx] = it; } @@ -1511,7 +1855,8 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const auto it = iterMap[dictIdx]; if (it < -1) { - it = indexes.findRow(volumeUnitIdx); + //it = indexes.findRow(volumeUnitIdx); + it = hashMap.find(volumeUnitIdx); iterMap[dictIdx] = it; } @@ -1647,12 +1992,24 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Mat(pose.matrix).copyTo(volPoseGpu); Mat(pose.inv().matrix).copyTo(invPoseGpu); + UMat hashesGpu(hashMap.hashDivisor, 1, CV_32S); + Mat(hashMap.hashes, false).copyTo(hashesGpu); + + UMat hashDataGpu(hashMap.capacity, 1, CV_32SC4); + Mat(hashMap.data, false).copyTo(hashDataGpu); + k.args( + ocl::KernelArg::PtrReadOnly(hashesGpu), + ocl::KernelArg::PtrReadOnly(hashDataGpu), + (int)hashMap.hashDivisor, + /* ocl::KernelArg::PtrReadOnly(indexes.volumes.getUMat(ACCESS_RW)), (int)indexes.list_size, (int)indexes.bufferNums, (int)indexes.hash_divisor, - (int)lastVolIndex, + */ + + ocl::KernelArg::WriteOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), frameSize, @@ -1688,12 +2045,14 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { //TODO: remove it when it works w/o CPU code volUnitsData.copyTo(volUnitsDataCopy); - Mat volumeUnitIndicesCopy(buff_lvl, 1, CV_32SC4); - volumeUnitIndices.copyTo(volumeUnitIndicesCopy); + //TODO: remove it when it works w/o CPU code + //TODO: enable it when it's on GPU + //UMat hashDataGpu(hashMap.capacity, 1, CV_32SC4); + //Mat(hashMap.data, false).copyTo(hashDataGpu); std::vector> pVecs, nVecs; - Range _fetchRange(0, lastVolIndex); + Range _fetchRange(0, hashMap.last); const int nstripes = -1; @@ -1706,7 +2065,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor std::vector points, normals; for (int row = range.start; row < range.end; row++) { - cv::Vec4i idx4 = *volumeUnitIndicesCopy.ptr(row, 0); + cv::Vec4i idx4 = hashMap.data[row]; cv::Vec3i idx(idx4[0], idx4[1], idx4[2]); Point3f base_point = volume.volumeUnitIdxToVolume(idx); @@ -1798,7 +2157,7 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con int numVisibleBlocks = 0; //! TODO: Iterate over map parallely? - for (int i = 0; i < lastVolIndex; i++) + for (int i = 0; i < hashMap.last; i++) { if (*cpuIndices.ptr(i) > (currFrameId - frameThreshold)) numVisibleBlocks++; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 1689ed60f31..4817452f9c0 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -45,7 +45,7 @@ __kernel void preCalculationPixNorm (__global char * pixNormsPtr, int pixNormsRows, int pixNormsCols, const __global float * xx, const __global float * yy) -{ +{ int i = get_global_id(0); int j = get_global_id(1); if (i < pixNormsRows && j < pixNormsCols) @@ -83,6 +83,25 @@ static int findRow(__global const struct Volume_NODE * hash_table, int3 indx, return -1; } +//TODO: make hashDivisor a power of 2 +static int toy_find(int3 idx, const int hashDivisor, __global const int* hashes, + __global const int4* data) +{ + int hash = calc_hash(idx) % hashDivisor; + int place = hashes[hash]; + // search a place + while (place >= 0) + { + if (all(data[place].s012 == idx)) + break; + else + place = data[place].s3; + } + + return place; +} + + static void integrateVolumeUnit( int x, int y, @@ -243,10 +262,10 @@ __kernel void integrateAllVolumeUnits( __global const char * depthptr, int depth_step, int depth_offset, int depth_rows, int depth_cols, - // volumeUnitIndices - __global const int4 * volumeUnitIndices, - int volumeUnitIndices_step, int volumeUnitIndices_offset, - int volumeUnitIndices_rows, int volumeUnitIndices_cols, + // hashMap + __global const int* hashes, + __global const int4* data, + const int hash_divisor, // volUnitsData __global struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, @@ -276,7 +295,7 @@ __kernel void integrateAllVolumeUnits( int i = get_global_id(0); int j = get_global_id(1); int row = get_global_id(2); - int3 idx = volumeUnitIndices[row].xyz; + int3 idx = data[row].xyz; const int4 volResolution4 = (int4)(volUnitResolution, volUnitResolution, @@ -382,10 +401,17 @@ inline float interpolate(float3 t, float8 vz) inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolumePtr, int volumeUnitResolution, float voxelSizeInv, + + /* const __global struct Volume_NODE * hash_table, const int list_size, const int bufferNums, const int hash_divisor, + */ + const int hash_divisor, + __global const int* hashes, + __global const int4* data, + int3 volStrides, int table_offset) { float3 normal = (float3) (0.0f, 0.0f, 0.0f); @@ -437,7 +463,8 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum int it = iterMap[dictIdx]; if (it < -1) { - it = findRow(hash_table, volumeUnitIdx, list_size, bufferNums, hash_divisor); + //it = findRow(hash_table, volumeUnitIdx, list_size, bufferNums, hash_divisor); + it = toy_find(volumeUnitIdx, hash_divisor, hashes, data); iterMap[dictIdx] = it; } @@ -499,11 +526,16 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum typedef float4 ptype; __kernel void raycast( + __global const int* hashes, + __global const int4* data, + const int hash_divisor, + /* __global const struct Volume_NODE * hash_table, - const int list_size, - const int bufferNums, + const int list_size, + const int bufferNums, const int hash_divisor, - const int lastVolIndex, + */ + __global char * pointsptr, int points_step, int points_offset, __global char * normalsptr, @@ -570,7 +602,9 @@ __kernel void raycast( float3 currVolUnitIdxF = floor(currRayPos / volumeUnitSize); int3 currVolumeUnitIdx = convert_int3(currVolUnitIdxF); - int row = findRow(hash_table, currVolumeUnitIdx, list_size, bufferNums, hash_divisor); + //int row = findRow(hash_table, currVolumeUnitIdx, list_size, bufferNums, hash_divisor); + int row = toy_find(currVolumeUnitIdx, hash_divisor, hashes, data); + float currTsdf = prevTsdf; int currWeight = 0; float stepSize = 0.5 * volumeUnitSize; @@ -593,8 +627,9 @@ __kernel void raycast( { float3 pv = orig + tInterp * dir; float3 nv = getNormalVoxel( pv, allVolumePtr, volumeUnitResolution, - voxelSizeInv, hash_table, - list_size, bufferNums, hash_divisor, + voxelSizeInv, + //hash_table, list_size, bufferNums, hash_divisor, + hash_divisor, hashes, data, volStrides, table_offset); if(!any(isnan(nv))) @@ -624,9 +659,9 @@ __kernel void raycast( __kernel void markActive ( - const __global char* volumeUnitIndicesPtr, - int volumeUnitIndicesStep, int volumeUnitIndicesOffset, - int volumeUnitIndicesRows, int volumeUnitIndicesCols, + __global const int* hashes, + __global const int4* data, + const int hash_divisor, __global char* isActiveFlagsPtr, int isActiveFlagsStep, int isActiveFlagsOffset, @@ -650,8 +685,7 @@ __kernel void markActive ( if (row < lastVolIndex) { - int3 idx = (*(__global const int4*)(volumeUnitIndicesPtr + volumeUnitIndicesOffset + - row * volumeUnitIndicesStep)).xyz; + int3 idx = data[row].xyz; float3 volumeUnitPos = convert_float3(idx) * volumeUnitSize; From a4cc40deb1310b3dd4cb78bc7acc967915f3413b Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 4 Feb 2021 03:11:20 +0300 Subject: [PATCH 173/216] warning about local pix capacity --- modules/rgbd/src/hash_tsdf.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 4975f7976dc..ffb7428feb1 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1103,6 +1103,11 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstM : HashTSDFVolume(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) { + if (truncDist >= volumeUnitSize) + { + std::cout << "truncDist exceeds volume unit size, allocation may work incorrectly" << std::endl; + } + //TODO: move reset() contents here, reset() just clears the data } // zero volume, leave rest params the same From 4e3df85b5866c730be22e8169be79dab53f1dfd7 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 9 Feb 2021 02:12:21 +0300 Subject: [PATCH 174/216] CPU allocation: find in global HT first --- 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 ffb7428feb1..bb50195ad16 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -195,7 +195,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma for (int k = lower_bound[2]; k <= upper_bound[2]; k++) { const Vec3i tsdf_idx = Vec3i(i, j, k); - if (!localAccessVolUnits.count(tsdf_idx)) + if (localAccessVolUnits.count(tsdf_idx) <= 0 && this->volumeUnits.count(tsdf_idx) <= 0) { //! This volume unit will definitely be required for current integration localAccessVolUnits.emplace(tsdf_idx); @@ -208,10 +208,9 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma for (const auto& tsdf_idx : localAccessVolUnits) { //! If the insert into the global set passes - if (!this->volumeUnits.count(tsdf_idx)) + if (!newIndices.count(tsdf_idx)) { // Volume allocation can be performed outside of the lock - this->volumeUnits.emplace(tsdf_idx, VolumeUnit()); newIndices.emplace(tsdf_idx); } } @@ -222,7 +221,8 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma //! Perform the allocation for (auto idx : newIndices) { - VolumeUnit& vu = volumeUnits[idx]; + VolumeUnit& vu = this->volumeUnits.emplace(idx, VolumeUnit()).first->second; + Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; vu.pose = subvolumePose; From 399908820fb820dfe6b88034660074c62213cff9 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 9 Feb 2021 02:13:31 +0300 Subject: [PATCH 175/216] ToyHashMap: -> tsdf_functions.hpp --- modules/rgbd/src/hash_tsdf.cpp | 110 ---------------------------- modules/rgbd/src/tsdf_functions.hpp | 108 +++++++++++++++++++++++++++ 2 files changed, 108 insertions(+), 110 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index bb50195ad16..cd8413f0d51 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -883,116 +883,6 @@ int HashTSDFVolumeCPU::getVisibleBlocks(int currFrameId, int frameThreshold) con #ifdef HAVE_OPENCL - -//TODO: hash set, not hash map -class ToyHashMap -{ -public: - static const int hashDivisor = 32768; - static const int startCapacity = 1024; // 32768*4; - - std::vector hashes; - // 0-3 for key, 4th for internal use - // don't keep keep value - std::vector data; - int capacity; - int last; - - ToyHashMap() - { - hashes.resize(hashDivisor); - for (int i = 0; i < hashDivisor; i++) - hashes[i] = -1; - capacity = startCapacity; - - data.resize(capacity); - for (int i = 0; i < capacity; i++) - data[i] = { 0, 0, 0, -1 }; - - last = 0; - } - - ~ToyHashMap() { } - - inline size_t calc_hash(Vec3i x) const - { - uint32_t seed = 0; - constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; - for (int i = 0; i < 3; i++) - { - seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - } - return seed; - } - - // should work on existing elements too - int insert(Vec3i idx) - { - if (last < capacity) - { - int hash = int(calc_hash(idx) % hashDivisor); - int place = hashes[hash]; - if (place >= 0) - { - int oldPlace = place; - while (place >= 0) - { - if (data[place][0] == idx[0] && - data[place][1] == idx[1] && - data[place][2] == idx[2]) - return 2; - else - { - oldPlace = place; - place = data[place][3]; - //std::cout << "place=" << place << std::endl; - } - } - - // found, create here - data[oldPlace][3] = last; - } - else - { - // insert at last - hashes[hash] = last; - } - - data[last][0] = idx[0]; - data[last][1] = idx[1]; - data[last][2] = idx[2]; - data[last][3] = -1; - last++; - - return 1; - } - else - return 0; - } - - int find(Vec3i idx) const - { - int hash = int(calc_hash(idx) % hashDivisor); - int place = hashes[hash]; - // search a place - while (place >= 0) - { - if (data[place][0] == idx[0] && - data[place][1] == idx[1] && - data[place][2] == idx[2]) - break; - else - { - place = data[place][3]; - } - } - - return place; - } -}; - - - class HashTSDFVolumeGPU : public HashTSDFVolume { public: diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 70d3dc0a78c..f37f01e57b3 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -96,6 +96,114 @@ class VolumesTable }; +//TODO: hash set, not hash map +class ToyHashMap +{ +public: + static const int hashDivisor = 32768; + static const int startCapacity = 1024; // 32768*4; + + std::vector hashes; + // 0-3 for key, 4th for internal use + // don't keep keep value + std::vector data; + int capacity; + int last; + + ToyHashMap() + { + hashes.resize(hashDivisor); + for (int i = 0; i < hashDivisor; i++) + hashes[i] = -1; + capacity = startCapacity; + + data.resize(capacity); + for (int i = 0; i < capacity; i++) + data[i] = { 0, 0, 0, -1 }; + + last = 0; + } + + ~ToyHashMap() { } + + inline size_t calc_hash(Vec3i x) const + { + uint32_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + for (int i = 0; i < 3; i++) + { + seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + } + return seed; + } + + // should work on existing elements too + int insert(Vec3i idx) + { + if (last < capacity) + { + int hash = int(calc_hash(idx) % hashDivisor); + int place = hashes[hash]; + if (place >= 0) + { + int oldPlace = place; + while (place >= 0) + { + if (data[place][0] == idx[0] && + data[place][1] == idx[1] && + data[place][2] == idx[2]) + return 2; + else + { + oldPlace = place; + place = data[place][3]; + //std::cout << "place=" << place << std::endl; + } + } + + // found, create here + data[oldPlace][3] = last; + } + else + { + // insert at last + hashes[hash] = last; + } + + data[last][0] = idx[0]; + data[last][1] = idx[1]; + data[last][2] = idx[2]; + data[last][3] = -1; + last++; + + return 1; + } + else + return 0; + } + + int find(Vec3i idx) const + { + int hash = int(calc_hash(idx) % hashDivisor); + int place = hashes[hash]; + // search a place + while (place >= 0) + { + if (data[place][0] == idx[0] && + data[place][1] == idx[1] && + data[place][2] == idx[2]) + break; + else + { + place = data[place][3]; + } + } + + return place; + } +}; + + } // namespace kinfu } // namespace cv #endif From b1b00b15e8a073f1fb239c5edfcac7c4f5882ec3 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 9 Feb 2021 02:16:17 +0300 Subject: [PATCH 176/216] debugging code + to be removed --- modules/rgbd/src/hash_tsdf.cpp | 226 +++++++++++---------------------- 1 file changed, 72 insertions(+), 154 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index cd8413f0d51..e0640bcbdc2 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -717,7 +717,6 @@ void HashTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in volume.volumeUnitIdxToVolume(currVolumeUnitIdx); volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); - //! TODO: Figure out voxel interpolation TsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index); currTsdf = tsdfToFloat(currVoxel.tsdf); @@ -933,7 +932,9 @@ class HashTSDFVolumeGPU : public HashTSDFVolume public: Vec4i volStrides; Vec6f frameParams; + //TODO: rename this int degree; + //TODO: rename this int buff_lvl; // per-volume-unit data @@ -951,8 +952,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume //VolumesTable indexes; ToyHashMap hashMap; - - Vec8i neighbourCoords; }; @@ -998,12 +997,13 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstM std::cout << "truncDist exceeds volume unit size, allocation may work incorrectly" << std::endl; } - //TODO: move reset() contents here, reset() just clears the data + reset(); } // zero volume, leave rest params the same void HashTSDFVolumeGPU::reset() { CV_TRACE_FUNCTION(); + degree = 15; buff_lvl = (int) pow(2, degree); @@ -1016,7 +1016,6 @@ void HashTSDFVolumeGPU::reset() isActiveFlags = cv::UMat(buff_lvl, 1, CV_8U); - //indexes = VolumesTable(); hashMap = ToyHashMap(); frameParams = Vec6f(); @@ -1139,85 +1138,9 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto // for new indices ToyHashMap thm; - /* ----------------------- - - String errorStr; - String name = "toy_alloc"; - - ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; - String options = "-cl-mad-enable"; - ocl::Kernel k; - k.create(name.c_str(), source, options, &errorStr); - - if (k.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); - - ToyHashMap ths; - UMat newSetHashes(ths.hashDivisor, 1, CV_32S); - Mat(ths.hashes, false).copyTo(newSetHashes); - int dt = CV_MAKETYPE(CV_32S, 8); - UMat newSetData(ths.capacity, 1, dt); - Mat(ths.data, false).copyTo(newSetData); - UMat newSetLast(1, 1, CV_32S); - newSetLast = ths.last; - - k.args(ocl::KernelArg::PtrReadWrite(newSetHashes), - ocl::KernelArg::PtrReadWrite(newSetData), - ocl::KernelArg::PtrReadWrite(newSetLast), - ths.capacity, - ths.hashDivisor, - - - - ); - - size_t globalSize[2]; - globalSize[0] = divUp(depth.cols, depthStride); - globalSize[1] = divUp(depth.rows, depthStride); - - if (!k.run(2, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); - - newSetHashes.copyTo(Mat(ths.hashes, false)); - newSetData.copyTo(Mat(ths.data, false)); - - //TODO HUGE: add to globalMap instead - - ------------------------------------------- */ - /* - __global int* newSetHashes, - __global int8* newSetData, - __global int* newSetLast, - const int newSetCapacity, - const int newSetHashDivisor, - - __global const int* globalMapHashes, - __global const int8* globalMapData, - const int globalMapHashDivisor, - - __global const char * depthPtr, - int depthStep, int depthOffset, - int depthRows, int depthCols, - - const int depthStride, - const float invDepthFactor, - const float truncateThreshold, - - const float2 fixy, const float2 cxy, - - const float16 cam2vol, - const float volumeUnitSizeInv, - const float truncDist, - - __global volatile int* hsMutex, // set to 0 initially - __global int* full // set to 0 initially - - */ - - // ----------------------- - auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, ToyHashMap& ghm/*LocalVolUnits& localAccessVolUnits, int& locVolIdx*/) + auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, ToyHashMap& ghm) { for (int y = yrange.start; y < yrange.end; y += depthStride) { @@ -1357,29 +1280,6 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto //mutex.unlock(); } - - /* - if (loc_vol_idx > 0) - { - //DEBUG - //std::cout << "loc_vol_idx: " << loc_vol_idx << std::endl; - - mutex.lock(); - for (int i = 0; i < loc_vol_idx; i++) - { - Vec3i idx = localAccessVolUnits[i]; - - // if not found - if (indexes.findRow(idx) < 0) - { - Volume_NODE* node = indexes.insert(idx, lastVolIndex); - nodePtrs.push_back(node); - lastVolIndex++; - } - } - mutex.unlock(); - } - */ } } @@ -1389,11 +1289,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto { if (needReallocation) { - /* - std::cout << "reallocation!! from: " << hashMap.capacity << " to x2: " << hashMap.capacity * 2 << std::endl; - hashMap.capacity *= 2; - hashMap.data.resize(hashMap.capacity); - */ + //DEBUG std::cout << "reallocation group!! from: " << thm.capacity << " to x2: " << thm.capacity * 2 << std::endl; thm.capacity *= 2; thm.data.resize(thm.capacity); @@ -1402,7 +1298,6 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto } parallel_for_(Range(0, gg.height), allocateLambda); - //allocateLambda(Range(0, gg.height)); } while (needReallocation); @@ -1426,44 +1321,73 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto } } }; - /* - String errorStr; - String name = "push_to_global"; - - ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; - String options = "-cl-mad-enable"; - ocl::Kernel k; - k.create(name.c_str(), source, options, &errorStr); - - if (k.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); - ToyHashMap ths; - UMat newSetHashes(ths.hashDivisor, 1, CV_32S); - Mat(ths.hashes, false).copyTo(newSetHashes); - int dt = CV_MAKETYPE(CV_32S, 8); - UMat newSetData(ths.capacity, 1, dt); - Mat(ths.data, false).copyTo(newSetData); - UMat newSetLast(1, 1, CV_32S); - newSetLast = ths.last; - - k.args(ocl::KernelArg::PtrReadWrite(newSetHashes), - ocl::KernelArg::PtrReadWrite(newSetData), - ocl::KernelArg::PtrReadWrite(newSetLast), - ths.capacity, - ths.hashDivisor, - - ); - - size_t globalSize[1]; - globalSize[0] = thm.last; + //TODO: remove it + auto pushToGlobalGpu = [](const ToyHashMap thm, ToyHashMap& globalHashMap, bool& needReallocation) + { + //TODO: set needReallocation based on thm.last + globalHashMap.last < globalHashMap.capacity + + String errorStr; + String name = "push_to_global"; + + ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; + String options = "-cl-mad-enable"; + ocl::Kernel k; + k.create(name.c_str(), source, options, &errorStr); + + if (k.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); + + // new hash table + UMat newHashesGpu(thm.hashDivisor, 1, CV_32S); + Mat(thm.hashes, false).copyTo(newHashesGpu); + UMat newHashDataGpu(thm.capacity, 1, CV_32SC4); + Mat(thm.data, false).copyTo(newHashDataGpu); + + // global hash map + UMat globalHashesGpu(globalHashMap.hashDivisor, 1, CV_32S); + Mat(globalHashMap.hashes, false).copyTo(globalHashesGpu); + UMat globalHashDataGpu(globalHashMap.capacity, 1, CV_32SC4); + Mat(globalHashMap.data, false).copyTo(globalHashDataGpu); + UMat globalHashLast(1, 1, CV_32S, Scalar(globalHashMap.last)); + + UMat hashMapLock(1, 1, CV_32S, Scalar(0)); + UMat needReallocationGpu(1, 1, CV_32S, Scalar(needReallocation)); + + k.args(ocl::KernelArg::PtrReadOnly(newHashesGpu), + ocl::KernelArg::PtrReadOnly(newHashDataGpu), + (int)thm.hashDivisor, + (int)thm.last, + ocl::KernelArg::PtrReadWrite(hashMapLock), + ocl::KernelArg::PtrReadWrite(needReallocationGpu), + ocl::KernelArg::PtrReadWrite(globalHashesGpu), + ocl::KernelArg::PtrReadWrite(globalHashDataGpu), + ocl::KernelArg::PtrReadWrite(globalHashLast), + globalHashMap.capacity, + globalHashMap.hashDivisor); + + size_t globalSize[1]; + //DEBUG + globalSize[0] = thm.last; + globalSize[0] = 64; + + if (!k.run(1, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); + + globalHashesGpu.copyTo(Mat(globalHashMap.hashes, false)); + globalHashDataGpu.copyTo(Mat(globalHashMap.data, false)); + globalHashMap.last = globalHashLast.getMat(ACCESS_READ).at(0, 0); + + needReallocation = (bool)needReallocationGpu.getMat(ACCESS_READ).at(0, 0); + + //DEBUG + if (needReallocation) + { + std::cout << "need reallocation" << std::endl; + } + }; - if (!k.run(2, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); - newSetHashes.copyTo(Mat(ths.hashes, false)); - newSetData.copyTo(Mat(ths.data, false)); - */ needReallocation = false; do @@ -1478,6 +1402,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto } pushToGlobal(thm, hashMap, needReallocation, mutex); + //pushToGlobalGpu(thm, hashMap, needReallocation); } while (needReallocation); // --------------------- @@ -1596,6 +1521,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //TODO: replace .ptr<...> everywhere by .at<...> +//TODO: remove these functions when everything is done on GPU cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const { return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), @@ -1897,14 +1823,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in ocl::KernelArg::PtrReadOnly(hashesGpu), ocl::KernelArg::PtrReadOnly(hashDataGpu), (int)hashMap.hashDivisor, - /* - ocl::KernelArg::PtrReadOnly(indexes.volumes.getUMat(ACCESS_RW)), - (int)indexes.list_size, - (int)indexes.bufferNums, - (int)indexes.hash_divisor, - */ - - ocl::KernelArg::WriteOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), frameSize, From f5c0e1760690146eb39ca16c8eae3f3bd5543a7a Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 9 Feb 2021 02:17:02 +0300 Subject: [PATCH 177/216] ptr -> at --- modules/rgbd/src/hash_tsdf.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index e0640bcbdc2..6db216c7eba 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1030,9 +1030,9 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f UMat pixNorm(depth_rows, depth_cols, CV_32F); for (int i = 0; i < depth_cols; i++) - *x.ptr(0, i) = (i - cxy[0]) / fxy[0]; + x.at(i) = (i - cxy[0]) / fxy[0]; for (int i = 0; i < depth_rows; i++) - *y.ptr(0, i) = (i - cxy[1]) / fxy[1]; + y.at(i) = (i - cxy[1]) / fxy[1]; cv::String errorStr; cv::String name = "preCalculationPixNorm"; @@ -1519,7 +1519,6 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma integrateAllVolumeUnitsGPU(depth, depthFactor, cameraPose, intrinsics); } -//TODO: replace .ptr<...> everywhere by .at<...> //TODO: remove these functions when everything is done on GPU cv::Vec3i HashTSDFVolumeGPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const @@ -1972,7 +1971,7 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con //! TODO: Iterate over map parallely? for (int i = 0; i < hashMap.last; i++) { - if (*cpuIndices.ptr(i) > (currFrameId - frameThreshold)) + if (cpuIndices.at(i) > (currFrameId - frameThreshold)) numVisibleBlocks++; } return numVisibleBlocks; From 1a65ad8f3445193cbc598563d6ac621cb58208b4 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 9 Feb 2021 03:14:43 +0300 Subject: [PATCH 178/216] volumeUnitResolution is limited to a power of 2 --- modules/rgbd/src/hash_tsdf.cpp | 68 ++++++++++++++-------------- modules/rgbd/src/hash_tsdf.hpp | 1 + modules/rgbd/src/opencl/hash_tsdf.cl | 34 +++++++------- 3 files changed, 50 insertions(+), 53 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 6db216c7eba..cffc1eaf6d2 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -36,6 +36,16 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca zFirstMemOrder(_zFirstMemOrder) { truncDist = std::max(_truncDist, 4.0f * voxelSize); + + if (!(volumeUnitResolution & (volumeUnitResolution - 1))) + { + // vuRes is a power of 2, let's get this power + volumeUnitDegree = trailingZeros32(volumeUnitResolution); + } + else + { + CV_Error(Error::StsBadArg, "Volume unit resolution should be a power of 2"); + } } //! Spatial hashing @@ -165,7 +175,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma Depth depth = _depth.getMat(); //! Compute volumes to be allocated - const int depthStride = int(log2(volumeUnitResolution)); + const int depthStride = volumeUnitDegree; const float invDepthFactor = 1.f / depthFactor; const Intr::Reprojector reproj(intrinsics.makeReprojector()); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); @@ -352,9 +362,9 @@ inline TsdfVoxel HashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, int indx) co inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { - Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), - cvFloor(volumeIdx[1] / volumeUnitResolution), - cvFloor(volumeIdx[2] / volumeUnitResolution)); + Vec3i volumeUnitIdx = Vec3i(volumeIdx[0] >> volumeUnitDegree, + volumeIdx[1] >> volumeUnitDegree, + volumeIdx[2] >> volumeUnitDegree); VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); @@ -362,10 +372,10 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { return TsdfVoxel(floatToTsdf(1.f), 0); } - - cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, - volumeUnitIdx[1] * volumeUnitResolution, - volumeUnitIdx[2] * volumeUnitResolution); + + cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] << volumeUnitDegree, + volumeUnitIdx[1] << volumeUnitDegree, + volumeUnitIdx[2] << volumeUnitDegree); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); @@ -375,7 +385,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const TsdfVoxel HashTSDFVolumeCPU::at(const Point3f& point) const { - cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); + cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); if (it == volumeUnits.end()) @@ -390,29 +400,15 @@ TsdfVoxel HashTSDFVolumeCPU::at(const Point3f& point) const return _at(volUnitLocalIdx, it->second.index); } -static inline Vec3i voxelToVolumeUnitIdx(const Vec3i& pt, const int vuRes) -{ - if (!(vuRes & (vuRes - 1))) - { - // vuRes is a power of 2, let's get this power - const int p2 = trailingZeros32(vuRes); - return Vec3i(pt[0] >> p2, pt[1] >> p2, pt[2] >> p2); - } - else - { - return Vec3i(cvFloor(float(pt[0]) / vuRes), - cvFloor(float(pt[1]) / vuRes), - cvFloor(float(pt[2]) / vuRes)); - } -} - TsdfVoxel HashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const { if (it == volumeUnits.end()) { return TsdfVoxel(floatToTsdf(1.f), 0); } - Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; + Vec3i volUnitLocalIdx = point - Vec3i(volumeUnitIdx[0] << volumeUnitDegree, + volumeUnitIdx[1] << volumeUnitDegree, + volumeUnitIdx[2] << volumeUnitDegree); // expanding at(), removing bounds check const TsdfVoxel* volData = volUnitsData.ptr(it->second.index); @@ -482,7 +478,7 @@ float HashTSDFVolumeCPU::interpolateVoxelPoint(const Point3f& point) const { Vec3i pt = iv + neighbourCoords[i]; - Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); + Vec3i volumeUnitIdx = Vec3i(pt[0] >> volumeUnitDegree, pt[1] >> volumeUnitDegree, pt[2] >> volumeUnitDegree); int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; auto it = iterMap[dictIdx]; if (!queried[dictIdx]) @@ -544,7 +540,7 @@ Point3f HashTSDFVolumeCPU::getNormalVoxel(const Point3f &point) const { Vec3i pt = iptVox + offsets[i]; - Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); + Vec3i volumeUnitIdx = Vec3i(pt[0] >> volumeUnitDegree, pt[1] >> volumeUnitDegree, pt[2] >> volumeUnitDegree); int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; auto it = iterMap[dictIdx]; @@ -1110,8 +1106,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept int resol = volumeUnitResolution; size_t globalSize[3]; - globalSize[0] = (size_t)resol; // volumeUnitResolution - globalSize[1] = (size_t)resol; // volumeUnitResolution + globalSize[0] = (size_t)resol; + globalSize[1] = (size_t)resol; globalSize[2] = (size_t)hashMap.last; // num of volume units if (!k.run(3, globalSize, NULL, true)) @@ -1128,7 +1124,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto Depth depth = _depth.getMat(ACCESS_READ); //! Compute volumes to be allocated - const int depthStride = int(log2(volumeUnitResolution)); + const int depthStride = volumeUnitDegree; const float invDepthFactor = 1.f / depthFactor; const Intr::Reprojector reproj(intrinsics.makeReprojector()); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); @@ -1568,7 +1564,9 @@ TsdfVoxel HashTSDFVolumeGPU::new_atVolumeUnit(const Vec3i& point, const Vec3i& v { return TsdfVoxel(floatToTsdf(1.f), 0); } - Vec3i volUnitLocalIdx = point - volumeUnitIdx * volumeUnitResolution; + Vec3i volUnitLocalIdx = point - Vec3i(volumeUnitIdx[0] << volumeUnitDegree, + volumeUnitIdx[1] << volumeUnitDegree, + volumeUnitIdx[2] << volumeUnitDegree); // expanding at(), removing bounds check const TsdfVoxel* volData = volUnitsDataCopy.ptr(indx); @@ -1607,7 +1605,7 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const { Vec3i pt = iv + local_neighbourCoords[i]; - Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); + Vec3i volumeUnitIdx = Vec3i(pt[0] >> volumeUnitDegree, pt[1] >> volumeUnitDegree, pt[2] >> volumeUnitDegree); int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; auto it = iterMap[dictIdx]; if (it < -1) @@ -1669,7 +1667,7 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const { Vec3i pt = iptVox + offsets[i]; - Vec3i volumeUnitIdx = voxelToVolumeUnitIdx(pt, volumeUnitResolution); + Vec3i volumeUnitIdx = Vec3i(pt[0] >> volumeUnitDegree, pt[1] >> volumeUnitDegree, pt[2] >> volumeUnitDegree); int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; auto it = iterMap[dictIdx]; @@ -1837,7 +1835,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in voxelSizeInv, volumeUnitSize, volume.truncDist, - volumeUnitResolution, + volumeUnitDegree, volStrides ); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index a6420266140..50502cf2556 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -34,6 +34,7 @@ class HashTSDFVolume : public Volume float truncDist; float truncateThreshold; int volumeUnitResolution; + int volumeUnitDegree; float volumeUnitSize; bool zFirstMemOrder; }; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 4817452f9c0..62d99d52ec9 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -344,14 +344,13 @@ __kernel void integrateAllVolumeUnits( } -static struct TsdfVoxel at(int3 volumeIdx, int row, int volumeUnitResolution, +static struct TsdfVoxel at(int3 volumeIdx, int row, int volumeUnitDegree, int3 volStrides, __global const struct TsdfVoxel * allVolumePtr, int table_offset) { //! Out of bounds - if (any(volumeIdx >= volumeUnitResolution) || + if (any(volumeIdx >= (1 << volumeUnitDegree)) || any(volumeIdx < 0)) - { struct TsdfVoxel dummy; dummy.tsdf = floatToTsdf(1.0f); @@ -359,7 +358,7 @@ static struct TsdfVoxel at(int3 volumeIdx, int row, int volumeUnitResolution, return dummy; } - int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; + int volCubed = 1 << (volumeUnitDegree*3); __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) (allVolumePtr + table_offset + row * volCubed); int3 ismul = volumeIdx * volStrides; @@ -369,7 +368,7 @@ static struct TsdfVoxel at(int3 volumeIdx, int row, int volumeUnitResolution, static struct TsdfVoxel atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row, - int volumeUnitResolution, int3 volStrides, + int volumeUnitDegree, int3 volStrides, __global const struct TsdfVoxel * allVolumePtr, int table_offset) { @@ -382,8 +381,8 @@ static struct TsdfVoxel atVolumeUnit(int3 volumeIdx, int3 volumeUnitIdx, int row return dummy; } - int3 volUnitLocalIdx = volumeIdx - volumeUnitIdx * volumeUnitResolution; - int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; + int3 volUnitLocalIdx = volumeIdx - (volumeUnitIdx << volumeUnitDegree); + int volCubed = 1 << (volumeUnitDegree*3); __global struct TsdfVoxel * volData = (__global struct TsdfVoxel*) (allVolumePtr + table_offset + row * volCubed); int3 ismul = volUnitLocalIdx * volStrides; @@ -399,7 +398,7 @@ inline float interpolate(float3 t, float8 vz) } inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolumePtr, - int volumeUnitResolution, + int volumeUnitDegree, float voxelSizeInv, /* @@ -453,9 +452,8 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum { int3 pt = iptVox + offsets[i]; - // VoxelToVolumeUnitIdx() - // TODO: add assertion - if (!(vuRes & (vuRes - 1))) - int3 volumeUnitIdx = convert_int3(floor(convert_float3(pt.s012) / (float)(volumeUnitResolution))); + // VoxelToVolumeUnitIdx() + int3 volumeUnitIdx = pt >> volumeUnitDegree; int3 vand = (volumeUnitIdx & 1); int dictIdx = vand.s0 + vand.s1 * 2 + vand.s2 * 4; @@ -468,7 +466,7 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum iterMap[dictIdx] = it; } - struct TsdfVoxel tmp = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnitResolution, volStrides, allVolumePtr, table_offset); + struct TsdfVoxel tmp = atVolumeUnit(pt, volumeUnitIdx, it, volumeUnitDegree, volStrides, allVolumePtr, table_offset); vals[i] = tsdfToFloat( tmp.tsdf ); } @@ -555,7 +553,7 @@ __kernel void raycast( const float voxelSizeInv, float volumeUnitSize, float truncDist, - int volumeUnitResolution, + int volumeUnitDegree, int4 volStrides4 ) { @@ -599,8 +597,8 @@ __kernel void raycast( float3 currRayPos = orig + tcurr * dir; // VolumeToVolumeUnitIdx() - float3 currVolUnitIdxF = floor(currRayPos / volumeUnitSize); - int3 currVolumeUnitIdx = convert_int3(currVolUnitIdxF); + int3 currVoxel = convert_int3(floor(currRayPos * voxelSizeInv)); + int3 currVolumeUnitIdx = currVoxel >> volumeUnitDegree; //int row = findRow(hash_table, currVolumeUnitIdx, list_size, bufferNums, hash_divisor); int row = toy_find(currVolumeUnitIdx, hash_divisor, hashes, data); @@ -612,8 +610,8 @@ __kernel void raycast( if (row >= 0) { - volUnitLocalIdx = convert_int3(currRayPos*voxelSizeInv - currVolUnitIdxF*(float)volumeUnitResolution); - struct TsdfVoxel currVoxel = at(volUnitLocalIdx, row, volumeUnitResolution, volStrides, allVolumePtr, table_offset); + volUnitLocalIdx = currVoxel - (currVolumeUnitIdx << volumeUnitDegree); + struct TsdfVoxel currVoxel = at(volUnitLocalIdx, row, volumeUnitDegree, volStrides, allVolumePtr, table_offset); currTsdf = tsdfToFloat(currVoxel.tsdf); currWeight = currVoxel.weight; @@ -626,7 +624,7 @@ __kernel void raycast( if ( !isnan(tInterp) && !isinf(tInterp) ) { float3 pv = orig + tInterp * dir; - float3 nv = getNormalVoxel( pv, allVolumePtr, volumeUnitResolution, + float3 nv = getNormalVoxel( pv, allVolumePtr, volumeUnitDegree, voxelSizeInv, //hash_table, list_size, bufferNums, hash_divisor, hash_divisor, hashes, data, From 97cc435e35c3c82fc97a04bbe573b22d5e44a5f3 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 9 Feb 2021 03:30:16 +0300 Subject: [PATCH 179/216] rename ToyHashMap to ToyHashSet, hashMap to hashTable --- modules/rgbd/src/hash_tsdf.cpp | 89 +++++++++++++--------------- modules/rgbd/src/opencl/hash_tsdf.cl | 2 +- modules/rgbd/src/tsdf_functions.hpp | 9 ++- 3 files changed, 47 insertions(+), 53 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index cffc1eaf6d2..7fcd09cce9a 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -903,7 +903,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; - size_t getTotalVolumeUnits() const override { return size_t(hashMap.last); } + size_t getTotalVolumeUnits() const override { return size_t(hashTable.last); } int getVisibleBlocks(int currFrameId, int frameThreshold) const override; @@ -945,8 +945,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::UMat pixNorms; //TODO: move indexes.volumes to GPU - //VolumesTable indexes; - ToyHashMap hashMap; + ToyHashSet hashTable; Vec8i neighbourCoords; }; @@ -1012,7 +1011,7 @@ void HashTSDFVolumeGPU::reset() isActiveFlags = cv::UMat(buff_lvl, 1, CV_8U); - hashMap = ToyHashMap(); + hashTable = ToyHashSet(); frameParams = Vec6f(); pixNorms = UMat(); @@ -1079,16 +1078,16 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept Matx44f vol2camMatrix = (Affine3f(cameraPose).inv() * pose).matrix; Matx44f camInvMatrix = Affine3f(cameraPose).inv().matrix; - UMat hashesGpu(hashMap.hashDivisor, 1, CV_32S); - Mat(hashMap.hashes, false).copyTo(hashesGpu); + UMat hashesGpu(hashTable.hashDivisor, 1, CV_32S); + Mat(hashTable.hashes, false).copyTo(hashesGpu); - UMat hashDataGpu(hashMap.capacity, 1, CV_32SC4); - Mat(hashMap.data, false).copyTo(hashDataGpu); + UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); + Mat(hashTable.data, false).copyTo(hashDataGpu); k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadOnly(hashesGpu), ocl::KernelArg::PtrReadOnly(hashDataGpu), - (int)hashMap.hashDivisor, + (int)hashTable.hashDivisor, ocl::KernelArg::ReadWrite(volUnitsData), ocl::KernelArg::ReadOnly(pixNorms), ocl::KernelArg::ReadOnly(isActiveFlags), @@ -1108,7 +1107,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept size_t globalSize[3]; globalSize[0] = (size_t)resol; globalSize[1] = (size_t)resol; - globalSize[2] = (size_t)hashMap.last; // num of volume units + globalSize[2] = (size_t)hashTable.last; // num of volume units if (!k.run(3, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); @@ -1132,11 +1131,11 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto Mutex mutex; // for new indices - ToyHashMap thm; + ToyHashSet thm; // ----------------------- - auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, ToyHashMap& ghm) + auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, ToyHashSet& ghm) { for (int y = yrange.start; y < yrange.end; y += depthStride) { @@ -1160,8 +1159,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto { const Vec3i tsdf_idx = Vec3i(i, j, k); - //if (indexes.findRow(tsdf_idx) < 0) - if (hashMap.find(tsdf_idx) < 0) + if (hashTable.find(tsdf_idx) < 0) { bool found = false; for (int i = 0; i < pixLocalCounter; i++) @@ -1221,7 +1219,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto int loc_vol_idx = 0; */ - ToyHashMap ghm; + ToyHashSet ghm; fillLocalAcessVolUnits(xr, yr, ghm /*localAccessVolUnits, loc_vol_idx*/); @@ -1297,7 +1295,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto } while (needReallocation); - auto pushToGlobal = [](const ToyHashMap thm, ToyHashMap& globalHashMap, + auto pushToGlobal = [](const ToyHashSet thm, ToyHashSet& globalHashMap, bool& needReallocation, Mutex& mutex) { for (int i = 0; i < thm.last; i++) @@ -1319,7 +1317,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto }; //TODO: remove it - auto pushToGlobalGpu = [](const ToyHashMap thm, ToyHashMap& globalHashMap, bool& needReallocation) + auto pushToGlobalGpu = [](const ToyHashSet thm, ToyHashSet& globalHashMap, bool& needReallocation) { //TODO: set needReallocation based on thm.last + globalHashMap.last < globalHashMap.capacity @@ -1383,22 +1381,21 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto } }; - - needReallocation = false; do { if (needReallocation) { - std::cout << "reallocation global!! from: " << hashMap.capacity << " to x2: " << hashMap.capacity * 2 << std::endl; - hashMap.capacity *= 2; - hashMap.data.resize(hashMap.capacity); + //DEBUG + std::cout << "reallocation global!! from: " << hashTable.capacity << " to x2: " << hashTable.capacity * 2 << std::endl; + hashTable.capacity *= 2; + hashTable.data.resize(hashTable.capacity); needReallocation = false; } - pushToGlobal(thm, hashMap, needReallocation, mutex); - //pushToGlobalGpu(thm, hashMap, needReallocation); + pushToGlobal(thm, hashTable, needReallocation, mutex); + //pushToGlobalGpu(thm, hashTable, needReallocation); } while (needReallocation); // --------------------- @@ -1422,16 +1419,16 @@ void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrin const Intr::Projector proj(intrinsics.makeProjector()); Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); - UMat hashesGpu(hashMap.hashDivisor, 1, CV_32S); - Mat(hashMap.hashes, false).copyTo(hashesGpu); + UMat hashesGpu(hashTable.hashDivisor, 1, CV_32S); + Mat(hashTable.hashes, false).copyTo(hashesGpu); - UMat hashDataGpu(hashMap.capacity, 1, CV_32SC4); - Mat(hashMap.data, false).copyTo(hashDataGpu); + UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); + Mat(hashTable.data, false).copyTo(hashDataGpu); k.args( ocl::KernelArg::PtrReadOnly(hashesGpu), ocl::KernelArg::PtrReadOnly(hashDataGpu), - (int)hashMap.hashDivisor, + (int)hashTable.hashDivisor, ocl::KernelArg::WriteOnly(isActiveFlags), ocl::KernelArg::WriteOnly(lastVisibleIndices), vol2cam.matrix, @@ -1439,12 +1436,12 @@ void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrin cxy, frameSz, volumeUnitSize, - hashMap.last, + hashTable.last, truncateThreshold, frameId ); - size_t globalSize[1] = { (size_t)hashMap.last }; + size_t globalSize[1] = { (size_t)hashTable.last }; if (!k.run(1, globalSize, nullptr, true)) throw std::runtime_error("Failed to run kernel"); } @@ -1458,9 +1455,9 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma UMat depth = _depth.getUMat(); // Save length to fill new data in ranges - int sizeBefore = hashMap.last; + int sizeBefore = hashTable.last; allocateVolumeUnits(depth, depthFactor, cameraPose, intrinsics); - int sizeAfter = hashMap.last; + int sizeAfter = hashTable.last; //! Perform the allocation // Grow buffers @@ -1610,8 +1607,7 @@ float HashTSDFVolumeGPU::interpolateVoxelPoint(const Point3f& point) const auto it = iterMap[dictIdx]; if (it < -1) { - //it = indexes.findRow(volumeUnitIdx); - it = hashMap.find(volumeUnitIdx); + it = hashTable.find(volumeUnitIdx); iterMap[dictIdx] = it; } @@ -1673,8 +1669,7 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const auto it = iterMap[dictIdx]; if (it < -1) { - //it = indexes.findRow(volumeUnitIdx); - it = hashMap.find(volumeUnitIdx); + it = hashTable.find(volumeUnitIdx); iterMap[dictIdx] = it; } @@ -1759,8 +1754,8 @@ Point3f HashTSDFVolumeGPU::getNormalVoxel(const Point3f& point) const normal[2] = interpolate(tx, ty, tz, czv); #endif float nv = sqrt(normal[0] * normal[0] + - normal[1] * normal[1] + - normal[2] * normal[2]); + normal[1] * normal[1] + + normal[2] * normal[2]); return nv < 0.0001f ? nan3 : normal / nv; } @@ -1810,16 +1805,16 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Mat(pose.matrix).copyTo(volPoseGpu); Mat(pose.inv().matrix).copyTo(invPoseGpu); - UMat hashesGpu(hashMap.hashDivisor, 1, CV_32S); - Mat(hashMap.hashes, false).copyTo(hashesGpu); + UMat hashesGpu(hashTable.hashDivisor, 1, CV_32S); + Mat(hashTable.hashes, false).copyTo(hashesGpu); - UMat hashDataGpu(hashMap.capacity, 1, CV_32SC4); - Mat(hashMap.data, false).copyTo(hashDataGpu); + UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); + Mat(hashTable.data, false).copyTo(hashDataGpu); k.args( ocl::KernelArg::PtrReadOnly(hashesGpu), ocl::KernelArg::PtrReadOnly(hashDataGpu), - (int)hashMap.hashDivisor, + (int)hashTable.hashDivisor, ocl::KernelArg::WriteOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), frameSize, @@ -1862,7 +1857,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor std::vector> pVecs, nVecs; - Range _fetchRange(0, hashMap.last); + Range _fetchRange(0, hashTable.last); const int nstripes = -1; @@ -1875,7 +1870,7 @@ void HashTSDFVolumeGPU::fetchPointsNormals(OutputArray _points, OutputArray _nor std::vector points, normals; for (int row = range.start; row < range.end; row++) { - cv::Vec4i idx4 = hashMap.data[row]; + cv::Vec4i idx4 = hashTable.data[row]; cv::Vec3i idx(idx4[0], idx4[1], idx4[2]); Point3f base_point = volume.volumeUnitIdxToVolume(idx); @@ -1967,7 +1962,7 @@ int HashTSDFVolumeGPU::getVisibleBlocks(int currFrameId, int frameThreshold) con int numVisibleBlocks = 0; //! TODO: Iterate over map parallely? - for (int i = 0; i < hashMap.last; i++) + for (int i = 0; i < hashTable.last; i++) { if (cpuIndices.at(i) > (currFrameId - frameThreshold)) numVisibleBlocks++; diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 62d99d52ec9..0eaff549cf0 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -84,6 +84,7 @@ static int findRow(__global const struct Volume_NODE * hash_table, int3 indx, } //TODO: make hashDivisor a power of 2 +//TODO: put it to this .cl file as a constant static int toy_find(int3 idx, const int hashDivisor, __global const int* hashes, __global const int4* data) { @@ -461,7 +462,6 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum int it = iterMap[dictIdx]; if (it < -1) { - //it = findRow(hash_table, volumeUnitIdx, list_size, bufferNums, hash_divisor); it = toy_find(volumeUnitIdx, hash_divisor, hashes, data); iterMap[dictIdx] = it; } diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index f37f01e57b3..9aeda1b8356 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -96,12 +96,11 @@ class VolumesTable }; -//TODO: hash set, not hash map -class ToyHashMap +class ToyHashSet { public: static const int hashDivisor = 32768; - static const int startCapacity = 1024; // 32768*4; + static const int startCapacity = 16384; std::vector hashes; // 0-3 for key, 4th for internal use @@ -110,7 +109,7 @@ class ToyHashMap int capacity; int last; - ToyHashMap() + ToyHashSet() { hashes.resize(hashDivisor); for (int i = 0; i < hashDivisor; i++) @@ -124,7 +123,7 @@ class ToyHashMap last = 0; } - ~ToyHashMap() { } + ~ToyHashSet() { } inline size_t calc_hash(Vec3i x) const { From 0a168b6f58fffc81f5e0bc77531535c5ee4ab398 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Tue, 9 Feb 2021 03:34:21 +0300 Subject: [PATCH 180/216] minor --- 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 9aeda1b8356..3ebba881140 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -100,7 +100,7 @@ class ToyHashSet { public: static const int hashDivisor = 32768; - static const int startCapacity = 16384; + static const int startCapacity = 1024; std::vector hashes; // 0-3 for key, 4th for internal use From 53f91d25610d45d756bd672a6eca1493331794ea Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 10 Feb 2021 15:19:33 +0300 Subject: [PATCH 181/216] getNormalVoxel .cl: no float conversions --- modules/rgbd/src/opencl/hash_tsdf.cl | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 0eaff549cf0..0de918e3e0f 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -416,13 +416,14 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum { float3 normal = (float3) (0.0f, 0.0f, 0.0f); float3 ptVox = p * voxelSizeInv; - int3 iptVox = convert_int3(floor(ptVox)); + float3 fip = floor(ptVox); + int3 iptVox = convert_int3(fip); // A small hash table to reduce a number of findRow() calls // -2 and lower means not queried yet // -1 means not found // 0+ means found - int iterMap[8]; + int iterMap[8]; for (int i = 0; i < 8; i++) { iterMap[i] = -2; @@ -486,12 +487,12 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum // where each digit corresponds to shift by x, y, z axis respectively. // 2. Add +1 for next or -1 for prev to each coordinate to corresponding axis // 3. Search corresponding values in offsets - const int idxxp[8] = { 8, 9, 10, 11, 0, 1, 2, 3 }; - const int idxxn[8] = { 4, 5, 6, 7, 12, 13, 14, 15 }; - const int idxyp[8] = { 16, 17, 0, 1, 18, 19, 4, 5 }; - const int idxyn[8] = { 2, 3, 20, 21, 6, 7, 22, 23 }; - const int idxzp[8] = { 24, 0, 25, 2, 26, 4, 27, 6 }; - const int idxzn[8] = { 1, 28, 3, 29, 5, 30, 7, 31 }; + const int idxxn[8] = { 8, 9, 10, 11, 0, 1, 2, 3 }; + const int idxxp[8] = { 4, 5, 6, 7, 12, 13, 14, 15 }; + const int idxyn[8] = { 16, 17, 0, 1, 18, 19, 4, 5 }; + const int idxyp[8] = { 2, 3, 20, 21, 6, 7, 22, 23 }; + const int idxzn[8] = { 24, 0, 25, 2, 26, 4, 27, 6 }; + const int idxzp[8] = { 1, 28, 3, 29, 5, 30, 7, 31 }; float vcxp[8], vcxn[8]; float vcyp[8], vcyn[8]; @@ -507,11 +508,11 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum float8 cxp = vload8(0, vcxp), cxn = vload8(0, vcxn); float8 cyp = vload8(0, vcyp), cyn = vload8(0, vcyn); float8 czp = vload8(0, vczp), czn = vload8(0, vczn); - float8 cx = cxn - cxp; - float8 cy = cyn - cyp; - float8 cz = czn - czp; + float8 cx = cxp - cxn; + float8 cy = cyp - cyn; + float8 cz = czp - czn; - float3 tv = ptVox - convert_float3(iptVox); + float3 tv = ptVox - fip; normal.x = interpolate(tv, cx); normal.y = interpolate(tv, cy); normal.z = interpolate(tv, cz); From 7a7f04e9dd75d545f0c1edfe670eedf3e13d937e Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 10 Feb 2021 15:32:09 +0300 Subject: [PATCH 182/216] getNormalVoxel does not need voxelSizeInv anymore; orig and dir pre-scaled to get rid of extra multiplication --- modules/rgbd/src/opencl/hash_tsdf.cl | 25 ++++++++----------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 0de918e3e0f..4d0e7d4afd1 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -398,16 +398,8 @@ inline float interpolate(float3 t, float8 vz) return mix(vx.s0, vx.s1, t.x); } -inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolumePtr, +inline float3 getNormalVoxel(float3 ptVox, __global const struct TsdfVoxel* allVolumePtr, int volumeUnitDegree, - float voxelSizeInv, - - /* - const __global struct Volume_NODE * hash_table, - const int list_size, - const int bufferNums, - const int hash_divisor, - */ const int hash_divisor, __global const int* hashes, __global const int4* data, @@ -415,7 +407,6 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* allVolum int3 volStrides, int table_offset) { float3 normal = (float3) (0.0f, 0.0f, 0.0f); - float3 ptVox = p * voxelSizeInv; float3 fip = floor(ptVox); int3 iptVox = convert_int3(fip); @@ -584,6 +575,8 @@ __kernel void raycast( float3 orig = (float3) (cam2volTransGPU.s0, cam2volTransGPU.s1, cam2volTransGPU.s2); float3 dir = fast_normalize(planed); + float3 origScaled = orig * voxelSizeInv; + float3 dirScaled = dir * voxelSizeInv; float tmin = 0; float tmax = truncateThreshold; @@ -595,13 +588,12 @@ __kernel void raycast( while (tcurr < tmax) { - float3 currRayPos = orig + tcurr * dir; + float3 currRayPosVox = origScaled + tcurr * dirScaled; // VolumeToVolumeUnitIdx() - int3 currVoxel = convert_int3(floor(currRayPos * voxelSizeInv)); + int3 currVoxel = convert_int3(floor(currRayPosVox)); int3 currVolumeUnitIdx = currVoxel >> volumeUnitDegree; - //int row = findRow(hash_table, currVolumeUnitIdx, list_size, bufferNums, hash_divisor); int row = toy_find(currVolumeUnitIdx, hash_divisor, hashes, data); float currTsdf = prevTsdf; @@ -624,10 +616,8 @@ __kernel void raycast( float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if ( !isnan(tInterp) && !isinf(tInterp) ) { - float3 pv = orig + tInterp * dir; - float3 nv = getNormalVoxel( pv, allVolumePtr, volumeUnitDegree, - voxelSizeInv, - //hash_table, list_size, bufferNums, hash_divisor, + float3 pvox = origScaled + tInterp * dirScaled; + float3 nv = getNormalVoxel( pvox, allVolumePtr, volumeUnitDegree, hash_divisor, hashes, data, volStrides, table_offset); @@ -638,6 +628,7 @@ __kernel void raycast( dot(nv, volRot1), dot(nv, volRot2)); // interpolation optimized a little + float3 pv = pvox * voxelSize; point = (float3)(dot(pv, volRot0), dot(pv, volRot1), dot(pv, volRot2)) + volTrans; From 789e0c9f50bf3571c1a2b7a39104b8fd376630d6 Mon Sep 17 00:00:00 2001 From: batters21 Date: Thu, 11 Feb 2021 16:28:04 +0300 Subject: [PATCH 183/216] minor fix --- modules/rgbd/src/opencl/hash_tsdf.cl | 20 ++++++++++---------- modules/rgbd/src/tsdf_functions.cpp | 1 + 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 4d0e7d4afd1..d6c102ccba9 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -421,21 +421,21 @@ inline float3 getNormalVoxel(float3 ptVox, __global const struct TsdfVoxel* allV } #if !USE_INTERPOLATION_IN_GETNORMAL - int3 offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3 - { 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7 + int3 offsets[] = { (int3)( 1, 0, 0), (int3)(-1, 0, 0), (int3)( 0, 1, 0), // 0-3 + (int3)( 0, -1, 0), (int3)( 0, 0, 1), (int3)( 0, 0, -1) // 4-7 }; const int nVals = 6; float vals[6]; #else - int3 offsets[]={{ 0, 0, 0}, { 0, 0, 1}, { 0, 1, 0}, { 0, 1, 1}, // 0-3 - { 1, 0, 0}, { 1, 0, 1}, { 1, 1, 0}, { 1, 1, 1}, // 4-7 - {-1, 0, 0}, {-1, 0, 1}, {-1, 1, 0}, {-1, 1, 1}, // 8-11 - { 2, 0, 0}, { 2, 0, 1}, { 2, 1, 0}, { 2, 1, 1}, // 12-15 - { 0, -1, 0}, { 0, -1, 1}, { 1, -1, 0}, { 1, -1, 1}, // 16-19 - { 0, 2, 0}, { 0, 2, 1}, { 1, 2, 0}, { 1, 2, 1}, // 20-23 - { 0, 0, -1}, { 0, 1, -1}, { 1, 0, -1}, { 1, 1, -1}, // 24-27 - { 0, 0, 2}, { 0, 1, 2}, { 1, 0, 2}, { 1, 1, 2}, // 28-31 + int3 offsets[]={(int3)( 0, 0, 0), (int3)( 0, 0, 1), (int3)( 0, 1, 0), (int3)( 0, 1, 1), // 0-3 + (int3)( 1, 0, 0), (int3)( 1, 0, 1), (int3)( 1, 1, 0), (int3)( 1, 1, 1), // 4-7 + (int3)(-1, 0, 0), (int3)(-1, 0, 1), (int3)(-1, 1, 0), (int3)(-1, 1, 1), // 8-11 + (int3)( 2, 0, 0), (int3)( 2, 0, 1), (int3)( 2, 1, 0), (int3)( 2, 1, 1), // 12-15 + (int3)( 0, -1, 0), (int3)( 0, -1, 1), (int3)( 1, -1, 0), (int3)( 1, -1, 1), // 16-19 + (int3)( 0, 2, 0), (int3)( 0, 2, 1), (int3)( 1, 2, 0), (int3)( 1, 2, 1), // 20-23 + (int3)( 0, 0, -1), (int3)( 0, 1, -1), (int3)( 1, 0, -1), (int3)( 1, 1, -1), // 24-27 + (int3)( 0, 0, 2), (int3)( 0, 1, 2), (int3)( 1, 0, 2), (int3)( 1, 1, 2), // 28-31 }; const int nVals = 32; float vals[32]; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 8a4a4a8c970..09d142c98c0 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -436,6 +436,7 @@ bool VolumesTable::insert(Vec3i idx, int row) i = v->nextVolumeRow; } + return false; } From c79611b9c69ed31cf6583f0d4d9219b0318eea41 Mon Sep 17 00:00:00 2001 From: batters21 Date: Thu, 11 Feb 2021 16:41:57 +0300 Subject: [PATCH 184/216] linux critical bug fix --- modules/rgbd/src/opencl/hash_tsdf.cl | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index d6c102ccba9..11a15fccd5d 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -421,21 +421,21 @@ inline float3 getNormalVoxel(float3 ptVox, __global const struct TsdfVoxel* allV } #if !USE_INTERPOLATION_IN_GETNORMAL - int3 offsets[] = { (int3)( 1, 0, 0), (int3)(-1, 0, 0), (int3)( 0, 1, 0), // 0-3 - (int3)( 0, -1, 0), (int3)( 0, 0, 1), (int3)( 0, 0, -1) // 4-7 + int4 offsets[] = { (int4)( 1, 0, 0, 0), (int4)(-1, 0, 0, 0), (int4)( 0, 1, 0, 0), // 0-3 + (int4)( 0, -1, 0, 0), (int4)( 0, 0, 1, 0), (int4)( 0, 0, -1, 0) // 4-7 }; const int nVals = 6; float vals[6]; #else - int3 offsets[]={(int3)( 0, 0, 0), (int3)( 0, 0, 1), (int3)( 0, 1, 0), (int3)( 0, 1, 1), // 0-3 - (int3)( 1, 0, 0), (int3)( 1, 0, 1), (int3)( 1, 1, 0), (int3)( 1, 1, 1), // 4-7 - (int3)(-1, 0, 0), (int3)(-1, 0, 1), (int3)(-1, 1, 0), (int3)(-1, 1, 1), // 8-11 - (int3)( 2, 0, 0), (int3)( 2, 0, 1), (int3)( 2, 1, 0), (int3)( 2, 1, 1), // 12-15 - (int3)( 0, -1, 0), (int3)( 0, -1, 1), (int3)( 1, -1, 0), (int3)( 1, -1, 1), // 16-19 - (int3)( 0, 2, 0), (int3)( 0, 2, 1), (int3)( 1, 2, 0), (int3)( 1, 2, 1), // 20-23 - (int3)( 0, 0, -1), (int3)( 0, 1, -1), (int3)( 1, 0, -1), (int3)( 1, 1, -1), // 24-27 - (int3)( 0, 0, 2), (int3)( 0, 1, 2), (int3)( 1, 0, 2), (int3)( 1, 1, 2), // 28-31 + int4 offsets[]={(int4)( 0, 0, 0, 0), (int4)( 0, 0, 1, 0), (int4)( 0, 1, 0, 0), (int4)( 0, 1, 1, 0), // 0-3 + (int4)( 1, 0, 0, 0), (int4)( 1, 0, 1, 0), (int4)( 1, 1, 0, 0), (int4)( 1, 1, 1, 0), // 4-7 + (int4)(-1, 0, 0, 0), (int4)(-1, 0, 1, 0), (int4)(-1, 1, 0, 0), (int4)(-1, 1, 1, 0), // 8-11 + (int4)( 2, 0, 0, 0), (int4)( 2, 0, 1, 0), (int4)( 2, 1, 0, 0), (int4)( 2, 1, 1, 0), // 12-15 + (int4)( 0, -1, 0, 0), (int4)( 0, -1, 1, 0), (int4)( 1, -1, 0, 0), (int4)( 1, -1, 1, 0), // 16-19 + (int4)( 0, 2, 0, 0), (int4)( 0, 2, 1, 0), (int4)( 1, 2, 0, 0), (int4)( 1, 2, 1, 0), // 20-23 + (int4)( 0, 0, -1, 0), (int4)( 0, 1, -1, 0), (int4)( 1, 0, -1, 0), (int4)( 1, 1, -1, 0), // 24-27 + (int4)( 0, 0, 2, 0), (int4)( 0, 1, 2, 0), (int4)( 1, 0, 2, 0), (int4)( 1, 1, 2, 0), // 28-31 }; const int nVals = 32; float vals[32]; @@ -443,7 +443,7 @@ inline float3 getNormalVoxel(float3 ptVox, __global const struct TsdfVoxel* allV for (int i = 0; i < nVals; i++) { - int3 pt = iptVox + offsets[i]; + int3 pt = iptVox + offsets[i].s012; // VoxelToVolumeUnitIdx() int3 volumeUnitIdx = pt >> volumeUnitDegree; From 594a7681e15102bad6ff443a7eb49ed1651a1e5e Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Feb 2021 17:08:01 +0300 Subject: [PATCH 185/216] docs fix --- modules/rgbd/src/hash_tsdf.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 7fcd09cce9a..412e831a1c5 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -372,7 +372,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { return TsdfVoxel(floatToTsdf(1.f), 0); } - + cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] << volumeUnitDegree, volumeUnitIdx[1] << volumeUnitDegree, volumeUnitIdx[2] << volumeUnitDegree); @@ -1129,7 +1129,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Point3f truncPt(truncDist, truncDist, truncDist); Mutex mutex; - + // for new indices ToyHashSet thm; @@ -1150,7 +1150,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto //! Find accessed TSDF volume unit for valid 3D vertex Vec3i lower_bound = this->volumeToVolumeUnitIdx(volPoint - truncPt); Vec3i upper_bound = this->volumeToVolumeUnitIdx(volPoint + truncPt); - + int pixLocalCounter = 0; LocalVolUnits pixLocalVolUnits; for (int i = lower_bound[0]; i <= upper_bound[0]; i++) @@ -1232,7 +1232,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto { Vec4i node = ghm.data[i]; Vec3i idx(node[0], node[1], node[2]); - + //TODO: 1. add to separate hash map instead, then merge on GPU side int result = thm.insert(idx); @@ -1315,7 +1315,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto } } }; - + //TODO: remove it auto pushToGlobalGpu = [](const ToyHashSet thm, ToyHashSet& globalHashMap, bool& needReallocation) { @@ -1397,7 +1397,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto pushToGlobal(thm, hashTable, needReallocation, mutex); //pushToGlobalGpu(thm, hashTable, needReallocation); } while (needReallocation); - + // --------------------- } @@ -1474,7 +1474,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma UMat newData(buff_lvl, volCubed, CV_8UC2); volUnitsData.copyTo(newData.rowRange(oldr)); volUnitsData = newData; - + UMat newLastVisibleIndices(buff_lvl, 1, CV_32S); lastVisibleIndices.copyTo(newLastVisibleIndices.rowRange(oldr)); lastVisibleIndices = newLastVisibleIndices; @@ -1784,7 +1784,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Intr::Reprojector r = intrinsics.makeReprojector(); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); - + Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, volumeUnitSize - voxelSize, volumeUnitSize - voxelSize); From 9e65354df2d6225753d482245221d8fc238832e8 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Feb 2021 17:11:55 +0300 Subject: [PATCH 186/216] mac warning fix --- modules/rgbd/src/hash_tsdf.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 412e831a1c5..11ce57cad94 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1116,8 +1116,7 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { - const int newIndicesCapacity = VOLUMES_SIZE; - constexpr size_t pixCapacity = 16; + constexpr int pixCapacity = 16; typedef std::array LocalVolUnits; Depth depth = _depth.getMat(ACCESS_READ); @@ -1162,9 +1161,9 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto if (hashTable.find(tsdf_idx) < 0) { bool found = false; - for (int i = 0; i < pixLocalCounter; i++) + for (int c = 0; c < pixLocalCounter; c++) { - if (pixLocalVolUnits[i] == tsdf_idx) + if (pixLocalVolUnits[c] == tsdf_idx) { found = true; break; } @@ -1317,6 +1316,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto }; //TODO: remove it + /* auto pushToGlobalGpu = [](const ToyHashSet thm, ToyHashSet& globalHashMap, bool& needReallocation) { //TODO: set needReallocation based on thm.last + globalHashMap.last < globalHashMap.capacity @@ -1380,7 +1380,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto std::cout << "need reallocation" << std::endl; } }; - + */ needReallocation = false; do { From 7db345decb03b5002a2b00b64fac5645df8d08ca Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Feb 2021 19:37:13 +0300 Subject: [PATCH 187/216] remove extra test code --- modules/rgbd/test/test_tsdf.cpp | 71 --------------------------------- 1 file changed, 71 deletions(-) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 70b7893edcf..5a9b23afb95 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -473,8 +473,6 @@ void valid_points_test(bool isHashTSDF) ASSERT_LT(0.5 - percentValidity, 0.3); } -#ifndef HAVE_OPENCL - TEST(TSDF, raycast_normals) { normal_test(false, true, false, false); @@ -515,74 +513,5 @@ TEST(HashTSDF, valid_points) valid_points_test(true); } -#else - -TEST(TSDF_GPU, raycast_normals) { normal_test(false, true, false, false); } -TEST(TSDF_GPU, fetch_points_normals) { normal_test(false, false, true, false); } -TEST(TSDF_GPU, fetch_normals) { normal_test(false, false, false, true); } -TEST(TSDF_GPU, valid_points) { valid_points_test(false); } - -TEST(HashTSDF_GPU, raycast_normals) { normal_test(true, true, false, false); } -TEST(HashTSDF_GPU, fetch_points_normals) { normal_test(true, false, true, false); } -TEST(HashTSDF_GPU, fetch_normals) { normal_test(true, false, false, true); } -TEST(HashTSDF_GPU, valid_points) { valid_points_test(true); } - -TEST(TSDF_CPU, raycast_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(false, true, false, false); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, fetch_points_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(false, false, true, false); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, fetch_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(false, false, false, true); - cv::ocl::setUseOpenCL(true); -} - -TEST(TSDF_CPU, valid_points) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test(false); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, raycast_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(true, true, false, false); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, fetch_points_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(true, false, true, false); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, fetch_normals) -{ - cv::ocl::setUseOpenCL(false); - normal_test(true, false, false, true); - cv::ocl::setUseOpenCL(true); -} - -TEST(HashTSDF_CPU, valid_points) -{ - cv::ocl::setUseOpenCL(false); - valid_points_test(true); - cv::ocl::setUseOpenCL(true); -} - -#endif }} // namespace From 9b7a0f2ac1164b4274ad2ab0c00760729f6b6a1d Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Feb 2021 19:40:43 +0300 Subject: [PATCH 188/216] linux warning fix --- modules/rgbd/src/hash_tsdf.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 11ce57cad94..744569a47d8 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1294,20 +1294,20 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto } while (needReallocation); - auto pushToGlobal = [](const ToyHashSet thm, ToyHashSet& globalHashMap, - bool& needReallocation, Mutex& mutex) + auto pushToGlobal = [](const ToyHashSet _thm, ToyHashSet& _globalHashMap, + bool& _needReallocation, Mutex& _mutex) { - for (int i = 0; i < thm.last; i++) + for (int i = 0; i < _thm.last; i++) { - Vec4i node = thm.data[i]; + Vec4i node = _thm.data[i]; Vec3i idx(node[0], node[1], node[2]); - std::lock_guard al(mutex); + std::lock_guard al(_mutex); - int result = globalHashMap.insert(idx); + int result = _globalHashMap.insert(idx); if (result == 0) { - needReallocation = true; + _needReallocation = true; //DEBUG std::cout << "need reallocation, exiting" << std::endl; return; From a39ee06376cf4b8ff03c2b648324c805ca309913 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Sat, 13 Feb 2021 09:42:51 +0300 Subject: [PATCH 189/216] remove comments --- modules/rgbd/samples/large_kinfu_demo.cpp | 1 - modules/rgbd/src/volume.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp index b1d638ecb0b..e78c3511215 100644 --- a/modules/rgbd/samples/large_kinfu_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -127,7 +127,6 @@ int main(int argc, char** argv) // These params can be different for each depth sensor ds->updateParams(*params); - // Disabled until there is no OpenCL accelerated HashTSDF is available cv::setUseOptimized(true); if (!idle) diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index 73a26966d22..88c46fe4b67 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -68,7 +68,6 @@ Ptr makeVolume(const VolumeParams& _volumeParams) if(_volumeParams.type == VolumeType::TSDF) return kinfu::makeTSDFVolume(_volumeParams); else if(_volumeParams.type == VolumeType::HASHTSDF) - //return kinfu::makeHashTSDFVolume(_volumeParams); return kinfu::makeHashTSDFVolume(_volumeParams); CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } From 5f42078b6a5ac02f0196d709a6fb39de7da6a7b7 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Sat, 13 Feb 2021 19:54:59 +0300 Subject: [PATCH 190/216] capacity = 2048 --- 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 3ebba881140..a5bcaba3cb6 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -100,7 +100,7 @@ class ToyHashSet { public: static const int hashDivisor = 32768; - static const int startCapacity = 1024; + static const int startCapacity = 2048; std::vector hashes; // 0-3 for key, 4th for internal use From 617492314b5b77df631935141a6d1cff92148f4b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Sat, 13 Feb 2021 22:24:54 +0300 Subject: [PATCH 191/216] capacity = 1024 --- 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 a5bcaba3cb6..3ebba881140 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -100,7 +100,7 @@ class ToyHashSet { public: static const int hashDivisor = 32768; - static const int startCapacity = 2048; + static const int startCapacity = 1024; std::vector hashes; // 0-3 for key, 4th for internal use From 09437084e71a7d9738b4a0328aed7ff9b4d154f1 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 16 Feb 2021 12:20:42 +0300 Subject: [PATCH 192/216] hashtables fixes --- modules/rgbd/src/hash_tsdf.cpp | 12 +- modules/rgbd/src/opencl/hash_tsdf.cl | 6 +- modules/rgbd/src/tsdf_functions.hpp | 192 +++++++++++++++++++-------- 3 files changed, 145 insertions(+), 65 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 744569a47d8..7bb42639880 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -945,7 +945,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume cv::UMat pixNorms; //TODO: move indexes.volumes to GPU - ToyHashSet hashTable; + CustomHashSet hashTable; Vec8i neighbourCoords; }; @@ -1011,7 +1011,7 @@ void HashTSDFVolumeGPU::reset() isActiveFlags = cv::UMat(buff_lvl, 1, CV_8U); - hashTable = ToyHashSet(); + hashTable = CustomHashSet(); frameParams = Vec6f(); pixNorms = UMat(); @@ -1130,11 +1130,11 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto Mutex mutex; // for new indices - ToyHashSet thm; + CustomHashSet thm; // ----------------------- - auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, ToyHashSet& ghm) + auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, CustomHashSet& ghm) { for (int y = yrange.start; y < yrange.end; y += depthStride) { @@ -1218,7 +1218,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto int loc_vol_idx = 0; */ - ToyHashSet ghm; + CustomHashSet ghm; fillLocalAcessVolUnits(xr, yr, ghm /*localAccessVolUnits, loc_vol_idx*/); @@ -1294,7 +1294,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto } while (needReallocation); - auto pushToGlobal = [](const ToyHashSet _thm, ToyHashSet& _globalHashMap, + auto pushToGlobal = [](const CustomHashSet _thm, CustomHashSet& _globalHashMap, bool& _needReallocation, Mutex& _mutex) { for (int i = 0; i < _thm.last; i++) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 11a15fccd5d..29b59fe511a 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -85,7 +85,7 @@ static int findRow(__global const struct Volume_NODE * hash_table, int3 indx, //TODO: make hashDivisor a power of 2 //TODO: put it to this .cl file as a constant -static int toy_find(int3 idx, const int hashDivisor, __global const int* hashes, +static int custom_find(int3 idx, const int hashDivisor, __global const int* hashes, __global const int4* data) { int hash = calc_hash(idx) % hashDivisor; @@ -454,7 +454,7 @@ inline float3 getNormalVoxel(float3 ptVox, __global const struct TsdfVoxel* allV int it = iterMap[dictIdx]; if (it < -1) { - it = toy_find(volumeUnitIdx, hash_divisor, hashes, data); + it = custom_find(volumeUnitIdx, hash_divisor, hashes, data); iterMap[dictIdx] = it; } @@ -594,7 +594,7 @@ __kernel void raycast( int3 currVoxel = convert_int3(floor(currRayPosVox)); int3 currVolumeUnitIdx = currVoxel >> volumeUnitDegree; - int row = toy_find(currVolumeUnitIdx, hash_divisor, hashes, data); + int row = custom_find(currVolumeUnitIdx, hash_divisor, hashes, data); float currTsdf = prevTsdf; int currWeight = 0; diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 3ebba881140..e62c1de9cad 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -43,64 +43,12 @@ void integrateVolumeUnit( InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); -const int NAN_ELEMENT = -2147483647; - -struct Volume_NODE -{ - Vec4i idx = Vec4i(NAN_ELEMENT); - int32_t row = -1; - int32_t nextVolumeRow = -1; - int32_t dummy = 0; - int32_t dummy2 = 0; -}; - -const int _hash_divisor = 32768; -const int _list_size = 4; - -class VolumesTable -{ -public: - const int hash_divisor = _hash_divisor; - const int list_size = _list_size; - const int32_t free_row = -1; - const int32_t free_isActive = 0; - - const cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT); - - int bufferNums; - cv::Mat volumes; - - VolumesTable(); - const VolumesTable& operator=(const VolumesTable&); - ~VolumesTable() {}; - - bool insert(Vec3i idx, int row); - int findRow(Vec3i idx) const; - - inline int getPos(Vec3i idx, int bufferNum) const - { - int hash = int(calc_hash(idx) % hash_divisor); - return (bufferNum * hash_divisor + hash) * list_size; - } - - inline size_t calc_hash(Vec3i x) const - { - uint32_t seed = 0; - constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; - for (int i = 0; i < 3; i++) - { - seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - } - return seed; - } -}; - -class ToyHashSet +class CustomHashSet { public: static const int hashDivisor = 32768; - static const int startCapacity = 1024; + static const int startCapacity = 2048; std::vector hashes; // 0-3 for key, 4th for internal use @@ -109,7 +57,7 @@ class ToyHashSet int capacity; int last; - ToyHashSet() + CustomHashSet() { hashes.resize(hashDivisor); for (int i = 0; i < hashDivisor; i++) @@ -123,7 +71,7 @@ class ToyHashSet last = 0; } - ~ToyHashSet() { } + ~CustomHashSet() { } inline size_t calc_hash(Vec3i x) const { @@ -137,6 +85,9 @@ class ToyHashSet } // should work on existing elements too + // 0 - need resize + // 1 - idx is inserted + // 2 - idx already exists int insert(Vec3i idx) { if (last < capacity) @@ -202,6 +153,135 @@ class ToyHashSet } }; +// TODO: remove this structure as soon as HashTSDFGPU data is completely on GPU; +// until then CustomHashTable can be replaced by this one if needed + +const int NAN_ELEMENT = -2147483647; + +struct Volume_NODE +{ + Vec4i idx = Vec4i(NAN_ELEMENT); + int32_t row = -1; + int32_t nextVolumeRow = -1; + int32_t dummy = 0; + int32_t dummy2 = 0; +}; + +const int _hash_divisor = 32768; +const int _list_size = 4; + +class VolumesTable +{ +public: + const int hash_divisor = _hash_divisor; + const int list_size = _list_size; + const int32_t free_row = -1; + const int32_t free_isActive = 0; + + const cv::Vec4i nan4 = cv::Vec4i(NAN_ELEMENT); + + int bufferNums; + cv::Mat volumes; + + VolumesTable() : bufferNums(1) + { + this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType()); + for (int i = 0; i < volumes.size().height; i++) + { + Volume_NODE* v = volumes.ptr(i); + v->idx = nan4; + v->row = -1; + v->nextVolumeRow = -1; + } + } + const VolumesTable& operator=(const VolumesTable& vt) + { + this->volumes = vt.volumes; + this->bufferNums = vt.bufferNums; + return *this; + } + ~VolumesTable() {}; + + bool insert(Vec3i idx, int row) + { + CV_Assert(row >= 0); + + int bufferNum = 0; + int hash = int(calc_hash(idx) % hash_divisor); + int start = getPos(idx, bufferNum); + int i = start; + + while (i >= 0) + { + Volume_NODE* v = volumes.ptr(i); + + if (v->idx[0] == NAN_ELEMENT) + { + Vec4i idx4(idx[0], idx[1], idx[2], 0); + + bool extend = false; + if (i != start && i % list_size == 0) + { + if (bufferNum >= bufferNums - 1) + { + extend = true; + volumes.resize(hash_divisor * bufferNums); + bufferNums++; + } + bufferNum++; + v->nextVolumeRow = (bufferNum * hash_divisor + hash) * list_size; + } + else + { + v->nextVolumeRow = i + 1; + } + + v->idx = idx4; + v->row = row; + + return extend; + } + + i = v->nextVolumeRow; + } + return false; + } + int findRow(Vec3i idx) const + { + int bufferNum = 0; + int i = getPos(idx, bufferNum); + + while (i >= 0) + { + const Volume_NODE* v = volumes.ptr(i); + + if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0)) + return v->row; + else + i = v->nextVolumeRow; + } + + return -1; + } + + inline int getPos(Vec3i idx, int bufferNum) const + { + int hash = int(calc_hash(idx) % hash_divisor); + return (bufferNum * hash_divisor + hash) * list_size; + } + + inline size_t calc_hash(Vec3i x) const + { + uint32_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + for (int i = 0; i < 3; i++) + { + seed ^= x[i] + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + } + return seed; + } +}; + } // namespace kinfu } // namespace cv From fbc1825c51d1becd563f478861241239578f0906 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 16 Feb 2021 12:29:09 +0300 Subject: [PATCH 193/216] minor code removement --- modules/rgbd/src/tsdf_functions.cpp | 85 ----------------------------- 1 file changed, 85 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 09d142c98c0..a2447035acd 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -373,90 +373,5 @@ void integrateVolumeUnit( } -/// Custom Volume Hash Table - -VolumesTable::VolumesTable() : bufferNums(1) -{ - this->volumes = cv::Mat(hash_divisor * list_size, 1, rawType()); - for (int i = 0; i < volumes.size().height; i++) - { - Volume_NODE* v = volumes.ptr(i); - v->idx = nan4; - v->row = -1; - v->nextVolumeRow = -1; - } -} - -const VolumesTable& VolumesTable::operator=(const VolumesTable& vt) -{ - this->volumes = vt.volumes; - this->bufferNums = vt.bufferNums; - return *this; -} - -bool VolumesTable::insert(Vec3i idx, int row) -{ - CV_Assert(row >= 0); - - int bufferNum = 0; - int hash = int(calc_hash(idx) % hash_divisor); - int start = getPos(idx, bufferNum); - int i = start; - - while (i >= 0) - { - Volume_NODE* v = volumes.ptr(i); - - if (v->idx[0] == NAN_ELEMENT) - { - Vec4i idx4(idx[0], idx[1], idx[2], 0); - - bool extend = false; - if (i != start && i % list_size == 0) - { - if (bufferNum >= bufferNums - 1) - { - extend = true; - volumes.resize(hash_divisor * bufferNums); - bufferNums++; - } - bufferNum++; - v->nextVolumeRow = (bufferNum * hash_divisor + hash) * list_size; - } - else - { - v->nextVolumeRow = i + 1; - } - - v->idx = idx4; - v->row = row; - - return extend; - } - - i = v->nextVolumeRow; - } - return false; -} - - -int VolumesTable::findRow(Vec3i idx) const -{ - int bufferNum = 0; - int i = getPos(idx, bufferNum); - - while (i >= 0) - { - const Volume_NODE* v = volumes.ptr(i); - - if (v->idx == Vec4i(idx[0], idx[1], idx[2], 0)) - return v->row; - else - i = v->nextVolumeRow; - } - - return -1; -} - } // namespace kinfu } // namespace cv From 7d9fda1540be92672719693d0c2293e3c4bf3aa1 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 09:55:20 +0300 Subject: [PATCH 194/216] change pixNorm calculation --- modules/rgbd/src/tsdf_functions.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index a2447035acd..122bfd6a403 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -253,8 +253,8 @@ void integrateVolumeUnit( 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)); + 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: From dfd35e70e834670a1f8d4c144ab766c0e7e8f58f Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 12:48:26 +0300 Subject: [PATCH 195/216] remove buff_lvl and neighboorCoords; rename degree --- modules/rgbd/src/hash_tsdf.cpp | 27 ++++++--------------------- 1 file changed, 6 insertions(+), 21 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 7bb42639880..3d995422360 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -928,10 +928,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume public: Vec4i volStrides; Vec6f frameParams; - //TODO: rename this - int degree; - //TODO: rename this - int buff_lvl; + int bufferSizeDegree; // per-volume-unit data cv::UMat lastVisibleIndices; @@ -946,8 +943,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume //TODO: move indexes.volumes to GPU CustomHashSet hashTable; - - Vec8i neighbourCoords; }; HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, @@ -969,17 +964,6 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, flo } volStrides = Vec4i(xdim, ydim, zdim); - neighbourCoords = Vec8i( - volStrides.dot(Vec4i(0, 0, 0)), - volStrides.dot(Vec4i(0, 0, 1)), - volStrides.dot(Vec4i(0, 1, 0)), - volStrides.dot(Vec4i(0, 1, 1)), - volStrides.dot(Vec4i(1, 0, 0)), - volStrides.dot(Vec4i(1, 0, 1)), - volStrides.dot(Vec4i(1, 1, 0)), - volStrides.dot(Vec4i(1, 1, 1)) - ); - reset(); } @@ -999,8 +983,8 @@ void HashTSDFVolumeGPU::reset() { CV_TRACE_FUNCTION(); - degree = 15; - buff_lvl = (int) pow(2, degree); + bufferSizeDegree = 15; + int buff_lvl = (int) (1 << bufferSizeDegree); int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; volUnitsDataCopy = cv::Mat(buff_lvl, volCubed, rawType()); @@ -1461,11 +1445,12 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma //! Perform the allocation // Grow buffers + int buff_lvl = (int)(1 << bufferSizeDegree); if (sizeAfter >= buff_lvl) { - degree = (int)(log2(sizeAfter) + 1); // clz() would be better + bufferSizeDegree = (int)(log2(sizeAfter) + 1); // clz() would be better int oldBuffSize = buff_lvl; - buff_lvl = (int)pow(2, degree); + buff_lvl = (int)pow(2, bufferSizeDegree); volUnitsDataCopy.resize(buff_lvl); From ec037bd67e5307a1ff2d3d710a505e0916c30258 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 12:57:02 +0300 Subject: [PATCH 196/216] move volStrides to HashTSDFVolume --- modules/rgbd/src/hash_tsdf.cpp | 47 +++++++++++----------------------- modules/rgbd/src/hash_tsdf.hpp | 1 + 2 files changed, 16 insertions(+), 32 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 3d995422360..87cb6dd56aa 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -46,6 +46,21 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca { CV_Error(Error::StsBadArg, "Volume unit resolution should be a power of 2"); } + + int xdim, ydim, zdim; + if (zFirstMemOrder) + { + xdim = volumeUnitResolution * volumeUnitResolution; + ydim = volumeUnitResolution; + zdim = 1; + } + else + { + xdim = 1; + ydim = volumeUnitResolution; + zdim = volumeUnitResolution * volumeUnitResolution; + } + volStrides = Vec4i(xdim, ydim, zdim); } //! Spatial hashing @@ -119,7 +134,6 @@ class HashTSDFVolumeCPU : public HashTSDFVolume Vec3i volumeToVoxelCoord(const Point3f& point) const; public: - Vec4i volStrides; Vec6f frameParams; Mat pixNorms; VolumeUnitIndexes volumeUnits; @@ -133,21 +147,6 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, flo :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { - int xdim, ydim, zdim; - if (zFirstMemOrder) - { - xdim = volumeUnitResolution * volumeUnitResolution; - ydim = volumeUnitResolution; - zdim = 1; - } - else - { - xdim = 1; - ydim = volumeUnitResolution; - zdim = volumeUnitResolution * volumeUnitResolution; - } - volStrides = Vec4i(xdim, ydim, zdim); - reset(); } @@ -926,7 +925,6 @@ class HashTSDFVolumeGPU : public HashTSDFVolume Vec3i volumeToVoxelCoord(const Point3f& point) const; public: - Vec4i volStrides; Vec6f frameParams; int bufferSizeDegree; @@ -949,21 +947,6 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, flo float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { - int xdim, ydim, zdim; - if (zFirstMemOrder) - { - xdim = volumeUnitResolution * volumeUnitResolution; - ydim = volumeUnitResolution; - zdim = 1; - } - else - { - xdim = 1; - ydim = volumeUnitResolution; - zdim = volumeUnitResolution * volumeUnitResolution; - } - volStrides = Vec4i(xdim, ydim, zdim); - reset(); } diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 50502cf2556..34e73f30383 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -37,6 +37,7 @@ class HashTSDFVolume : public Volume int volumeUnitDegree; float volumeUnitSize; bool zFirstMemOrder; + Vec4i volStrides; }; //template From 45a1b346cc070167187b05ade4c9e24ef22b2cb5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 13:06:58 +0300 Subject: [PATCH 197/216] HashTSDFVolumeGPU constructor 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 87cb6dd56aa..b7d3087e7eb 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -951,7 +951,7 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, flo } HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstMemOrder) - : HashTSDFVolume(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + : HashTSDFVolumeGPU(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) { if (truncDist >= volumeUnitSize) From b3507d1da7339551460c98d900cc818fc43ac9dd Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 16:31:31 +0300 Subject: [PATCH 198/216] move preCalculatePixNormsGPU to tsdf_functions --- modules/rgbd/src/hash_tsdf.cpp | 43 +----------------- modules/rgbd/src/opencl/hash_tsdf.cl | 14 ------ modules/rgbd/src/opencl/tsdf_functions.cl | 19 ++++++++ modules/rgbd/src/tsdf.cpp | 55 +++-------------------- modules/rgbd/src/tsdf_functions.cpp | 44 ++++++++++++++++++ modules/rgbd/src/tsdf_functions.hpp | 2 + 6 files changed, 72 insertions(+), 105 deletions(-) create mode 100644 modules/rgbd/src/opencl/tsdf_functions.cl diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b7d3087e7eb..5fa24f21edf 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -985,46 +985,6 @@ void HashTSDFVolumeGPU::reset() } -static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f fxy, Vec2f cxy) -{ - Mat x(1, depth_cols, CV_32FC1); - Mat y(1, depth_rows, CV_32FC1); - UMat pixNorm(depth_rows, depth_cols, CV_32F); - - for (int i = 0; i < depth_cols; i++) - x.at(i) = (i - cxy[0]) / fxy[0]; - for (int i = 0; i < depth_rows; i++) - y.at(i) = (i - cxy[1]) / fxy[1]; - - cv::String errorStr; - cv::String name = "preCalculationPixNorm"; - ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; - ocl::Kernel kk; - kk.create(name.c_str(), source, options, &errorStr); - - if (kk.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); - - AccessFlag af = ACCESS_READ; - UMat xx = x.getUMat(af); - UMat yy = y.getUMat(af); - - kk.args(ocl::KernelArg::WriteOnly(pixNorm), - ocl::KernelArg::PtrReadOnly(xx), - ocl::KernelArg::PtrReadOnly(yy)); - - size_t globalSize[2]; - globalSize[0] = depth_rows; - globalSize[1] = depth_cols; - - if (!kk.run(2, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); - - return pixNorm; -} - - void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) { CV_TRACE_FUNCTION(); @@ -1472,8 +1432,7 @@ void HashTSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Ma if (!(frameParams == newParams)) { frameParams = newParams; - Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); + pixNorms = preCalculationPixNormGPU(depth, intrinsics); } //! Integrate the correct volumeUnits diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 29b59fe511a..c7a4807e2ee 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -40,20 +40,6 @@ static inline float tsdfToFloat(TsdfType num) return ( (float) num ) / (-128); } -__kernel void preCalculationPixNorm (__global char * pixNormsPtr, - int pixNormsStep, int pixNormsOffset, - int pixNormsRows, int pixNormsCols, - const __global float * xx, - const __global float * yy) -{ - int i = get_global_id(0); - int j = get_global_id(1); - if (i < pixNormsRows && j < pixNormsCols) - { - *(__global float*)(pixNormsPtr + pixNormsOffset + i*pixNormsStep + j*sizeof(float)) = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); - } -} - static uint calc_hash(int3 x) { unsigned int seed = 0; diff --git a/modules/rgbd/src/opencl/tsdf_functions.cl b/modules/rgbd/src/opencl/tsdf_functions.cl new file mode 100644 index 00000000000..40efeca57c8 --- /dev/null +++ b/modules/rgbd/src/opencl/tsdf_functions.cl @@ -0,0 +1,19 @@ +// 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 + +__kernel void preCalculationPixNorm (__global char * pixNormsPtr, + int pixNormsStep, int pixNormsOffset, + int pixNormsRows, int pixNormsCols, + const __global float * xx, + const __global float * yy) +{ + int i = get_global_id(0); + int j = get_global_id(1); + if (i < pixNormsRows && j < pixNormsCols) + { + *(__global float*)(pixNormsPtr + pixNormsOffset + i*pixNormsStep + j*sizeof(float)) = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); + } +} \ No newline at end of file diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index c689bd7997b..195ac3d4884 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -830,47 +830,6 @@ void TSDFVolumeGPU::reset() volume.setTo(Scalar(0, 0)); } -static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f fxy, Vec2f cxy) -{ - Mat x(1, depth_cols, CV_32F); - Mat y(1, depth_rows, CV_32F); - Mat _pixNorm(1, depth_rows * depth_cols, CV_32F); - - for (int i = 0; i < depth_cols; i++) - x.at(0, i) = (i - cxy[0]) / fxy[0]; - for (int i = 0; i < depth_rows; i++) - y.at(0, i) = (i - cxy[1]) / fxy[1]; - - cv::String errorStr; - cv::String name = "preCalculationPixNorm"; - ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; - ocl::Kernel kk; - kk.create(name.c_str(), source, options, &errorStr); - - - if (kk.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); - - AccessFlag af = ACCESS_READ; - UMat pixNorm = _pixNorm.getUMat(af); - UMat xx = x.getUMat(af); - UMat yy = y.getUMat(af); - - kk.args(ocl::KernelArg::PtrReadWrite(pixNorm), - ocl::KernelArg::PtrReadOnly(xx), - ocl::KernelArg::PtrReadOnly(yy), - depth_cols); - - size_t globalSize[2]; - globalSize[0] = depth_rows; - globalSize[1] = depth_cols; - - if (!kk.run(2, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); - - return pixNorm; -} // use depth instead of distance (optimization) void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, @@ -896,15 +855,13 @@ void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, float dfac = 1.f/depthFactor; Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); - 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; - - pixNorms = preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy); + frameParams = newParams; + pixNorms = preCalculationPixNormGPU(depth, intrinsics); } // TODO: optimization possible diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 122bfd6a403..d60eb02fa73 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -6,6 +6,7 @@ #include "precomp.hpp" #include "tsdf_functions.hpp" +#include "opencl_kernels_rgbd.hpp" namespace cv { @@ -35,6 +36,49 @@ cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) return pixNorm; } +cv::UMat preCalculationPixNormGPU(const UMat& depth, const Intr& intrinsics) +{ + int depth_cols = depth.cols; + int depth_rows = depth.rows; + Point2f fl(intrinsics.fx, intrinsics.fy); + Point2f pp(intrinsics.cx, intrinsics.cy); + Mat x(1, depth_cols, CV_32FC1); + Mat y(1, depth_rows, CV_32FC1); + UMat pixNorm(depth_rows, depth_cols, CV_32F); + + for (int i = 0; i < depth_cols; i++) + x.at(i) = (i - pp.x) / fl.x; + for (int i = 0; i < depth_rows; i++) + y.at(i) = (i - pp.y) / fl.y; + + cv::String errorStr; + cv::String name = "preCalculationPixNorm"; + ocl::ProgramSource source = ocl::rgbd::tsdf_functions_oclsrc; + cv::String options = "-cl-mad-enable"; + ocl::Kernel kk; + kk.create(name.c_str(), source, options, &errorStr); + + if (kk.empty()) + throw std::runtime_error("Failed to create kernel: " + errorStr); + + AccessFlag af = ACCESS_READ; + UMat xx = x.getUMat(af); + UMat yy = y.getUMat(af); + + kk.args(ocl::KernelArg::WriteOnly(pixNorm), + ocl::KernelArg::PtrReadOnly(xx), + ocl::KernelArg::PtrReadOnly(yy)); + + size_t globalSize[2]; + globalSize[0] = depth_rows; + globalSize[1] = depth_cols; + + if (!kk.run(2, globalSize, NULL, true)) + throw std::runtime_error("Failed to run kernel"); + + return pixNorm; +} + const bool fixMissingData = false; depthType bilinearDepth(const Depth& m, cv::Point2f pt) { diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index e62c1de9cad..09efecf1252 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -35,6 +35,8 @@ inline float tsdfToFloat(TsdfType num) } cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics); +cv::UMat preCalculationPixNormGPU(const UMat& depth, const Intr& intrinsics); + depthType bilinearDepth(const Depth& m, cv::Point2f pt); void integrateVolumeUnit( From 22817a11ae0219ac30225d2ed264f9b6834cc136 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 16:32:59 +0300 Subject: [PATCH 199/216] minor code removement --- modules/rgbd/src/opencl/tsdf.cl | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/modules/rgbd/src/opencl/tsdf.cl b/modules/rgbd/src/opencl/tsdf.cl index e57bbdbabae..d92d8375651 100644 --- a/modules/rgbd/src/opencl/tsdf.cl +++ b/modules/rgbd/src/opencl/tsdf.cl @@ -27,16 +27,6 @@ static inline float tsdfToFloat(TsdfType num) return ( (float) num ) / (-128); } -__kernel void preCalculationPixNorm (__global float * pixNorms, - const __global float * xx, - const __global float * yy, - int width) -{ - int i = get_global_id(0); - int j = get_global_id(1); - int idx = i*width + j; - pixNorms[idx] = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); -} __kernel void integrate(__global const char * depthptr, int depth_step, int depth_offset, From b3be2d1f9e3eaca14bebe9db7a349adfe8480123 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 16:39:32 +0300 Subject: [PATCH 200/216] replace copyto() with getUMat() --- 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 5fa24f21edf..49f57b889a2 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1729,11 +1729,11 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Matx44f vol2camRotGPU = vol2cam.matrix; UMat volPoseGpu, invPoseGpu; - Mat(pose.matrix).copyTo(volPoseGpu); - Mat(pose.inv().matrix).copyTo(invPoseGpu); + volPoseGpu = Mat(pose.matrix).getUMat(ACCESS_READ); + invPoseGpu = Mat(pose.inv().matrix).getUMat(ACCESS_READ);; UMat hashesGpu(hashTable.hashDivisor, 1, CV_32S); - Mat(hashTable.hashes, false).copyTo(hashesGpu); + hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); Mat(hashTable.data, false).copyTo(hashDataGpu); From 5a6e7a406fffb5e134bf2bb27bdcfb43e13e234e Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 16:59:14 +0300 Subject: [PATCH 201/216] remove debug code --- modules/rgbd/src/hash_tsdf.cpp | 120 +-------------------------------- 1 file changed, 1 insertion(+), 119 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 49f57b889a2..16e205e2143 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1059,8 +1059,6 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto // for new indices CustomHashSet thm; - // ----------------------- - auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, CustomHashSet& ghm) { for (int y = yrange.start; y < yrange.end; y += depthStride) @@ -1100,8 +1098,6 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto pixLocalVolUnits[pixLocalCounter++] = tsdf_idx; if (pixLocalCounter >= pixCapacity) { - //DEBUG - std::cout << "allocate: pix capacity exhausted" << std::endl; return; } } @@ -1114,8 +1110,6 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto Vec3i idx = pixLocalVolUnits[i]; if (!ghm.insert(idx)) { - //DEBUG - std::cout << "allocate: local capacity exhausted" << std::endl; //return; } } @@ -1140,20 +1134,14 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto gr = gr & dim; Range xr(gr.tl().x, gr.br().x), yr(gr.tl().y, gr.br().y); - /* - LocalVolUnits localAccessVolUnits; - int loc_vol_idx = 0; - */ - CustomHashSet ghm; - fillLocalAcessVolUnits(xr, yr, ghm /*localAccessVolUnits, loc_vol_idx*/); + fillLocalAcessVolUnits(xr, yr, ghm); if (ghm.last) { std::lock_guard al(mutex); - //mutex.lock(); for (int i = 0; i < ghm.last; i++) { Vec4i node = ghm.data[i]; @@ -1165,40 +1153,9 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto if (!result) { needReallocation = true; - //DEBUG - std::cout << "new indices: need reallocation, exiting" << std::endl; - return; } - - /* - // if not found - //if (indexes.findRow(idx) < 0) - if (hashMap.find(idx) < 0) - { - //bool extend = indexes.insert(idx, lastVolIndex); - int result = hashMap.insert(idx, lastVolIndex); - //TODO: replace 1 by enum - if (result == 1) - { - Vec4i idx4(idx[0], idx[1], idx[2], 0); - nodeIndices.push_back(idx4); - lastVolIndex++; - } - else if (result == 0) - { - needReallocation = true; - //DEBUG - std::cout << "need reallocation, exiting" << std::endl; - - return; - } - } - */ - } - //mutex.unlock(); - } } } @@ -1209,8 +1166,6 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto { if (needReallocation) { - //DEBUG - std::cout << "reallocation group!! from: " << thm.capacity << " to x2: " << thm.capacity * 2 << std::endl; thm.capacity *= 2; thm.data.resize(thm.capacity); @@ -1235,86 +1190,16 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto if (result == 0) { _needReallocation = true; - //DEBUG - std::cout << "need reallocation, exiting" << std::endl; return; } } }; - //TODO: remove it - /* - auto pushToGlobalGpu = [](const ToyHashSet thm, ToyHashSet& globalHashMap, bool& needReallocation) - { - //TODO: set needReallocation based on thm.last + globalHashMap.last < globalHashMap.capacity - - String errorStr; - String name = "push_to_global"; - - ocl::ProgramSource source = ocl::rgbd::hash_tsdf_oclsrc; - String options = "-cl-mad-enable"; - ocl::Kernel k; - k.create(name.c_str(), source, options, &errorStr); - - if (k.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); - - // new hash table - UMat newHashesGpu(thm.hashDivisor, 1, CV_32S); - Mat(thm.hashes, false).copyTo(newHashesGpu); - UMat newHashDataGpu(thm.capacity, 1, CV_32SC4); - Mat(thm.data, false).copyTo(newHashDataGpu); - - // global hash map - UMat globalHashesGpu(globalHashMap.hashDivisor, 1, CV_32S); - Mat(globalHashMap.hashes, false).copyTo(globalHashesGpu); - UMat globalHashDataGpu(globalHashMap.capacity, 1, CV_32SC4); - Mat(globalHashMap.data, false).copyTo(globalHashDataGpu); - UMat globalHashLast(1, 1, CV_32S, Scalar(globalHashMap.last)); - - UMat hashMapLock(1, 1, CV_32S, Scalar(0)); - UMat needReallocationGpu(1, 1, CV_32S, Scalar(needReallocation)); - - k.args(ocl::KernelArg::PtrReadOnly(newHashesGpu), - ocl::KernelArg::PtrReadOnly(newHashDataGpu), - (int)thm.hashDivisor, - (int)thm.last, - ocl::KernelArg::PtrReadWrite(hashMapLock), - ocl::KernelArg::PtrReadWrite(needReallocationGpu), - ocl::KernelArg::PtrReadWrite(globalHashesGpu), - ocl::KernelArg::PtrReadWrite(globalHashDataGpu), - ocl::KernelArg::PtrReadWrite(globalHashLast), - globalHashMap.capacity, - globalHashMap.hashDivisor); - - size_t globalSize[1]; - //DEBUG - globalSize[0] = thm.last; - globalSize[0] = 64; - - if (!k.run(1, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); - - globalHashesGpu.copyTo(Mat(globalHashMap.hashes, false)); - globalHashDataGpu.copyTo(Mat(globalHashMap.data, false)); - globalHashMap.last = globalHashLast.getMat(ACCESS_READ).at(0, 0); - - needReallocation = (bool)needReallocationGpu.getMat(ACCESS_READ).at(0, 0); - - //DEBUG - if (needReallocation) - { - std::cout << "need reallocation" << std::endl; - } - }; - */ needReallocation = false; do { if (needReallocation) { - //DEBUG - std::cout << "reallocation global!! from: " << hashTable.capacity << " to x2: " << hashTable.capacity * 2 << std::endl; hashTable.capacity *= 2; hashTable.data.resize(hashTable.capacity); @@ -1322,10 +1207,7 @@ void HashTSDFVolumeGPU::allocateVolumeUnits(const UMat& _depth, float depthFacto } pushToGlobal(thm, hashTable, needReallocation, mutex); - //pushToGlobalGpu(thm, hashTable, needReallocation); } while (needReallocation); - - // --------------------- } From 41dc901b36aa97e6896f40da00974d93ad7e1d14 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 17:13:45 +0300 Subject: [PATCH 202/216] replace copyTo with getUmat again --- modules/rgbd/src/hash_tsdf.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 16e205e2143..1f944407455 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1006,10 +1006,10 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept Matx44f camInvMatrix = Affine3f(cameraPose).inv().matrix; UMat hashesGpu(hashTable.hashDivisor, 1, CV_32S); - Mat(hashTable.hashes, false).copyTo(hashesGpu); + hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); - Mat(hashTable.data, false).copyTo(hashDataGpu); + hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadOnly(hashesGpu), @@ -1229,10 +1229,10 @@ void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrin Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); UMat hashesGpu(hashTable.hashDivisor, 1, CV_32S); - Mat(hashTable.hashes, false).copyTo(hashesGpu); + hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); - Mat(hashTable.data, false).copyTo(hashDataGpu); + hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); k.args( ocl::KernelArg::PtrReadOnly(hashesGpu), @@ -1618,7 +1618,7 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); - Mat(hashTable.data, false).copyTo(hashDataGpu); + hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); k.args( ocl::KernelArg::PtrReadOnly(hashesGpu), From 3bbd6eae1235c1996402b8c5f2170351e4a43b0a Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 17:24:10 +0300 Subject: [PATCH 203/216] replace types in cl --- modules/rgbd/src/opencl/hash_tsdf.cl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index c7a4807e2ee..dfe37a7bc16 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -7,8 +7,8 @@ #define NAN_ELEMENT -2147483647 #define USE_INTERPOLATION_IN_GETNORMAL 1 -typedef __INT8_TYPE__ int8_t; -typedef __INT32_TYPE__ int32_t; +typedef char int8_t; +typedef uint int32_t; typedef int8_t TsdfType; typedef uchar WeightType; From 852fca69cde86bfc63692d28fbfd96ba6c95cfe2 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 17:26:55 +0300 Subject: [PATCH 204/216] remove NAN_ELEMENT --- modules/rgbd/src/opencl/hash_tsdf.cl | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index dfe37a7bc16..109319cd282 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -4,7 +4,6 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory -#define NAN_ELEMENT -2147483647 #define USE_INTERPOLATION_IN_GETNORMAL 1 typedef char int8_t; From bb87cb6b564e47d160bec442aca27025537b40ba Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 17:30:40 +0300 Subject: [PATCH 205/216] remove Volume_NODE struct and findRow() --- modules/rgbd/src/opencl/hash_tsdf.cl | 33 ---------------------------- 1 file changed, 33 deletions(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 109319cd282..fdad75abc4f 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -18,14 +18,6 @@ struct TsdfVoxel WeightType weight; }; -struct Volume_NODE -{ - int4 idx; - int32_t row; - int32_t nextVolumeRow; - int32_t dummy; - int32_t dummy2; -}; static inline TsdfType floatToTsdf(float num) { @@ -50,24 +42,6 @@ static uint calc_hash(int3 x) } -static int findRow(__global const struct Volume_NODE * hash_table, int3 indx, - int list_size, int bufferNums, int hash_divisor) -{ - int bufferNum = 0; - int hash = calc_hash(indx) % hash_divisor; - int i = (bufferNum * hash_divisor + hash) * list_size; - while (i >= 0) - { - struct Volume_NODE v = hash_table[i]; - if (all(v.idx.s012 == indx.s012)) - return v.row; - else - i = v.nextVolumeRow; - } - - return -1; -} - //TODO: make hashDivisor a power of 2 //TODO: put it to this .cl file as a constant static int custom_find(int3 idx, const int hashDivisor, __global const int* hashes, @@ -504,13 +478,6 @@ __kernel void raycast( __global const int* hashes, __global const int4* data, const int hash_divisor, - /* - __global const struct Volume_NODE * hash_table, - const int list_size, - const int bufferNums, - const int hash_divisor, - */ - __global char * pointsptr, int points_step, int points_offset, __global char * normalsptr, From d2bcdb019b5ebeb1efc778622f2f870561ea90f0 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 17:55:10 +0300 Subject: [PATCH 206/216] make hash_divisor constant --- modules/rgbd/src/hash_tsdf.cpp | 3 --- modules/rgbd/src/opencl/hash_tsdf.cl | 7 ++++--- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 1f944407455..dd11f873d16 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1014,7 +1014,6 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadOnly(hashesGpu), ocl::KernelArg::PtrReadOnly(hashDataGpu), - (int)hashTable.hashDivisor, ocl::KernelArg::ReadWrite(volUnitsData), ocl::KernelArg::ReadOnly(pixNorms), ocl::KernelArg::ReadOnly(isActiveFlags), @@ -1237,7 +1236,6 @@ void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrin k.args( ocl::KernelArg::PtrReadOnly(hashesGpu), ocl::KernelArg::PtrReadOnly(hashDataGpu), - (int)hashTable.hashDivisor, ocl::KernelArg::WriteOnly(isActiveFlags), ocl::KernelArg::WriteOnly(lastVisibleIndices), vol2cam.matrix, @@ -1623,7 +1621,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in k.args( ocl::KernelArg::PtrReadOnly(hashesGpu), ocl::KernelArg::PtrReadOnly(hashDataGpu), - (int)hashTable.hashDivisor, ocl::KernelArg::WriteOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), frameSize, diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index fdad75abc4f..71f19a6e695 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -5,6 +5,7 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory #define USE_INTERPOLATION_IN_GETNORMAL 1 +#define HASH_DIVISOR 32768 typedef char int8_t; typedef uint int32_t; @@ -225,7 +226,6 @@ __kernel void integrateAllVolumeUnits( // hashMap __global const int* hashes, __global const int4* data, - const int hash_divisor, // volUnitsData __global struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, @@ -252,6 +252,7 @@ __kernel void integrateAllVolumeUnits( const int maxWeight ) { + const int hash_divisor = HASH_DIVISOR; int i = get_global_id(0); int j = get_global_id(1); int row = get_global_id(2); @@ -477,7 +478,6 @@ typedef float4 ptype; __kernel void raycast( __global const int* hashes, __global const int4* data, - const int hash_divisor, __global char * pointsptr, int points_step, int points_offset, __global char * normalsptr, @@ -501,6 +501,7 @@ __kernel void raycast( int4 volStrides4 ) { + const int hash_divisor = HASH_DIVISOR; int x = get_global_id(0); int y = get_global_id(1); @@ -603,7 +604,6 @@ __kernel void raycast( __kernel void markActive ( __global const int* hashes, __global const int4* data, - const int hash_divisor, __global char* isActiveFlagsPtr, int isActiveFlagsStep, int isActiveFlagsOffset, @@ -623,6 +623,7 @@ __kernel void markActive ( const int frameId ) { + const int hash_divisor = HASH_DIVISOR; int row = get_global_id(0); if (row < lastVolIndex) From df7b4e4024dfa9605740615c9bff4cc0f8da7ebd Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 17:57:04 +0300 Subject: [PATCH 207/216] uncomment camTrans --- modules/rgbd/src/opencl/hash_tsdf.cl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 71f19a6e695..569b72563e3 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -514,7 +514,7 @@ __kernel void raycast( const float3 camRot0 = cam2volRotGPU.s012; const float3 camRot1 = cam2volRotGPU.s456; const float3 camRot2 = cam2volRotGPU.s89a; - //const float3 camTrans = cam2volRotGPU.s37b; + const float3 camTrans = cam2volRotGPU.s37b; const float3 volRot0 = vol2camRotGPU.s012; const float3 volRot1 = vol2camRotGPU.s456; From 5f8bd0dcc29ce5432cc54512c0fe84bf55a5ccde Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 17:59:57 +0300 Subject: [PATCH 208/216] rename data; remove extra val --- modules/rgbd/src/hash_tsdf.cpp | 4 ---- modules/rgbd/src/opencl/hash_tsdf.cl | 5 ++--- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index dd11f873d16..dd8522b3419 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1227,14 +1227,10 @@ void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrin const Intr::Projector proj(intrinsics.makeProjector()); Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); - UMat hashesGpu(hashTable.hashDivisor, 1, CV_32S); - hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); - UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); k.args( - ocl::KernelArg::PtrReadOnly(hashesGpu), ocl::KernelArg::PtrReadOnly(hashDataGpu), ocl::KernelArg::WriteOnly(isActiveFlags), ocl::KernelArg::WriteOnly(lastVisibleIndices), diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index 569b72563e3..ad39b57c131 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -602,8 +602,7 @@ __kernel void raycast( __kernel void markActive ( - __global const int* hashes, - __global const int4* data, + __global const int4* hashSetData, __global char* isActiveFlagsPtr, int isActiveFlagsStep, int isActiveFlagsOffset, @@ -628,7 +627,7 @@ __kernel void markActive ( if (row < lastVolIndex) { - int3 idx = data[row].xyz; + int3 idx = hashSetData[row].xyz; float3 volumeUnitPos = convert_float3(idx) * volumeUnitSize; From b9891c5a4636bfbd2cf07ee74c95d859398f830d Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 18:07:12 +0300 Subject: [PATCH 209/216] fix merge conflict --- modules/rgbd/src/tsdf.cpp | 52 --------------------------------------- 1 file changed, 52 deletions(-) diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 60e29e7bae4..0a0fcfacf05 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -833,50 +833,6 @@ void TSDFVolumeGPU::reset() volume.setTo(Scalar(0, 0)); } -<<<<<<< HEAD -======= -static void preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f fxy, Vec2f cxy, UMat& pixNorm) -{ - Mat x(1, depth_cols, CV_32F); - Mat y(1, depth_rows, CV_32F); - pixNorm.create(1, depth_rows * depth_cols, CV_32F); - - for (int i = 0; i < depth_cols; i++) - x.at(0, i) = (i - cxy[0]) / fxy[0]; - for (int i = 0; i < depth_rows; i++) - y.at(0, i) = (i - cxy[1]) / fxy[1]; - - cv::String errorStr; - cv::String name = "preCalculationPixNorm"; - ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; - ocl::Kernel kk; - kk.create(name.c_str(), source, options, &errorStr); - - - if (kk.empty()) - throw std::runtime_error("Failed to create kernel: " + errorStr); - - AccessFlag af = ACCESS_READ; - UMat xx = x.getUMat(af); - UMat yy = y.getUMat(af); - - kk.args(ocl::KernelArg::ReadWrite(pixNorm), - ocl::KernelArg::PtrReadOnly(xx), - ocl::KernelArg::PtrReadOnly(yy), - depth_cols, depth_rows); - - size_t globalSize[2]; - globalSize[0] = depth_rows; - globalSize[1] = depth_cols; - - if (!kk.run(2, globalSize, NULL, true)) - throw std::runtime_error("Failed to run kernel"); - - return; -} ->>>>>>> 4e85f8c6dc50e954a2caf253c32fca8ac2be6888 - // use depth instead of distance (optimization) void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) @@ -906,16 +862,8 @@ void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, intrinsics.cx, intrinsics.cy); if (!(frameParams == newParams)) { -<<<<<<< HEAD frameParams = newParams; pixNorms = preCalculationPixNormGPU(depth, intrinsics); -======= - 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; - - preCalculationPixNormGPU(depth.rows, depth.cols, fxy, cxy, pixNorms); ->>>>>>> 4e85f8c6dc50e954a2caf253c32fca8ac2be6888 } // TODO: optimization possible From 2b3fce750afbc0204527d09438e4fc112d44807a Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 22:03:21 +0300 Subject: [PATCH 210/216] create empty UMat --- modules/rgbd/src/hash_tsdf.cpp | 22 +++++++--------------- 1 file changed, 7 insertions(+), 15 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index dd8522b3419..030f988e125 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1005,11 +1005,8 @@ void HashTSDFVolumeGPU::integrateAllVolumeUnitsGPU(const UMat& depth, float dept Matx44f vol2camMatrix = (Affine3f(cameraPose).inv() * pose).matrix; Matx44f camInvMatrix = Affine3f(cameraPose).inv().matrix; - UMat hashesGpu(hashTable.hashDivisor, 1, CV_32S); - hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); - - UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); - hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); + UMat hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); + UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadOnly(hashesGpu), @@ -1227,8 +1224,7 @@ void HashTSDFVolumeGPU::markActive(const Matx44f& cameraPose, const Intr& intrin const Intr::Projector proj(intrinsics.makeProjector()); Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); - UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); - hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); + UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); k.args( ocl::KernelArg::PtrReadOnly(hashDataGpu), @@ -1604,15 +1600,11 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in Matx44f cam2volRotGPU = cam2vol.matrix; Matx44f vol2camRotGPU = vol2cam.matrix; - UMat volPoseGpu, invPoseGpu; - volPoseGpu = Mat(pose.matrix).getUMat(ACCESS_READ); - invPoseGpu = Mat(pose.inv().matrix).getUMat(ACCESS_READ);; - - UMat hashesGpu(hashTable.hashDivisor, 1, CV_32S); - hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); + UMat volPoseGpu = Mat(pose.matrix).getUMat(ACCESS_READ); + UMat invPoseGpu = Mat(pose.inv().matrix).getUMat(ACCESS_READ); - UMat hashDataGpu(hashTable.capacity, 1, CV_32SC4); - hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); + UMat hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); + UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); k.args( ocl::KernelArg::PtrReadOnly(hashesGpu), From 36d15a4114c70c2117d5845270ab5697c87266c9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Feb 2021 22:09:49 +0300 Subject: [PATCH 211/216] cl fix --- modules/rgbd/src/opencl/tsdf.cl | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/modules/rgbd/src/opencl/tsdf.cl b/modules/rgbd/src/opencl/tsdf.cl index a1e55d689b1..3a13d3b8336 100644 --- a/modules/rgbd/src/opencl/tsdf.cl +++ b/modules/rgbd/src/opencl/tsdf.cl @@ -26,23 +26,6 @@ static inline float tsdfToFloat(TsdfType num) return ( (float) num ) / (-128); } -<<<<<<< HEAD -======= -__kernel void preCalculationPixNorm (__global float * pixNorms, - int pix_step, int pix_offset, - int pix_rows, int pix_cols, - const __global float * xx, - const __global float * yy, - int width, int height) -{ - int i = get_global_id(0); - int j = get_global_id(1); - int idx = i*width + j; - if(i < height && j < width && idx < pix_cols) - pixNorms[idx] = sqrt(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f); -} ->>>>>>> 4e85f8c6dc50e954a2caf253c32fca8ac2be6888 - __kernel void integrate(__global const char * depthptr, int depth_step, int depth_offset, int depth_rows, int depth_cols, From 63d516c4b6bfdc74961269c456461ab0a78624fa Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 18 Feb 2021 11:21:22 +0300 Subject: [PATCH 212/216] build error fix --- modules/rgbd/src/tsdf_functions.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index d60eb02fa73..b0e5276cba7 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -36,6 +36,7 @@ cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) return pixNorm; } +#ifdef HAVE_OPENCL cv::UMat preCalculationPixNormGPU(const UMat& depth, const Intr& intrinsics) { int depth_cols = depth.cols; @@ -78,6 +79,7 @@ cv::UMat preCalculationPixNormGPU(const UMat& depth, const Intr& intrinsics) return pixNorm; } +#endif const bool fixMissingData = false; depthType bilinearDepth(const Depth& m, cv::Point2f pt) From ec12ba7099f16be553a907b44d9acda21e5d67a5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 18 Feb 2021 14:08:09 +0300 Subject: [PATCH 213/216] clear HashTSDFVolumeGPU() --- modules/rgbd/src/hash_tsdf.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 030f988e125..51e01aaaaa8 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -954,13 +954,8 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(const VolumeParams & _params, bool _zFirstM : HashTSDFVolumeGPU(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) { - if (truncDist >= volumeUnitSize) - { - std::cout << "truncDist exceeds volume unit size, allocation may work incorrectly" << std::endl; - } - - reset(); } + // zero volume, leave rest params the same void HashTSDFVolumeGPU::reset() { From f274e15d46bbfeb4f358df69c0e8aa5153efa9b4 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 18 Feb 2021 14:12:23 +0300 Subject: [PATCH 214/216] use constuctor delegation at HashTSDFVolumeCPU --- modules/rgbd/src/hash_tsdf.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 51e01aaaaa8..1ea5012ad2a 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -151,10 +151,11 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, flo } HashTSDFVolumeCPU::HashTSDFVolumeCPU(const VolumeParams& _params, bool _zFirstMemOrder) - : HashTSDFVolume(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + : HashTSDFVolumeCPU(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) { } + // zero volume, leave rest params the same void HashTSDFVolumeCPU::reset() { From dd45b0a7ab145e8500be7402a3ac1d2a55ab7867 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 18 Feb 2021 16:27:24 +0300 Subject: [PATCH 215/216] remove cam2volTransGPU --- modules/rgbd/src/hash_tsdf.cpp | 4 ---- modules/rgbd/src/opencl/hash_tsdf.cl | 3 +-- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 1ea5012ad2a..93440b00c4f 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1590,9 +1590,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in const Affine3f cam2vol(volume.pose.inv() * Affine3f(cameraPose)); const Affine3f vol2cam(Affine3f(cameraPose.inv()) * volume.pose); - const Point3f cam2volTrans = cam2vol.translation(); - - Vec4f cam2volTransGPU(cam2volTrans.x, cam2volTrans.y, cam2volTrans.z, 0); Matx44f cam2volRotGPU = cam2vol.matrix; Matx44f vol2camRotGPU = vol2cam.matrix; @@ -1609,7 +1606,6 @@ void HashTSDFVolumeGPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& in ocl::KernelArg::WriteOnlyNoSize(normals), frameSize, ocl::KernelArg::ReadOnly(volUnitsData), - cam2volTransGPU, cam2volRotGPU, vol2camRotGPU, float(volume.truncateThreshold), diff --git a/modules/rgbd/src/opencl/hash_tsdf.cl b/modules/rgbd/src/opencl/hash_tsdf.cl index ad39b57c131..0e611c1d3dd 100644 --- a/modules/rgbd/src/opencl/hash_tsdf.cl +++ b/modules/rgbd/src/opencl/hash_tsdf.cl @@ -486,7 +486,6 @@ __kernel void raycast( __global const struct TsdfVoxel * allVolumePtr, int table_step, int table_offset, int table_rows, int table_cols, - float4 cam2volTransGPU, float16 cam2volRotGPU, float16 vol2camRotGPU, float truncateThreshold, @@ -526,7 +525,7 @@ __kernel void raycast( dot(planed, camRot1), dot(planed, camRot2)); - float3 orig = (float3) (cam2volTransGPU.s0, cam2volTransGPU.s1, cam2volTransGPU.s2); + float3 orig = (float3) (camTrans.s0, camTrans.s1, camTrans.s2); float3 dir = fast_normalize(planed); float3 origScaled = orig * voxelSizeInv; float3 dirScaled = dir * voxelSizeInv; From 499047d60a5e6a708d88d285d69318d2894e7767 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 18 Feb 2021 16:49:39 +0300 Subject: [PATCH 216/216] add minor fix --- modules/rgbd/src/hash_tsdf.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 93440b00c4f..08338fe83ef 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -948,6 +948,11 @@ HashTSDFVolumeGPU::HashTSDFVolumeGPU(float _voxelSize, const Matx44f& _pose, flo float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { + if (truncDist >= volumeUnitSize) + { + std::cout << "truncDist exceeds volume unit size, allocation may work incorrectly" << std::endl; + } + reset(); }