diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp index 3aac8592846..9ac87305256 100644 --- a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -75,10 +75,15 @@ struct CV_EXPORTS_W Params CV_WRAP static Ptr hashTSDFParams(bool isCoarse); /** @brief ColoredTSDF parameters - A set of parameters suitable for use with HashTSDFVolume + A set of parameters suitable for use with ColoredTSDFVolume */ CV_WRAP static Ptr coloredTSDFParams(bool isCoarse); + /** @brief ColoredHashTSDF parameters + A set of parameters suitable for use with ColoredHashTSDFVolume + */ + CV_WRAP static Ptr coloredHashTSDFParams(bool isCoarse); + /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 696b7265587..9aa18df237e 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -79,6 +79,11 @@ struct CV_EXPORTS_W Params */ CV_WRAP static Ptr coloredTSDFParams(bool isCoarse); + /** @brief ColoredHashTSDF parameters + A set of parameters suitable for use with ColoredHashTSDFVolume + */ + CV_WRAP static Ptr coloredHashTSDFParams(bool isCoarse); + /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 8dc0dce5fa8..2440160ad42 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -56,7 +56,8 @@ enum class VolumeType { TSDF = 0, HASHTSDF = 1, - COLOREDTSDF = 2 + COLOREDTSDF = 2, + COLOREDHASHTSDF = 3 }; struct CV_EXPORTS_W VolumeParams diff --git a/modules/rgbd/src/colored_hash_tsdf.cpp b/modules/rgbd/src/colored_hash_tsdf.cpp new file mode 100644 index 00000000000..281776aae2b --- /dev/null +++ b/modules/rgbd/src/colored_hash_tsdf.cpp @@ -0,0 +1,1000 @@ +// 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 +#include "precomp.hpp" +#include "colored_hash_tsdf.hpp" + +#include +#include +#include +#include +#include + +#include "kinfu_frame.hpp" +#include "opencv2/core/cvstd.hpp" +#include "opencv2/core/utility.hpp" +#include "opencv2/core/utils/trace.hpp" +#include "utils.hpp" +#include "opencl_kernels_rgbd.hpp" + +#define USE_INTERPOLATION_IN_GETNORMAL 1 +#define VOLUMES_SIZE 8192 + +namespace cv +{ +namespace kinfu +{ + +ColoredHashTSDFVolume::ColoredHashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, float _truncateThreshold, + int _volumeUnitRes, bool _zFirstMemOrder) + : Volume(_voxelSize, _pose, _raycastStepFactor), + maxWeight(_maxWeight), + truncateThreshold(_truncateThreshold), + volumeUnitResolution(_volumeUnitRes), + volumeUnitSize(voxelSize* volumeUnitResolution), + zFirstMemOrder(_zFirstMemOrder) +{ + truncDist = std::max(_truncDist, 4.0f * voxelSize); + + if (!(volumeUnitResolution & (volumeUnitResolution - 1))) + { + // vuRes is a power of 2, let's get this power + volumeUnitDegree = trailingZeros32(volumeUnitResolution); + } + else + { + CV_Error(Error::StsBadArg, "Volume unit resolution should be a power of 2"); + } + + int xdim, ydim, zdim; + if (zFirstMemOrder) + { + xdim = volumeUnitResolution * volumeUnitResolution; + ydim = volumeUnitResolution; + zdim = 1; + } + else + { + xdim = 1; + ydim = volumeUnitResolution; + zdim = volumeUnitResolution * volumeUnitResolution; + } + volStrides = Vec4i(xdim, ydim, zdim); +} + +//! Spatial hashing +struct tsdf_hash +{ + size_t operator()(const Vec3i& x) const noexcept + { + size_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + for (uint16_t i = 0; i < 3; i++) + { + seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + } + return seed; + } +}; + +struct VolumeUnit +{ + cv::Vec3i coord; + int index; + cv::Matx44f pose; + int lastVisibleIndex = 0; + bool isActive; +}; + +typedef std::unordered_set VolumeUnitIndexSet; +typedef std::unordered_map VolumeUnitIndexes; + + + +class ColoredHashTSDFVolumeCPU : public ColoredHashTSDFVolume +{ +public: + // dimension in voxels, size in meters + ColoredHashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); + + ColoredHashTSDFVolumeCPU(const VolumeParams& _params, bool zFirstMemOrder = true); + + void integrate(InputArray, float, const Matx44f&, const kinfu::Intr&, const int) override + {/* CV_Error(Error::StsNotImplemented, "Not implemented"); */}; + + // Added rgb array and rgb intrinscis + void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& depth_intrinsics, + const Intr& rgb_intrinsics, const int frameId = 0) override; + + void raycast(const Matx44f&, const kinfu::Intr&, const Size&, OutputArray, OutputArray) const override + {/*CV_Error(Error::StsNotImplemented, "Not implemented");*/}; + + // Added colors output array + void raycast(const Matx44f& cameraPose, const kinfu::Intr& depth_intrinsics, const Size& frameSize, + OutputArray points, OutputArray normals, OutputArray colors) const override; + + void fetchNormals(InputArray points, OutputArray _normals) const override; + void fetchPointsNormals(OutputArray points, OutputArray normals) const override + { + fetchPointsNormalsColors(points, normals, noArray()); + } + void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override; + + void reset() override; + size_t getTotalVolumeUnits() const override { return volumeUnits.size(); } + int getVisibleBlocks(int currFrameId, int frameThreshold) const override; + + //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) + RGBTsdfVoxel at(const Vec3i& volumeIdx) const; + + //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = + //! 1m) + virtual RGBTsdfVoxel at(const cv::Point3f& point) const; + virtual RGBTsdfVoxel _at(const cv::Vec3i& volumeIdx, int indx) const; + + RGBTsdfVoxel atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const; + + float interpolateVoxelPoint(const Point3f& point) const; + float interpolateVoxel(const cv::Point3f& point) const; + Point3f getNormalVoxel(const cv::Point3f& p) const; + + // for color support + float interpolateColor(float tx, float ty, float tz, float vx[8]) const; + + Point3f getColorVoxel(const cv::Point3f& p) const; + + //! Utility functions for coordinate transformations + Vec3i volumeToVolumeUnitIdx(const Point3f& point) const; + Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const; + + Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; + Vec3i volumeToVoxelCoord(const Point3f& point) const; + +public: + Vec6f frameParams; + Mat pixNorms; + VolumeUnitIndexes volumeUnits; + cv::Mat volUnitsData; + int lastVolIndex; +}; + + +ColoredHashTSDFVolumeCPU::ColoredHashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, + float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) + : ColoredHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, + _truncDist, _maxWeight, _truncateThreshold, + _volumeUnitRes, _zFirstMemOrder) +{ + reset(); +} + +ColoredHashTSDFVolumeCPU::ColoredHashTSDFVolumeCPU(const VolumeParams& _params, bool _zFirstMemOrder) + : ColoredHashTSDFVolumeCPU(_params.voxelSize, _params.pose.matrix, + _params.raycastStepFactor, _params.tsdfTruncDist, + _params.maxWeight, _params.depthTruncThreshold, + _params.unitResolution, _zFirstMemOrder) +{ +} + +// zero volume, leave rest params the same +void ColoredHashTSDFVolumeCPU::reset() +{ + CV_TRACE_FUNCTION(); + lastVolIndex = 0; + volUnitsData = cv::Mat(VOLUMES_SIZE, volumeUnitResolution * volumeUnitResolution * volumeUnitResolution, rawType()); + frameParams = Vec6f(); + pixNorms = Mat(); + volumeUnits = VolumeUnitIndexes(); +} + +void ColoredHashTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& depth_intrinsics, + const Intr& rgb_intrinsics, const int frameId) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + Depth depth = _depth.getMat(); + + Colors rgb = _rgb.getMat(); + + //! Compute volumes to be allocated + const int depthStride = volumeUnitDegree; + const float invDepthFactor = 1.f / depthFactor; + const Intr::Reprojector reproj(depth_intrinsics.makeReprojector()); + const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); + const Point3f truncPt(truncDist, truncDist, truncDist); + VolumeUnitIndexSet newIndices; + Mutex mutex; + Range allocateRange(0, depth.rows); + + auto AllocateVolumeUnitsInvoker = [&](const Range& range) { + VolumeUnitIndexSet localAccessVolUnits; + for (int y = range.start; y < range.end; y += depthStride) + { + const depthType* depthRow = depth[y]; + for (int x = 0; x < depth.cols; x += depthStride) + { + depthType z = depthRow[x] * invDepthFactor; + if (z <= 0 || z > this->truncateThreshold) + continue; + Point3f camPoint = reproj(Point3f((float)x, (float)y, z)); + Point3f volPoint = cam2vol * camPoint; + //! Find accessed TSDF volume unit for valid 3D vertex + Vec3i lower_bound = this->volumeToVolumeUnitIdx(volPoint - truncPt); + Vec3i upper_bound = this->volumeToVolumeUnitIdx(volPoint + truncPt); + + for (int i = lower_bound[0]; i <= upper_bound[0]; i++) + for (int j = lower_bound[1]; j <= upper_bound[1]; j++) + for (int k = lower_bound[2]; k <= upper_bound[2]; k++) + { + const Vec3i tsdf_idx = Vec3i(i, j, k); + if (localAccessVolUnits.count(tsdf_idx) <= 0 && this->volumeUnits.count(tsdf_idx) <= 0) + { + //! This volume unit will definitely be required for current integration + localAccessVolUnits.emplace(tsdf_idx); + + } + } + } + } + + 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 = this->volumeUnits.emplace(idx, VolumeUnit()).first->second; + + Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; + + vu.pose = subvolumePose; + vu.index = lastVolIndex; lastVolIndex++; + if (lastVolIndex > int(volUnitsData.size().height)) + { + volUnitsData.resize((lastVolIndex - 1) * 2); + } + volUnitsData.row(vu.index).forEach([](VecRGBTsdfVoxel& vv, const int* /* position */) + { + RGBTsdfVoxel& 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(depth_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); + Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; + if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) + { + 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; + } + } + }); + + Vec6f newParams((float)depth.rows, (float)depth.cols, + depth_intrinsics.fx, depth_intrinsics.fy, + depth_intrinsics.cx, depth_intrinsics.cy); + if ( !(frameParams==newParams) ) + { + frameParams = newParams; + pixNorms = preCalculationPixNorm(depth, depth_intrinsics); + } + + //! Integrate the correct volumeUnits + parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) { + for (int i = range.start; i < range.end; i++) + { + Vec3i tsdf_idx = totalVolUnits[i]; + VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx); + if (it == volumeUnits.end()) + return; + + VolumeUnit& volumeUnit = it->second; + if (volumeUnit.isActive) + { + integrateRGBVolumeUnit(truncDist, voxelSize, maxWeight, volumeUnit.pose, + Point3i(volumeUnitResolution, volumeUnitResolution, volumeUnitResolution), volStrides, + depth, rgb, depthFactor, cameraPose, depth_intrinsics, rgb_intrinsics, + pixNorms, volUnitsData.row(volumeUnit.index)); + //! Ensure all active volumeUnits are set to inactive for next integration + volumeUnit.isActive = false; + } + } + }); +} + +// Volume and Index + +cv::Vec3i ColoredHashTSDFVolumeCPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const +{ + return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), + cvFloor(p.z / volumeUnitSize)); +} + +cv::Point3f ColoredHashTSDFVolumeCPU::volumeUnitIdxToVolume(const cv::Vec3i& volumeUnitIdx) const +{ + return cv::Point3f(volumeUnitIdx[0] * volumeUnitSize, volumeUnitIdx[1] * volumeUnitSize, + volumeUnitIdx[2] * volumeUnitSize); +} + +cv::Point3f ColoredHashTSDFVolumeCPU::voxelCoordToVolume(const cv::Vec3i& voxelIdx) const +{ + return cv::Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); +} + +cv::Vec3i ColoredHashTSDFVolumeCPU::volumeToVoxelCoord(const cv::Point3f& point) const +{ + return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), + cvFloor(point.z * voxelSizeInv)); +} + + +// function At + +inline RGBTsdfVoxel ColoredHashTSDFVolumeCPU::_at(const cv::Vec3i& volumeIdx, int indx) const +{ + // Out of limits + 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, 160, 160, 160}; + } + + const RGBTsdfVoxel* volData = volUnitsData.ptr(indx); + int coordBase = volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2]; + return volData[coordBase]; +} + +inline RGBTsdfVoxel ColoredHashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const +{ + Vec3i volumeUnitIdx = Vec3i(volumeIdx[0] >> volumeUnitDegree, + volumeIdx[1] >> volumeUnitDegree, + volumeIdx[2] >> volumeUnitDegree); + + VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); + + if (it == volumeUnits.end()) + { + return RGBTsdfVoxel{floatToTsdf(1.f), 0, 160, 160, 160}; + } + + cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] << volumeUnitDegree, + volumeUnitIdx[1] << volumeUnitDegree, + volumeUnitIdx[2] << volumeUnitDegree); + + volUnitLocalIdx = + cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + return _at(volUnitLocalIdx, it->second.index); + +} + +RGBTsdfVoxel ColoredHashTSDFVolumeCPU::at(const Point3f& point) const +{ + Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); + VolumeUnitIndexes::const_iterator it = volumeUnits.find(volumeUnitIdx); + + if (it == volumeUnits.end()) + { + return RGBTsdfVoxel{floatToTsdf(1.f), 0, 160, 160, 160}; + } + + Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); + Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); + volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + + return _at(volUnitLocalIdx, it->second.index); +} + + +RGBTsdfVoxel ColoredHashTSDFVolumeCPU::atVolumeUnit(const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it) const +{ + if (it == volumeUnits.end()) + { + return RGBTsdfVoxel{floatToTsdf(1.f), 0, 160, 160, 160}; + } + Vec3i volUnitLocalIdx = point - Vec3i(volumeUnitIdx[0] << volumeUnitDegree, + volumeUnitIdx[1] << volumeUnitDegree, + volumeUnitIdx[2] << volumeUnitDegree); + + // expanding at(), removing bounds check + 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]; +} + + + +//Interpolate + +#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 + + +float ColoredHashTSDFVolumeCPU::interpolateVoxelPoint(const Point3f& point) const +{ + const Vec3i neighbourCoords[] = { {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, + {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1} }; + + // A small hash table to reduce a number of find() calls + bool queried[8]; + VolumeUnitIndexes::const_iterator iterMap[8]; + for (int i = 0; i < 8; i++) + { + iterMap[i] = volumeUnits.end(); + queried[i] = false; + } + + int ix = cvFloor(point.x); + int iy = cvFloor(point.y); + int iz = cvFloor(point.z); + + float tx = point.x - ix; + float ty = point.y - iy; + float tz = point.z - iz; + + Vec3i iv(ix, iy, iz); + float vx[8]; + for (int i = 0; i < 8; i++) + { + Vec3i pt = iv + neighbourCoords[i]; + + Vec3i volumeUnitIdx = 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; + } + + vx[i] = atVolumeUnit(pt, volumeUnitIdx, it).tsdf; + } + + return interpolate(tx, ty, tz, vx); +} + +inline float ColoredHashTSDFVolumeCPU::interpolateVoxel(const cv::Point3f& point) const +{ + return interpolateVoxelPoint(point * voxelSizeInv); +} + +float ColoredHashTSDFVolumeCPU::interpolateColor(float tx, float ty, float tz, float vx[8]) const +{ + return interpolate(tx, ty, tz, vx); +} + + +Point3f ColoredHashTSDFVolumeCPU::getNormalVoxel(const Point3f &point) const +{ + Vec3f normal = Vec3f(0, 0, 0); + + Point3f ptVox = point * voxelSizeInv; + Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); + + // A small hash table to reduce a number of find() calls + bool queried[8]; + VolumeUnitIndexes::const_iterator iterMap[8]; + for (int i = 0; i < 8; i++) + { + iterMap[i] = volumeUnits.end(); + queried[i] = false; + } + +#if !USE_INTERPOLATION_IN_GETNORMAL + const Vec3i offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3 + { 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7 + }; + const int nVals = 6; + +#else + const Vec3i offsets[] = { { 0, 0, 0}, { 0, 0, 1}, { 0, 1, 0}, { 0, 1, 1}, // 0-3 + { 1, 0, 0}, { 1, 0, 1}, { 1, 1, 0}, { 1, 1, 1}, // 4-7 + {-1, 0, 0}, {-1, 0, 1}, {-1, 1, 0}, {-1, 1, 1}, // 8-11 + { 2, 0, 0}, { 2, 0, 1}, { 2, 1, 0}, { 2, 1, 1}, // 12-15 + { 0, -1, 0}, { 0, -1, 1}, { 1, -1, 0}, { 1, -1, 1}, // 16-19 + { 0, 2, 0}, { 0, 2, 1}, { 1, 2, 0}, { 1, 2, 1}, // 20-23 + { 0, 0, -1}, { 0, 1, -1}, { 1, 0, -1}, { 1, 1, -1}, // 24-27 + { 0, 0, 2}, { 0, 1, 2}, { 1, 0, 2}, { 1, 1, 2}, // 28-31 + }; + const int nVals = 32; +#endif + + float vals[nVals]; + for (int i = 0; i < nVals; i++) + { + Vec3i pt = iptVox + offsets[i]; + + Vec3i volumeUnitIdx = 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(pt, volumeUnitIdx, it).tsdf); + } + +#if !USE_INTERPOLATION_IN_GETNORMAL + for (int c = 0; c < 3; c++) + { + normal[c] = vals[c * 2] - vals[c * 2 + 1]; + } +#else + + float cxv[8], cyv[8], czv[8]; + + // How these numbers were obtained: + // 1. Take the basic interpolation sequence: + // 000, 001, 010, 011, 100, 101, 110, 111 + // where each digit corresponds to shift by x, y, z axis respectively. + // 2. Add +1 for next or -1 for prev to each coordinate to corresponding axis + // 3. Search corresponding values in offsets + const int idxxp[8] = { 8, 9, 10, 11, 0, 1, 2, 3 }; + const int idxxn[8] = { 4, 5, 6, 7, 12, 13, 14, 15 }; + const int idxyp[8] = { 16, 17, 0, 1, 18, 19, 4, 5 }; + const int idxyn[8] = { 2, 3, 20, 21, 6, 7, 22, 23 }; + const int idxzp[8] = { 24, 0, 25, 2, 26, 4, 27, 6 }; + const int idxzn[8] = { 1, 28, 3, 29, 5, 30, 7, 31 }; + +#if !USE_INTRINSICS + for (int i = 0; i < 8; i++) + { + cxv[i] = vals[idxxn[i]] - vals[idxxp[i]]; + cyv[i] = vals[idxyn[i]] - vals[idxyp[i]]; + czv[i] = vals[idxzn[i]] - vals[idxzp[i]]; + } +#else + +# if CV_SIMD >= 32 + v_float32x8 cxp = v_lut(vals, idxxp); + v_float32x8 cxn = v_lut(vals, idxxn); + + v_float32x8 cyp = v_lut(vals, idxyp); + v_float32x8 cyn = v_lut(vals, idxyn); + + v_float32x8 czp = v_lut(vals, idxzp); + v_float32x8 czn = v_lut(vals, idxzn); + + v_float32x8 vcxv = 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; +} + +Point3f ColoredHashTSDFVolumeCPU::getColorVoxel(const Point3f& point) const +{ + Point3f ptVox = point * voxelSizeInv; + Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); + + const Vec3i neighbourCoords[] = { + {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {0, 1, 1}, + {1, 0, 0}, {1, 0, 1}, {1, 1, 0}, {1, 1, 1} + }; + + float tx = ptVox.x - iptVox[0]; + float ty = ptVox.y - iptVox[1]; + float tz = ptVox.z - iptVox[2]; + + float r[8], g[8], b[8]; + + bool queried[8] = { false }; + VolumeUnitIndexes::const_iterator iterMap[8]; + for (int i = 0; i < 8; i++) iterMap[i] = volumeUnits.end(); + + for (int i = 0; i < 8; i++) + { + Vec3i pt = iptVox + neighbourCoords[i]; + Vec3i volumeUnitIdx(pt[0] >> volumeUnitDegree, pt[1] >> volumeUnitDegree, pt[2] >> volumeUnitDegree); + int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4; + + if (!queried[dictIdx]) + { + iterMap[dictIdx] = volumeUnits.find(volumeUnitIdx); + queried[dictIdx] = true; + } + + RGBTsdfVoxel v = atVolumeUnit(pt, volumeUnitIdx, iterMap[dictIdx]); + r[i] = (float)v.r; + g[i] = (float)v.g; + b[i] = (float)v.b; + } + + Point3f res; + res.x = interpolateColor(tx, ty, tz, r); + res.y = interpolateColor(tx, ty, tz, g); + res.z = interpolateColor(tx, ty, tz, b); + + colorFix(res); + return res; +} + + +void ColoredHashTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, + OutputArray _points, OutputArray _normals, OutputArray _colors) const +{ + CV_TRACE_FUNCTION(); + CV_Assert(frameSize.area() > 0); + + _points.create(frameSize, POINT_TYPE); + _normals.create(frameSize, POINT_TYPE); + _colors.create(frameSize, POINT_TYPE); + + Points points1 = _points.getMat(); + Normals normals1 = _normals.getMat(); + Colors colors1 = _colors.getMat(); + + Points& points(points1); + Normals& normals(normals1); + Colors& colors(colors1); + const ColoredHashTSDFVolumeCPU& volume(*this); + const float tstep(volume.truncDist * volume.raycastStepFactor); + const Affine3f cam2vol(volume.pose.inv() * Affine3f(cameraPose)); + const Affine3f vol2cam(Affine3f(cameraPose.inv()) * volume.pose); + const Intr::Reprojector reproj(intrinsics.makeReprojector()); + + const int nstripes = -1; + + auto _ColoredHashRaycastInvoker = [&](const Range& range) + { + const Point3f cam2volTrans = cam2vol.translation(); + const Matx33f cam2volRot = cam2vol.rotation(); + const Matx33f vol2camRot = vol2cam.rotation(); + + const float blockSize = volume.volumeUnitSize; + + for (int y = range.start; y < range.end; y++) + { + ptype* ptsRow = points[y]; + ptype* nrmRow = normals[y]; + ptype* colRow = colors[y]; + + for (int x = 0; x < points.cols; x++) + { + //! Initialize default value + Point3f point = nan3, normal = nan3, color = nan3; + + //! Ray origin and direction in the volume coordinate frame + Point3f orig = cam2volTrans; + Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); + + float tmin = 0; + float tmax = volume.truncateThreshold; + float tcurr = tmin; + + cv::Vec3i prevVolumeUnitIdx = + cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), + std::numeric_limits::min()); + + float tprev = tcurr; + float prevTsdf = volume.truncDist; + while (tcurr < tmax) + { + Point3f currRayPos = orig + tcurr * rayDirV; + cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); + + VolumeUnitIndexes::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); + + float currTsdf = prevTsdf; + int currWeight = 0; + float stepSize = 0.5f * blockSize; + cv::Vec3i volUnitLocalIdx; + + + //! The subvolume exists in hashtable + if (it != volume.volumeUnits.end()) + { + cv::Point3f currVolUnitPos = + volume.volumeUnitIdxToVolume(currVolumeUnitIdx); + volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); + + //! TODO: Figure out voxel interpolation + RGBTsdfVoxel currVoxel = _at(volUnitLocalIdx, it->second.index); + currTsdf = tsdfToFloat(currVoxel.tsdf); + currWeight = currVoxel.weight; + stepSize = tstep; + } + //! Surface crossing + if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) + { + float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); + if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) + { + Point3f pv = orig + tInterp * rayDirV; + Point3f nv = volume.getNormalVoxel(pv); + Point3f cv = volume.getColorVoxel(pv); + + if (!isNaN(nv)) + { + normal = vol2camRot * nv; + point = vol2cam * pv; + color = cv; + } + } + break; + } + prevVolumeUnitIdx = currVolumeUnitIdx; + prevTsdf = currTsdf; + tprev = tcurr; + tcurr += stepSize; + } + ptsRow[x] = toPtype(point); + nrmRow[x] = toPtype(normal); + colRow[x] = toPtype(color); + } + } + }; + + parallel_for_(Range(0, points.rows), _ColoredHashRaycastInvoker, nstripes); +} + +void ColoredHashTSDFVolumeCPU::fetchPointsNormalsColors(OutputArray _points, OutputArray _normals, OutputArray _colors) const +{ + CV_TRACE_FUNCTION(); + + if (_points.needed()) + { + std::vector> pVecs, nVecs, cVecs; + + std::vector totalVolUnits; + for (const auto& keyvalue : volumeUnits) + { + totalVolUnits.push_back(keyvalue.first); + } + Range fetchRange(0, (int)totalVolUnits.size()); + const int nstripes = -1; + + const ColoredHashTSDFVolumeCPU& volume(*this); + bool needNormals(_normals.needed()); + bool needColors(_colors.needed()); + Mutex mutex; + + auto ColoredHashFetchPointsNormalsColorsInvoker = [&](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 = volume.volumeUnits.find(tsdf_idx); + Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); + if (it != volume.volumeUnits.end()) + { + std::vector localPoints; + std::vector localNormals; + std::vector localColors; + for (int x = 0; x < volume.volumeUnitResolution; x++) + for (int y = 0; y < volume.volumeUnitResolution; y++) + for (int z = 0; z < volume.volumeUnitResolution; z++) + { + cv::Vec3i voxelIdx(x, y, z); + RGBTsdfVoxel voxel = _at(voxelIdx, it->second.index); + + if (voxel.tsdf != -128 && voxel.weight != 0) + { + Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); + localPoints.push_back(toPtype(this->pose * point)); + if (needNormals) + { + Point3f normal = volume.getNormalVoxel(point); + localNormals.push_back(toPtype(this->pose.rotation() * normal)); + } + if(needColors) + { + Point3f color = Point3f(voxel.r/255.0f, voxel.g/255.0f, voxel.b/255.0f); + localColors.push_back(toPtype(color)); + } + } + } + + AutoLock al(mutex); + pVecs.push_back(localPoints); + nVecs.push_back(localNormals); + cVecs.push_back(localColors); + } + } + }; + + parallel_for_(fetchRange, ColoredHashFetchPointsNormalsColorsInvoker, 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, POINT_TYPE); + if (!colors.empty()) + Mat((int)colors.size(), 1, POINT_TYPE, &colors[0]).copyTo(_colors.getMat()); + } + } +} + +void ColoredHashTSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const +{ + CV_TRACE_FUNCTION(); + + if (_normals.needed()) + { + Points points = _points.getMat(); + CV_Assert(points.type() == POINT_TYPE); + + _normals.createSameSize(_points, _points.type()); + Normals normals = _normals.getMat(); + + const ColoredHashTSDFVolumeCPU& _volume = *this; + auto HashPushNormals = [&](const ptype& point, const int* position) { + const ColoredHashTSDFVolumeCPU& volume(_volume); + Affine3f invPose(volume.pose.inv()); + Point3f p = fromPtype(point); + Point3f n = nan3; + if (!isNaN(p)) + { + Point3f voxelPoint = invPose * p; + n = volume.pose.rotation() * volume.getNormalVoxel(voxelPoint); + } + normals(position[0], position[1]) = toPtype(n); + }; + points.forEach(HashPushNormals); + } +} + +int ColoredHashTSDFVolumeCPU::getVisibleBlocks(int currFrameId, int frameThreshold) const +{ + int numVisibleBlocks = 0; + //! TODO: Iterate over map parallely? + for (const auto& keyvalue : volumeUnits) + { + const VolumeUnit& volumeUnit = keyvalue.second; + if (volumeUnit.lastVisibleIndex > (currFrameId - frameThreshold)) + numVisibleBlocks++; + } + return numVisibleBlocks; +} + +Ptr makeColoredHashTSDFVolume(const VolumeParams& _params) +{ + return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + _params.depthTruncThreshold, _params.unitResolution); +} + +Ptr makeColoredHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float truncateThreshold, int volumeUnitResolution) +{ + return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, + volumeUnitResolution); +} + +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/colored_hash_tsdf.hpp b/modules/rgbd/src/colored_hash_tsdf.hpp new file mode 100644 index 00000000000..f56238fb9ce --- /dev/null +++ b/modules/rgbd/src/colored_hash_tsdf.hpp @@ -0,0 +1,51 @@ +// 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_COLORED_HASH_TSDF_H__ +#define __OPENCV_COLORED_HASH_TSDF_H__ + +#include +#include +#include + +#include "tsdf_functions.hpp" + +namespace cv +{ +namespace kinfu +{ +class ColoredHashTSDFVolume : public Volume +{ + public: + // dimension in voxels, size in meters + //! Use fixed volume cuboid + ColoredHashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float _truncateThreshold, int _volumeUnitRes, + bool zFirstMemOrder = true); + + virtual ~ColoredHashTSDFVolume() = default; + + virtual int getVisibleBlocks(int currFrameId, int frameThreshold) const = 0; + virtual size_t getTotalVolumeUnits() const = 0; + + public: + int maxWeight; + float truncDist; + float truncateThreshold; + int volumeUnitResolution; + int volumeUnitDegree; + float volumeUnitSize; + bool zFirstMemOrder; + Vec4i volStrides; +}; + +Ptr makeColoredHashTSDFVolume(const VolumeParams& _volumeParams); +Ptr makeColoredHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16); + +} // namespace kinfu +} // namespace cv + + +#endif diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 449acd3ccf2..c1be8940fac 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -131,6 +131,18 @@ Ptr Params::coloredTSDFParams(bool isCoarse) return p; } +Ptr Params::coloredHashTSDFParams(bool isCoarse) +{ + Ptr p; + if (isCoarse) + p = coarseParams(); + else + p = defaultParams(); + p->volumeType = VolumeType::COLOREDHASHTSDF; + p->truncateThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); + return p; +} + // MatType should be Mat or UMat template< typename MatType> class ColoredKinFuImpl : public ColoredKinFu diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index 14dbdc7aef1..f0404df6c2f 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -8,6 +8,7 @@ #include "fast_icp.hpp" #include "tsdf.hpp" #include "hash_tsdf.hpp" +#include "colored_hash_tsdf.hpp" #include "kinfu_frame.hpp" namespace cv { @@ -119,6 +120,18 @@ Ptr Params::coloredTSDFParams(bool isCoarse) return p; } +Ptr Params::coloredHashTSDFParams(bool isCoarse) +{ + Ptr p; + if (isCoarse) + p = coarseParams(); + else + p = defaultParams(); + p->volumeType = VolumeType::COLOREDHASHTSDF; + p->truncateThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); + return p; +} + // MatType should be Mat or UMat template< typename MatType> class KinFuImpl : public KinFu diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index c05238f2602..248b1f843fe 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -9,6 +9,7 @@ #include "tsdf.hpp" #include "hash_tsdf.hpp" #include "colored_tsdf.hpp" +#include "colored_hash_tsdf.hpp" namespace cv { @@ -47,6 +48,14 @@ Ptr VolumeParams::defaultParams(VolumeType _volumeType) params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters return makePtr(params); } + else if (params.type == VolumeType::COLOREDHASHTSDF) + { + params.unitResolution = 16; + params.voxelSize = volumeSize / 512.f; + params.depthTruncThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); + params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters + return makePtr(params); + } CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } @@ -76,6 +85,12 @@ Ptr VolumeParams::coarseParams(VolumeType _volumeType) params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters return params; } + else if (params->type == VolumeType::COLOREDHASHTSDF) + { + params->voxelSize = volumeSize / 128.f; + params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters + return params; + } CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } @@ -87,6 +102,8 @@ Ptr makeVolume(const VolumeParams& _volumeParams) return kinfu::makeHashTSDFVolume(_volumeParams); else if(_volumeParams.type == VolumeType::COLOREDTSDF) return kinfu::makeColoredTSDFVolume(_volumeParams); + else if(_volumeParams.type == VolumeType::COLOREDHASHTSDF) + return kinfu::makeColoredHashTSDFVolume(_volumeParams); CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } @@ -107,6 +124,11 @@ Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, { return makeColoredTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _presolution); } + else if (_volumeType == VolumeType::COLOREDHASHTSDF) + { + return makeColoredHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); + } + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index f5a88180923..fdf646ae872 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -518,5 +518,225 @@ TEST(HashTSDF_CPU, valid_points) cv::ocl::setUseOpenCL(true); } #endif + +// Tests ColoredHashTSDF +Ptr createColoredHashTSDFParams() { + auto params = kinfu::Params::hashTSDFParams(true); + + params->volumeType = kinfu::VolumeType::COLOREDHASHTSDF; + + return params; +} + +void colored_normal_test(bool isRaycast, bool isFetchPointsNormals, bool isFetchNormals) +{ + auto normalCheck = [](Vec4f& vector, const int*) + { + if (!cvIsNaN(vector[0])) + { + float length = vector[0] * vector[0] + + vector[1] * vector[1] + + vector[2] * vector[2]; + ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1"; + } + }; + + // Create parameters for ColoredHashTSDF + auto params = createColoredHashTSDFParams(); + + Ptr volume = kinfu::makeVolume(params->volumeType, params->voxelSize, params->volumePose.matrix, + params->raycast_step_factor, params->tsdf_trunc_dist, params->tsdf_max_weight, + params->truncateThreshold, params->volumeDims); + + Ptr scene = Scene::create(params->frameSize, params->intr, params->depthFactor, false); + std::vector poses = scene->getPoses(); + + Mat depth = scene->depth(poses[0]); + UMat _points, _normals, _tmpnormals; + UMat _newPoints, _newNormals; + Mat points, normals; + AccessFlag af = ACCESS_READ; + + volume->integrate(depth, params->depthFactor, poses[0].matrix, params->intr); + + if (isRaycast) + { + volume->raycast(poses[0].matrix, params->intr, params->frameSize, _points, _normals); + } + if (isFetchPointsNormals) + { + volume->fetchPointsNormals(_points, _normals); + } + if (isFetchNormals) + { + volume->fetchPointsNormals(_points, _tmpnormals); + + if (!_points.empty()) + { + volume->fetchNormals(_points, _normals); + } + else + { + _normals.create(_points.size(), CV_32FC4); + _normals.setTo(Scalar::all(0)); + } + } + + if (!_normals.empty() && !_points.empty()) + { + normals = _normals.getMat(af); + points = _points.getMat(af); + + if (normals.type() != CV_32FC4) + { + normals.convertTo(normals, CV_32FC4); + } + if (points.type() != CV_32FC4) + { + points.convertTo(points, CV_32FC4); + } + + if (parallelCheck && !normals.empty()) + normals.forEach(normalCheck); + else if (!normals.empty()) + normalsCheck(normals); + + if (isRaycast && display) + displayImage(depth, points, normals, params->depthFactor, params->lightPose); + } + + if (isRaycast && !poses.empty() && poses.size() > 17) + { + volume->raycast(poses[17].matrix, params->intr, params->frameSize, _newPoints, _newNormals); + + if (!_newNormals.empty() && !_newPoints.empty()) + { + normals = _newNormals.getMat(af); + points = _newPoints.getMat(af); + + if (!normals.empty()) + normalsCheck(normals); + + if (parallelCheck && !normals.empty()) + normals.forEach(normalCheck); + else if (!normals.empty()) + normalsCheck(normals); + + if (display) + displayImage(depth, points, normals, params->depthFactor, params->lightPose); + } + } + + if (!points.empty()) points.release(); + if (!normals.empty()) normals.release(); +} + +void colored_valid_points() +{ + auto params = createColoredHashTSDFParams(); + + Ptr volume = kinfu::makeVolume(params->volumeType, params->voxelSize, params->volumePose.matrix, + params->raycast_step_factor, params->tsdf_trunc_dist, params->tsdf_max_weight, + params->truncateThreshold, params->volumeDims); + + Ptr scene = Scene::create(params->frameSize, params->intr, params->depthFactor, true); + std::vector poses = scene->getPoses(); + + Mat depth = scene->depth(poses[0]); + Mat depthInMeters = depth / params->depthFactor; + + try { + volume->integrate(depth, params->depthFactor, poses[0].matrix, params->intr); + } catch (const std::exception&) {} + + UMat _points, _normals; + try { + volume->raycast(poses[0].matrix, params->intr, params->frameSize, _points, _normals); + } catch (const std::exception&) {} + + auto altParams1 = createColoredHashTSDFParams(); + altParams1->volumeDims = Vec3i(256, 256, 256); + altParams1->voxelSize = 0.01f; + + Ptr altVolume1 = kinfu::makeVolume(altParams1->volumeType, altParams1->voxelSize, altParams1->volumePose.matrix, + altParams1->raycast_step_factor, altParams1->tsdf_trunc_dist, altParams1->tsdf_max_weight, + altParams1->truncateThreshold, altParams1->volumeDims); + + altVolume1->integrate(depth, altParams1->depthFactor, poses[0].matrix, altParams1->intr); + UMat altPoints1, altNormals1; + altVolume1->raycast(poses[0].matrix, altParams1->intr, altParams1->frameSize, altPoints1, altNormals1); + + auto altParams2 = createColoredHashTSDFParams(); + altParams2->volumePose = Affine3f::Identity(); + altParams2->voxelSize = 0.005f; + + Ptr altVolume2 = kinfu::makeVolume(altParams2->volumeType, altParams2->voxelSize, altParams2->volumePose.matrix, + altParams2->raycast_step_factor, altParams2->tsdf_trunc_dist, altParams2->tsdf_max_weight, + altParams2->truncateThreshold, altParams2->volumeDims); + + altVolume2->integrate(depth, altParams2->depthFactor, poses[0].matrix, altParams2->intr); + UMat altPoints2, altNormals2; + altVolume2->raycast(poses[0].matrix, altParams2->intr, altParams2->frameSize, altPoints2, altNormals2); + + auto altParams3 = createColoredHashTSDFParams(); + altParams3->volumeDims = Vec3i(192, 192, 192); + altParams3->voxelSize = 0.0156f; + + Ptr altVolume3 = kinfu::makeVolume(altParams3->volumeType, altParams3->voxelSize, altParams3->volumePose.matrix, + altParams3->raycast_step_factor, altParams3->tsdf_trunc_dist, altParams3->tsdf_max_weight, + altParams3->truncateThreshold, altParams3->volumeDims); + + for (int i = 0; i < std::min(5, (int)poses.size()); i++) { + Mat frameDepth = scene->depth(poses[i]); + if (!frameDepth.empty()) { + altVolume3->integrate(frameDepth, altParams3->depthFactor, poses[i].matrix, altParams3->intr); + } + } + + UMat altPoints3, altNormals3; + altVolume3->raycast(poses[0].matrix, altParams3->intr, altParams3->frameSize, altPoints3, altNormals3); + + if (!altPoints3.empty()) { + Mat points3Mat = altPoints3.getMat(ACCESS_READ); + if (points3Mat.type() != CV_32FC4) points3Mat.convertTo(points3Mat, CV_32FC4); + patchNaNs(points3Mat); + } + + for (int i = 0; i < 3; i++) { + Mat testDepth = scene->depth(poses[i]); + double minD, maxD; + cv::minMaxLoc(testDepth, &minD, &maxD); + } +} + +// Tests +TEST(ColoredHashTSDF_CPU, raycast_normals) +{ + cv::ocl::setUseOpenCL(false); + colored_normal_test(true, false, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(ColoredHashTSDF_CPU, fetch_points_normals) +{ + cv::ocl::setUseOpenCL(false); + colored_normal_test(false, true, false); + cv::ocl::setUseOpenCL(true); +} + +TEST(ColoredHashTSDF_CPU, fetch_normals) +{ + cv::ocl::setUseOpenCL(false); + colored_normal_test(false, false, true); + cv::ocl::setUseOpenCL(true); +} + +TEST(ColoredHashTSDF_CPU, valid_points) +{ + cv::ocl::setUseOpenCL(false); + colored_valid_points(); + cv::ocl::setUseOpenCL(true); +} + } } // namespace