diff --git a/modules/ptcloud/include/opencv2/ptcloud/volume.hpp b/modules/ptcloud/include/opencv2/ptcloud/volume.hpp index 98c8eea03e..6fe1800238 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/volume.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/volume.hpp @@ -20,7 +20,7 @@ class CV_EXPORTS_W Volume { public: /** @brief Constructor of custom volume. - * @param vtype the volume type [TSDF, HashTSDF, ColorTSDF]. + * @param vtype the volume type [TSDF, HashTSDF, ColorTSDF, ColorHashTSDF]. * @param settings the custom settings for volume. */ CV_WRAP explicit Volume(VolumeType vtype = VolumeType::TSDF, @@ -52,7 +52,7 @@ public: Camera intrinsics are taken from volume settings structure. * @param depth the depth image. - * @param image the color image (only for ColorTSDF). + * @param image the color image (only for ColorTSDF and ColorHashTSDF). For color TSDF a depth data should be registered with color data, i.e. have the same intrinsics & camera pose. This can be done using function registerDepth() from 3d module. * @param pose the pose of camera in global coordinates. @@ -78,7 +78,7 @@ public: * @param cameraPose the pose of camera in global coordinates. * @param points image to store rendered points. * @param normals image to store rendered normals corresponding to points. - * @param colors image to store rendered colors corresponding to points (only for ColorTSDF). + * @param colors image to store rendered colors corresponding to points (only for ColorTSDF and ColorHashTSDF). */ CV_WRAP_AS(raycastColor) void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const; @@ -106,7 +106,7 @@ public: * @param K camera raycast intrinsics * @param points image to store rendered points. * @param normals image to store rendered normals corresponding to points. - * @param colors image to store rendered colors corresponding to points (only for ColorTSDF). + * @param colors image to store rendered colors corresponding to points (only for ColorTSDF and ColorHashTSDF). */ CV_WRAP_AS(raycastExColor) void raycast(InputArray cameraPose, int height, int width, InputArray K, OutputArray points, OutputArray normals, OutputArray colors) const; @@ -123,7 +123,7 @@ public: /** @brief Extract the all data from volume. * @param points the storage of all points. * @param normals the storage of all normals, corresponding to points. - * @param colors the storage of all colors, corresponding to points (only for ColorTSDF). + * @param colors the storage of all colors, corresponding to points (only for ColorTSDF and ColorHashTSDF). */ CV_WRAP void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const; @@ -157,12 +157,12 @@ public: /** * @brief Enables or disables new volume unit allocation during integration. - * Makes sense for HashTSDF only. + * Makes sense for HashTSDF and ColorHashTSDF only. */ CV_WRAP void setEnableGrowth(bool v); /** * @brief Returns if new volume units are allocated during integration or not. - * Makes sense for HashTSDF only. + * Makes sense for HashTSDF and ColorHashTSDF only. */ CV_WRAP bool getEnableGrowth() const; diff --git a/modules/ptcloud/include/opencv2/ptcloud/volume_settings.hpp b/modules/ptcloud/include/opencv2/ptcloud/volume_settings.hpp index 428e1343a8..27b02ad809 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/volume_settings.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/volume_settings.hpp @@ -19,7 +19,8 @@ enum class VolumeType { TSDF = 0, HashTSDF = 1, - ColorTSDF = 2 + ColorTSDF = 2, + ColorHashTSDF = 3 }; @@ -98,6 +99,26 @@ public: */ CV_WRAP float getTsdfTruncateDistance() const; + /** @brief Sets gradient delta factor used for normal estimation in TSDF volumes. + * @param val input value. + */ + CV_WRAP void setGradientDeltaFactor(float val); + + /** @brief Returns gradient delta factor used for normal estimation in TSDF volumes. + */ + CV_WRAP float getGradientDeltaFactor() const; + + /** @brief Sets threshold for number of frames after which invisible volume units are hidden/removed. + * For HashTSDF and ColorHashTSDF volumes, units that haven't been visible for this many frames will be removed. + * @param val input value. + */ + CV_WRAP void setVolumeUnitHideThreshold(int val); + + /** @brief Returns threshold for number of frames after which invisible volume units are hidden/removed. + * For HashTSDF and ColorHashTSDF volumes, units that haven't been visible for this many frames will be removed. + */ + CV_WRAP int getVolumeUnitHideThreshold() const; + /** @brief Sets threshold for depth truncation in meters. Truncates the depth greater than threshold to 0. * @param val input value. */ @@ -141,16 +162,16 @@ public: /** @brief Resolution of voxel space. Number of voxels in each dimension. - Applicable only for TSDF Volume. - HashTSDF volume only supports equal resolution in all three dimensions. + Applicable only for TSDF and ColorTSDF volumes. + HashTSDF and ColorHashTSDF volumes only support equal resolution in all three dimensions. * @param val input value. */ CV_WRAP void setVolumeResolution(InputArray val); /** @brief Resolution of voxel space. Number of voxels in each dimension. - Applicable only for TSDF Volume. - HashTSDF volume only supports equal resolution in all three dimensions. + Applicable only for TSDF and ColorTSDF volumes. + HashTSDF and ColorHashTSDF volumes only support equal resolution in all three dimensions. * @param val output value. */ CV_WRAP void getVolumeResolution(OutputArray val) const; diff --git a/modules/ptcloud/src/color_hash_tsdf_functions.cpp b/modules/ptcloud/src/color_hash_tsdf_functions.cpp new file mode 100644 index 0000000000..0118ece856 --- /dev/null +++ b/modules/ptcloud/src/color_hash_tsdf_functions.cpp @@ -0,0 +1,712 @@ +// 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 + +// ColorHashTSDF is built on top of HashTSDF: it shares the spatially-hashed +// volume-unit storage and the raycast/fetch structure, but stores RGBTsdfVoxel +// voxels (TSDF + running-average RGB) and integrates color alongside depth. +// It is a CPU-only implementation. + +#include "precomp.hpp" +#include "color_hash_tsdf_functions.hpp" + +namespace cv { + +namespace { + +inline Vec3i volumeToVolumeUnitIdx(const Point3f& point, const float volumeUnitSize) +{ + return cv::Vec3i( + cvFloor(point.x / volumeUnitSize), + cvFloor(point.y / volumeUnitSize), + cvFloor(point.z / volumeUnitSize)); +} + +inline cv::Point3f volumeUnitIdxToVolume(const cv::Vec3i& volumeUnitIdx, const float volumeUnitSize) +{ + return cv::Point3f( + volumeUnitIdx[0] * volumeUnitSize, + volumeUnitIdx[1] * volumeUnitSize, + volumeUnitIdx[2] * volumeUnitSize); +} + +inline cv::Point3f voxelCoordToVolume(const cv::Vec3i& voxelIdx, const float voxelSize) +{ + return cv::Point3f( + voxelIdx[0] * voxelSize, + voxelIdx[1] * voxelSize, + voxelIdx[2] * voxelSize); +} + +inline 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)); +} + +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); +} + +// Out-of-bounds sentinel reused from HashTSDF: tsdf == floatToTsdf(1.f) (-128), weight 0. +RGBTsdfVoxel atColorHashVolumeUnit( + 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 RGBTsdfVoxel(floatToTsdf(1.f), 0, 0, 0, 0); + + Vec3i volUnitLocalIdx = point - Vec3i(volumeUnitIdx[0] << volumeUnitDegree, + volumeUnitIdx[1] << volumeUnitDegree, + volumeUnitIdx[2] << volumeUnitDegree); + + const RGBTsdfVoxel* volData = volUnitsData.ptr(it->second.index); + int coordBase = volUnitLocalIdx[0] * volStrides[0] + + volUnitLocalIdx[1] * volStrides[1] + + volUnitLocalIdx[2] * volStrides[2]; + return volData[coordBase]; +} + +inline RGBTsdfVoxel _atColorHash(Mat& volUnitsData, const cv::Vec3i& volumeIdx, int indx, + const int volumeUnitResolution, const Vec4i volStrides) +{ + if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) || + (volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) || + (volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0)) + { + return RGBTsdfVoxel(floatToTsdf(1.f), 0, 0, 0, 0); + } + const RGBTsdfVoxel* volData = volUnitsData.ptr(indx); + int coordBase = + volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2]; + return volData[coordBase]; +} + +// Normal estimation reuses the HashTSDF gradient scheme; only the .tsdf field is read. +Point3f getNormalColorHashVoxel( + const Point3f& point, const float voxelSizeInv, + const int volumeUnitDegree, const Vec4i volStrides, + const Mat& volUnitsData, const VolumeUnitIndexes& volumeUnits) +{ + Vec3f normal = Vec3f(0, 0, 0); + + Point3f ptVox = point * voxelSizeInv; + Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); + + 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, -1, 0}, { 0, 0, 1}, { 0, 0, -1} }; + const int nVals = 6; +#else + const Vec3i offsets[] = { { 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}, + { 2, 0, 0}, { 2, 0, 1}, { 2, 1, 0}, { 2, 1, 1}, + { 0, -1, 0}, { 0, -1, 1}, { 1, -1, 0}, { 1, -1, 1}, + { 0, 2, 0}, { 0, 2, 1}, { 1, 2, 0}, { 1, 2, 1}, + { 0, 0, -1}, { 0, 1, -1}, { 1, 0, -1}, { 1, 1, -1}, + { 0, 0, 2}, { 0, 1, 2}, { 1, 0, 2}, { 1, 1, 2} }; + 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(atColorHashVolumeUnit(volUnitsData, 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 + 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 }; + + float cxv[8], cyv[8], czv[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]]; + } + + 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; +} + +// Trilinear color interpolation at a point given in volume coordinates (meters). +// Neighbors are looked up across volume-unit boundaries through the hash map. +Point3f getColorHashVoxel( + const Point3f& point, const float voxelSizeInv, + const int volumeUnitDegree, const Vec4i volStrides, + const Mat& volUnitsData, const VolumeUnitIndexes& volumeUnits) +{ + Point3f ptVox = point * voxelSizeInv; + Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); + float tx = ptVox.x - iptVox[0]; + float ty = ptVox.y - iptVox[1]; + float tz = ptVox.z - iptVox[2]; + + const Vec3i offsets[8] = { + {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, + {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1} + }; + + float r[8], g[8], b[8]; + bool allValid = true; + for (int i = 0; i < 8; i++) + { + Vec3i pt = iptVox + offsets[i]; + Vec3i volumeUnitIdx(pt[0] >> volumeUnitDegree, pt[1] >> volumeUnitDegree, pt[2] >> volumeUnitDegree); + auto it = volumeUnits.find(volumeUnitIdx); + RGBTsdfVoxel v = atColorHashVolumeUnit(volUnitsData, volumeUnits, pt, volumeUnitIdx, it, + volumeUnitDegree, volStrides); + if (v.weight == 0) + { + allValid = false; + break; + } + r[i] = float(v.r); + g[i] = float(v.g); + b[i] = float(v.b); + } + + if (!allValid) + { + // Fall back to the nearest voxel if any corner of the cube is empty. + Vec3i volumeUnitIdx(iptVox[0] >> volumeUnitDegree, + iptVox[1] >> volumeUnitDegree, + iptVox[2] >> volumeUnitDegree); + auto it = volumeUnits.find(volumeUnitIdx); + RGBTsdfVoxel v = atColorHashVolumeUnit(volUnitsData, volumeUnits, iptVox, volumeUnitIdx, it, + volumeUnitDegree, volStrides); + return Point3f(float(v.r), float(v.g), float(v.b)); + } + + Point3f res(interpolate(tx, ty, tz, r), + interpolate(tx, ty, tz, g), + interpolate(tx, ty, tz, b)); + colorFix(res); + return res; +} + +} // namespace + +void integrateColorHashTsdfVolumeUnit( + const VolumeSettings& settings, const Matx44f& cameraPose, + int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth, + InputArray _depth, InputArray _rgb, InputArray _pixNorms, + InputOutputArray _volUnitsData, VolumeUnitIndexes& volumeUnits) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + Depth depth = _depth.getMat(); + Mat rgb = _rgb.getMat(); + Mat& volUnitsData = _volUnitsData.getMatRef(); + Mat pixNorms = _pixNorms.getMat(); + + CV_Assert(!rgb.empty()); + CV_Assert(depth.size() == rgb.size()); + + 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) + { + 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; + 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) + localAccessVolUnits.emplace(tsdf_idx); + } + } + } + + mutex.lock(); + for (const auto& tsdf_idx : localAccessVolUnits) + { + if (!newIndices.count(tsdf_idx)) + newIndices.emplace(tsdf_idx); + } + mutex.unlock(); + }; + parallel_for_(allocateRange, AllocateVolumeUnitsInvoker); + + 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, "ColorHashTSDF storage extended from " << lastVolIndex << " to " << lastVolIndex * 2 << " volume units"); + } + lastVolIndex++; + volUnitsData.row(vu.index).forEach([](VecRGBTsdfVoxel& vv, const int* /*position*/) + { + RGBTsdfVoxel& v = reinterpret_cast(vv); + v.tsdf = floatToTsdf(0.0f); v.weight = 0; + v.r = v.g = v.b = 0; + }); + vu.lastVisibleIndex = frameId; + vu.isActive = true; + } + } + + std::vector totalVolUnits; + for (const auto& keyvalue : volumeUnits) + totalVolUnits.push_back(keyvalue.first); + + 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) + { + it->second.lastVisibleIndex = frameId; + it->second.isActive = true; + } + } + }); + + 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) + { + integrateColorTsdfVolumeUnit(settings, volumeUnit.pose, cameraPose, + depth, rgb, pixNorms, volUnitsData.row(volumeUnit.index)); + volumeUnit.isActive = false; + } + } + }); +} + +void raycastColorHashTsdfVolumeUnit( + const VolumeSettings& settings, const Matx44f& cameraPose, + int height, int width, InputArray intr, const int volumeUnitDegree, + InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, + OutputArray _points, OutputArray _normals, OutputArray _colors) +{ + 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); + if (_colors.needed()) + _colors.create(frameSize, COLOR_TYPE); + + Points points1 = _points.getMat(); + Normals normals1 = _normals.getMat(); + Points& points(points1); + Normals& normals(normals1); + + Colors colors1; + Colors* colors = nullptr; + if (_colors.needed()) + { + colors1 = _colors.getMat(); + colors = &colors1; + } + + 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]; + ptype* clrRow = colors ? (*colors)[y] : nullptr; + + for (int x = 0; x < points.cols; x++) + { + Point3f point = nan3, normal = nan3, color = nan3; + + 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(std::numeric_limits::min(), + std::numeric_limits::min(), + std::numeric_limits::min()); + float tprev = tcurr; + float prevTsdf = truncDist; + float currTsdf = truncDist; + (void)prevVolumeUnitIdx; + + 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; + + if (it != volumeUnits.end()) + { + cv::Point3f currVolUnitPos = volumeUnitIdxToVolume(currVolumeUnitIdx, volumeUnitSize); + volUnitLocalIdx = volumeToVoxelCoord(currRayPos - currVolUnitPos, voxelSizeInv); + RGBTsdfVoxel currVoxel = _atColorHash(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; + } + + 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 = getNormalColorHashVoxel(pv, voxelSizeInv, volumeUnitDegree, volDims, + volUnitsData, volumeUnits); + if (!isNaN(nv)) + { + normal = vol2camRot * nv; + point = vol2cam * pv; + if (colors) + color = getColorHashVoxel(pv, voxelSizeInv, volumeUnitDegree, volDims, + volUnitsData, volumeUnits); + } + } + } + + ptsRow[x] = toPtype(point); + nrmRow[x] = toPtype(normal); + if (clrRow) + clrRow[x] = toPtype(color); + } + } + }; + + parallel_for_(Range(0, points.rows), HashRaycastInvoker, nstripes); +} + +void fetchNormalsFromColorHashTsdfVolumeUnit( + 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() * getNormalColorHashVoxel(voxelPoint, voxelSizeInv, volumeUnitDegree, + volDims, volUnitsData, volumeUnits); + } + normals(position[0], position[1]) = toPtype(n); + }; + points.forEach(HashPushNormals); +} + +void fetchPointsNormalsColorsFromColorHashTsdfVolumeUnit( + const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, + const int volumeUnitDegree, OutputArray _points, OutputArray _normals, OutputArray _colors) +{ + CV_TRACE_FUNCTION(); + + if (!_points.needed()) + return; + + std::vector> pVecs, nVecs, cVecs; + 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()); + bool needColors(_colors.needed()); + Mutex mutex; + + //TODO: same as HashTSDF - a 0-surface should be captured instead of all non-zero voxels + auto HashFetchPointsNormalsColorsInvoker = [&](const Range& range) + { + std::vector points, normals, colors; + 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, localNormals, localColors; + 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); + RGBTsdfVoxel voxel = _atColorHash(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 = getNormalColorHashVoxel(point, voxelSizeInv, volumeUnitDegree, + volDims, volUnitsData, volumeUnits); + localNormals.push_back(toPtype(pose.rotation() * normal)); + } + if (needColors) + { + Point3f c(float(voxel.r), float(voxel.g), float(voxel.b)); + localColors.push_back(toPtype(c)); + } + } + } + + AutoLock al(mutex); + pVecs.push_back(localPoints); + nVecs.push_back(localNormals); + cVecs.push_back(localColors); + } + } + }; + + parallel_for_(fetchRange, HashFetchPointsNormalsColorsInvoker, nstripes); + + std::vector points, normals, colors; + 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()); + colors.insert(colors.end(), cVecs[i].begin(), cVecs[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()); + } + + if (_colors.needed()) + { + _colors.create((int)colors.size(), 1, COLOR_TYPE); + if (!colors.empty()) + Mat((int)colors.size(), 1, COLOR_TYPE, &colors[0]).copyTo(_colors.getMat()); + } +} + +} // namespace cv diff --git a/modules/ptcloud/src/color_hash_tsdf_functions.hpp b/modules/ptcloud/src/color_hash_tsdf_functions.hpp new file mode 100644 index 0000000000..fd78aa57a4 --- /dev/null +++ b/modules/ptcloud/src/color_hash_tsdf_functions.hpp @@ -0,0 +1,41 @@ +// 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 + +#ifndef OPENCV_3D_COLOR_HASH_TSDF_FUNCTIONS_HPP +#define OPENCV_3D_COLOR_HASH_TSDF_FUNCTIONS_HPP + +#include "hash_tsdf_functions.hpp" +#include "color_tsdf_functions.hpp" + +namespace cv +{ + +// ColorHashTSDF uses the same voxel layout as ColorTSDF (TSDF + running-average RGB), +// but stores voxels in spatially hashed volume units like HashTSDF. +// It is a CPU-only implementation; the Volume::Impl interface falls back to CPU +// regardless of the OpenCL availability (same as ColorTSDF). + +void integrateColorHashTsdfVolumeUnit( + const VolumeSettings& settings, const Matx44f& cameraPose, + int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth, + InputArray _depth, InputArray _rgb, InputArray _pixNorms, + InputOutputArray _volUnitsData, VolumeUnitIndexes& volumeUnits); + +void raycastColorHashTsdfVolumeUnit( + const VolumeSettings& settings, const Matx44f& cameraPose, + int height, int width, InputArray intr, const int volumeUnitDegree, + InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, + OutputArray _points, OutputArray _normals, OutputArray _colors); + +void fetchNormalsFromColorHashTsdfVolumeUnit( + const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, + const int volumeUnitDegree, InputArray _points, OutputArray _normals); + +void fetchPointsNormalsColorsFromColorHashTsdfVolumeUnit( + const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, + const int volumeUnitDegree, OutputArray _points, OutputArray _normals, OutputArray _colors); + +} // namespace cv + +#endif // OPENCV_3D_COLOR_HASH_TSDF_FUNCTIONS_HPP diff --git a/modules/ptcloud/src/color_tsdf_functions.cpp b/modules/ptcloud/src/color_tsdf_functions.cpp index 52c92d51ef..858a4fb246 100644 --- a/modules/ptcloud/src/color_tsdf_functions.cpp +++ b/modules/ptcloud/src/color_tsdf_functions.cpp @@ -577,7 +577,7 @@ inline float interpolateColor(float tx, float ty, float tz, float vx[8]) inline v_float32x4 getColorVoxel(const Mat& volume, const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, - const float voxelSizeInv, const v_float32x4& p) + const v_float32x4& p) { if (v_check_any(v_lt(p, v_float32x4(1.f, 1.f, 1.f, 0.f))) || v_check_any(v_ge(p, v_float32x4((float)(volResolution.x - 2), @@ -586,14 +586,15 @@ inline v_float32x4 getColorVoxel(const Mat& volume, ) return nanv; - v_int32x4 ip = v_floor(p); + const v_int32x4 ip = v_floor(p); const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const RGBTsdfVoxel* volData = volume.ptr(); - int ix = v_get0(ip); ip = v_rotate_right<1>(ip); - int iy = v_get0(ip); ip = v_rotate_right<1>(ip); - int iz = v_get0(ip); + v_int32x4 ipr = ip; + int ix = v_get0(ipr); ipr = v_rotate_right<1>(ipr); + int iy = v_get0(ipr); ipr = v_rotate_right<1>(ipr); + int iz = v_get0(ipr); int coordBase = ix * xdim + iy * ydim + iz * zdim; float CV_DECL_ALIGNED(16) rgb[4]; @@ -607,10 +608,10 @@ inline v_float32x4 getColorVoxel(const Mat& volume, b[i] = (float)volData[neighbourCoords[i] + coordBase].b; } - v_float32x4 vsi(voxelSizeInv, voxelSizeInv, voxelSizeInv, voxelSizeInv); - v_float32x4 ptVox = v_mul(p, vsi); - v_int32x4 iptVox = v_floor(ptVox); - v_float32x4 t = v_sub(ptVox, v_cvt_f32(iptVox)); + // p is already in voxel-index units (corners were gathered at floor(p)), + // so the trilinear weights are just its fractional part. Do NOT rescale by + // voxelSizeInv again here — that double-scaling scrambled the weights. + v_float32x4 t = v_sub(p, v_cvt_f32(ip)); float tx = v_get0(t); t = v_rotate_right<1>(t); float ty = v_get0(t); t = v_rotate_right<1>(t); float tz = v_get0(t); @@ -630,10 +631,10 @@ inline v_float32x4 getColorVoxel(const Mat& volume, inline Point3f getColorVoxel(const Mat& volume, const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, - const float voxelSizeInv, const Point3f& _p) + const Point3f& _p) { v_float32x4 p(_p.x, _p.y, _p.z, 0.f); - v_float32x4 result = getColorVoxel(volume, volDims, neighbourCoords, volResolution, voxelSizeInv, p); + v_float32x4 result = getColorVoxel(volume, volDims, neighbourCoords, volResolution, p); float CV_DECL_ALIGNED(16) ares[4]; v_store_aligned(ares, result); return Point3f(ares[0], ares[1], ares[2]); @@ -643,7 +644,7 @@ inline Point3f getColorVoxel(const Mat& volume, #else inline Point3f getColorVoxel(const Mat& volume, const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution, - const float voxelSizeInv, const Point3f& p) + const Point3f& p) { const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const RGBTsdfVoxel* volData = volume.ptr(); @@ -670,11 +671,11 @@ inline Point3f getColorVoxel(const Mat& volume, b[i] = (float)volData[neighbourCoords[i] + coordBase].b; } - Point3f ptVox = p * voxelSizeInv; - Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); - float tx = ptVox.x - iptVox[0]; - float ty = ptVox.y - iptVox[1]; - float tz = ptVox.z - iptVox[2]; + // p is already in voxel-index units (corners gathered at floor(p) above), + // so weights are its fractional part — do not rescale by voxelSizeInv again. + float tx = p.x - ix; + float ty = p.y - iy; + float tz = p.z - iz; res = Point3f(interpolateColor(tx, ty, tz, r), interpolateColor(tx, ty, tz, g), @@ -868,7 +869,7 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &c { v_float32x4 pv = v_add(orig, v_mul(dir, v_setall_f32(ts))); v_float32x4 nv = getNormalColorVoxel(volume, volDims, neighbourCoords, volResolution, pv); - v_float32x4 cv = getColorVoxel(volume, volDims, neighbourCoords, volResolution, voxelSizeInv, pv); + v_float32x4 cv = getColorVoxel(volume, volDims, neighbourCoords, volResolution, pv); if (!isNaN(nv)) { @@ -980,7 +981,7 @@ void raycastColorTsdfVolumeUnit(const VolumeSettings &settings, const Matx44f &c { Point3f pv = (orig + dir * ts); Point3f nv = getNormalColorVoxel(volume, volDims, neighbourCoords, volResolution, pv); - Point3f cv = getColorVoxel(volume, volDims, neighbourCoords, volResolution, voxelSizeInv, pv); + Point3f cv = getColorVoxel(volume, volDims, neighbourCoords, volResolution, pv); if (!isNaN(nv)) { //convert pv and nv to camera space @@ -1114,7 +1115,7 @@ inline void coord( getNormalColorVoxel(volume, volDims, neighbourCoords, volResolution, p * voxelSizeInv))); if (needColors) colors.push_back(toPtype(pose.rotation() * - getColorVoxel(volume, volDims, neighbourCoords, volResolution, voxelSizeInv, p * voxelSizeInv))); + getColorVoxel(volume, volDims, neighbourCoords, volResolution, p * voxelSizeInv))); } } } diff --git a/modules/ptcloud/src/hash_tsdf_functions.cpp b/modules/ptcloud/src/hash_tsdf_functions.cpp index fe57723632..8376e45b46 100644 --- a/modules/ptcloud/src/hash_tsdf_functions.cpp +++ b/modules/ptcloud/src/hash_tsdf_functions.cpp @@ -1066,6 +1066,7 @@ void raycastHashTsdfVolumeUnit( float tprev = tcurr; float prevTsdf = truncDist; + float currTsdf = truncDist; while (tcurr < tmax) { @@ -1074,8 +1075,7 @@ void raycastHashTsdfVolumeUnit( VolumeUnitIndexes::const_iterator it = volumeUnits.find(currVolumeUnitIdx); - float currTsdf = prevTsdf; - int currWeight = 0; + currTsdf = prevTsdf; float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; @@ -1089,32 +1089,42 @@ void raycastHashTsdfVolumeUnit( //! TODO: Figure out voxel interpolation TsdfVoxel currVoxel = _at(volUnitsData, volUnitLocalIdx, it->second.index, volResolution.x, volDims); currTsdf = tsdfToFloat(currVoxel.tsdf); - currWeight = currVoxel.weight; + + 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; } - //! 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 = getNormalVoxel(pv, voxelSizeInv, volumeUnitDegree, volDims, volUnitsData, volumeUnits); - - if (!isNaN(nv)) - { - normal = vol2camRot * nv; - point = vol2cam * pv; - } - } - break; - } 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); } diff --git a/modules/ptcloud/src/volume_impl.cpp b/modules/ptcloud/src/volume_impl.cpp index 0df8defc01..3c7856f39a 100644 --- a/modules/ptcloud/src/volume_impl.cpp +++ b/modules/ptcloud/src/volume_impl.cpp @@ -8,6 +8,7 @@ #include "tsdf_functions.hpp" #include "hash_tsdf_functions.hpp" #include "color_tsdf_functions.hpp" +#include "color_hash_tsdf_functions.hpp" #include "opencv2/imgproc.hpp" namespace cv @@ -308,37 +309,37 @@ void HashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, Inpu const Matx44f cameraPose = _cameraPose.getMat(); -#ifndef HAVE_OPENCL - raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals); -#else +#ifdef HAVE_OPENCL if (useGPU) ocl_raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, hashTable, gpu_volUnitsData, _points, _normals); else raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, cpu_volUnitsData, cpu_volumeUnits, _points, _normals); +#else + raycastHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, volUnitsData, volumeUnits, _points, _normals); #endif } void HashTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const { -#ifndef HAVE_OPENCL - fetchNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals); -#else +#ifdef HAVE_OPENCL if (useGPU) ocl_fetchNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals); else fetchNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals); - +#else + fetchNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals); #endif } + void HashTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const { -#ifndef HAVE_OPENCL - fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals); -#else +#ifdef HAVE_OPENCL if (useGPU) ocl_fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volumeUnitDegree, gpu_volUnitsData, volUnitsDataCopy, hashTable, points, normals); else fetchPointsNormalsFromHashTsdfVolumeUnit(settings, cpu_volUnitsData, cpu_volumeUnits, volumeUnitDegree, points, normals); +#else + fetchPointsNormalsFromHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, points, normals); #endif } @@ -353,14 +354,7 @@ void HashTsdfVolume::reset() lastVolIndex = 0; lastFrameId = 0; enableGrowth = true; -#ifndef HAVE_OPENCL - volUnitsData.forEach([](VecTsdfVoxel& vv, const int* /* position */) - { - TsdfVoxel& v = reinterpret_cast(vv); - v.tsdf = floatToTsdf(0.0f); v.weight = 0; - }); - volumeUnits = VolumeUnitIndexes(); -#else +#ifdef HAVE_OPENCL if (useGPU) { Vec3i resolution; @@ -387,6 +381,13 @@ void HashTsdfVolume::reset() }); cpu_volumeUnits = VolumeUnitIndexes(); } +#else + volUnitsData.forEach([](VecTsdfVoxel& vv, const int* /* position */) + { + TsdfVoxel& v = reinterpret_cast(vv); + v.tsdf = floatToTsdf(0.0f); v.weight = 0; + }); + volumeUnits = VolumeUnitIndexes(); #endif } @@ -417,29 +418,26 @@ void HashTsdfVolume::getBoundingBox(OutputArray boundingBox, int precision) cons float side = res[0] * voxelSize; std::vector vi; -#ifndef HAVE_OPENCL - for (const auto& keyvalue : volumeUnits) +#ifdef HAVE_OPENCL + if (useGPU) + { + for (int row = 0; row < hashTable.last; row++) { + Vec4i idx4 = hashTable.data[row]; + vi.push_back(Vec3i(idx4[0], idx4[1], idx4[2])); + } + } + else + { + for (const auto& keyvalue : cpu_volumeUnits) vi.push_back(keyvalue.first); - } + } #else - if (useGPU) - { - for (int row = 0; row < hashTable.last; row++) - { - Vec4i idx4 = hashTable.data[row]; - vi.push_back(Vec3i(idx4[0], idx4[1], idx4[2])); - } - } - else - { - for (const auto& keyvalue : cpu_volumeUnits) - { - vi.push_back(keyvalue.first); - } - } + for (const auto& keyvalue : volumeUnits) + vi.push_back(keyvalue.first); #endif + if (vi.empty()) { boundingBox.setZero(); @@ -593,4 +591,183 @@ bool ColorTsdfVolume::getEnableGrowth() const return false; } +// COLOR_HASH_TSDF +// +// CPU-only hash-based colored TSDF. Mirrors HashTsdfVolume's storage layout +// (VOLUMES_SIZE volume units, spatially hashed via VolumeUnitIndexes) but uses +// RGBTsdfVoxel and integrates color alongside depth. There is no OpenCL path +// (same as ColorTsdfVolume); the GPU flag from Volume::Impl is ignored. + +ColorHashTsdfVolume::ColorHashTsdfVolume(const VolumeSettings& _settings) : + Volume::Impl(_settings), + lastVolIndex(0), + lastFrameId(0), + volumeUnitDegree(0), + enableGrowth(true) +{ + Vec3i resolution; + settings.getVolumeResolution(resolution); + const Point3i volResolution = Point3i(resolution); + volumeUnitDegree = calcVolumeUnitDegree(volResolution); + + volUnitsData = cv::Mat(VOLUMES_SIZE, resolution[0] * resolution[1] * resolution[2], rawType()); + reset(); +} + +ColorHashTsdfVolume::~ColorHashTsdfVolume() {} + +void ColorHashTsdfVolume::integrate(const OdometryFrame& frame, InputArray _cameraPose) +{ + CV_TRACE_FUNCTION(); + Mat depth, image; + frame.getDepth(depth); + frame.getImage(image); + integrate(depth, image, _cameraPose); +} + +void ColorHashTsdfVolume::integrate(InputArray, InputArray) +{ + CV_Error(cv::Error::StsBadFunc, "Color data should be passed for this volume type"); +} + +void ColorHashTsdfVolume::integrate(InputArray _depth, InputArray _image, InputArray _cameraPose) +{ + Mat depth = _depth.getMat(); + Mat image = _image.getMat(); + const Matx44f cameraPose = _cameraPose.getMat(); + Matx33f intr; + settings.getCameraIntegrateIntrinsics(intr); + Intr intrinsics(intr); + Vec6f newParams((float)depth.rows, (float)depth.cols, + intrinsics.fx, intrinsics.fy, + intrinsics.cx, intrinsics.cy); + if (!(frameParams == newParams)) + { + frameParams = newParams; + preCalculationPixNorm(depth.size(), intrinsics, pixNorms); + } + + integrateColorHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, + volumeUnitDegree, enableGrowth, + depth, image, pixNorms, volUnitsData, volumeUnits); + lastFrameId++; +} + +void ColorHashTsdfVolume::raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const +{ + Matx33f intr; + settings.getCameraRaycastIntrinsics(intr); + raycast(cameraPose, settings.getRaycastHeight(), settings.getRaycastWidth(), intr, points, normals, colors); +} + +void ColorHashTsdfVolume::raycast(InputArray _cameraPose, int height, int width, InputArray intr, OutputArray _points, OutputArray _normals, OutputArray _colors) const +{ + const Matx44f cameraPose = _cameraPose.getMat(); + raycastColorHashTsdfVolumeUnit(settings, cameraPose, height, width, intr, volumeUnitDegree, + volUnitsData, volumeUnits, _points, _normals, _colors); +} + +void ColorHashTsdfVolume::fetchNormals(InputArray points, OutputArray normals) const +{ + fetchNormalsFromColorHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, + points, normals); +} + +void ColorHashTsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) const +{ + fetchPointsNormalsColorsFromColorHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, + points, normals, noArray()); +} + +void ColorHashTsdfVolume::fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const +{ + fetchPointsNormalsColorsFromColorHashTsdfVolumeUnit(settings, volUnitsData, volumeUnits, volumeUnitDegree, + points, normals, colors); +} + +void ColorHashTsdfVolume::reset() +{ + CV_TRACE_FUNCTION(); + lastVolIndex = 0; + lastFrameId = 0; + enableGrowth = true; + volUnitsData.forEach([](VecRGBTsdfVoxel& vv, const int* /* position */) + { + RGBTsdfVoxel& v = reinterpret_cast(vv); + v.tsdf = floatToTsdf(0.0f); v.weight = 0; + v.r = v.g = v.b = 0; + }); + volumeUnits = VolumeUnitIndexes(); +} + +int ColorHashTsdfVolume::getVisibleBlocks() const { return (int)volumeUnits.size(); } +size_t ColorHashTsdfVolume::getTotalVolumeUnits() const { return volumeUnits.size(); } + +void ColorHashTsdfVolume::setEnableGrowth(bool v) +{ + enableGrowth = v; +} + +bool ColorHashTsdfVolume::getEnableGrowth() const +{ + return enableGrowth; +} + +void ColorHashTsdfVolume::getBoundingBox(OutputArray boundingBox, int precision) const +{ + // Same VOLUME_UNIT-level bounding box as HashTsdfVolume, derived from the + // set of allocated volume units. + if (precision == Volume::BoundingBoxPrecision::VOXEL) + { + CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet"); + } + else + { + Vec3i res; + this->settings.getVolumeResolution(res); + float voxelSize = this->settings.getVoxelSize(); + float side = res[0] * voxelSize; + + std::vector vi; + for (const auto& keyvalue : volumeUnits) + vi.push_back(keyvalue.first); + + if (vi.empty()) + { + boundingBox.setZero(); + } + else + { + std::vector pts; + for (Vec3i idx : vi) + { + Point3f base = Point3f((float)idx[0], (float)idx[1], (float)idx[2]) * side; + pts.push_back(base); + pts.push_back(base + Point3f(side, 0, 0)); + pts.push_back(base + Point3f(0, side, 0)); + pts.push_back(base + Point3f(0, 0, side)); + pts.push_back(base + Point3f(side, side, 0)); + pts.push_back(base + Point3f(side, 0, side)); + pts.push_back(base + Point3f(0, side, side)); + pts.push_back(base + Point3f(side, side, side)); + } + + const float mval = std::numeric_limits::max(); + Vec6f bb(mval, mval, mval, -mval, -mval, -mval); + for (auto p : pts) + { + Point3f pg = p; + bb[0] = min(bb[0], pg.x); + bb[1] = min(bb[1], pg.y); + bb[2] = min(bb[2], pg.z); + bb[3] = max(bb[3], pg.x); + bb[4] = max(bb[4], pg.y); + bb[5] = max(bb[5], pg.z); + } + + bb.copyTo(boundingBox); + } + } +} + } diff --git a/modules/ptcloud/src/volume_impl.hpp b/modules/ptcloud/src/volume_impl.hpp index 98941b33f8..94cd4ed862 100644 --- a/modules/ptcloud/src/volume_impl.hpp +++ b/modules/ptcloud/src/volume_impl.hpp @@ -211,6 +211,55 @@ private: }; +// ColorHashTSDF is a CPU-only hash-based colored TSDF volume. +// It does not inherit ColorTsdfVolume (which owns a dense volume Mat) on purpose: +// the whole point of hashing is to avoid allocating the dense volume. +class ColorHashTsdfVolume : public Volume::Impl +{ +public: + ColorHashTsdfVolume(const VolumeSettings& settings); + ~ColorHashTsdfVolume(); + + virtual void integrate(const OdometryFrame& frame, InputArray pose) override; + virtual void integrate(InputArray depth, InputArray pose) override; + virtual void integrate(InputArray depth, InputArray image, InputArray pose) override; + virtual void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const override; + virtual void raycast(InputArray cameraPose, int height, int width, InputArray intr, OutputArray points, OutputArray normals, OutputArray colors) const override; + + virtual void fetchNormals(InputArray points, OutputArray normals) const override; + virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; + virtual void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override; + + virtual void reset() override; + virtual int getVisibleBlocks() const override; + virtual size_t getTotalVolumeUnits() const override; + + // Enables or disables new volume unit allocation during integration + // Applicable for HashTSDF and ColorHashTSDF only + virtual void setEnableGrowth(bool v) override; + // Returns if new volume units are allocated during integration or not + // Applicable for HashTSDF and ColorHashTSDF only + virtual bool getEnableGrowth() const override; + + // Gets bounding box in volume coordinates with given precision: + // VOLUME_UNIT - up to volume unit + // VOXEL - up to voxel + // returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates + virtual void getBoundingBox(OutputArray bb, int precision) const override; + +public: + int lastVolIndex; + int lastFrameId; + Vec6f frameParams; + int volumeUnitDegree; + bool enableGrowth; + + Mat volUnitsData; + Mat pixNorms; + VolumeUnitIndexes volumeUnits; +}; + + Volume::Volume(VolumeType vtype, const VolumeSettings& settings) { switch (vtype) @@ -224,8 +273,11 @@ Volume::Volume(VolumeType vtype, const VolumeSettings& settings) case VolumeType::ColorTSDF: this->impl = makePtr(settings); break; + case VolumeType::ColorHashTSDF: + this->impl = makePtr(settings); + break; default: - CV_Error(Error::StsInternal, "Incorrect OdometryType, you are able to use only { ICP, RGB, RGBD }"); + CV_Error(Error::StsInternal, "Incorrect VolumeType"); break; } } @@ -254,4 +306,4 @@ bool Volume::getEnableGrowth() const { return this->impl->getEnableGrowth(); } } -#endif // !OPENCV_3D_VOLUME_IMPL_HPP +#endif // OPENCV_3D_VOLUME_IMPL_HPP diff --git a/modules/ptcloud/src/volume_settings_impl.cpp b/modules/ptcloud/src/volume_settings_impl.cpp index a7783ae9bb..ba66e97aba 100644 --- a/modules/ptcloud/src/volume_settings_impl.cpp +++ b/modules/ptcloud/src/volume_settings_impl.cpp @@ -56,6 +56,10 @@ public: virtual int getMaxWeight() const = 0; virtual void setRaycastStepFactor(float val) = 0; virtual float getRaycastStepFactor() const = 0; + virtual void setGradientDeltaFactor(float val) = 0; + virtual float getGradientDeltaFactor() const = 0; + virtual void setVolumeUnitHideThreshold(int val) = 0; + virtual int getVolumeUnitHideThreshold() const = 0; virtual void setVolumePose(InputArray val) = 0; virtual void getVolumePose(OutputArray val) const = 0; @@ -95,6 +99,10 @@ public: virtual int getMaxWeight() const override; virtual void setRaycastStepFactor(float val) override; virtual float getRaycastStepFactor() const override; + virtual void setGradientDeltaFactor(float val) override; + virtual float getGradientDeltaFactor() const override; + virtual void setVolumeUnitHideThreshold(int val) override; + virtual int getVolumeUnitHideThreshold() const override; virtual void setVolumePose(InputArray val) override; virtual void getVolumePose(OutputArray val) const override; @@ -120,6 +128,8 @@ private: int maxWeight; float raycastStepFactor; bool zFirstMemOrder; + float gradientDeltaFactor; + int volumeUnitHideThreshold; Matx44f volumePose; Point3i volumeResolution; @@ -156,13 +166,15 @@ public: static const int maxWeight = 64; // number of frames static constexpr float raycastStepFactor = 0.75f; static const bool zFirstMemOrder = true; // order of voxels in volume + static constexpr float gradientDeltaFactor = 1.0f; + static const int volumeUnitHideThreshold = 10; const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)); const Matx44f volumePoseMatrix = volumePose.matrix; // Unlike original code, this should work with any volume size // Not only when (x,y,z % 32) == 0 const Point3i volumeResolution = Vec3i::all(128); //number of voxels - }; +}; class DefaultHashTsdfSets { public: @@ -190,6 +202,8 @@ public: static const int maxWeight = 64; // number of frames static constexpr float raycastStepFactor = 0.25f; static const bool zFirstMemOrder = true; // order of voxels in volume + static constexpr float gradientDeltaFactor = 1.0f; + static const int volumeUnitHideThreshold = 10; const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)); const Matx44f volumePoseMatrix = volumePose.matrix; @@ -232,6 +246,8 @@ public: static const int maxWeight = 64; // number of frames static constexpr float raycastStepFactor = 0.75f; static const bool zFirstMemOrder = true; // order of voxels in volume + static constexpr float gradientDeltaFactor = 1.0f; + static const int volumeUnitHideThreshold = 10; const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)); const Matx44f volumePoseMatrix = volumePose.matrix; @@ -240,6 +256,42 @@ public: const Point3i volumeResolution = Vec3i::all(128); //number of voxels }; + class DefaultColorHashTsdfSets { + public: + static const int integrateWidth = 640; + static const int integrateHeight = 480; + float ifx = 525.f; // focus point x axis + float ify = 525.f; // focus point y axis + float icx = float(integrateWidth) / 2.f - 0.5f; // central point x axis + float icy = float(integrateHeight) / 2.f - 0.5f; // central point y axis + const Matx33f cameraIntegrateIntrinsics = Matx33f(ifx, 0, icx, 0, ify, icy, 0, 0, 1); // camera settings + + static const int raycastWidth = 640; + static const int raycastHeight = 480; + float rfx = 525.f; // focus point x axis + float rfy = 525.f; // focus point y axis + float rcx = float(raycastWidth) / 2.f - 0.5f; // central point x axis + float rcy = float(raycastHeight) / 2.f - 0.5f; // central point y axis + const Matx33f cameraRaycastIntrinsics = Matx33f(rfx, 0, rcx, 0, rfy, rcy, 0, 0, 1); // camera settings + + static constexpr float depthFactor = 5000.f; // 5000 for the 16-bit PNG files, 1 for the 32-bit float images in the ROS bag files + static constexpr float volumeSize = 3.f; // meters + static constexpr float voxelSize = volumeSize / 512.f; //meters (similar to HashTSDF) + static constexpr float tsdfTruncateDistance = 7 * voxelSize; // similar to HashTSDF + static constexpr float maxDepth = 4.f; + static const int maxWeight = 64; // number of frames + static constexpr float raycastStepFactor = 0.25f; // similar to HashTSDF + static const bool zFirstMemOrder = true; // order of voxels in volume + static constexpr float gradientDeltaFactor = 1.0f; + static const int volumeUnitHideThreshold = 10; + + const Affine3f volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)); + const Matx44f volumePoseMatrix = volumePose.matrix; + // Unlike original code, this should work with any volume size + // Not only when (x,y,z % 32) == 0 + const Point3i volumeResolution = Vec3i::all(16); //number of voxels (similar to HashTSDF) +}; + }; @@ -291,7 +343,10 @@ void VolumeSettings::setCameraIntegrateIntrinsics(InputArray val) { this->impl-> void VolumeSettings::getCameraIntegrateIntrinsics(OutputArray val) const { this->impl->getCameraIntegrateIntrinsics(val); }; void VolumeSettings::setCameraRaycastIntrinsics(InputArray val) { this->impl->setCameraRaycastIntrinsics(val); }; void VolumeSettings::getCameraRaycastIntrinsics(OutputArray val) const { this->impl->getCameraRaycastIntrinsics(val); }; - +void VolumeSettings::setGradientDeltaFactor(float val) { this->impl->setGradientDeltaFactor(val); }; +float VolumeSettings::getGradientDeltaFactor() const { return this->impl->getGradientDeltaFactor(); }; +void VolumeSettings::setVolumeUnitHideThreshold(int val) { this->impl->setVolumeUnitHideThreshold(val); }; +int VolumeSettings::getVolumeUnitHideThreshold() const { return this->impl->getVolumeUnitHideThreshold(); }; VolumeSettingsImpl::VolumeSettingsImpl() : VolumeSettingsImpl(VolumeType::TSDF) @@ -322,6 +377,8 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType) this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder); this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; + this->gradientDeltaFactor = ds.gradientDeltaFactor; + this->volumeUnitHideThreshold = ds.volumeUnitHideThreshold; } else if (volumeType == VolumeType::HashTSDF) { @@ -344,6 +401,8 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType) this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder); this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; + this->gradientDeltaFactor = ds.gradientDeltaFactor; + this->volumeUnitHideThreshold = ds.volumeUnitHideThreshold; } else if (volumeType == VolumeType::ColorTSDF) { @@ -366,6 +425,32 @@ VolumeSettingsImpl::VolumeSettingsImpl(VolumeType _volumeType) this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder); this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; + this->gradientDeltaFactor = ds.gradientDeltaFactor; + this->volumeUnitHideThreshold = ds.volumeUnitHideThreshold; + } + else if (volumeType == VolumeType::ColorHashTSDF) + { + DefaultColorHashTsdfSets ds = DefaultColorHashTsdfSets(); + + this->integrateWidth = ds.integrateWidth; + this->integrateHeight = ds.integrateHeight; + this->raycastWidth = ds.raycastWidth; + this->raycastHeight = ds.raycastHeight; + this->depthFactor = ds.depthFactor; + this->voxelSize = ds.voxelSize; + this->tsdfTruncateDistance = ds.tsdfTruncateDistance; + this->maxDepth = ds.maxDepth; + this->maxWeight = ds.maxWeight; + this->raycastStepFactor = ds.raycastStepFactor; + this->zFirstMemOrder = ds.zFirstMemOrder; + + this->volumePose = ds.volumePoseMatrix; + this->volumeResolution = ds.volumeResolution; + this->volumeStrides = calcVolumeStrides(ds.volumeResolution, ds.zFirstMemOrder); + this->cameraIntegrateIntrinsics = ds.cameraIntegrateIntrinsics; + this->cameraRaycastIntrinsics = ds.cameraRaycastIntrinsics; + this->gradientDeltaFactor = ds.gradientDeltaFactor; + this->volumeUnitHideThreshold = ds.volumeUnitHideThreshold; } } @@ -532,4 +617,26 @@ void VolumeSettingsImpl::getCameraRaycastIntrinsics(OutputArray val) const Mat(this->cameraRaycastIntrinsics).copyTo(val); } +void VolumeSettingsImpl::setGradientDeltaFactor(float val) +{ + this->gradientDeltaFactor = val; +} + +float VolumeSettingsImpl::getGradientDeltaFactor() const +{ + return this->gradientDeltaFactor; +} + +void VolumeSettingsImpl::setVolumeUnitHideThreshold(int val) +{ + this->volumeUnitHideThreshold = val; +} + +int VolumeSettingsImpl::getVolumeUnitHideThreshold() const +{ + return this->volumeUnitHideThreshold; +} + + + } diff --git a/modules/ptcloud/test/test_tsdf.cpp b/modules/ptcloud/test/test_tsdf.cpp index d907b9f56c..e422abc4de 100644 --- a/modules/ptcloud/test/test_tsdf.cpp +++ b/modules/ptcloud/test/test_tsdf.cpp @@ -517,11 +517,11 @@ void staticBoundingBoxTest(VolumeType volumeType) } -// For HashTSDF only -void boundingBoxGrowthTest(bool enableGrowth) +// For HashTSDF and ColorHashTSDF +void boundingBoxGrowthTest(VolumeType volumeType, bool enableGrowth) { - VolumeSettings vs(VolumeType::HashTSDF); - Volume volume(VolumeType::HashTSDF, vs); + VolumeSettings vs(volumeType); + Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Matx33f intrIntegrate, intrRaycast; @@ -536,11 +536,25 @@ void boundingBoxGrowthTest(bool enableGrowth) UMat udepth; depth.copyTo(udepth); + const bool isColor = (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF); + Mat rgb; + UMat urgb; + if (isColor) + { + rgb = scene->rgb(poses[0]); + rgb.copyTo(urgb); + } + // depth is integrated with multiple weight // TODO: add weight parameter to integrate() call (both scalar and array of 8u/32f) const int nIntegrations = 1; for (int i = 0; i < nIntegrations; i++) - volume.integrate(udepth, poses[0].matrix); + { + if (isColor) + volume.integrate(udepth, urgb, poses[0].matrix); + else + volume.integrate(udepth, poses[0].matrix); + } Vec6f bb; volume.getBoundingBox(bb, Volume::BoundingBoxPrecision::VOLUME_UNIT); @@ -569,7 +583,12 @@ void boundingBoxGrowthTest(bool enableGrowth) volume.setEnableGrowth(enableGrowth); for (int i = 0; i < nIntegrations; i++) - volume.integrate(udepth2, poses[0].matrix); + { + if (isColor) + volume.integrate(udepth2, urgb, poses[0].matrix); + else + volume.integrate(udepth2, poses[0].matrix); + } Vec6f bb2; volume.getBoundingBox(bb2, Volume::BoundingBoxPrecision::VOLUME_UNIT); @@ -675,12 +694,12 @@ Ptr makeRepeatableScene(Size sz, Matx33f _intr, float _depthFactor) return makePtr(sz, _intr, _depthFactor); } -// For HashTSDF only -void hugeSceneGrowthTest() +// For HashTSDF and ColorHashTSDF +void hugeSceneGrowthTest(VolumeType volumeType) { - VolumeSettings vs(VolumeType::HashTSDF); + VolumeSettings vs(volumeType); vs.setMaxDepth(10); - Volume volume(VolumeType::HashTSDF, vs); + Volume volume(volumeType, vs); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Matx33f intrIntegrate, intrRaycast; @@ -690,12 +709,24 @@ void hugeSceneGrowthTest() Ptr scene = makeRepeatableScene(frameSize, intrIntegrate, depthFactor); std::vector poses = scene->getPoses(); + const bool isColor = (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF); + // this should exceed the standard size of 8192 volume units // to grow volume more, use more poses Mat depth = scene->depth(poses[0]); UMat udepth; depth.copyTo(udepth); - volume.integrate(udepth, poses[0].matrix); + if (isColor) + { + Mat rgb = scene->rgb(poses[0]); + UMat urgb; + rgb.copyTo(urgb); + volume.integrate(udepth, urgb, poses[0].matrix); + } + else + { + volume.integrate(udepth, poses[0].matrix); + } if (cvtest::debugLevel > 0) { debugVolumeDraw(volume, poses[0], depth, depthFactor, "pts.obj"); @@ -769,18 +800,18 @@ static Mat normalsError(Mat srcNormals, Mat dstNormals) return Mat(); } -void regressionVolPoseRot() +void regressionVolPoseRot(VolumeType volumeType) { // Make 2 volumes which differ only in their pose (especially rotation) - VolumeSettings vs(VolumeType::HashTSDF); - Volume volume0(VolumeType::HashTSDF, vs); + VolumeSettings vs(volumeType); + Volume volume0(volumeType, vs); VolumeSettings vsRot(vs); Matx44f pose; vsRot.getVolumePose(pose); pose = Affine3f(Vec3f(1, 1, 1), Vec3f()).matrix; vsRot.setVolumePose(pose); - Volume volumeRot(VolumeType::HashTSDF, vsRot); + Volume volumeRot(volumeType, vsRot); Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight()); Matx33f intrIntegrate, intrRaycast; @@ -796,8 +827,20 @@ void regressionVolPoseRot() UMat udepth; depth.copyTo(udepth); - volume0.integrate(udepth, poses[0].matrix); - volumeRot.integrate(udepth, poses[0].matrix); + const bool isColor = (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF); + UMat urgb; + if (isColor) + { + Mat rgb = scene->rgb(poses[0]); + rgb.copyTo(urgb); + volume0.integrate(udepth, urgb, poses[0].matrix); + volumeRot.integrate(udepth, urgb, poses[0].matrix); + } + else + { + volume0.integrate(udepth, poses[0].matrix); + volumeRot.integrate(udepth, poses[0].matrix); + } UMat upts, unrm, uptsRot, unrmRot; @@ -875,15 +918,15 @@ namespace { struct VolumeTypeEnum { - static const std::array vals; - static const std::array svals; + static const std::array vals; + static const std::array svals; VolumeTypeEnum(VolumeType v = VolumeType::TSDF) : val(v) {} operator VolumeType() const { return val; } void PrintTo(std::ostream *os) const { int v = int(val); - if (v >= 0 && v < 3) + if (v >= 0 && v < 4) { *os << svals[v]; } @@ -894,14 +937,24 @@ namespace } static ::testing::internal::ParamGenerator all() { - return ::testing::Values(VolumeTypeEnum(vals[0]), VolumeTypeEnum(vals[1]), VolumeTypeEnum(vals[2])); + return ::testing::Values(VolumeTypeEnum(vals[0]), VolumeTypeEnum(vals[1]), VolumeTypeEnum(vals[2]), VolumeTypeEnum(vals[3])); } private: VolumeType val; }; - const std::array VolumeTypeEnum::vals{VolumeType::TSDF, VolumeType::HashTSDF, VolumeType::ColorTSDF}; - const std::array VolumeTypeEnum::svals{std::string("TSDF"), std::string("HashTSDF"), std::string("ColorTSDF")}; + const std::array VolumeTypeEnum::vals{ + VolumeType::TSDF, + VolumeType::HashTSDF, + VolumeType::ColorTSDF, + VolumeType::ColorHashTSDF + }; + const std::array VolumeTypeEnum::svals{ + std::string("TSDF"), + std::string("HashTSDF"), + std::string("ColorTSDF"), + std::string("ColorHashTSDF") + }; static inline void PrintTo(const VolumeTypeEnum &t, std::ostream *os) { t.PrintTo(os); } @@ -1011,7 +1064,7 @@ protected: if (testSrcType == VolumeTestSrcType::MAT) { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) volume->integrate(udepth, urgb, poses[0].matrix); else volume->integrate(udepth, poses[0].matrix); @@ -1056,7 +1109,8 @@ void VolumeTestFixture::saveObj(std::string funcName, Mat points, Mat normals) string platformString = gpu ? "GPU" : "CPU"; string volumeTypeString = volumeType == VolumeType::TSDF ? "TSDF" : volumeType == VolumeType::HashTSDF ? "HashTSDF" : - volumeType == VolumeType::ColorTSDF ? "ColorTSDF" : ""; + volumeType == VolumeType::ColorTSDF ? "ColorTSDF" : + volumeType == VolumeType::ColorHashTSDF ? "ColorHashTSDF" : ""; string testSrcTypeString = testSrcType == VolumeTestSrcType::MAT ? "MAT" : testSrcType == VolumeTestSrcType::ODOMETRY_FRAME ? "OFRAME" : ""; string frameSizeSpecifiedString = frameSizeSpecified == FrameSizeType::DEFAULT ? "DefaultSize" : @@ -1071,14 +1125,14 @@ void VolumeTestFixture::raycast_test() UMat upoints, unormals, ucolors; if (frameSizeSpecified == FrameSizeType::CUSTOM) { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) volume->raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals, ucolors); else volume->raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals); } else if (frameSizeSpecified == FrameSizeType::DEFAULT) { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) volume->raycast(poses[0].matrix, upoints, unormals, ucolors); else volume->raycast(poses[0].matrix, upoints, unormals); @@ -1091,7 +1145,7 @@ void VolumeTestFixture::raycast_test() if (cvtest::debugLevel > 0) { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) displayColorImage(depth, rgb, points, normals, colors, depthFactor, lightPose); else displayImage(depth, points, normals, depthFactor, lightPose); @@ -1146,14 +1200,14 @@ void VolumeTestFixture::valid_points_test() UMat upoints, unormals, ucolors; if (frameSizeSpecified == FrameSizeType::CUSTOM) { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) volume->raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals, ucolors); else volume->raycast(poses[0].matrix, frameSize.height, frameSize.width, intrRaycast, upoints, unormals); } else if (frameSizeSpecified == FrameSizeType::DEFAULT) { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) volume->raycast(poses[0].matrix, upoints, unormals, ucolors); else volume->raycast(poses[0].matrix, upoints, unormals); @@ -1169,7 +1223,7 @@ void VolumeTestFixture::valid_points_test() if (cvtest::debugLevel > 0) { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) displayColorImage(depth, rgb, points, normals, colors, depthFactor, lightPose); else displayImage(depth, points, normals, depthFactor, lightPose); @@ -1180,14 +1234,14 @@ void VolumeTestFixture::valid_points_test() if (frameSizeSpecified == FrameSizeType::CUSTOM) { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) volume->raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, upoints2, unormals2, ucolors2); else volume->raycast(poses[17].matrix, frameSize.height, frameSize.width, intrRaycast, upoints2, unormals2); } else { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) volume->raycast(poses[17].matrix, upoints2, unormals2, ucolors2); else volume->raycast(poses[17].matrix, upoints2, unormals2); @@ -1202,7 +1256,7 @@ void VolumeTestFixture::valid_points_test() if (cvtest::debugLevel > 0) { - if (volumeType == VolumeType::ColorTSDF) + if (volumeType == VolumeType::ColorTSDF || volumeType == VolumeType::ColorHashTSDF) displayColorImage(depth, rgb, points2, normals2, colors2, depthFactor, lightPose); else displayImage(depth, points2, normals2, depthFactor, lightPose); @@ -1239,14 +1293,21 @@ TEST_P(VolumeTestFixture, fetch_normals) } //TODO: fix it when ColorTSDF gets GPU version -INSTANTIATE_TEST_CASE_P(Volume, VolumeTestFixture, /*::testing::Combine(PlatformTypeEnum::all(), VolumeTypeEnum::all())*/ - ::testing::Combine( - ::testing::Values(PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF}, - PlatformVolumeType {PlatformType::CPU, VolumeType::HashTSDF}, - PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF}, - PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF}, - PlatformVolumeType {PlatformType::GPU, VolumeType::HashTSDF}), - VolumeTestSrcTypeEnum::all(), FrameSizeTypeEnum::all())); +INSTANTIATE_TEST_CASE_P(Volume, VolumeTestFixture, + ::testing::Combine( + ::testing::Values( + PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF}, + PlatformVolumeType {PlatformType::CPU, VolumeType::HashTSDF}, + PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF}, + PlatformVolumeType {PlatformType::CPU, VolumeType::ColorHashTSDF}, + PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF}, + PlatformVolumeType {PlatformType::GPU, VolumeType::HashTSDF} + // Note: Color types don't support GPU yet + ), + VolumeTestSrcTypeEnum::all(), + FrameSizeTypeEnum::all() + ) +); class StaticVolumeBoundingBox : public ::testing::TestWithParam @@ -1267,27 +1328,33 @@ TEST_P(StaticVolumeBoundingBox, staticBoundingBox) //TODO: edit this list when ColorTSDF gets GPU support INSTANTIATE_TEST_CASE_P(Volume, StaticVolumeBoundingBox, ::testing::Values( - PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF}, - PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF}, - PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF})); + PlatformVolumeType {PlatformType::CPU, VolumeType::TSDF}, + PlatformVolumeType {PlatformType::CPU, VolumeType::ColorTSDF}, + PlatformVolumeType {PlatformType::GPU, VolumeType::TSDF} +)); -class ReproduceVolPoseRotTest : public ::testing::TestWithParam +class ReproduceVolPoseRotTest : public ::testing::TestWithParam> { }; TEST_P(ReproduceVolPoseRotTest, reproduce_volPoseRot) { - bool gpu = (GetParam() == PlatformType::GPU); + auto p = GetParam(); + bool gpu = (std::get<0>(p) == PlatformType::GPU); + VolumeType volumeType = std::get<1>(p); OpenCLStatusRevert oclStatus; if (!gpu) oclStatus.off(); - regressionVolPoseRot(); + regressionVolPoseRot(volumeType); } -INSTANTIATE_TEST_CASE_P(Volume, ReproduceVolPoseRotTest, PlatformTypeEnum::all()); +INSTANTIATE_TEST_CASE_P(Volume, ReproduceVolPoseRotTest, + ::testing::Combine(PlatformTypeEnum::all(), + ::testing::Values(VolumeTypeEnum(VolumeType::HashTSDF), + VolumeTypeEnum(VolumeType::ColorHashTSDF)))); enum Growth @@ -1296,7 +1363,7 @@ enum Growth }; CV_ENUM(GrowthEnum, Growth::OFF, Growth::ON); -class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam> +class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam> { }; TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth) @@ -1304,35 +1371,43 @@ TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth) auto p = GetParam(); bool gpu = (std::get<0>(p) == PlatformType::GPU); bool enableGrowth = (std::get<1>(p) == Growth::ON); + VolumeType volumeType = std::get<2>(p); OpenCLStatusRevert oclStatus; if (!gpu) oclStatus.off(); - boundingBoxGrowthTest(enableGrowth); + boundingBoxGrowthTest(volumeType, enableGrowth); } -INSTANTIATE_TEST_CASE_P(Volume, BoundingBoxEnableGrowthTest, ::testing::Combine(PlatformTypeEnum::all(), GrowthEnum::all())); +INSTANTIATE_TEST_CASE_P(Volume, BoundingBoxEnableGrowthTest, + ::testing::Combine(PlatformTypeEnum::all(), GrowthEnum::all(), + ::testing::Values(VolumeTypeEnum(VolumeType::HashTSDF), + VolumeTypeEnum(VolumeType::ColorHashTSDF)))); -class HugeSceneGrowthTest : public ::testing::TestWithParam +class HugeSceneGrowthTest : public ::testing::TestWithParam> { }; TEST_P(HugeSceneGrowthTest, boundingBoxEnableGrowth) { auto p = GetParam(); - bool gpu = (p == PlatformType::GPU); + bool gpu = (std::get<0>(p) == PlatformType::GPU); + VolumeType volumeType = std::get<1>(p); OpenCLStatusRevert oclStatus; if (!gpu) oclStatus.off(); - hugeSceneGrowthTest(); + hugeSceneGrowthTest(volumeType); } -INSTANTIATE_TEST_CASE_P(Volume, HugeSceneGrowthTest, PlatformTypeEnum::all()); +INSTANTIATE_TEST_CASE_P(Volume, HugeSceneGrowthTest, + ::testing::Combine(PlatformTypeEnum::all(), + ::testing::Values(VolumeTypeEnum(VolumeType::HashTSDF), + VolumeTypeEnum(VolumeType::ColorHashTSDF)))); } } // namespace