// 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 // Partially rewritten from https://github.com/Nerei/kinfu_remake // Copyright(c) 2012, Anatoly Baksheev. All rights reserved. #include "precomp.hpp" #include "hash_tsdf_functions.hpp" #include "opencl_kernels_ptcloud.hpp" namespace cv { static Vec3i volumeToVolumeUnitIdx(const Point3f& point, const float volumeUnitSize) { return cv::Vec3i( cvFloor(point.x / volumeUnitSize), cvFloor(point.y / volumeUnitSize), cvFloor(point.z / volumeUnitSize)); } static cv::Point3f volumeUnitIdxToVolume(const cv::Vec3i& volumeUnitIdx, const float volumeUnitSize) { return cv::Point3f( volumeUnitIdx[0] * volumeUnitSize, volumeUnitIdx[1] * volumeUnitSize, volumeUnitIdx[2] * volumeUnitSize); } static cv::Point3f voxelCoordToVolume(const cv::Vec3i& voxelIdx, const float voxelSize) { return cv::Point3f( voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); } static cv::Vec3i volumeToVoxelCoord(const cv::Point3f& point, const float voxelSizeInv) { return cv::Vec3i( cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } int calcVolumeUnitDegree(Point3i volumeResolution) { if (!(volumeResolution.x & (volumeResolution.x - 1))) { // vuRes is a power of 2, let's get this power return trailingZeros32(volumeResolution.x); } else { CV_Error(Error::StsBadArg, "Volume unit resolution should be a power of 2"); } } void allocateVolumeUnits( const UMat& _depth, float depthFactor, const Affine3f volumePose, const Matx44f& cameraPose, const Intr& intrinsics, CustomHashSet& hashTable, const int volumeUnitDegree, const float truncDist, const float maxDepth, const float volumeUnitSize); TsdfVoxel atVolumeUnit( const Mat& volUnitsData, const VolumeUnitIndexes& volumeUnits, const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, const int volumeUnitDegree, const Vec4i volStrides); TsdfVoxel new_atVolumeUnit( const Mat& volUnitsDataCopy, const Vec3i& point, const Vec3i& volumeUnitIdx, const int indx, const int volumeUnitDegree, const Vec4i volStrides); Point3f getNormalVoxel( const Point3f& point, const float voxelSizeInv, const int volumeUnitDegree, const Vec4i volStrides, const Mat& volUnitsDataCopy, const VolumeUnitIndexes& volumeUnits); #ifdef HAVE_OPENCL void markActive( const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId, const Affine3f volumePose, CustomHashSet& hashTable, UMat& isActiveFlags, UMat& lastVisibleIndices, const float maxDepth, const float volumeUnitSize); Point3f ocl_getNormalVoxel( const Point3f& point, const float voxelSizeInv, const int volumeUnitDegree, const Vec4i volStrides, const Mat& volUnitsData, const CustomHashSet& hashTable); #endif void integrateHashTsdfVolumeUnit( const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth, InputArray _depth, InputArray _pixNorms, InputOutputArray _volUnitsData, VolumeUnitIndexes& volumeUnits) { CV_TRACE_FUNCTION(); CV_Assert(_depth.type() == DEPTH_TYPE); Depth depth = _depth.getMat(); Mat& volUnitsData = _volUnitsData.getMatRef(); Mat pixNorms = _pixNorms.getMat(); Matx44f _pose; settings.getVolumePose(_pose); const Affine3f pose = Affine3f(_pose); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); Matx33f intr; settings.getCameraIntegrateIntrinsics(intr); const Intr intrinsics(intr); const Intr::Reprojector reproj(intrinsics.makeReprojector()); const float maxDepth = settings.getMaxDepth(); const float voxelSize = settings.getVoxelSize(); Vec3i resolution; settings.getVolumeResolution(resolution); const float volumeUnitSize = voxelSize * resolution[0]; if (enableGrowth) { //! Compute volumes to be allocated const int depthStride = volumeUnitDegree; const float invDepthFactor = 1.f / settings.getDepthFactor(); const float truncDist = settings.getTsdfTruncateDistance(); const Point3f truncPt(truncDist, truncDist, truncDist); std::unordered_set newIndices; Mutex mutex; Range allocateRange(0, depth.rows); auto AllocateVolumeUnitsInvoker = [&](const Range& range) { std::unordered_set localAccessVolUnits; 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 > maxDepth) 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 = volumeToVolumeUnitIdx(volPoint - truncPt, volumeUnitSize); Vec3i upper_bound = volumeToVolumeUnitIdx(volPoint + truncPt, volumeUnitSize); 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 (localAccessVolUnits.count(tsdf_idx) <= 0 && volumeUnits.count(tsdf_idx) <= 0) { //! This volume unit will definitely be required for current integration localAccessVolUnits.emplace(tsdf_idx); } } } } mutex.lock(); for (const auto& tsdf_idx : localAccessVolUnits) { //! If the insert into the global set passes if (!newIndices.count(tsdf_idx)) { // Volume allocation can be performed outside of the lock newIndices.emplace(tsdf_idx); } } mutex.unlock(); }; parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); //! Perform the allocation for (auto idx : newIndices) { VolumeUnit& vu = volumeUnits.emplace(idx, VolumeUnit()).first->second; Matx44f subvolumePose = pose.translate(pose.rotation() * volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix; vu.pose = subvolumePose; vu.index = lastVolIndex; if (lastVolIndex >= int(volUnitsData.size().height)) { volUnitsData.resize(lastVolIndex * 2); CV_LOG_DEBUG(NULL, "HashTSDF storage extended from " << lastVolIndex << " to " << lastVolIndex * 2 << " volume units"); } lastVolIndex++; 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; } } //! Get keys for all the allocated volume Units std::vector totalVolUnits; for (const auto& keyvalue : volumeUnits) { totalVolUnits.push_back(keyvalue.first); } //! Mark volumes in the camera frustum as active Range inFrustumRange(0, (int)volumeUnits.size()); parallel_for_(inFrustumRange, [&](const Range& range) { 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()) continue; Point3f volumeUnitPos = volumeUnitIdxToVolume(it->first, volumeUnitSize); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > maxDepth) { it->second.isActive = false; continue; } 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; } } }); //! 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 integrateTsdfVolumeUnit(settings, volumeUnit.pose, cameraPose, depth, pixNorms, volUnitsData.row(volumeUnit.index)); //! Ensure all active volumeUnits are set to inactive for next integration volumeUnit.isActive = false; } } }); } void allocateVolumeUnits( const UMat& _depth, float depthFactor, const Affine3f volumePose, const Matx44f& cameraPose, const Intr& intrinsics, CustomHashSet& hashTable, const int volumeUnitDegree, const float truncDist, const float maxDepth, const float volumeUnitSize) { constexpr int pixCapacity = 16; typedef std::array LocalVolUnits; Depth depth = _depth.getMat(ACCESS_READ); //! Compute volumes to be allocated const int depthStride = volumeUnitDegree; const float invDepthFactor = 1.f / depthFactor; const Intr::Reprojector reproj(intrinsics.makeReprojector()); const Affine3f cam2vol(volumePose.inv() * Affine3f(cameraPose)); const Point3f truncPt(truncDist, truncDist, truncDist); Mutex mutex; // 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) { const depthType* depthRow = depth[y]; for (int x = xrange.start; x < xrange.end; x += depthStride) { depthType z = depthRow[x] * invDepthFactor; if (z <= 0 || z > maxDepth) 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 = volumeToVolumeUnitIdx(volPoint - truncPt, volumeUnitSize); Vec3i upper_bound = volumeToVolumeUnitIdx(volPoint + truncPt, volumeUnitSize); 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 (hashTable.find(tsdf_idx) < 0) { bool found = false; for (int c = 0; c < pixLocalCounter; c++) { if (pixLocalVolUnits[c] == tsdf_idx) { found = true; break; } } if (!found) { pixLocalVolUnits[pixLocalCounter++] = tsdf_idx; if (pixLocalCounter >= pixCapacity) { return; } } } } // lock localAccessVolUnits somehow for (int i = 0; i < pixLocalCounter; i++) { Vec3i idx = pixLocalVolUnits[i]; if (!ghm.insert(idx)) { //return; } } // unlock } } }; Rect dim(0, 0, depth.cols, depth.rows); Size gsz(32, 32); Size gg(divUp(dim.width, gsz.width), divUp(dim.height, gsz.height)); bool needReallocation = false; auto allocateLambda = [&](const Range& r) { for (int yg = r.start; yg < r.end; yg++) { for (int xg = 0; xg < gg.width; xg++) { 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); CustomHashSet ghm; fillLocalAcessVolUnits(xr, yr, ghm); if (ghm.last) { cv::AutoLock al(mutex); for (int i = 0; i < ghm.last; i++) { 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; return; } } } } } }; do { if (needReallocation) { thm.capacity *= 2; thm.data.resize(thm.capacity); needReallocation = false; } parallel_for_(Range(0, gg.height), allocateLambda); } while (needReallocation); auto pushToGlobal = [](const CustomHashSet _thm, CustomHashSet& _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; return; } } }; needReallocation = false; do { if (needReallocation) { hashTable.capacity *= 2; hashTable.data.resize(hashTable.capacity); needReallocation = false; } pushToGlobal(thm, hashTable, needReallocation, mutex); } while (needReallocation); } #ifdef HAVE_OPENCL void markActive( const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId, const Affine3f volumePose, CustomHashSet& hashTable, UMat& isActiveFlags, UMat& lastVisibleIndices, const float maxDepth, const float volumeUnitSize) { //! Mark volumes in the camera frustum as active String errorStr; String name = "markActive"; ocl::ProgramSource source = ocl::ptcloud::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); const Affine3f vol2cam(Affine3f(cameraPose.inv()) * volumePose); const Intr::Projector proj(intrinsics.makeProjector()); Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); k.args( ocl::KernelArg::PtrReadOnly(hashDataGpu), ocl::KernelArg::WriteOnly(isActiveFlags), ocl::KernelArg::WriteOnly(lastVisibleIndices), vol2cam.matrix, fxy, cxy, frameSz, volumeUnitSize, hashTable.last, maxDepth, frameId ); size_t globalSize[1] = { (size_t)hashTable.last }; if (!k.run(1, globalSize, nullptr, true)) throw std::runtime_error("Failed to run kernel"); } void ocl_integrateHashTsdfVolumeUnit( const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree, bool enableGrowth, InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputOutputArray _volUnitsDataCopy, InputOutputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags) { CV_TRACE_FUNCTION(); UMat depth = _depth.getUMat(); CV_Assert(!depth.empty()); CV_Assert(lastVolIndex >= 0); UMat pixNorms = _pixNorms.getUMat(); UMat& volUnitsData = _volUnitsData.getUMatRef(); Mat& volUnitsDataCopy = _volUnitsDataCopy.getMatRef(); UMat isActiveFlags = _isActiveFlags.getUMat(); UMat lastVisibleIndices = _lastVisibleIndices.getUMat(); Matx33f intr; settings.getCameraIntegrateIntrinsics(intr); const Intr intrinsics(intr); Vec4i volStrides; settings.getVolumeStrides(volStrides); Vec3i resolution; settings.getVolumeResolution(resolution); const int volumeUnitResolution = resolution[0]; const int maxWeight = settings.getMaxWeight(); const float truncDist = settings.getTsdfTruncateDistance(); const float maxDepth = settings.getMaxDepth(); const float voxelSize = settings.getVoxelSize(); const float depthFactor = settings.getDepthFactor(); const float dfac = 1.f / depthFactor; const float volumeUnitSize = voxelSize * resolution[0]; Matx44f _pose; settings.getVolumePose(_pose); const Affine3f pose = Affine3f(_pose); Matx44f vol2camMatrix = (Affine3f(cameraPose).inv() * pose).matrix; if (enableGrowth) { // Save length to fill new data in ranges int sizeBefore = hashTable.last; allocateVolumeUnits(depth, depthFactor, pose, cameraPose, intrinsics, hashTable, volumeUnitDegree, truncDist, maxDepth, volumeUnitSize); int sizeAfter = hashTable.last; //! Perform the allocation // Grow buffers int buff_lvl = (int)(1 << bufferSizeDegree); if (sizeAfter >= buff_lvl) { bufferSizeDegree = (int)(log2(sizeAfter) + 1); // clz() would be better int oldBuffSize = buff_lvl; buff_lvl = (int)pow(2, bufferSizeDegree); volUnitsDataCopy.resize(buff_lvl); Range oldr(0, oldBuffSize); int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution; 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; UMat newIsActiveFlags(buff_lvl, 1, CV_8U); isActiveFlags.copyTo(newIsActiveFlags.rowRange(oldr)); isActiveFlags = newIsActiveFlags; } // Fill data for new volume units Range r(sizeBefore, sizeAfter); if (r.start < r.end) { 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 markActive(cameraPose, intrinsics, depth.size(), frameId, pose, hashTable, isActiveFlags, lastVisibleIndices, maxDepth, volumeUnitSize); //! Integrate the correct volumeUnits String errorStr; String name = "integrateAllVolumeUnits"; ocl::ProgramSource source = ocl::ptcloud::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); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); 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), ocl::KernelArg::PtrReadOnly(hashDataGpu), ocl::KernelArg::ReadWrite(volUnitsData), ocl::KernelArg::ReadOnly(pixNorms), ocl::KernelArg::ReadOnly(isActiveFlags), vol2camMatrix, voxelSize, volumeUnitResolution, volStrides.val, fxy.val, cxy.val, dfac, truncDist, int(maxWeight) ); int resol = volumeUnitResolution; size_t globalSize[3]; globalSize[0] = (size_t)resol; globalSize[1] = (size_t)resol; 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"); } #endif inline TsdfVoxel _at(Mat& volUnitsData, const cv::Vec3i& volumeIdx, int indx, const int volumeUnitResolution, const Vec4i volStrides) { //! Out of bounds if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) { return TsdfVoxel(floatToTsdf(1.f), 0); } const TsdfVoxel* volData = volUnitsData.ptr(indx); int coordBase = volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[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 = v_add(v0246, v_mul(v_setall_f32(tz), v_sub(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 = v_add(v00_10, v_mul(v_setall_f32(ty), v_sub(v01_11, v00_10))); float v0 = v_get0(v0_1); v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); float v1 = v_get0(v0_1); 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 TsdfVoxel atVolumeUnit( const Mat& volUnitsData, const VolumeUnitIndexes& volumeUnits, const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it, const int volumeUnitDegree, const Vec4i volStrides) { if (it == volumeUnits.end()) { return TsdfVoxel(floatToTsdf(1.f), 0); } 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); int coordBase = volUnitLocalIdx[0] * volStrides[0] + volUnitLocalIdx[1] * volStrides[1] + volUnitLocalIdx[2] * volStrides[2]; return volData[coordBase]; } TsdfVoxel new_atVolumeUnit( const Mat& volUnitsDataCopy, const Vec3i& point, const Vec3i& volumeUnitIdx, const int indx, const int volumeUnitDegree, const Vec4i volStrides) { if (indx < 0) { return TsdfVoxel(floatToTsdf(1.f), 0); } Vec3i volUnitLocalIdx = point - Vec3i(volumeUnitIdx[0] << volumeUnitDegree, volumeUnitIdx[1] << volumeUnitDegree, volumeUnitIdx[2] << volumeUnitDegree); // expanding at(), removing bounds check const TsdfVoxel* volData = volUnitsDataCopy.ptr(indx); int coordBase = volUnitLocalIdx[0] * volStrides[0] + volUnitLocalIdx[1] * volStrides[1] + volUnitLocalIdx[2] * volStrides[2]; return volData[coordBase]; } Point3f getNormalVoxel( const Point3f& point, const float voxelSizeInv, const int volumeUnitDegree, const Vec4i volStrides, const Mat& volUnitsDataCopy, const VolumeUnitIndexes& volumeUnits) { 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 = 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]) { it = volumeUnits.find(volumeUnitIdx); iterMap[dictIdx] = it; queried[dictIdx] = true; } vals[i] = tsdfToFloat(atVolumeUnit(volUnitsDataCopy, volumeUnits, pt, volumeUnitIdx, it, volumeUnitDegree, volStrides).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_WIDTH >= 32 v_float32x8 cxp = v_lut(vals, v256_load(idxxp)); v_float32x8 cxn = v_lut(vals, v256_load(idxxn)); v_float32x8 cyp = v_lut(vals, v256_load(idxyp)); v_float32x8 cyn = v_lut(vals, v256_load(idxyn)); v_float32x8 czp = v_lut(vals, v256_load(idxzp)); v_float32x8 czn = v_lut(vals, v256_load(idxzn)); v_float32x8 vcxv = v_sub(cxn, cxp); v_float32x8 vcyv = v_sub(cyn, cyp); v_float32x8 vczv = v_sub(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 = v_sub(cxn0, cxp0); v_float32x4 cxv1 = v_sub(cxn1, cxp1); v_float32x4 cyv0 = v_sub(cyn0, cyp0); v_float32x4 cyv1 = v_sub(cyn1, cyp1); v_float32x4 czv0 = v_sub(czn0, czp0); v_float32x4 czv1 = v_sub(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; } #ifdef HAVE_OPENCL Point3f ocl_getNormalVoxel( const Point3f& point, const float voxelSizeInv, const int volumeUnitDegree, const Vec4i volStrides, const Mat& volUnitsData, const CustomHashSet& hashTable) { 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 // -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] = -2; } #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 = 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) { it = hashTable.find(volumeUnitIdx); iterMap[dictIdx] = it; } vals[i] = tsdfToFloat(new_atVolumeUnit(volUnitsData, pt, volumeUnitIdx, it, volumeUnitDegree, volStrides).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_WIDTH >= 32 v_float32x8 cxp = v_lut(vals, v256_load(idxxp)); v_float32x8 cxn = v_lut(vals, v256_load(idxxn)); v_float32x8 cyp = v_lut(vals, v256_load(idxyp)); v_float32x8 cyn = v_lut(vals, v256_load(idxyn)); v_float32x8 czp = v_lut(vals, v256_load(idxzp)); v_float32x8 czn = v_lut(vals, v256_load(idxzn)); v_float32x8 vcxv = v_sub(cxn, cxp); v_float32x8 vcyv = v_sub(cyn, cyp); v_float32x8 vczv = v_sub(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 = v_sub(cxn0, cxp0); v_float32x4 cxv1 = v_sub(cxn1, cxp1); v_float32x4 cyv0 = v_sub(cyn0, cyp0); v_float32x4 cyv1 = v_sub(cyn1, cyp1); v_float32x4 czv0 = v_sub(czn0, czp0); v_float32x4 czv1 = v_sub(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; } #endif void raycastHashTsdfVolumeUnit( const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals) { CV_TRACE_FUNCTION(); Size frameSize(width, height); CV_Assert(frameSize.area() > 0); Matx33f mintr(intr.getMat()); Mat volUnitsData = _volUnitsData.getMat(); _points.create(frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE); Points points1 = _points.getMat(); Normals normals1 = _normals.getMat(); Points& points(points1); Normals& normals(normals1); const float truncDist = settings.getTsdfTruncateDistance(); const float raycastStepFactor = settings.getRaycastStepFactor(); const float tstep = truncDist * raycastStepFactor; const float maxDepth = settings.getMaxDepth(); const float voxelSize = settings.getVoxelSize(); const float voxelSizeInv = 1.f / voxelSize; const Vec4i volDims; settings.getVolumeStrides(volDims); Vec3i resolution; settings.getVolumeResolution(resolution); const Point3i volResolution = Point3i(resolution); const float volumeUnitSize = voxelSize * resolution[0]; Matx44f _pose; settings.getVolumePose(_pose); const Affine3f pose = Affine3f(_pose); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); const Intr intrinsics(mintr); 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 = 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 = maxDepth; 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; float currTsdf = truncDist; while (tcurr < tmax) { Point3f currRayPos = orig + tcurr * rayDirV; cv::Vec3i currVolumeUnitIdx = volumeToVolumeUnitIdx(currRayPos, volumeUnitSize); VolumeUnitIndexes::const_iterator it = volumeUnits.find(currVolumeUnitIdx); currTsdf = prevTsdf; float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; //! The subvolume exists in hashtable if (it != volumeUnits.end()) { cv::Point3f currVolUnitPos = volumeUnitIdxToVolume(currVolumeUnitIdx, volumeUnitSize); volUnitLocalIdx = volumeToVoxelCoord(currRayPos - currVolUnitPos, voxelSizeInv); //! TODO: Figure out voxel interpolation TsdfVoxel currVoxel = _at(volUnitsData, volUnitLocalIdx, it->second.index, volResolution.x, volDims); currTsdf = tsdfToFloat(currVoxel.tsdf); if (currTsdf != prevTsdf) { // from pos to zero or negative // or from neg to zero or positive bool posCurr = currTsdf > 0.f, posPrev = prevTsdf > 0.f; bool negCurr = currTsdf < 0.f, negPrev = prevTsdf < 0.f; if (posCurr != posPrev || negCurr != negPrev) break; } stepSize = tstep; } prevVolumeUnitIdx = currVolumeUnitIdx; prevTsdf = currTsdf; tprev = tcurr; tcurr += stepSize; } //! Surface crossing if (prevTsdf >= 0.f && currTsdf <= 0.f && prevTsdf > currTsdf) { float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { Point3f pv = orig + tInterp * rayDirV; Point3f nv = getNormalVoxel(pv, voxelSizeInv, volumeUnitDegree, volDims, volUnitsData, volumeUnits); if (!isNaN(nv)) { normal = vol2camRot * nv; point = vol2cam * pv; } } } ptsRow[x] = toPtype(point); nrmRow[x] = toPtype(normal); } } }; parallel_for_(Range(0, points.rows), _HashRaycastInvoker, nstripes); } #ifdef HAVE_OPENCL void ocl_raycastHashTsdfVolumeUnit( const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree, const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals) { CV_TRACE_FUNCTION(); Size frameSize(width, height); CV_Assert(frameSize.area() > 0); Matx33f mintr(intr.getMat()); UMat volUnitsData = _volUnitsData.getUMat(); String errorStr; String name = "raycast"; ocl::ProgramSource source = ocl::ptcloud::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); _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); UMat points = _points.getUMat(); UMat normals = _normals.getUMat(); Intr intrinsics(mintr); Intr::Reprojector r = intrinsics.makeReprojector(); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); const float truncDist = settings.getTsdfTruncateDistance(); const float raycastStepFactor = settings.getRaycastStepFactor(); const float tstep = truncDist * raycastStepFactor; const float maxDepth = settings.getMaxDepth(); const float voxelSize = settings.getVoxelSize(); const float voxelSizeInv = 1.f / voxelSize; const Vec4i volStrides; settings.getVolumeStrides(volStrides); Vec3i resolution; settings.getVolumeResolution(resolution); const Point3i volResolution = Point3i(resolution); const float volumeUnitSize = voxelSize * volResolution.x; Vec4f boxMin, boxMax(volumeUnitSize - voxelSize, volumeUnitSize - voxelSize, volumeUnitSize - voxelSize); Matx44f _pose; settings.getVolumePose(_pose); const Affine3f pose = Affine3f(_pose); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); Matx44f cam2volRotGPU = cam2vol.matrix; Matx44f vol2camRotGPU = vol2cam.matrix; UMat hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ); UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ); k.args( ocl::KernelArg::PtrReadOnly(hashesGpu), ocl::KernelArg::PtrReadOnly(hashDataGpu), ocl::KernelArg::WriteOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), frameSize, ocl::KernelArg::ReadOnly(volUnitsData), cam2volRotGPU, vol2camRotGPU, float(maxDepth), finv.val, cxy.val, boxMin.val, boxMax.val, tstep, voxelSize, voxelSizeInv, volumeUnitSize, truncDist, volumeUnitDegree, 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"); } #endif void fetchNormalsFromHashTsdfVolumeUnit( const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, const int volumeUnitDegree, InputArray _points, OutputArray _normals) { CV_TRACE_FUNCTION(); if (!_normals.needed()) return; Points points = _points.getMat(); CV_Assert(points.type() == POINT_TYPE); _normals.createSameSize(_points, _points.type()); Normals normals = _normals.getMat(); Mat volUnitsData = _volUnitsData.getMat(); const float voxelSize = settings.getVoxelSize(); const float voxelSizeInv = 1.f / voxelSize; const Vec4i volDims; settings.getVolumeStrides(volDims); Matx44f _pose; settings.getVolumePose(_pose); const Affine3f pose(_pose); auto HashPushNormals = [&](const ptype& point, const int* position) { Affine3f invPose(pose.inv()); Point3f p = fromPtype(point); Point3f n = nan3; if (!isNaN(p)) { Point3f voxelPoint = invPose * p; n = pose.rotation() * getNormalVoxel(voxelPoint, voxelSizeInv, volumeUnitDegree, volDims, volUnitsData, volumeUnits); } normals(position[0], position[1]) = toPtype(n); }; points.forEach(HashPushNormals); } #ifdef HAVE_OPENCL void ocl_fetchNormalsFromHashTsdfVolumeUnit( const VolumeSettings& settings, const int volumeUnitDegree, InputArray _volUnitsData, InputArray _volUnitsDataCopy, const CustomHashSet& hashTable, InputArray _points, OutputArray _normals) { CV_TRACE_FUNCTION(); UMat volUnitsData = _volUnitsData.getUMat(); Mat volUnitsDataCopy = _volUnitsDataCopy.getMat(); if (!_normals.needed()) return; //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()); Normals normals = _normals.getMat(); const float voxelSize = settings.getVoxelSize(); const float voxelSizeInv = 1.f / voxelSize; const Vec4i volDims; settings.getVolumeStrides(volDims); Matx44f _pose; settings.getVolumePose(_pose); const Affine3f pose = Affine3f(_pose); auto HashPushNormals = [&](const ptype& point, const int* position) { Affine3f invPose(pose.inv()); Point3f p = fromPtype(point); Point3f n = nan3; if (!isNaN(p)) { Point3f voxelPoint = invPose * p; n = pose.rotation() * ocl_getNormalVoxel(voxelPoint, voxelSizeInv, volumeUnitDegree, volDims, volUnitsDataCopy, hashTable); } normals(position[0], position[1]) = toPtype(n); }; points.forEach(HashPushNormals); } #endif void fetchPointsNormalsFromHashTsdfVolumeUnit( const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, const int volumeUnitDegree, OutputArray _points, OutputArray _normals) { //std::cout << "fetchNormalsFromHashTsdfVolumeUnit()" << std::endl; CV_TRACE_FUNCTION(); if (!_points.needed()) return; std::vector> pVecs, nVecs; Mat volUnitsData = _volUnitsData.getMat(); const float voxelSize = settings.getVoxelSize(); const float voxelSizeInv = 1.f / voxelSize; Vec3i resolution; settings.getVolumeResolution(resolution); const Point3i volResolution = Point3i(resolution); const int volumeUnitResolution = volResolution.x; const float volumeUnitSize = voxelSize * resolution[0]; const Vec4i volDims; settings.getVolumeStrides(volDims); Matx44f mpose; settings.getVolumePose(mpose); const Affine3f pose(mpose); std::vector totalVolUnits; for (const auto& keyvalue : volumeUnits) { totalVolUnits.push_back(keyvalue.first); } Range fetchRange(0, (int)totalVolUnits.size()); const int nstripes = -1; bool needNormals(_normals.needed()); Mutex mutex; //TODO: this is incorrect; a 0-surface should be captured instead of all non-zero voxels 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 = volumeUnits.find(tsdf_idx); Point3f base_point = volumeUnitIdxToVolume(tsdf_idx, volumeUnitSize); if (it != volumeUnits.end()) { std::vector localPoints; std::vector localNormals; for (int x = 0; x < volumeUnitResolution; x++) for (int y = 0; y < volumeUnitResolution; y++) for (int z = 0; z < volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); TsdfVoxel voxel = _at(volUnitsData, voxelIdx, it->second.index, volResolution.x, volDims); // floatToTsdf(1.0) == -128 if (voxel.tsdf != -128 && voxel.weight != 0) { Point3f point = base_point + voxelCoordToVolume(voxelIdx, voxelSize); localPoints.push_back(toPtype(pose * point)); if (needNormals) { Point3f normal = getNormalVoxel(point, voxelSizeInv, volumeUnitDegree, volDims, volUnitsData, volumeUnits); localNormals.push_back(toPtype(pose.rotation() * normal)); } } } AutoLock al(mutex); 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()); } } #ifdef HAVE_OPENCL inline TsdfVoxel new_at( Mat& volUnitsDataCopy, const cv::Vec3i& volumeIdx, int indx, const int volumeUnitResolution, const Vec4i volStrides) { //! Out of bounds if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) { return TsdfVoxel(floatToTsdf(1.0f), 0); } const TsdfVoxel* volData = volUnitsDataCopy.ptr(indx); int coordBase = volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2]; return volData[coordBase]; } void ocl_fetchPointsNormalsFromHashTsdfVolumeUnit( const VolumeSettings& settings, const int volumeUnitDegree, InputArray _volUnitsData, InputArray _volUnitsDataCopy, const CustomHashSet& hashTable, OutputArray _points, OutputArray _normals) { CV_TRACE_FUNCTION(); if (!_points.needed()) return; UMat volUnitsData = _volUnitsData.getUMat(); Mat volUnitsDataCopy = _volUnitsDataCopy.getMat(); //TODO: remove it when it works w/o CPU code volUnitsData.copyTo(volUnitsDataCopy); //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; const float voxelSize = settings.getVoxelSize(); const float voxelSizeInv = 1.f / voxelSize; Vec3i resolution; settings.getVolumeResolution(resolution); const Point3i volResolution = Point3i(resolution); const int volumeUnitResolution = volResolution.x; const float volumeUnitSize = voxelSize * resolution[0]; const Vec4i volDims; settings.getVolumeStrides(volDims); Matx44f mpose; settings.getVolumePose(mpose); const Affine3f pose(mpose); Range _fetchRange(0, hashTable.last); const int nstripes = -1; bool needNormals(_normals.needed()); Mutex mutex; //TODO: this is incorrect; a 0-surface should be captured instead of all non-zero voxels auto _HashFetchPointsNormalsInvoker = [&](const Range& range) { std::vector points, normals; for (int row = range.start; row < range.end; row++) { cv::Vec4i idx4 = hashTable.data[row]; cv::Vec3i idx(idx4[0], idx4[1], idx4[2]); Point3f base_point = volumeUnitIdxToVolume(idx, volumeUnitSize); std::vector localPoints; std::vector localNormals; for (int x = 0; x < volumeUnitResolution; x++) for (int y = 0; y < volumeUnitResolution; y++) for (int z = 0; z < volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); TsdfVoxel voxel = new_at(volUnitsDataCopy, voxelIdx, row, volumeUnitResolution, volDims); // floatToTsdf(1.0) == -128 if (voxel.tsdf != -128 && voxel.weight != 0) { Point3f point = base_point + voxelCoordToVolume(voxelIdx, voxelSize); localPoints.push_back(toPtype(pose * point)); if (needNormals) { Point3f normal = ocl_getNormalVoxel(point, voxelSizeInv, volumeUnitDegree, volDims, volUnitsDataCopy, hashTable); localNormals.push_back(toPtype(pose.rotation() * normal)); } } } AutoLock al(mutex); 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()); } } #endif } // namespace cv