From 3fa8a3c853125e3c2b91ba5b13eb73ff4b997738 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Sat, 23 May 2020 23:51:49 -0400 Subject: [PATCH 01/36] - Add HashTSDF class - Implement Integrate function (untested) --- modules/rgbd/src/hash_tsdf.cpp | 271 +++++++++++++++++++++++++++++++++ modules/rgbd/src/hash_tsdf.hpp | 54 +++++++ modules/rgbd/src/kinfu.cpp | 1 + modules/rgbd/src/tsdf.cpp | 24 +-- modules/rgbd/src/tsdf.hpp | 5 +- modules/rgbd/src/utils.hpp | 3 + 6 files changed, 345 insertions(+), 13 deletions(-) create mode 100644 modules/rgbd/src/hash_tsdf.cpp create mode 100644 modules/rgbd/src/hash_tsdf.hpp diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp new file mode 100644 index 00000000000..88706fdc187 --- /dev/null +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -0,0 +1,271 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory + +#include "opencv2/core/cvdef.h" +#include "opencv2/core/cvstd_wrapper.hpp" +#include "opencv2/core/hal/interface.h" +#include "opencv2/core/utility.hpp" +#include "opencv2/rgbd/depth.hpp" +#include "precomp.hpp" +#include "tsdf.hpp" +#include "hash_tsdf.hpp" +#include "opencl_kernels_rgbd.hpp" +#include +#include +#include +#include "depth_to_3d.hpp" +#include "utils.hpp" + +namespace cv { + +namespace kinfu { + +// TODO: Optimization possible: +// * volumeType can be FP16 +// * weight can be int16 +typedef float volumeType; +struct Voxel +{ + volumeType v; + int weight; +}; +typedef Vec VecT; + +struct VolumeUnit +{ + explicit VolumeUnit() : p_volume(nullptr) {}; + ~VolumeUnit() = default; + + cv::Ptr p_volume; + cv::Vec3i index; +}; + + +//! Spatial hashing +struct tsdf_hash +{ + size_t operator()(const cv::Vec3i & x) const noexcept + { + size_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + for (uint16_t i = 0; i < 3; i++) { + seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + } + return seed; + } +}; + +typedef std::unordered_set VolumeUnitIndexSet; +typedef std::unordered_map VolumeUnitMap; + +class HashTSDFVolumeCPU : public HashTSDFVolume +{ +public: + // dimension in voxels, size in meters + HashTSDFVolumeCPU(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, + float _truncDist, int _maxWeight, + float _raycastStepFactor, bool zFirstMemOrder = true); + + virtual void integrate(InputArray _depth, float depthFactor, + cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; + /* virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, */ + /* cv::OutputArray points, cv::OutputArray normals) const override; */ + + /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; */ + /* virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; */ + + virtual void reset() override; + + cv::Vec3i findVoxelUnitIndex(cv::Point3f p) const; + /* volumeType interpolateVoxel(cv::Point3f p) const; */ + /* Point3f getNormalVoxel(cv::Point3f p) const; */ +//! TODO: Make this private +public: + //! Hashtable of individual smaller volume units + VolumeUnitMap volume_units_; +}; + +HashTSDFVolume::HashTSDFVolume(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, float _truncDist, + int _maxWeight, float _raycastStepFactor, bool _zFirstMemOrder) + : voxelSize(_voxelSize) + , voxelSizeInv(1.0f / _voxelSize) + , pose(_pose) + , maxWeight(_maxWeight) + , raycastStepFactor(_raycastStepFactor) + , volumeUnitResolution(_volume_unit_res) + , volumeUnitSize(voxelSize * volumeUnitResolution) + , zFirstMemOrder(_zFirstMemOrder) +{ + truncDist = std::max(_truncDist, 2.1f * voxelSize); +} + +HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, + float _truncDist, int _maxWeight, + float _raycastStepFactor, bool _zFirstMemOrder) + : HashTSDFVolume(_voxelSize, _volume_unit_res, _pose, _truncDist, _maxWeight, _raycastStepFactor, _zFirstMemOrder) +{ +} + +// zero volume, leave rest params the same +void HashTSDFVolumeCPU::reset() +{ + CV_TRACE_FUNCTION(); + volume_units_.clear(); +} + +struct AccessedVolumeUnitsInvoker : ParallelLoopBody +{ + AccessedVolumeUnitsInvoker(HashTSDFVolumeCPU& _volume, VolumeUnitIndexSet& _accessVolUnits, + const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose, + float _depthFactor, int _depthStride = 4) : + ParallelLoopBody(), + volume(_volume), + accessVolUnits(_accessVolUnits), + depth(_depth), + reproj(intrinsics.makeReprojector()), + cam2vol(_volume.pose.inv() * cameraPose), + dfac(1.0f/_depthFactor), + depthStride(_depthStride) + { + } + + virtual void operator() (const Range& range) const override + { + 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]*dfac; + if (z <= 0) + continue; + + Point3f camPoint = reproj(Point3f((float)x, (float)y, z)); + Point3f volPoint = cam2vol * camPoint; + + //! Find accessed TSDF volume unit for valid 3D vertex + cv::Vec3i lower_bound = volume.findVoxelUnitIndex( + volPoint - cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); + cv::Vec3i upper_bound = volume.findVoxelUnitIndex( + volPoint + cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); + + //! TODO(Akash): Optimize this using differential analyzer algorithm + for(int i = lower_bound[0]; i < upper_bound[0]; i++) + for(int j = lower_bound[1]; j < upper_bound[1]; j++) + for(int k = lower_bound[2]; k < lower_bound[2]; k++) + { + const cv::Vec3i tsdf_idx = cv::Vec3i(i, j, k); + //! If the index has not already been accessed + if(accessVolUnits.find(tsdf_idx) == accessVolUnits.end()) + { + accessVolUnits.insert(tsdf_idx); + //! Adds entry to unordered_map + //! and allocate memory for the volume unit + VolumeUnit volume_unit = volume.volume_units_[tsdf_idx]; + if(!volume_unit.p_volume) + { + cv::Point3i volumeDims(volume.volumeUnitResolution, + volume.volumeUnitResolution, + volume.volumeUnitResolution); + cv::Point3f volume_unit_origin = cv::Point3f(tsdf_idx); + volume_unit.p_volume = makeTSDFVolume(volumeDims, + volume.volumeUnitSize, + volume.pose, + volume.truncDist, + volume.maxWeight, + volume.raycastStepFactor, volume_unit_origin); + } + } + } + } + } + + } + + HashTSDFVolumeCPU& volume; + VolumeUnitIndexSet& accessVolUnits; + const Depth& depth; + const Intr::Reprojector reproj; + const cv::Affine3f cam2vol; + const float dfac; + const int depthStride; +}; + +struct IntegrateSubvolumeInvoker : ParallelLoopBody +{ + IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, std::vector _accessVolUnitVec, + const Depth& _depth, Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : + ParallelLoopBody(), + volume(_volume), + accessVolUnitsVec(_accessVolUnitVec), + depth(_depth), + depthFactor(_depthFactor), + cameraPose(_cameraPose), + intrinsics(_intrinsics) + { + } + + virtual void operator() (const Range& range) const override + { + for (int i = range.start; i < range.end; i++) + { + cv::Vec3i tsdf_idx = accessVolUnitsVec[i]; + + VolumeUnitMap::iterator accessedVolUnit = volume.volume_units_.find(tsdf_idx); + + //! The volume unit should already be added into the Volume from the allocator + assert(accessedVolUnit != volume.volume_units_.end()); + accessedVolUnit->second.p_volume->integrate(depth, depthFactor, cameraPose, intrinsics); + } + } + + HashTSDFVolumeCPU& volume; + std::vector accessVolUnitsVec; + const Depth& depth; + float depthFactor; + cv::Affine3f cameraPose; + Intr intrinsics; +}; + + +// use depth instead of distance (optimization) +void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, Intr intrinsics) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + Depth depth = _depth.getMat(); + VolumeUnitIndexSet accessVolUnits; + + //TODO(Akash): Consider reusing pyrPoints and transform the points + AccessedVolumeUnitsInvoker allocate_i(*this, accessVolUnits, depth, intrinsics, cameraPose, depthFactor); + Range range(0, depth.rows); + parallel_for_(range, allocate_i); + + std::vector accessVolUnitsVec; + accessVolUnitsVec.assign(accessVolUnits.begin(), accessVolUnits.end()); + + IntegrateSubvolumeInvoker integrate_i(*this, accessVolUnitsVec, depth, intrinsics, cameraPose, depthFactor); + Range accessed_units_range(0, accessVolUnitsVec.size()); + parallel_for_(accessed_units_range, integrate_i); +} + +cv::Vec3i HashTSDFVolumeCPU::findVoxelUnitIndex(cv::Point3f p) const +{ + return cv::Vec3i(cvFloor(p.x / volumeUnitSize), + cvFloor(p.y / volumeUnitSize), + cvFloor(p.z / volumeUnitSize)); +} + +cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, + float _raycastStepFactor, int volumeUnitResolution) +{ + return cv::makePtr(_voxelSize, volumeUnitResolution, _pose, _truncDist, _maxWeight, _raycastStepFactor); +} + +} // namespace kinfu +} // namespace cv + diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp new file mode 100644 index 00000000000..1b5e20cb0f0 --- /dev/null +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -0,0 +1,54 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory + +#ifndef __OPENCV_HASH_TSDF_H__ +#define __OPENCV_HASH_TSDF_H__ + +#include "opencv2/core/affine.hpp" +#include "kinfu_frame.hpp" + +namespace cv { +namespace kinfu { + + +class HashTSDFVolume +{ +public: + // dimension in voxels, size in meters + //! Use fixed volume cuboid + explicit HashTSDFVolume(float _voxelSize, int _voxel_unit_res, cv::Affine3f _pose, + float _truncDist, int _maxWeight, + float _raycastStepFactor, bool zFirstMemOrder = true); + + virtual ~HashTSDFVolume() = default; + virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) = 0; + /* virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, */ + /* cv::OutputArray points, cv::OutputArray normals) const = 0; */ + + /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; */ + /* virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; */ + + virtual void reset() = 0; + + +public: + float voxelSize; + float voxelSizeInv; + cv::Affine3f pose; + int maxWeight; + float raycastStepFactor; + float truncDist; + uint16_t volumeUnitResolution; + uint16_t volumeUnitSize; + bool zFirstMemOrder; + +}; +cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, + float _raycastStepFactor, int volumeUnitResolution = 16); +} // namespace kinfu +} // namespace cv +#endif + diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index 61644385f1a..2a1b02368a9 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -7,6 +7,7 @@ #include "precomp.hpp" #include "fast_icp.hpp" #include "tsdf.hpp" +#include "hash_tsdf.hpp" #include "kinfu_frame.hpp" namespace cv { diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index c60a7ccc915..f0dd937c601 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -29,7 +29,7 @@ class TSDFVolumeCPU : public TSDFVolume public: // dimension in voxels, size in meters TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder = true); + float _raycastStepFactor, Point3f _origin, bool zFirstMemOrder = true); virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, @@ -56,10 +56,11 @@ class TSDFVolumeCPU : public TSDFVolume TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder) : + float _raycastStepFactor, Point3f _origin, bool zFirstMemOrder) : voxelSize(_voxelSize), voxelSizeInv(1.f/_voxelSize), volResolution(_res), + origin(_origin), maxWeight(_maxWeight), pose(_pose), raycastStepFactor(_raycastStepFactor) @@ -104,8 +105,8 @@ TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _tr // dimension in voxels, size in meters TSDFVolumeCPU::TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder) : - TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, zFirstMemOrder) + float _raycastStepFactor, Point3f _origin, bool zFirstMemOrder) : + TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, _origin, zFirstMemOrder) { volume = Mat(1, volResolution.x * volResolution.y * volResolution.z, rawType()); @@ -367,8 +368,9 @@ struct IntegrateInvoker : ParallelLoopBody Voxel* volDataY = volDataX+y*volume.volDims[1]; // optimization of camSpace transformation (vector addition instead of matmul at each z) Point3f basePt = vol2cam*(Point3f(x, y, 0)*volume.voxelSize); - Point3f camSpacePt = basePt; + Point3f camSpacePt = basePt + volume.origin; // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; + // zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize Point3f zStep = Point3f(vol2cam.matrix(0, 2), vol2cam.matrix(1, 2), vol2cam.matrix(2, 2))*volume.voxelSize; @@ -1155,7 +1157,7 @@ class TSDFVolumeGPU : public TSDFVolume public: // dimension in voxels, size in meters TSDFVolumeGPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor); + float _raycastStepFactor, Point3f _origin); virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, @@ -1175,8 +1177,8 @@ class TSDFVolumeGPU : public TSDFVolume TSDFVolumeGPU::TSDFVolumeGPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor) : - TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, false) + float _raycastStepFactor, Point3f _origin) : + TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, _origin, false) { volume = UMat(1, volResolution.x * volResolution.y * volResolution.z, CV_32FC2); @@ -1473,13 +1475,13 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) #endif cv::Ptr makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor) + float _raycastStepFactor, Point3f _origin) { #ifdef HAVE_OPENCL if(cv::ocl::useOpenCL()) - return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor); + return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, _origin); #endif - return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor); + return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, _origin); } } // namespace kinfu diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 347935ddf29..23ce191b772 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -18,7 +18,7 @@ class TSDFVolume public: // dimension in voxels, size in meters TSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder = true); + float _raycastStepFactor, cv::Point3f _origin, bool zFirstMemOrder = true); virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) = 0; virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, @@ -34,6 +34,7 @@ class TSDFVolume float voxelSize; float voxelSizeInv; Point3i volResolution; + Point3f origin; int maxWeight; cv::Affine3f pose; float raycastStepFactor; @@ -45,7 +46,7 @@ class TSDFVolume }; cv::Ptr makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor); + float _raycastStepFactor, cv::Point3f origin = Point3f(0, 0, 0)); } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/utils.hpp b/modules/rgbd/src/utils.hpp index b1b3028677f..764134b2bb1 100644 --- a/modules/rgbd/src/utils.hpp +++ b/modules/rgbd/src/utils.hpp @@ -9,6 +9,7 @@ #ifndef __OPENCV_RGBD_UTILS_HPP__ #define __OPENCV_RGBD_UTILS_HPP__ +#include "opencv2/core/matx.hpp" namespace cv { namespace rgbd @@ -125,6 +126,8 @@ struct Intr inline Reprojector makeReprojector() const { return Reprojector(*this); } inline Projector makeProjector() const { return Projector(*this); } + inline cv::Matx33f getMat() const { return Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); } + float fx, fy, cx, cy; }; From 98ce3d0f3108adb58a31eb8b3319b23eec5c597b Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Wed, 3 Jun 2020 15:20:03 -0400 Subject: [PATCH 02/36] Integration seems to be working, raycasting does not --- modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 6 +- modules/rgbd/samples/kinfu_demo.cpp | 8 +- modules/rgbd/src/hash_tsdf.cpp | 439 ++++++++++++++++---- modules/rgbd/src/hash_tsdf.hpp | 75 +++- modules/rgbd/src/kinfu.cpp | 61 +-- modules/rgbd/src/tsdf.cpp | 210 ++++------ modules/rgbd/src/tsdf.hpp | 71 +++- modules/rgbd/src/utils.hpp | 1 + 8 files changed, 616 insertions(+), 255 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index a2c55bd150f..9589a00bf61 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -202,7 +202,7 @@ class CV_EXPORTS_W KinFu @param points vector of points which are 4-float vectors @param normals vector of normals which are 4-float vectors */ - CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0; + /* CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0; */ /** @brief Gets points of current 3d mesh @@ -210,13 +210,13 @@ class CV_EXPORTS_W KinFu @param points vector of points which are 4-float vectors */ - CV_WRAP virtual void getPoints(OutputArray points) const = 0; + /* CV_WRAP virtual void getPoints(OutputArray points) const = 0; */ /** @brief Calculates normals for given points @param points input vector of points which are 4-float vectors @param normals output vector of corresponding normals which are 4-float vectors */ - CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0; + /* CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0; */ /** @brief Resets the algorithm diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index ba151a6c69e..e97a356ecad 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -386,7 +386,7 @@ int main(int argc, char **argv) ds->updateParams(*params); // Enables OpenCL explicitly (by default can be switched-off) - cv::setUseOptimized(true); + cv::setUseOptimized(false); // Scene-specific params should be tuned for each scene individually //float cubeSize = 1.f; @@ -420,7 +420,7 @@ int main(int argc, char **argv) if(pause) { // doesn't happen in idle mode - kf->getCloud(points, normals); + /* kf->getCloud(points, normals); */ if(!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); @@ -457,7 +457,7 @@ int main(int argc, char **argv) if(!kf->update(frame)) { - kf->reset(); + /* kf->reset(); */ std::cout << "reset" << std::endl; } #ifdef HAVE_OPENCV_VIZ @@ -465,7 +465,7 @@ int main(int argc, char **argv) { if(coarse) { - kf->getCloud(points, normals); + /* kf->getCloud(points, normals); */ if(!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 88706fdc187..b25d8aed5d4 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -4,18 +4,22 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +#include "opencv2/core.hpp" +#include "opencv2/core/affine.hpp" +#include "opencv2/core/base.hpp" #include "opencv2/core/cvdef.h" #include "opencv2/core/cvstd_wrapper.hpp" +#include "opencv2/core/fast_math.hpp" #include "opencv2/core/hal/interface.h" +#include "opencv2/core/types.hpp" #include "opencv2/core/utility.hpp" +#include "opencv2/core/utils/trace.hpp" #include "opencv2/rgbd/depth.hpp" #include "precomp.hpp" -#include "tsdf.hpp" #include "hash_tsdf.hpp" #include "opencl_kernels_rgbd.hpp" #include -#include -#include +#include #include "depth_to_3d.hpp" #include "utils.hpp" @@ -23,70 +27,6 @@ namespace cv { namespace kinfu { -// TODO: Optimization possible: -// * volumeType can be FP16 -// * weight can be int16 -typedef float volumeType; -struct Voxel -{ - volumeType v; - int weight; -}; -typedef Vec VecT; - -struct VolumeUnit -{ - explicit VolumeUnit() : p_volume(nullptr) {}; - ~VolumeUnit() = default; - - cv::Ptr p_volume; - cv::Vec3i index; -}; - - -//! Spatial hashing -struct tsdf_hash -{ - size_t operator()(const cv::Vec3i & x) const noexcept - { - size_t seed = 0; - constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; - for (uint16_t i = 0; i < 3; i++) { - seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); - } - return seed; - } -}; - -typedef std::unordered_set VolumeUnitIndexSet; -typedef std::unordered_map VolumeUnitMap; - -class HashTSDFVolumeCPU : public HashTSDFVolume -{ -public: - // dimension in voxels, size in meters - HashTSDFVolumeCPU(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, - float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder = true); - - virtual void integrate(InputArray _depth, float depthFactor, - cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; - /* virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, */ - /* cv::OutputArray points, cv::OutputArray normals) const override; */ - - /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; */ - /* virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; */ - - virtual void reset() override; - - cv::Vec3i findVoxelUnitIndex(cv::Point3f p) const; - /* volumeType interpolateVoxel(cv::Point3f p) const; */ - /* Point3f getNormalVoxel(cv::Point3f p) const; */ -//! TODO: Make this private -public: - //! Hashtable of individual smaller volume units - VolumeUnitMap volume_units_; -}; HashTSDFVolume::HashTSDFVolume(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, float _truncDist, int _maxWeight, float _raycastStepFactor, bool _zFirstMemOrder) @@ -146,38 +86,45 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody Point3f camPoint = reproj(Point3f((float)x, (float)y, z)); Point3f volPoint = cam2vol * camPoint; + /* std::cout << "volPoint" << volPoint << "\n"; */ + //! Find accessed TSDF volume unit for valid 3D vertex - cv::Vec3i lower_bound = volume.findVoxelUnitIndex( + cv::Vec3i lower_bound = volume.volumeToVolumeUnitIdx( volPoint - cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); - cv::Vec3i upper_bound = volume.findVoxelUnitIndex( + cv::Vec3i upper_bound = volume.volumeToVolumeUnitIdx( volPoint + cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); //! TODO(Akash): Optimize this using differential analyzer algorithm - for(int i = lower_bound[0]; i < upper_bound[0]; i++) - for(int j = lower_bound[1]; j < upper_bound[1]; j++) - for(int k = lower_bound[2]; k < lower_bound[2]; k++) + for(int i = lower_bound[0]; i <= upper_bound[0]; i++) + for(int j = lower_bound[1]; j <= upper_bound[1]; j++) + for(int k = lower_bound[2]; k <= lower_bound[2]; k++) { const cv::Vec3i tsdf_idx = cv::Vec3i(i, j, k); - //! If the index has not already been accessed - if(accessVolUnits.find(tsdf_idx) == accessVolUnits.end()) + //! If the index does not exist + AutoLock al(mutex); + if(!accessVolUnits.count(tsdf_idx)) { accessVolUnits.insert(tsdf_idx); + /* std::cout << "CamPoint: " << camPoint << " volPoint: " << volPoint << "\n"; */ + /* std::cout << "Inserted tsdf_idx: (" << tsdf_idx[0] << ", " << tsdf_idx[1] << ", " << tsdf_idx[2] << ")\n"; */ //! Adds entry to unordered_map //! and allocate memory for the volume unit - VolumeUnit volume_unit = volume.volume_units_[tsdf_idx]; - if(!volume_unit.p_volume) - { - cv::Point3i volumeDims(volume.volumeUnitResolution, + VolumeUnit volumeUnit; + /* std::cout << "Allocated volumeUnit in map" << std::endl; */ + cv::Point3i volumeDims(volume.volumeUnitResolution, volume.volumeUnitResolution, volume.volumeUnitResolution); - cv::Point3f volume_unit_origin = cv::Point3f(tsdf_idx); - volume_unit.p_volume = makeTSDFVolume(volumeDims, - volume.volumeUnitSize, - volume.pose, - volume.truncDist, - volume.maxWeight, - volume.raycastStepFactor, volume_unit_origin); - } + //! Translate the origin of the subvolume to the correct position in volume coordinate frame + cv::Affine3f subvolumePose = volume.pose.translate(volume.volumeUnitIdxToVolume(-tsdf_idx)); + volumeUnit.p_volume = cv::makePtr(volumeDims, + volume.volumeUnitSize, + subvolumePose, + volume.truncDist, + volume.maxWeight, + volume.raycastStepFactor); + volumeUnit.index = tsdf_idx; + + volume.volume_units_[tsdf_idx] = volumeUnit; } } } @@ -192,6 +139,7 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody const cv::Affine3f cam2vol; const float dfac; const int depthStride; + mutable Mutex mutex; }; struct IntegrateSubvolumeInvoker : ParallelLoopBody @@ -215,10 +163,12 @@ struct IntegrateSubvolumeInvoker : ParallelLoopBody cv::Vec3i tsdf_idx = accessVolUnitsVec[i]; VolumeUnitMap::iterator accessedVolUnit = volume.volume_units_.find(tsdf_idx); + assert(accessedVolUnit != volume.volume_units_.end()); + VolumeUnit volumeUnit = accessedVolUnit->second; + /* std::cout << "Integrating unit: " << accessedVolUnit->first << std::endl; */ //! The volume unit should already be added into the Volume from the allocator - assert(accessedVolUnit != volume.volume_units_.end()); - accessedVolUnit->second.p_volume->integrate(depth, depthFactor, cameraPose, intrinsics); + volumeUnit.p_volume->integrate(depth, depthFactor, cameraPose, intrinsics); } } @@ -231,7 +181,6 @@ struct IntegrateSubvolumeInvoker : ParallelLoopBody }; -// use depth instead of distance (optimization) void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, Intr intrinsics) { CV_TRACE_FUNCTION(); @@ -247,19 +196,327 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affi std::vector accessVolUnitsVec; accessVolUnitsVec.assign(accessVolUnits.begin(), accessVolUnits.end()); - + std::cout << "Number of active subvolumes: " << accessVolUnitsVec.size() << "\n"; IntegrateSubvolumeInvoker integrate_i(*this, accessVolUnitsVec, depth, intrinsics, cameraPose, depthFactor); Range accessed_units_range(0, accessVolUnitsVec.size()); parallel_for_(accessed_units_range, integrate_i); + std::cout << "Integration complete \n"; } -cv::Vec3i HashTSDFVolumeCPU::findVoxelUnitIndex(cv::Point3f p) const +cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx(cv::Point3f p) const { return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), cvFloor(p.z / volumeUnitSize)); } +cv::Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const +{ + return cv::Point3f(volumeUnitIdx[0] * volumeUnitSize, + volumeUnitIdx[1] * volumeUnitSize, + volumeUnitIdx[2] * volumeUnitSize); +} + +cv::Point3f HashTSDFVolumeCPU::voxelCoordToVolume(cv::Vec3i voxelIdx) const +{ + return cv::Point3f(voxelIdx[0] * voxelSize, + voxelIdx[1] * voxelSize, + voxelIdx[2] * voxelSize); +} + +cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const +{ + return cv::Vec3i(cvFloor(point.x * voxelSizeInv), + cvFloor(point.y * voxelSizeInv), + cvFloor(point.z * voxelSizeInv)); +} + +inline Voxel HashTSDFVolumeCPU::at(const cv::Vec3i &volumeIdx) const +{ + cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), + cvFloor(volumeIdx[1] / volumeUnitResolution), + cvFloor(volumeIdx[2] / volumeUnitResolution)); + + cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, + volumeUnitIdx[1] * volumeUnitResolution, + volumeUnitIdx[2] * volumeUnitResolution); + /* std::cout << "VolumeUnitIdx: " << volumeUnitIdx << "\n"; */ + /* std::cout << "subvolumeCoords: " << subVolumeCoords << "\n"; */ + VolumeUnitMap::const_iterator it = volume_units_.find(volumeUnitIdx); + if(it == volume_units_.end()) + { + Voxel dummy; + dummy.tsdf = 0.f; dummy.weight = 0; + return dummy; + } + cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.p_volume); + return volumeUnit->at(volUnitLocalIdx); +} + +inline Voxel HashTSDFVolumeCPU::at(const cv::Point3f &point) const +{ + cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); + cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); + cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); + VolumeUnitMap::const_iterator it = volume_units_.find(volumeUnitIdx); + if(it == volume_units_.end()) + { + Voxel dummy; + dummy.tsdf = 0; dummy.weight = 0; + return dummy; + } + cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.p_volume); + /* std::cout << "volumeUnitIdx: " << volumeUnitIdx << "volUnitLocalIdx: " << volUnitLocalIdx << std::endl; */ + /* std::cout << volumeUnit->at(volUnitLocalIdx).tsdf << std::endl; */ + return volumeUnit->at(volUnitLocalIdx); +} + + +inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(cv::Point3f p) const +{ + int ix = cvFloor(p.x); + int iy = cvFloor(p.y); + int iz = cvFloor(p.z); + + float tx = p.x - ix; + float ty = p.y - iy; + float tz = p.z - iz; + + TsdfType vx[8]; + //! This fetches the tsdf value from the correct subvolumes + vx[0] = at(cv::Vec3i(0, 0, 0) + cv::Vec3i(ix, iy, iz)).tsdf; + vx[1] = at(cv::Vec3i(0, 0, 1) + cv::Vec3i(ix, iy, iz)).tsdf; + vx[2] = at(cv::Vec3i(0, 1, 0) + cv::Vec3i(ix, iy, iz)).tsdf; + vx[3] = at(cv::Vec3i(0, 1, 1) + cv::Vec3i(ix, iy, iz)).tsdf; + vx[4] = at(cv::Vec3i(1, 0, 0) + cv::Vec3i(ix, iy, iz)).tsdf; + vx[5] = at(cv::Vec3i(1, 0, 1) + cv::Vec3i(ix, iy, iz)).tsdf; + vx[6] = at(cv::Vec3i(0, 1, 0) + cv::Vec3i(ix, iy, iz)).tsdf; + vx[7] = at(cv::Vec3i(1, 1, 1) + cv::Vec3i(ix, iy, iz)).tsdf; + + /* std::cout << "tsdf 7th: " << vx[7] << "\n"; */ + TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); + + TsdfType v0 = v00 + ty*(v01 - v00); + TsdfType v1 = v10 + ty*(v11 - v10); + + return v0 + tx*(v1 - v0); +} + +inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const +{ + + Vec3f pointVec(point); + Vec3f normal = Vec3f(0, 0, 0); + + Vec3f pointPrev = point; + Vec3f pointNext = point; + + for(int c = 0; c < 3; c++) + { + pointPrev[c] -= voxelSize*0.5; + pointNext[c] += voxelSize*0.5; + + normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; + /* std::cout << "pointPrev, pointNext: " << at(Point3f(pointPrev)).tsdf << ", " << at(Point3f(pointNext)).tsdf << std::endl; */ + normal[c] *= 0.5; + /* std::cout << "TSDF diff: " << normal[c] << std::endl; */ + + pointPrev[c] = pointVec[c]; + pointNext[c] = pointVec[c]; + } + return normalize(normal); + + /* int ix = cvFloor(p.x); */ + /* int iy = cvFloor(p.y); */ + /* int iz = cvFloor(p.z); */ + + /* float tx = p.x - ix; */ + /* float ty = p.y - iy; */ + /* float tz = p.z - iz; */ + + /* Vec3i coordBase0 = cv::Vec3i(ix, iy, iz); */ + /* Vec3i coordBase1 = cv::Vec3i(ix, iy, iz); */ + /* Vec3f an; */ + /* for(int c = 0; c < 3; c++) */ + /* { */ + /* float& nv = an[c]; */ + + /* TsdfType vx[8]; */ + /* coordBase0[c] -= 1; */ + /* coordBase1[c] += 1; */ + + /* vx[0] = at(cv::Vec3i(0, 0, 0) + coordBase1).tsdf - at(cv::Vec3i(0, 0, 0) + coordBase0).tsdf; */ + /* vx[1] = at(cv::Vec3i(0, 0, 1) + coordBase1).tsdf - at(cv::Vec3i(0, 0, 1) + coordBase0).tsdf; */ + /* vx[2] = at(cv::Vec3i(0, 1, 0) + coordBase1).tsdf - at(cv::Vec3i(0, 1, 0) + coordBase0).tsdf; */ + /* vx[3] = at(cv::Vec3i(0, 1, 1) + coordBase1).tsdf - at(cv::Vec3i(0, 1, 1) + coordBase0).tsdf; */ + /* vx[4] = at(cv::Vec3i(1, 0, 0) + coordBase1).tsdf - at(cv::Vec3i(1, 0, 0) + coordBase0).tsdf; */ + /* vx[5] = at(cv::Vec3i(1, 0, 1) + coordBase1).tsdf - at(cv::Vec3i(1, 0, 1) + coordBase0).tsdf; */ + /* vx[6] = at(cv::Vec3i(1, 1, 0) + coordBase1).tsdf - at(cv::Vec3i(1, 1, 0) + coordBase0).tsdf; */ + /* vx[7] = at(cv::Vec3i(1, 1, 1) + coordBase1).tsdf - at(cv::Vec3i(1, 1, 1) + coordBase0).tsdf; */ + + /* TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); */ + /* TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); */ + /* TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); */ + /* TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); */ + + /* TsdfType v0 = v00 + ty*(v01 - v00); */ + /* TsdfType v1 = v10 + ty*(v11 - v10); */ + + /* nv = v0 + tx*(v1 - v0); */ + /* } */ + + /* return normalize(an); */ +} + + +struct RaycastInvoker : ParallelLoopBody +{ + RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, Intr intrinsics, const HashTSDFVolumeCPU& _volume) : + ParallelLoopBody(), + points(_points), + normals(_normals), + volume(_volume), + tstep(_volume.truncDist * _volume.raycastStepFactor), + cam2vol(volume.pose.inv() * cameraPose), + vol2cam(cameraPose.inv() * volume.pose), + reproj(intrinsics.makeReprojector()) + {} + + virtual void operator() (const Range& range) const override + { + const Point3f cam2volTrans = cam2vol.translation(); + const Matx33f cam2volRot = cam2vol.rotation(); + const Matx33f vol2camRot = vol2cam.rotation(); + + const float blockSize = volume.volumeUnitSize; + + for (int y = range.start; y < range.end; y++) + { + + ptype* ptsRow = points[y]; + ptype* nrmRow = normals[y]; + + for(int x = 0; x < points.cols; x++) + { + Point3f point = nan3, normal = nan3; + + //! Ray origin in the volume coordinate frame + Point3f orig = cam2volTrans; + //! Ray direction in the volume coordinate frame + Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(x, y, 1.f)))); + float tmin = rgbd::Odometry::DEFAULT_MIN_DEPTH()/rayDirV.z; + float tmax = rgbd::Odometry::DEFAULT_MAX_DEPTH()/rayDirV.z; + + /* std::cout << "tmin, tmax :" << tmin << ", " << tmax << "\n"; */ + /* std::cout << "Origin: " << orig << " rayDirection: " << rayDirV << "\n"; */ + float tprev = tmin; + float tcurr = tmin + tstep; + //! Is this a reasonable initialization? + TsdfType prevTsdf = volume.truncDist; + cv::Vec3i prevVolumeUnitIdx = cv::Vec3i(std::numeric_limits::min(), + std::numeric_limits::min(), + std::numeric_limits::min()); + cv::Ptr currVolumeUnit; + while(tcurr < tmax) + { + Point3f currRayPos = orig + tcurr * rayDirV; + cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); + /* std::cout << "tCurr: " << tcurr << "\n"; */ + /* std::cout << "Current Ray cast position: " << currRayPos << "\n"; */ + /* std::cout << "Previous volume unit Index: " << prevVolumeUnitIdx << "\n"; */ + /* std::cout << "Current volume unit Index: " << currVolumeUnitIdx << "\n"; */ + + VolumeUnitMap::const_iterator it; + if(currVolumeUnitIdx != prevVolumeUnitIdx) + it = volume.volume_units_.find(currVolumeUnitIdx); + + TsdfType currTsdf = prevTsdf; + int currWeight = 0; + float stepSize = blockSize; + cv::Vec3i volUnitLocalIdx; + //! Is the subvolume exists in hashtable + if(it != volume.volume_units_.end()) + { + currVolumeUnit = std::dynamic_pointer_cast(it->second.p_volume); + cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); + volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); + + //! Figure out voxel interpolation + Voxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); + currTsdf = currVoxel.tsdf; + currWeight = currVoxel.weight; + stepSize = max(currTsdf * volume.truncDist, tstep); + + } + //! Surface crossing + if(prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) + { + std::cout << "subvolume coords: " << volUnitLocalIdx << "\n"; + std::cout << "current TSDF: " << currTsdf << "\n"; + std::cout << "current weight: " << currWeight << "\n"; + std::cout << "previous TSDF: " << prevTsdf << "\n"; + std::cout << "tcurr: " << tcurr << "\n"; + float tInterp = (tcurr * prevTsdf - tprev * currTsdf)/(prevTsdf - currTsdf); + + if(!cvIsNaN(tInterp) && !cvIsInf(tInterp)) + { + Point3f pv = orig + tInterp * rayDirV; + Point3f nv = volume.getNormalVoxel(pv); + /* std::cout << "normal: " << nv << std::endl; */ + + if(!isNaN(nv)) + { + normal = vol2camRot * nv; + point = vol2cam * pv; + + /* std::cout << "Point: " << point << "\n"; */ + /* std::cout << "normal: " << normal << "\n"; */ + break; + } + } + } + + prevVolumeUnitIdx = currVolumeUnitIdx; + prevTsdf = currTsdf; + tprev = tcurr; + tcurr += stepSize; + } + ptsRow[x] = toPtype(point); + nrmRow[x] = toPtype(normal); + } + } + } + + Points& points; + Normals& normals; + const HashTSDFVolumeCPU& volume; + const float tstep; + const Affine3f cam2vol; + const Affine3f vol2cam; + const Intr::Reprojector reproj; +}; + +void HashTSDFVolumeCPU::raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, + cv::Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const +{ + CV_TRACE_FUNCTION(); + CV_Assert(frameSize.area() > 0); + + _points.create (frameSize, POINT_TYPE); + _normals.create (frameSize, POINT_TYPE); + + Points points = _points.getMat(); + Normals normals = _normals.getMat(); + RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); + + const int nstripes = -1; + parallel_for_(Range(0, points.rows), ri, nstripes); + +} + cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, float _raycastStepFactor, int volumeUnitResolution) { diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 1b5e20cb0f0..d567cb713df 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -7,13 +7,15 @@ #ifndef __OPENCV_HASH_TSDF_H__ #define __OPENCV_HASH_TSDF_H__ +#include +#include #include "opencv2/core/affine.hpp" #include "kinfu_frame.hpp" +#include "tsdf.hpp" namespace cv { namespace kinfu { - class HashTSDFVolume { public: @@ -25,8 +27,8 @@ class HashTSDFVolume virtual ~HashTSDFVolume() = default; virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) = 0; - /* virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, */ - /* cv::OutputArray points, cv::OutputArray normals) const = 0; */ + virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, + cv::OutputArray points, cv::OutputArray normals) const = 0; /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; */ /* virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; */ @@ -42,10 +44,75 @@ class HashTSDFVolume float raycastStepFactor; float truncDist; uint16_t volumeUnitResolution; - uint16_t volumeUnitSize; + float volumeUnitSize; bool zFirstMemOrder; }; + +struct VolumeUnit +{ + explicit VolumeUnit() : p_volume(nullptr) {}; + ~VolumeUnit() = default; + + cv::Ptr p_volume; + cv::Vec3i index; +}; + +//! Spatial hashing +struct tsdf_hash +{ + size_t operator()(const cv::Vec3i & x) const noexcept + { + size_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + for (uint16_t i = 0; i < 3; i++) { + seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + } + return seed; + } +}; + +typedef std::unordered_set VolumeUnitIndexSet; +typedef std::unordered_map VolumeUnitMap; + + +class HashTSDFVolumeCPU : public HashTSDFVolume +{ +public: + // dimension in voxels, size in meters + HashTSDFVolumeCPU(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, + float _truncDist, int _maxWeight, + float _raycastStepFactor, bool zFirstMemOrder = true); + + virtual void integrate(InputArray _depth, float depthFactor, + cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; + virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, + cv::OutputArray points, cv::OutputArray normals) const override; + + /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; */ + /* virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; */ + + virtual void reset() override; + virtual Voxel at(const cv::Vec3i &volumeIdx) const; + virtual Voxel at(const cv::Point3f &point) const; + + TsdfType interpolateVoxel(cv::Point3f p) const; + Point3f getNormalVoxel(cv::Point3f p) const; + + //! Utility functions for coordinate transformations + cv::Vec3i volumeToVolumeUnitIdx(cv::Point3f point) const; + cv::Point3f volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const; + + cv::Point3f voxelCoordToVolume(cv::Vec3i voxelIdx) const; + cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const; + + +//! TODO: Make this private +public: + //! Hashtable of individual smaller volume units + VolumeUnitMap volume_units_; +}; + cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, float _raycastStepFactor, int volumeUnitResolution = 16); } // namespace kinfu diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index 2a1b02368a9..cdb59865ca7 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -61,7 +61,8 @@ Ptr Params::defaultParams() // default pose of volume cube p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f)); - p.tsdf_trunc_dist = 0.04f; //meters; + /* p.volumePose = Affine3f(); */ + p.tsdf_trunc_dist = 5 * p.voxelSize; //meters; p.tsdf_max_weight = 64; //frames p.raycast_step_factor = 0.25f; //in voxel sizes @@ -87,6 +88,7 @@ Ptr Params::coarseParams() float volSize = 3.f; p->volumeDims = Vec3i::all(128); //number of voxels p->voxelSize = volSize/128.f; + p->tsdf_trunc_dist = 5 * p->voxelSize; p->raycast_step_factor = 0.75f; //in voxel sizes @@ -105,9 +107,10 @@ class KinFuImpl : public KinFu void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE; - void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE; - void getPoints(OutputArray points) const CV_OVERRIDE; - void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; + //! TODO(Akash): Add back later + /* void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE; */ + /* void getPoints(OutputArray points) const CV_OVERRIDE; */ + /* void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; */ void reset() CV_OVERRIDE; @@ -121,7 +124,7 @@ class KinFuImpl : public KinFu Params params; cv::Ptr icp; - cv::Ptr volume; + cv::Ptr volume; int frameCounter; Affine3f pose; @@ -134,7 +137,7 @@ template< typename T > KinFuImpl::KinFuImpl(const Params &_params) : params(_params), icp(makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)), - volume(makeTSDFVolume(params.volumeDims, params.voxelSize, params.volumePose, + volume(makeHashTSDFVolume(params.voxelSize, params.volumePose, params.tsdf_trunc_dist, params.tsdf_max_weight, params.raycast_step_factor)), pyrPoints(), pyrNormals() @@ -222,12 +225,11 @@ bool KinFuImpl::updateT(const T& _depth) params.bilateral_sigma_spatial, params.bilateral_kernel_size, params.truncateThreshold); - + std::cout << "Created Frame from depth FrameID: " << frameCounter << "\n"; if(frameCounter == 0) { // use depth instead of distance volume->integrate(depth, params.depthFactor, pose, params.intr); - pyrPoints = newPoints; pyrNormals = newNormals; } @@ -239,6 +241,7 @@ bool KinFuImpl::updateT(const T& _depth) return false; pose = pose * affine; + std::cout << "Obtained pose:\n " << pose.matrix << "\n"; float rnorm = (float)cv::norm(affine.rvec()); float tnorm = (float)cv::norm(affine.translation()); @@ -246,15 +249,23 @@ bool KinFuImpl::updateT(const T& _depth) if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement) { // use depth instead of distance + std::cout << "Starting integration: " << std::endl; volume->integrate(depth, params.depthFactor, pose, params.intr); + std::cout << "Completed integration: " << frameCounter << std::endl; } - + /* int number; */ + /* std::cout << "Waiting: " << std::endl; */ + /* std::cin >> number; */ T& points = pyrPoints [0]; T& normals = pyrNormals[0]; volume->raycast(pose, params.intr, params.frameSize, points, normals); + std::cout << "Raycast complete \n"; + /* std::cout << "Waiting: " << std::endl; */ + /* std::cin >> number; */ // build a pyramid of points and normals buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, params.pyramidLevels); + std::cout << "Built Point normal pyramids\n"; } frameCounter++; @@ -279,30 +290,31 @@ void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const { T points, normals; volume->raycast(cameraPose, params.intr, params.frameSize, points, normals); + std::cout << "Raycasted render" << std::endl; renderPointsNormals(points, normals, image, params.lightPose); } } -template< typename T > -void KinFuImpl::getCloud(OutputArray p, OutputArray n) const -{ - volume->fetchPointsNormals(p, n); -} +/* template< typename T > */ +/* void KinFuImpl::getCloud(OutputArray p, OutputArray n) const */ +/* { */ +/* volume->fetchPointsNormals(p, n); */ +/* } */ -template< typename T > -void KinFuImpl::getPoints(OutputArray points) const -{ - volume->fetchPointsNormals(points, noArray()); -} +/* template< typename T > */ +/* void KinFuImpl::getPoints(OutputArray points) const */ +/* { */ +/* volume->fetchPointsNormals(points, noArray()); */ +/* } */ -template< typename T > -void KinFuImpl::getNormals(InputArray points, OutputArray normals) const -{ - volume->fetchNormals(points, normals); -} +/* template< typename T > */ +/* void KinFuImpl::getNormals(InputArray points, OutputArray normals) const */ +/* { */ +/* volume->fetchNormals(points, normals); */ +/* } */ // importing class @@ -332,3 +344,4 @@ KinFu::~KinFu() {} } // namespace kinfu } // namespace cv + diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index f0dd937c601..b13dff8c3d8 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -12,55 +12,11 @@ namespace cv { namespace kinfu { -// TODO: Optimization possible: -// * volumeType can be FP16 -// * weight can be int16 -typedef float volumeType; -struct Voxel -{ - volumeType v; - int weight; -}; -typedef Vec VecT; - - -class TSDFVolumeCPU : public TSDFVolume -{ -public: - // dimension in voxels, size in meters - TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, Point3f _origin, bool zFirstMemOrder = true); - - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; - virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, - cv::OutputArray points, cv::OutputArray normals) const override; - - virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; - virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; - - virtual void reset() override; - - volumeType interpolateVoxel(cv::Point3f p) const; - Point3f getNormalVoxel(cv::Point3f p) const; - -#if USE_INTRINSICS - volumeType interpolateVoxel(const v_float32x4& p) const; - v_float32x4 getNormalVoxel(const v_float32x4& p) const; -#endif - - // See zFirstMemOrder arg of parent class constructor - // for the array layout info - // Consist of Voxel elements - Mat volume; -}; - - TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, Point3f _origin, bool zFirstMemOrder) : + float _raycastStepFactor, bool zFirstMemOrder) : voxelSize(_voxelSize), voxelSizeInv(1.f/_voxelSize), volResolution(_res), - origin(_origin), maxWeight(_maxWeight), pose(_pose), raycastStepFactor(_raycastStepFactor) @@ -105,8 +61,8 @@ TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _tr // dimension in voxels, size in meters TSDFVolumeCPU::TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, Point3f _origin, bool zFirstMemOrder) : - TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, _origin, zFirstMemOrder) + float _raycastStepFactor, bool zFirstMemOrder) : + TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, zFirstMemOrder) { volume = Mat(1, volResolution.x * volResolution.y * volResolution.z, rawType()); @@ -121,10 +77,30 @@ void TSDFVolumeCPU::reset() volume.forEach([](VecT& vv, const int* /* position */) { Voxel& v = reinterpret_cast(vv); - v.v = 0; v.weight = 0; + v.tsdf = 0; v.weight = 0; }); } +Voxel TSDFVolumeCPU::at(const cv::Vec3i &volumeIdx) const +{ + //! Out of bounds + if (volumeIdx[0] >= volResolution.x || + volumeIdx[1] >= volResolution.y || + volumeIdx[2] >= volResolution.z) + { + Voxel dummy; + dummy.tsdf = 0.0; + dummy.weight = 0; + std::cout << "Returning dummy\n"; + return dummy; + } + + const Voxel* volData = volume.ptr(); + int coordBase = volumeIdx[0]*volDims[0] + volumeIdx[1]*volDims[1] + volumeIdx[2]*volDims[2]; + /* std::cout << " Current TSDF: " << volData[coordBase].tsdf << " current weight: " << volData[coordBase].weight << "\n"; */ + return volData[coordBase]; +} + // SIMD version of that code is manually inlined #if !USE_INTRINSICS static const bool fixMissingData = false; @@ -239,6 +215,7 @@ struct IntegrateInvoker : ParallelLoopBody // optimization of camSpace transformation (vector addition instead of matmul at each z) Point3f basePt = vol2cam*(Point3f((float)x, (float)y, 0)*volume.voxelSize); v_float32x4 camSpacePt(basePt.x, basePt.y, basePt.z, 0); + /* std::cout << "basePt: " << basePt << " zstep: " << zStepPt << "\n"; */ int startZ, endZ; if(abs(zStepPt.z) > 1e-5) @@ -269,6 +246,7 @@ struct IntegrateInvoker : ParallelLoopBody } startZ = max(0, startZ); endZ = min(volume.volResolution.z, endZ); + /* std::cout << "StartZ, EndZ: " << startZ << ", " << endZ << "\n"; */ for(int z = startZ; z < endZ; z++) { // optimization of the following: @@ -277,7 +255,7 @@ struct IntegrateInvoker : ParallelLoopBody camSpacePt += zStep; float zCamSpace = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(camSpacePt))).get0(); - + /* std::cout << "zCamSpace: " << zCamSpace << std::endl; */ if(zCamSpace <= 0.f) continue; @@ -337,21 +315,22 @@ struct IntegrateInvoker : ParallelLoopBody // norm(camPixVec) produces double which is too slow float pixNorm = sqrt(v_reduce_sum(camPixVec*camPixVec)); // difference between distances of point and of surface to camera - volumeType sdf = pixNorm*(v*dfac - zCamSpace); + TsdfType sdf = pixNorm*(v*dfac - zCamSpace); // possible alternative is: // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); - + /* std::cout << "SDF: " << sdf << "truncDist: " << -volume.truncDist << "\n"; */ if(sdf >= -volume.truncDist) { - volumeType tsdf = fmin(1.f, sdf * truncDistInv); + TsdfType tsdf = fmin(1.f, sdf * truncDistInv); Voxel& voxel = volDataY[z*volume.volDims[2]]; int& weight = voxel.weight; - volumeType& value = voxel.v; + TsdfType& value = voxel.tsdf; // update TSDF value = (value*weight+tsdf) / (weight + 1); weight = min(weight + 1, volume.maxWeight); + /* std::cout << "Updated TSDF: " << value << "\n"; */ } } } @@ -368,13 +347,12 @@ struct IntegrateInvoker : ParallelLoopBody Voxel* volDataY = volDataX+y*volume.volDims[1]; // optimization of camSpace transformation (vector addition instead of matmul at each z) Point3f basePt = vol2cam*(Point3f(x, y, 0)*volume.voxelSize); - Point3f camSpacePt = basePt + volume.origin; + Point3f camSpacePt = basePt; // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; // zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize Point3f zStep = Point3f(vol2cam.matrix(0, 2), vol2cam.matrix(1, 2), vol2cam.matrix(2, 2))*volume.voxelSize; - int startZ, endZ; if(abs(zStep.z) > 1e-5) { @@ -404,13 +382,13 @@ struct IntegrateInvoker : ParallelLoopBody } startZ = max(0, startZ); endZ = min(volume.volResolution.z, endZ); + /* std::cout << "startZ, endZ: (" << startZ << ", " << endZ << ")\n"; */ for(int z = startZ; z < endZ; z++) { // optimization of the following: //Point3f volPt = Point3f(x, y, z)*volume.voxelSize; //Point3f camSpacePt = vol2cam * volPt; camSpacePt += zStep; - if(camSpacePt.z <= 0) continue; @@ -424,21 +402,27 @@ struct IntegrateInvoker : ParallelLoopBody // norm(camPixVec) produces double which is too slow float pixNorm = sqrt(camPixVec.dot(camPixVec)); // difference between distances of point and of surface to camera - volumeType sdf = pixNorm*(v*dfac - camSpacePt.z); + TsdfType sdf = pixNorm*(v*dfac - camSpacePt.z); + /* std::cout << "camSpacePt: " << camSpacePt << "\n"; */ + /* std::cout << "projected: " << projected << "\n"; */ + /* std::cout << "depth at projected: " << v << "\n"; */ + /* std::cout << "SDF: " << sdf << "\n"; */ + // possible alternative is: // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); - if(sdf >= -volume.truncDist) { - volumeType tsdf = fmin(1.f, sdf * truncDistInv); + TsdfType tsdf = fmin(1.f, sdf * truncDistInv); Voxel& voxel = volDataY[z*volume.volDims[2]]; int& weight = voxel.weight; - volumeType& value = voxel.v; + TsdfType& value = voxel.tsdf; // update TSDF value = (value*weight+tsdf) / (weight + 1); weight = min(weight + 1, volume.maxWeight); + /* std::cout << "Voxel coord: (" << x << ", " << y << ", " << z << ") "; */ + /* std::cout << "Updated value: " << value << " Updated weight: " << weight << "\n"; */ } } } @@ -462,7 +446,6 @@ void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f CV_Assert(_depth.type() == DEPTH_TYPE); Depth depth = _depth.getMat(); - IntegrateInvoker ii(*this, depth, intrinsics, cameraPose, depthFactor); Range range(0, volResolution.x); parallel_for_(range, ii); @@ -470,13 +453,13 @@ void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f #if USE_INTRINSICS // all coordinate checks should be done in inclosing cycle -inline volumeType TSDFVolumeCPU::interpolateVoxel(Point3f _p) const +inline TsdfType TSDFVolumeCPU::interpolateVoxel(Point3f _p) const { v_float32x4 p(_p.x, _p.y, _p.z, 0); return interpolateVoxel(p); } -inline volumeType TSDFVolumeCPU::interpolateVoxel(const v_float32x4& p) const +inline TsdfType TSDFVolumeCPU::interpolateVoxel(const v_float32x4& p) const { // tx, ty, tz = floor(p) v_int32x4 ip = v_floor(p); @@ -496,9 +479,9 @@ inline volumeType TSDFVolumeCPU::interpolateVoxel(const v_float32x4& p) const int coordBase = ix*xdim + iy*ydim + iz*zdim; - volumeType vx[8]; + TsdfType vx[8]; for(int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase].v; + vx[i] = volData[neighbourCoords[i] + coordBase].tsdf; v_float32x4 v0246(vx[0], vx[2], vx[4], vx[6]), v1357(vx[1], vx[3], vx[5], vx[7]); v_float32x4 vxx = v0246 + v_setall_f32(tz)*(v1357 - v0246); @@ -514,7 +497,7 @@ inline volumeType TSDFVolumeCPU::interpolateVoxel(const v_float32x4& p) const return v0 + tx*(v1 - v0); } #else -inline volumeType TSDFVolumeCPU::interpolateVoxel(Point3f p) const +inline TsdfType TSDFVolumeCPU::interpolateVoxel(Point3f p) const { int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; @@ -529,17 +512,17 @@ inline volumeType TSDFVolumeCPU::interpolateVoxel(Point3f p) const int coordBase = ix*xdim + iy*ydim + iz*zdim; const Voxel* volData = volume.ptr(); - volumeType vx[8]; + TsdfType vx[8]; for(int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase].v; + vx[i] = volData[neighbourCoords[i] + coordBase].tsdf; - volumeType v00 = vx[0] + tz*(vx[1] - vx[0]); - volumeType v01 = vx[2] + tz*(vx[3] - vx[2]); - volumeType v10 = vx[4] + tz*(vx[5] - vx[4]); - volumeType v11 = vx[6] + tz*(vx[7] - vx[6]); + TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); - volumeType v0 = v00 + ty*(v01 - v00); - volumeType v1 = v10 + ty*(v11 - v10); + TsdfType v0 = v00 + ty*(v01 - v00); + TsdfType v1 = v10 + ty*(v11 - v10); return v0 + tx*(v1 - v0); } @@ -589,10 +572,10 @@ inline v_float32x4 TSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const const int dim = volDims[c]; float& nv = an[c]; - volumeType vx[8]; + TsdfType vx[8]; for(int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].v - - volData[neighbourCoords[i] + coordBase - 1*dim].v; + vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - + volData[neighbourCoords[i] + coordBase - 1*dim].tsdf; v_float32x4 v0246(vx[0], vx[2], vx[4], vx[6]), v1357(vx[1], vx[3], vx[5], vx[7]); v_float32x4 vxx = v0246 + v_setall_f32(tz)*(v1357 - v0246); @@ -639,18 +622,18 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const const int dim = volDims[c]; float& nv = an[c]; - volumeType vx[8]; + TsdfType vx[8]; for(int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].v - - volData[neighbourCoords[i] + coordBase - 1*dim].v; + vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - + volData[neighbourCoords[i] + coordBase - 1*dim].tsdf; - volumeType v00 = vx[0] + tz*(vx[1] - vx[0]); - volumeType v01 = vx[2] + tz*(vx[3] - vx[2]); - volumeType v10 = vx[4] + tz*(vx[5] - vx[4]); - volumeType v11 = vx[6] + tz*(vx[7] - vx[6]); + TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); - volumeType v0 = v00 + ty*(v01 - v00); - volumeType v1 = v10 + ty*(v11 - v10); + TsdfType v0 = v00 + ty*(v01 - v00); + TsdfType v1 = v10 + ty*(v11 - v10); nv = v0 + tx*(v1 - v0); } @@ -760,7 +743,7 @@ struct RaycastInvoker : ParallelLoopBody int zdim = volume.volDims[2]; v_float32x4 rayStep = dir * v_setall_f32(tstep); v_float32x4 next = (orig + dir * v_setall_f32(tmin)); - volumeType f = volume.interpolateVoxel(next), fnext = f; + TsdfType f = volume.interpolateVoxel(next), fnext = f; //raymarch int steps = 0; @@ -774,7 +757,7 @@ struct RaycastInvoker : ParallelLoopBody int iz = ip.get0(); int coord = ix*xdim + iy*ydim + iz*zdim; - fnext = volume.volume.at(coord).v; + fnext = volume.volume.at(coord).tsdf; if(fnext != f) { fnext = volume.interpolateVoxel(next); @@ -792,8 +775,8 @@ struct RaycastInvoker : ParallelLoopBody if(f > 0.f && fnext < 0.f) { v_float32x4 tp = next - rayStep; - volumeType ft = volume.interpolateVoxel(tp); - volumeType ftdt = volume.interpolateVoxel(next); + TsdfType ft = volume.interpolateVoxel(tp); + TsdfType ftdt = volume.interpolateVoxel(next); // float t = tmin + steps*tstep; // float ts = t - tstep*ft/(ftdt - ft); float ts = tmin + tstep*(steps - ft/(ftdt - ft)); @@ -869,7 +852,7 @@ struct RaycastInvoker : ParallelLoopBody Point3f rayStep = dir * tstep; Point3f next = (orig + dir * tmin); - volumeType f = volume.interpolateVoxel(next), fnext = f; + TsdfType f = volume.interpolateVoxel(next), fnext = f; //raymarch int steps = 0; @@ -883,7 +866,7 @@ struct RaycastInvoker : ParallelLoopBody int ix = cvRound(next.x); int iy = cvRound(next.y); int iz = cvRound(next.z); - fnext = volume.volume.at(ix*xdim + iy*ydim + iz*zdim).v; + fnext = volume.volume.at(ix*xdim + iy*ydim + iz*zdim).tsdf; if(fnext != f) { fnext = volume.interpolateVoxel(next); @@ -901,8 +884,8 @@ struct RaycastInvoker : ParallelLoopBody if(f > 0.f && fnext < 0.f) { Point3f tp = next - rayStep; - volumeType ft = volume.interpolateVoxel(tp); - volumeType ftdt = volume.interpolateVoxel(next); + TsdfType ft = volume.interpolateVoxel(tp); + TsdfType ftdt = volume.interpolateVoxel(next); // float t = tmin + steps*tstep; // float ts = t - tstep*ft/(ftdt - ft); float ts = tmin + tstep*(steps - ft/(ftdt - ft)); @@ -1012,7 +995,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody const Voxel& voxeld = volDataStart[(x+shift.x)*vol.volDims[0] + (y+shift.y)*vol.volDims[1] + (z+shift.z)*vol.volDims[2]]; - volumeType vd = voxeld.v; + TsdfType vd = voxeld.tsdf; if(voxeld.weight != 0 && vd != 1.f) { @@ -1049,7 +1032,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody for(int z = 0; z < vol.volResolution.z; z++) { const Voxel& voxel0 = volDataY[z*vol.volDims[2]]; - volumeType v0 = voxel0.v; + TsdfType v0 = voxel0.tsdf; if(voxel0.weight != 0 && v0 != 1.f) { Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f)*vol.voxelSize); @@ -1151,34 +1134,9 @@ void TSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const ///////// GPU implementation ///////// #ifdef HAVE_OPENCL - -class TSDFVolumeGPU : public TSDFVolume -{ -public: - // dimension in voxels, size in meters - TSDFVolumeGPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, Point3f _origin); - - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; - virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, - cv::OutputArray _points, cv::OutputArray _normals) const override; - - virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; - virtual void fetchNormals(cv::InputArray points, cv::OutputArray normals) const override; - - virtual void reset() override; - - // See zFirstMemOrder arg of parent class constructor - // for the array layout info - // Array elem is CV_32FC2, read as (float, int) - // TODO: optimization possible to (fp16, int16), see Voxel definition - UMat volume; -}; - - TSDFVolumeGPU::TSDFVolumeGPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, Point3f _origin) : - TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, _origin, false) + float _raycastStepFactor) : + TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, false) { volume = UMat(1, volResolution.x * volResolution.y * volResolution.z, CV_32FC2); @@ -1475,13 +1433,13 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) #endif cv::Ptr makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, Point3f _origin) + float _raycastStepFactor) { #ifdef HAVE_OPENCL if(cv::ocl::useOpenCL()) - return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, _origin); + return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor); #endif - return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, _origin); + return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor); } } // namespace kinfu diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 23ce191b772..b6de802fb48 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -18,7 +18,7 @@ class TSDFVolume public: // dimension in voxels, size in meters TSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, cv::Point3f _origin, bool zFirstMemOrder = true); + float _raycastStepFactor, bool zFirstMemOrder = true); virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) = 0; virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, @@ -34,7 +34,6 @@ class TSDFVolume float voxelSize; float voxelSizeInv; Point3i volResolution; - Point3f origin; int maxWeight; cv::Affine3f pose; float raycastStepFactor; @@ -45,8 +44,74 @@ class TSDFVolume Vec8i neighbourCoords; }; +// TODO: Optimization possible: +// * TsdfType can be FP16 +// * weight can be int16 +typedef float TsdfType; +struct Voxel +{ + TsdfType tsdf; + int weight; +}; +typedef Vec VecT; + +class TSDFVolumeCPU : public TSDFVolume +{ +public: + // dimension in voxels, size in meters + TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, + float _raycastStepFactor, bool zFirstMemOrder = true); + + virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; + virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, + cv::OutputArray points, cv::OutputArray normals) const override; + + virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; + virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; + + virtual void reset() override; + virtual Voxel at(const cv::Vec3i &volumeIdx) const; + + TsdfType interpolateVoxel(cv::Point3f p) const; + Point3f getNormalVoxel(cv::Point3f p) const; + +#if USE_INTRINSICS + TsdfType interpolateVoxel(const v_float32x4& p) const; + v_float32x4 getNormalVoxel(const v_float32x4& p) const; +#endif + + // See zFirstMemOrder arg of parent class constructor + // for the array layout info + // Consist of Voxel elements + Mat volume; +}; + +#ifdef HAVE_OPENCL +class TSDFVolumeGPU : public TSDFVolume +{ +public: + // dimension in voxels, size in meters + TSDFVolumeGPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, + float _raycastStepFactor); + + virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; + virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, + cv::OutputArray _points, cv::OutputArray _normals) const override; + + virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; + virtual void fetchNormals(cv::InputArray points, cv::OutputArray normals) const override; + + virtual void reset() override; + + // See zFirstMemOrder arg of parent class constructor + // for the array layout info + // Array elem is CV_32FC2, read as (float, int) + // TODO: optimization possible to (fp16, int16), see Voxel definition + UMat volume; +}; +#endif cv::Ptr makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, cv::Point3f origin = Point3f(0, 0, 0)); + float _raycastStepFactor); } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/utils.hpp b/modules/rgbd/src/utils.hpp index 764134b2bb1..6df1af86785 100644 --- a/modules/rgbd/src/utils.hpp +++ b/modules/rgbd/src/utils.hpp @@ -48,6 +48,7 @@ namespace kinfu { // One place to turn intrinsics on and off #define USE_INTRINSICS CV_SIMD128 +#undef USE_INTRINSICS typedef float depthType; From 01a172c8cf4ba3d029667f92b41839e804a4814d Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Thu, 4 Jun 2020 11:19:24 -0400 Subject: [PATCH 03/36] Update integration code --- modules/rgbd/src/hash_tsdf.cpp | 116 +++++++++++++++++++++++++-------- modules/rgbd/src/hash_tsdf.hpp | 5 +- modules/rgbd/src/tsdf.cpp | 4 +- 3 files changed, 95 insertions(+), 30 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b25d8aed5d4..4887fc2330e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -20,6 +20,7 @@ #include "opencl_kernels_rgbd.hpp" #include #include +#include #include "depth_to_3d.hpp" #include "utils.hpp" @@ -116,14 +117,14 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody volume.volumeUnitResolution); //! Translate the origin of the subvolume to the correct position in volume coordinate frame cv::Affine3f subvolumePose = volume.pose.translate(volume.volumeUnitIdxToVolume(-tsdf_idx)); - volumeUnit.p_volume = cv::makePtr(volumeDims, + volumeUnit.pVolume = cv::makePtr(volumeDims, volume.volumeUnitSize, subvolumePose, volume.truncDist, volume.maxWeight, volume.raycastStepFactor); volumeUnit.index = tsdf_idx; - + volumeUnit.isActive = false; volume.volume_units_[tsdf_idx] = volumeUnit; } } @@ -144,11 +145,11 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody struct IntegrateSubvolumeInvoker : ParallelLoopBody { - IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, std::vector _accessVolUnitVec, + IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, std::vector _allocatedVolUnits, const Depth& _depth, Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : ParallelLoopBody(), volume(_volume), - accessVolUnitsVec(_accessVolUnitVec), + allocatedVolUnits(_allocatedVolUnits), depth(_depth), depthFactor(_depthFactor), cameraPose(_cameraPose), @@ -160,26 +161,81 @@ struct IntegrateSubvolumeInvoker : ParallelLoopBody { for (int i = range.start; i < range.end; i++) { - cv::Vec3i tsdf_idx = accessVolUnitsVec[i]; - - VolumeUnitMap::iterator accessedVolUnit = volume.volume_units_.find(tsdf_idx); - assert(accessedVolUnit != volume.volume_units_.end()); + cv::Vec3i tsdf_idx = allocatedVolUnits[i]; - VolumeUnit volumeUnit = accessedVolUnit->second; - /* std::cout << "Integrating unit: " << accessedVolUnit->first << std::endl; */ - //! The volume unit should already be added into the Volume from the allocator - volumeUnit.p_volume->integrate(depth, depthFactor, cameraPose, intrinsics); + VolumeUnitMap::iterator it = volume.volume_units_.find(tsdf_idx); + assert(it != volume.volume_units_.end()); + VolumeUnit& volumeUnit = it->second; + AutoLock al(mutex); + if(volumeUnit.isActive) + { + std::cout << "Integrating unit: " << it->first << std::endl; + //! The volume unit should already be added into the Volume from the allocator + volumeUnit.pVolume->integrate(depth, depthFactor, cameraPose, intrinsics); + //! Ensure all active volumeUnits are set to inactive for next integration + volumeUnit.isActive = false; + } } } HashTSDFVolumeCPU& volume; - std::vector accessVolUnitsVec; + std::vector allocatedVolUnits; const Depth& depth; float depthFactor; cv::Affine3f cameraPose; Intr intrinsics; + mutable Mutex mutex; }; +struct VolumeUnitInFrustumInvoker : ParallelLoopBody +{ + VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, const std::vector& _allAllocatedUnits, const Depth& _depth, Intr _intrinsics, + cv::Affine3f _cameraPose, float _depthFactor) : + ParallelLoopBody(), + volume(_volume), + allAllocatedUnits(_allAllocatedUnits), + depth(_depth), + proj(_intrinsics.makeProjector()), + cameraPose(_cameraPose), + depthFactor(_depthFactor), + cam2vol(_volume.pose.inv() * cameraPose) + {} + + virtual void operator() (const Range& range) const override + { + for (int i = range.start; i < range.end; ++i) + { + cv::Vec3i tsdf_idx = allAllocatedUnits[i]; + Point3f volumeUnitPos = volume.volumeUnitIdxToVolume(tsdf_idx); + AutoLock al(mutex); + if (volumeUnitPos.z < rgbd::Odometry::DEFAULT_MIN_DEPTH() || + volumeUnitPos.z > rgbd::Odometry::DEFAULT_MAX_DEPTH()) + { + VolumeUnitMap::iterator it = volume.volume_units_.find(tsdf_idx); + assert(it != volume.volume_units_.end()); + it->second.isActive = false; + return; + } + Point2f cameraPoint = proj(volumeUnitPos); + if(cameraPoint.x >= 0 && cameraPoint.y >= 0 && + cameraPoint.x < (depth.rows - 1) && cameraPoint.y < (depth.cols - 1)) + { + VolumeUnitMap::iterator it = volume.volume_units_.find(tsdf_idx); + assert(it != volume.volume_units_.end()); + it->second.isActive = true; + /* std::cout << " Active VolumeUnit: " << tsdf_idx << "\n"; */ + } + } + } + HashTSDFVolumeCPU& volume; + const std::vector allAllocatedUnits; + const Depth& depth; + const Intr::Projector proj; + const Affine3f cameraPose; + const float depthFactor; + const Affine3f cam2vol; + mutable Mutex mutex; +}; void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, Intr intrinsics) { @@ -194,11 +250,19 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affi Range range(0, depth.rows); parallel_for_(range, allocate_i); - std::vector accessVolUnitsVec; - accessVolUnitsVec.assign(accessVolUnits.begin(), accessVolUnits.end()); - std::cout << "Number of active subvolumes: " << accessVolUnitsVec.size() << "\n"; - IntegrateSubvolumeInvoker integrate_i(*this, accessVolUnitsVec, depth, intrinsics, cameraPose, depthFactor); - Range accessed_units_range(0, accessVolUnitsVec.size()); + std::vector allocatedVolUnits; + //! TODO(Akash): Actually only need to integrate over volumeunits in current frustum + for(const auto& keyvalue : volume_units_) + { + allocatedVolUnits.push_back(keyvalue.first); + } + std::cout << "Number of allocated subvolumes: " << allocatedVolUnits.size() << "\n"; + VolumeUnitInFrustumInvoker infrustum_i(*this, allocatedVolUnits, depth, intrinsics, cameraPose, depthFactor); + Range in_frustum_range(0, allocatedVolUnits.size()); + parallel_for_(in_frustum_range, infrustum_i); + + IntegrateSubvolumeInvoker integrate_i(*this, allocatedVolUnits, depth, intrinsics, cameraPose, depthFactor); + Range accessed_units_range(0, allocatedVolUnits.size()); parallel_for_(accessed_units_range, integrate_i); std::cout << "Integration complete \n"; } @@ -249,7 +313,7 @@ inline Voxel HashTSDFVolumeCPU::at(const cv::Vec3i &volumeIdx) const dummy.tsdf = 0.f; dummy.weight = 0; return dummy; } - cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.p_volume); + cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); return volumeUnit->at(volUnitLocalIdx); } @@ -265,7 +329,7 @@ inline Voxel HashTSDFVolumeCPU::at(const cv::Point3f &point) const dummy.tsdf = 0; dummy.weight = 0; return dummy; } - cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.p_volume); + cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); /* std::cout << "volumeUnitIdx: " << volumeUnitIdx << "volUnitLocalIdx: " << volUnitLocalIdx << std::endl; */ /* std::cout << volumeUnit->at(volUnitLocalIdx).tsdf << std::endl; */ return volumeUnit->at(volUnitLocalIdx); @@ -440,7 +504,7 @@ struct RaycastInvoker : ParallelLoopBody //! Is the subvolume exists in hashtable if(it != volume.volume_units_.end()) { - currVolumeUnit = std::dynamic_pointer_cast(it->second.p_volume); + currVolumeUnit = std::dynamic_pointer_cast(it->second.pVolume); cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); @@ -454,11 +518,11 @@ struct RaycastInvoker : ParallelLoopBody //! Surface crossing if(prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) { - std::cout << "subvolume coords: " << volUnitLocalIdx << "\n"; - std::cout << "current TSDF: " << currTsdf << "\n"; - std::cout << "current weight: " << currWeight << "\n"; - std::cout << "previous TSDF: " << prevTsdf << "\n"; - std::cout << "tcurr: " << tcurr << "\n"; + /* std::cout << "subvolume coords: " << volUnitLocalIdx << "\n"; */ + /* std::cout << "current TSDF: " << currTsdf << "\n"; */ + /* std::cout << "current weight: " << currWeight << "\n"; */ + /* std::cout << "previous TSDF: " << prevTsdf << "\n"; */ + /* std::cout << "tcurr: " << tcurr << "\n"; */ float tInterp = (tcurr * prevTsdf - tprev * currTsdf)/(prevTsdf - currTsdf); if(!cvIsNaN(tInterp) && !cvIsInf(tInterp)) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index d567cb713df..4a5f5b8dccf 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -51,11 +51,12 @@ class HashTSDFVolume struct VolumeUnit { - explicit VolumeUnit() : p_volume(nullptr) {}; + explicit VolumeUnit() : pVolume(nullptr) {}; ~VolumeUnit() = default; - cv::Ptr p_volume; + cv::Ptr pVolume; cv::Vec3i index; + bool isActive; }; //! Spatial hashing diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index b13dff8c3d8..a08aefe708e 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -421,8 +421,8 @@ struct IntegrateInvoker : ParallelLoopBody // update TSDF value = (value*weight+tsdf) / (weight + 1); weight = min(weight + 1, volume.maxWeight); - /* std::cout << "Voxel coord: (" << x << ", " << y << ", " << z << ") "; */ - /* std::cout << "Updated value: " << value << " Updated weight: " << weight << "\n"; */ + std::cout << "Voxel coord: (" << x << ", " << y << ", " << z << ") "; + std::cout << "Updated value: " << value << " Updated weight: " << weight << "\n"; } } } From 5a3f53718e68bcd61c821242343263d81bc84461 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Sun, 7 Jun 2020 18:28:18 -0400 Subject: [PATCH 04/36] Integration and Raycasting fixes, (both work now) --- modules/rgbd/samples/kinfu_demo.cpp | 4 +- modules/rgbd/src/hash_tsdf.cpp | 106 +++++++++++++++------------- modules/rgbd/src/kinfu.cpp | 11 +-- modules/rgbd/src/tsdf.cpp | 5 +- 4 files changed, 68 insertions(+), 58 deletions(-) diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index e97a356ecad..c0e21813c97 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -420,7 +420,7 @@ int main(int argc, char **argv) if(pause) { // doesn't happen in idle mode - /* kf->getCloud(points, normals); */ + kf->getCloud(points, normals); if(!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); @@ -465,7 +465,7 @@ int main(int argc, char **argv) { if(coarse) { - /* kf->getCloud(points, normals); */ + kf->getCloud(points, normals); if(!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 4887fc2330e..6cebe76a20e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -18,6 +18,7 @@ #include "precomp.hpp" #include "hash_tsdf.hpp" #include "opencl_kernels_rgbd.hpp" +#include #include #include #include @@ -115,10 +116,10 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody cv::Point3i volumeDims(volume.volumeUnitResolution, volume.volumeUnitResolution, volume.volumeUnitResolution); - //! Translate the origin of the subvolume to the correct position in volume coordinate frame - cv::Affine3f subvolumePose = volume.pose.translate(volume.volumeUnitIdxToVolume(-tsdf_idx)); + //! Translate the origin of the subvolume to the correct position in volume coordinate frame + cv::Affine3f subvolumePose = volume.pose.translate(volume.volumeUnitIdxToVolume(tsdf_idx)); volumeUnit.pVolume = cv::makePtr(volumeDims, - volume.volumeUnitSize, + volume.voxelSize, subvolumePose, volume.truncDist, volume.maxWeight, @@ -145,11 +146,11 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody struct IntegrateSubvolumeInvoker : ParallelLoopBody { - IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, std::vector _allocatedVolUnits, + IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, std::vector _totalVolUnits, const Depth& _depth, Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : ParallelLoopBody(), volume(_volume), - allocatedVolUnits(_allocatedVolUnits), + totalVolUnits(_totalVolUnits), depth(_depth), depthFactor(_depthFactor), cameraPose(_cameraPose), @@ -161,25 +162,25 @@ struct IntegrateSubvolumeInvoker : ParallelLoopBody { for (int i = range.start; i < range.end; i++) { - cv::Vec3i tsdf_idx = allocatedVolUnits[i]; - + cv::Vec3i tsdf_idx = totalVolUnits[i]; VolumeUnitMap::iterator it = volume.volume_units_.find(tsdf_idx); - assert(it != volume.volume_units_.end()); + if(it == volume.volume_units_.end()) + return; + VolumeUnit& volumeUnit = it->second; - AutoLock al(mutex); - if(volumeUnit.isActive) - { - std::cout << "Integrating unit: " << it->first << std::endl; + /* if(volumeUnit.isActive) */ + /* { */ + /* std::cout << "Integrating unit: " << it->first << std::endl; */ //! The volume unit should already be added into the Volume from the allocator volumeUnit.pVolume->integrate(depth, depthFactor, cameraPose, intrinsics); //! Ensure all active volumeUnits are set to inactive for next integration volumeUnit.isActive = false; - } + /* } */ } } HashTSDFVolumeCPU& volume; - std::vector allocatedVolUnits; + std::vector totalVolUnits; const Depth& depth; float depthFactor; cv::Affine3f cameraPose; @@ -189,51 +190,53 @@ struct IntegrateSubvolumeInvoker : ParallelLoopBody struct VolumeUnitInFrustumInvoker : ParallelLoopBody { - VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, const std::vector& _allAllocatedUnits, const Depth& _depth, Intr _intrinsics, + VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, const std::vector& _totalVolUnits, const Depth& _depth, Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : ParallelLoopBody(), volume(_volume), - allAllocatedUnits(_allAllocatedUnits), + totalVolUnits(_totalVolUnits), depth(_depth), proj(_intrinsics.makeProjector()), cameraPose(_cameraPose), depthFactor(_depthFactor), - cam2vol(_volume.pose.inv() * cameraPose) - {} + vol2cam(cameraPose.inv() * _volume.pose) + { + } virtual void operator() (const Range& range) const override { for (int i = range.start; i < range.end; ++i) { - cv::Vec3i tsdf_idx = allAllocatedUnits[i]; - Point3f volumeUnitPos = volume.volumeUnitIdxToVolume(tsdf_idx); - AutoLock al(mutex); + cv::Vec3i tsdf_idx = totalVolUnits[i]; + + VolumeUnitMap::iterator it = volume.volume_units_.find(tsdf_idx); + if(it == volume.volume_units_.end()) + return; + + Point3f volumeUnitPos = volume.volumeUnitIdxToVolume(it->first); if (volumeUnitPos.z < rgbd::Odometry::DEFAULT_MIN_DEPTH() || volumeUnitPos.z > rgbd::Odometry::DEFAULT_MAX_DEPTH()) { - VolumeUnitMap::iterator it = volume.volume_units_.find(tsdf_idx); - assert(it != volume.volume_units_.end()); it->second.isActive = false; return; } - Point2f cameraPoint = proj(volumeUnitPos); + Point2f cameraPoint = proj(vol2cam * volumeUnitPos); + /* std::cout << i << " VolumeUnitPos: " << volumeUnitPos << " cameraPoint: " << cameraPoint << "\n"; */ if(cameraPoint.x >= 0 && cameraPoint.y >= 0 && - cameraPoint.x < (depth.rows - 1) && cameraPoint.y < (depth.cols - 1)) + cameraPoint.x < (depth.cols - 1) && cameraPoint.y < (depth.rows - 1)) { - VolumeUnitMap::iterator it = volume.volume_units_.find(tsdf_idx); assert(it != volume.volume_units_.end()); it->second.isActive = true; - /* std::cout << " Active VolumeUnit: " << tsdf_idx << "\n"; */ } } } HashTSDFVolumeCPU& volume; - const std::vector allAllocatedUnits; + const std::vector totalVolUnits; const Depth& depth; const Intr::Projector proj; const Affine3f cameraPose; const float depthFactor; - const Affine3f cam2vol; + const Affine3f vol2cam; mutable Mutex mutex; }; @@ -249,22 +252,21 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affi AccessedVolumeUnitsInvoker allocate_i(*this, accessVolUnits, depth, intrinsics, cameraPose, depthFactor); Range range(0, depth.rows); parallel_for_(range, allocate_i); - - std::vector allocatedVolUnits; - //! TODO(Akash): Actually only need to integrate over volumeunits in current frustum + std::vector totalVolUnits; for(const auto& keyvalue : volume_units_) { - allocatedVolUnits.push_back(keyvalue.first); + totalVolUnits.push_back(keyvalue.first); } - std::cout << "Number of allocated subvolumes: " << allocatedVolUnits.size() << "\n"; - VolumeUnitInFrustumInvoker infrustum_i(*this, allocatedVolUnits, depth, intrinsics, cameraPose, depthFactor); - Range in_frustum_range(0, allocatedVolUnits.size()); + std::cout << "Number of VolumeUnits in map: " << totalVolUnits.size() << "\n"; + VolumeUnitInFrustumInvoker infrustum_i(*this, totalVolUnits, depth, intrinsics, cameraPose, depthFactor); + Range in_frustum_range(0, volume_units_.size()); parallel_for_(in_frustum_range, infrustum_i); - - IntegrateSubvolumeInvoker integrate_i(*this, allocatedVolUnits, depth, intrinsics, cameraPose, depthFactor); - Range accessed_units_range(0, allocatedVolUnits.size()); + /* std::cout << "Number of subvolumes in Frustum: " << unitsInFrustum << " Waiting:\n"; */ + /* std::cin >> input; */ + IntegrateSubvolumeInvoker integrate_i(*this, totalVolUnits, depth, intrinsics, cameraPose, depthFactor); + Range accessed_units_range(0, totalVolUnits.size()); parallel_for_(accessed_units_range, integrate_i); - std::cout << "Integration complete \n"; + /* std::cout << "Integration complete \n"; */ } cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx(cv::Point3f p) const @@ -477,13 +479,19 @@ struct RaycastInvoker : ParallelLoopBody /* std::cout << "tmin, tmax :" << tmin << ", " << tmax << "\n"; */ /* std::cout << "Origin: " << orig << " rayDirection: " << rayDirV << "\n"; */ float tprev = tmin; - float tcurr = tmin + tstep; + float tcurr = tmin; //! Is this a reasonable initialization? TsdfType prevTsdf = volume.truncDist; cv::Vec3i prevVolumeUnitIdx = cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), std::numeric_limits::min()); cv::Ptr currVolumeUnit; + + /* Point3f initRayPos = orig + tmin */ + /* cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); */ + /* cv::Vec3f rayDeltaDistance(1/rayDirV.x, 1/rayDirV.y, 1/rayDirV.z); */ + + while(tcurr < tmax) { Point3f currRayPos = orig + tcurr * rayDirV; @@ -493,36 +501,35 @@ struct RaycastInvoker : ParallelLoopBody /* std::cout << "Previous volume unit Index: " << prevVolumeUnitIdx << "\n"; */ /* std::cout << "Current volume unit Index: " << currVolumeUnitIdx << "\n"; */ - VolumeUnitMap::const_iterator it; - if(currVolumeUnitIdx != prevVolumeUnitIdx) - it = volume.volume_units_.find(currVolumeUnitIdx); - + VolumeUnitMap::const_iterator it = volume.volume_units_.find(currVolumeUnitIdx); TsdfType currTsdf = prevTsdf; int currWeight = 0; float stepSize = blockSize; cv::Vec3i volUnitLocalIdx; - //! Is the subvolume exists in hashtable + //! Does the subvolume exist in hashtable if(it != volume.volume_units_.end()) { currVolumeUnit = std::dynamic_pointer_cast(it->second.pVolume); cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); + /* std::cout << "currRaypos: " << currRayPos << " currVolUnitPos: " << currVolUnitPos << "\n"; */ volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! Figure out voxel interpolation Voxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); currTsdf = currVoxel.tsdf; currWeight = currVoxel.weight; - stepSize = max(currTsdf * volume.truncDist, tstep); + stepSize = std::max(tstep, currTsdf * volume.truncDist); + } //! Surface crossing if(prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) { + /* std::cout << "Current pixel: (" << x << ", " << y << ")\n"; */ /* std::cout << "subvolume coords: " << volUnitLocalIdx << "\n"; */ /* std::cout << "current TSDF: " << currTsdf << "\n"; */ /* std::cout << "current weight: " << currWeight << "\n"; */ /* std::cout << "previous TSDF: " << prevTsdf << "\n"; */ - /* std::cout << "tcurr: " << tcurr << "\n"; */ float tInterp = (tcurr * prevTsdf - tprev * currTsdf)/(prevTsdf - currTsdf); if(!cvIsNaN(tInterp) && !cvIsInf(tInterp)) @@ -542,7 +549,7 @@ struct RaycastInvoker : ParallelLoopBody } } } - + /* std::cout << "stepSize: " << stepSize << "\n"; */ prevVolumeUnitIdx = currVolumeUnitIdx; prevTsdf = currTsdf; tprev = tcurr; @@ -574,6 +581,7 @@ void HashTSDFVolumeCPU::raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrins Points points = _points.getMat(); Normals normals = _normals.getMat(); + RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); const int nstripes = -1; diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index cdb59865ca7..ef832b50745 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -90,7 +90,7 @@ Ptr Params::coarseParams() p->voxelSize = volSize/128.f; p->tsdf_trunc_dist = 5 * p->voxelSize; - p->raycast_step_factor = 0.75f; //in voxel sizes + p->raycast_step_factor = 0.25f; //in voxel sizes return p; } @@ -108,7 +108,7 @@ class KinFuImpl : public KinFu void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE; //! TODO(Akash): Add back later - /* void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE; */ + /* virtual void getCloud(OutputArray points, OutputArray normals, const Matx44f& cameraPose) const CV_OVERRIDE; */ /* void getPoints(OutputArray points) const CV_OVERRIDE; */ /* void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; */ @@ -259,7 +259,7 @@ bool KinFuImpl::updateT(const T& _depth) T& points = pyrPoints [0]; T& normals = pyrNormals[0]; volume->raycast(pose, params.intr, params.frameSize, points, normals); - std::cout << "Raycast complete \n"; + /* std::cout << "Raycast complete \n"; */ /* std::cout << "Waiting: " << std::endl; */ /* std::cin >> number; */ // build a pyramid of points and normals @@ -297,9 +297,10 @@ void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const /* template< typename T > */ -/* void KinFuImpl::getCloud(OutputArray p, OutputArray n) const */ +/* void KinFuImpl::getCloud(cv::OutputArray p, OutputArray n, const Matx44f& _cameraPose) const */ /* { */ -/* volume->fetchPointsNormals(p, n); */ +/* /1* volume->fetchPointsNormals(p, n); *1/ */ +/* /1* volume->raycast(pose, params.intr, params.frameSize, p, n); *1/ */ /* } */ diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index a08aefe708e..22f899d67f4 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -347,6 +347,7 @@ struct IntegrateInvoker : ParallelLoopBody Voxel* volDataY = volDataX+y*volume.volDims[1]; // optimization of camSpace transformation (vector addition instead of matmul at each z) Point3f basePt = vol2cam*(Point3f(x, y, 0)*volume.voxelSize); + /* std::cout << "Base Point: " << basePt << " VoxelSize: " << volume.voxelSize << "\n"; */ Point3f camSpacePt = basePt; // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; // zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize @@ -421,8 +422,8 @@ struct IntegrateInvoker : ParallelLoopBody // update TSDF value = (value*weight+tsdf) / (weight + 1); weight = min(weight + 1, volume.maxWeight); - std::cout << "Voxel coord: (" << x << ", " << y << ", " << z << ") "; - std::cout << "Updated value: " << value << " Updated weight: " << weight << "\n"; + /* std::cout << "Voxel coord: (" << x << ", " << y << ", " << z << ") "; */ + /* std::cout << "Updated value: " << value << " Updated weight: " << weight << "\n"; */ } } } From 9b5b0ce67ca4c1a401e1a57d4dee97838ed090b4 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 9 Jun 2020 13:09:07 -0400 Subject: [PATCH 05/36] - Format code - Clean up comments and few fixes --- modules/rgbd/samples/kinfu_demo.cpp | 281 ++++++++-------- modules/rgbd/src/hash_tsdf.cpp | 499 +++++++++++++--------------- modules/rgbd/src/hash_tsdf.hpp | 10 +- modules/rgbd/src/kinfu.cpp | 9 - modules/rgbd/src/utils.hpp | 3 + 5 files changed, 376 insertions(+), 426 deletions(-) diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index c0e21813c97..291a7e54ecc 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -2,13 +2,14 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory -#include #include -#include +#include #include #include +#include #include using namespace cv; @@ -26,24 +27,25 @@ static vector readDepth(std::string fileList) vector v; fstream file(fileList); - if(!file.is_open()) + if (!file.is_open()) throw std::runtime_error("Failed to read depth list"); std::string dir; size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); - while(!file.eof()) + while (!file.eof()) { std::string s, imgPath; std::getline(file, s); - if(s.empty() || s[0] == '#') continue; + if (s.empty() || s[0] == '#') + continue; std::stringstream ss; ss << s; double thumb; ss >> thumb >> imgPath; - v.push_back(dir+'/'+imgPath); + v.push_back(dir + '/' + imgPath); } return v; @@ -51,14 +53,13 @@ static vector readDepth(std::string fileList) struct DepthWriter { - DepthWriter(string fileList) : - file(fileList, ios::out), count(0), dir() + DepthWriter(string fileList) : file(fileList, ios::out), count(0), dir() { size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); - if(!file.is_open()) + if (!file.is_open()) throw std::runtime_error("Failed to write depth list"); file << "# depth maps saved from device" << endl; @@ -67,10 +68,10 @@ struct DepthWriter void append(InputArray _depth) { - Mat depth = _depth.getMat(); - string depthFname = cv::format("%04d.png", count); + Mat depth = _depth.getMat(); + string depthFname = cv::format("%04d.png", count); string fullDepthFname = dir + '/' + depthFname; - if(!imwrite(fullDepthFname, depth)) + if (!imwrite(fullDepthFname, depth)) throw std::runtime_error("Failed to write depth to file " + fullDepthFname); file << count++ << " " << depthFname << endl; } @@ -82,19 +83,19 @@ struct DepthWriter namespace Kinect2Params { - static const Size frameSize = Size(512, 424); - // approximate values, no guarantee to be correct - static const float focal = 366.1f; - static const float cx = 258.2f; - static const float cy = 204.f; - static const float k1 = 0.12f; - static const float k2 = -0.34f; - static const float k3 = 0.12f; -}; +static const Size frameSize = Size(512, 424); +// approximate values, no guarantee to be correct +static const float focal = 366.1f; +static const float cx = 258.2f; +static const float cy = 204.f; +static const float k1 = 0.12f; +static const float k2 = -0.34f; +static const float k3 = 0.12f; +}; // namespace Kinect2Params struct DepthSource { -public: + public: enum Type { DEPTH_LIST, @@ -103,31 +104,27 @@ struct DepthSource DEPTH_REALSENSE }; - DepthSource(int cam) : - DepthSource("", cam) - { } + DepthSource(int cam) : DepthSource("", cam) {} - DepthSource(String fileListName) : - DepthSource(fileListName, -1) - { } + DepthSource(String fileListName) : DepthSource(fileListName, -1) {} - DepthSource(String fileListName, int cam) : - depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), - frameIdx(0), - undistortMap1(), - undistortMap2() + DepthSource(String fileListName, int cam) + : depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), + frameIdx(0), + undistortMap1(), + undistortMap2() { - if(cam >= 0) + if (cam >= 0) { vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); - if(vc.isOpened()) + if (vc.isOpened()) { sourceType = Type::DEPTH_KINECT2; } else { vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); - if(vc.isOpened()) + if (vc.isOpened()) { sourceType = Type::DEPTH_REALSENSE; } @@ -135,7 +132,7 @@ struct DepthSource } else { - vc = VideoCapture(); + vc = VideoCapture(); sourceType = Type::DEPTH_KINECT2_LIST; } } @@ -160,19 +157,15 @@ struct DepthSource vc.grab(); switch (sourceType) { - case Type::DEPTH_KINECT2: - vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); - break; - case Type::DEPTH_REALSENSE: - vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); - break; - default: - // unknown depth source - vc.retrieve(out); + case Type::DEPTH_KINECT2: vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); break; + case Type::DEPTH_REALSENSE: vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); break; + default: + // unknown depth source + vc.retrieve(out); } // workaround for Kinect 2 - if(sourceType == Type::DEPTH_KINECT2) + if (sourceType == Type::DEPTH_KINECT2) { out = out(Rect(Point(), Kinect2Params::frameSize)); @@ -189,10 +182,7 @@ struct DepthSource return out; } - bool empty() - { - return depthFileList.empty() && !(vc.isOpened()); - } + bool empty() { return depthFileList.empty() && !(vc.isOpened()); } void updateParams(Params& params) { @@ -206,66 +196,63 @@ struct DepthSource float fx, fy, cx, cy; float depthFactor = 1000.f; Size frameSize; - if(sourceType == Type::DEPTH_KINECT2) + if (sourceType == Type::DEPTH_KINECT2) { fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; + cx = Kinect2Params::cx; + cy = Kinect2Params::cy; frameSize = Kinect2Params::frameSize; } else { - if(sourceType == Type::DEPTH_REALSENSE) + if (sourceType == Type::DEPTH_REALSENSE) { - fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); - fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); - depthFactor = 1.f/(float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); + fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); + fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); + depthFactor = 1.f / (float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); } else { - fx = fy = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); + fx = fy = + (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); } - cx = w/2 - 0.5f; - cy = h/2 - 0.5f; + cx = w / 2 - 0.5f; + cy = h / 2 - 0.5f; frameSize = Size(w, h); } - Matx33f camMatrix = Matx33f(fx, 0, cx, - 0, fy, cy, - 0, 0, 1); + Matx33f camMatrix = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); - params.frameSize = frameSize; - params.intr = camMatrix; + params.frameSize = frameSize; + params.intr = camMatrix; params.depthFactor = depthFactor; // RealSense has shorter depth range, some params should be tuned - if(sourceType == Type::DEPTH_REALSENSE) + if (sourceType == Type::DEPTH_REALSENSE) { // all sizes in meters - float cubeSize = 1.f; - params.voxelSize = cubeSize/params.volumeDims[0]; + float cubeSize = 1.f; + params.voxelSize = cubeSize / params.volumeDims[0]; params.tsdf_trunc_dist = 0.01f; - params.icpDistThresh = 0.01f; - params.volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, - -cubeSize/2.f, - 0.05f)); - params.truncateThreshold = 2.5f; + params.icpDistThresh = 0.01f; + params.volumePose = + Affine3f().translate(Vec3f(-cubeSize / 2.f, -cubeSize / 2.f, 0.05f)); + params.truncateThreshold = 2.5f; params.bilateral_sigma_depth = 0.01f; } - if(sourceType == Type::DEPTH_KINECT2) + if (sourceType == Type::DEPTH_KINECT2) { Matx distCoeffs; distCoeffs(0) = Kinect2Params::k1; distCoeffs(1) = Kinect2Params::k2; distCoeffs(4) = Kinect2Params::k3; - initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), - camMatrix, frameSize, CV_16SC2, - undistortMap1, undistortMap2); + initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), camMatrix, frameSize, + CV_16SC2, undistortMap1, undistortMap2); } } } @@ -282,8 +269,7 @@ const std::string vizWindowName = "cloud"; struct PauseCallbackArgs { - PauseCallbackArgs(KinFu& _kf) : kf(_kf) - { } + PauseCallbackArgs(KinFu& _kf) : kf(_kf) {} KinFu& kf; }; @@ -291,9 +277,9 @@ struct PauseCallbackArgs void pauseCallback(const viz::MouseEvent& me, void* args); void pauseCallback(const viz::MouseEvent& me, void* args) { - if(me.type == viz::MouseEvent::Type::MouseMove || - me.type == viz::MouseEvent::Type::MouseScrollDown || - me.type == viz::MouseEvent::Type::MouseScrollUp) + if (me.type == viz::MouseEvent::Type::MouseMove || + me.type == viz::MouseEvent::Type::MouseScrollDown || + me.type == viz::MouseEvent::Type::MouseScrollUp) { PauseCallbackArgs pca = *((PauseCallbackArgs*)(args)); viz::Viz3d window(vizWindowName); @@ -305,54 +291,52 @@ void pauseCallback(const viz::MouseEvent& me, void* args) } #endif -static const char* keys = -{ +static const char* keys = { "{help h usage ? | | print this message }" "{depth | | Path to depth.txt file listing a set of depth images }" "{camera |0| Index of depth camera to be used as a depth source }" "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," - " in coarse mode points and normals are displayed }" + " in coarse mode points and normals are displayed }" "{idle | | Do not run KinFu, just display depth frames }" "{record | | Write depth frames to specified file list" - " (the same format as for the 'depth' key) }" + " (the same format as for the 'depth' key) }" }; static const std::string message = - "\nThis demo uses live depth input or RGB-D dataset taken from" - "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" - "\nto demonstrate KinectFusion implementation \n"; - + "\nThis demo uses live depth input or RGB-D dataset taken from" + "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" + "\nto demonstrate KinectFusion implementation \n"; -int main(int argc, char **argv) +int main(int argc, char** argv) { bool coarse = false; - bool idle = false; + bool idle = false; string recordPath; CommandLineParser parser(argc, argv, keys); parser.about(message); - if(!parser.check()) + if (!parser.check()) { parser.printMessage(); parser.printErrors(); return -1; } - if(parser.has("help")) + if (parser.has("help")) { parser.printMessage(); return 0; } - if(parser.has("coarse")) + if (parser.has("coarse")) { coarse = true; } - if(parser.has("record")) + if (parser.has("record")) { recordPath = parser.get("record"); } - if(parser.has("idle")) + if (parser.has("idle")) { idle = true; } @@ -371,13 +355,13 @@ int main(int argc, char **argv) } Ptr depthWriter; - if(!recordPath.empty()) + if (!recordPath.empty()) depthWriter = makePtr(recordPath); Ptr params; Ptr kf; - if(coarse) + if (coarse) params = Params::coarseParams(); else params = Params::defaultParams(); @@ -389,14 +373,14 @@ int main(int argc, char **argv) cv::setUseOptimized(false); // Scene-specific params should be tuned for each scene individually - //float cubeSize = 1.f; - //params->voxelSize = cubeSize/params->volumeDims[0]; //meters - //params->tsdf_trunc_dist = 0.01f; //meters - //params->icpDistThresh = 0.01f; //meters - //params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters - //params->tsdf_max_weight = 16; - - if(!idle) + // float cubeSize = 1.f; + // params->voxelSize = cubeSize/params->volumeDims[0]; //meters + // params->tsdf_trunc_dist = 0.01f; //meters + // params->icpDistThresh = 0.01f; //meters + // params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); + // //meters params->tsdf_max_weight = 16; + + if (!idle) kf = KinFu::create(params); #ifdef HAVE_OPENCV_VIZ @@ -411,31 +395,33 @@ int main(int argc, char **argv) int64 prevTime = getTickCount(); - for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) + for (UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) { - if(depthWriter) + if (depthWriter) depthWriter->append(frame); #ifdef HAVE_OPENCV_VIZ - if(pause) + if (pause) { // doesn't happen in idle mode kf->getCloud(points, normals); - if(!points.empty() && !normals.empty()) + if (!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); - viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05, + viz::Color::gray()); window.showWidget("cloud", cloudWidget); window.showWidget("normals", cloudNormals); - Vec3d volSize = kf->getParams().voxelSize*Vec3d(kf->getParams().volumeDims); - window.showWidget("cube", viz::WCube(Vec3d::all(0), - volSize), + Vec3d volSize = kf->getParams().voxelSize * Vec3d(kf->getParams().volumeDims); + window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), kf->getParams().volumePose); PauseCallbackArgs pca(*kf); window.registerMouseCallback(pauseCallback, (void*)&pca); - window.showWidget("text", viz::WText(cv::String("Move camera in this window. " - "Close the window or press Q to resume"), Point())); + window.showWidget("text", + viz::WText(cv::String("Move camera in this window. " + "Close the window or press Q to resume"), + Point())); window.spin(); window.removeWidget("text"); window.removeWidget("cloud"); @@ -450,35 +436,35 @@ int main(int argc, char **argv) { UMat cvt8; float depthFactor = params->depthFactor; - convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor); - if(!idle) + convertScaleAbs(frame, cvt8, 0.25 * 256. / depthFactor); + if (!idle) { imshow("depth", cvt8); - if(!kf->update(frame)) + if (!kf->update(frame)) { - /* kf->reset(); */ + kf->reset(); std::cout << "reset" << std::endl; } #ifdef HAVE_OPENCV_VIZ else { - if(coarse) + if (coarse) { kf->getCloud(points, normals); - if(!points.empty() && !normals.empty()) + if (!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); - viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, + /*scale*/ 0.05, viz::Color::gray()); window.showWidget("cloud", cloudWidget); window.showWidget("normals", cloudNormals); } } - //window.showWidget("worldAxes", viz::WCoordinateSystem()); - Vec3d volSize = kf->getParams().voxelSize*kf->getParams().volumeDims; - window.showWidget("cube", viz::WCube(Vec3d::all(0), - volSize), + // window.showWidget("worldAxes", viz::WCoordinateSystem()); + Vec3d volSize = kf->getParams().voxelSize * kf->getParams().volumeDims; + window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), kf->getParams().volumePose); window.setViewerPose(kf->getPose()); window.spinOnce(1, true); @@ -494,9 +480,10 @@ int main(int argc, char **argv) } int64 newTime = getTickCount(); - putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit", - (int)(getTickFrequency()/(newTime - prevTime))), - Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); + putText(rendered, + cv::format("FPS: %2d press R to reset, P to pause, Q to quit", + (int)(getTickFrequency() / (newTime - prevTime))), + Point(0, rendered.rows - 1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); prevTime = newTime; imshow("render", rendered); @@ -504,19 +491,17 @@ int main(int argc, char **argv) int c = waitKey(1); switch (c) { - case 'r': - if(!idle) - kf->reset(); - break; - case 'q': - return 0; + case 'r': + if (!idle) + kf->reset(); + break; + case 'q': return 0; #ifdef HAVE_OPENCV_VIZ - case 'p': - if(!idle) - pause = true; + case 'p': + if (!idle) + pause = true; #endif - default: - break; + default: break; } } diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 6cebe76a20e..aa62e9e5e38 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -2,52 +2,41 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory - -#include "opencv2/core.hpp" -#include "opencv2/core/affine.hpp" -#include "opencv2/core/base.hpp" -#include "opencv2/core/cvdef.h" -#include "opencv2/core/cvstd_wrapper.hpp" -#include "opencv2/core/fast_math.hpp" -#include "opencv2/core/hal/interface.h" -#include "opencv2/core/types.hpp" -#include "opencv2/core/utility.hpp" -#include "opencv2/core/utils/trace.hpp" -#include "opencv2/rgbd/depth.hpp" -#include "precomp.hpp" +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory #include "hash_tsdf.hpp" -#include "opencl_kernels_rgbd.hpp" + #include #include #include #include -#include "depth_to_3d.hpp" -#include "utils.hpp" - -namespace cv { - -namespace kinfu { +#include "opencv2/core/utils/trace.hpp" -HashTSDFVolume::HashTSDFVolume(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, float _truncDist, - int _maxWeight, float _raycastStepFactor, bool _zFirstMemOrder) - : voxelSize(_voxelSize) - , voxelSizeInv(1.0f / _voxelSize) - , pose(_pose) - , maxWeight(_maxWeight) - , raycastStepFactor(_raycastStepFactor) - , volumeUnitResolution(_volume_unit_res) - , volumeUnitSize(voxelSize * volumeUnitResolution) - , zFirstMemOrder(_zFirstMemOrder) +namespace cv +{ +namespace kinfu +{ +HashTSDFVolume::HashTSDFVolume(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, + float _truncDist, int _maxWeight, float _raycastStepFactor, + bool _zFirstMemOrder) + : voxelSize(_voxelSize), + voxelSizeInv(1.0f / _voxelSize), + pose(_pose), + maxWeight(_maxWeight), + raycastStepFactor(_raycastStepFactor), + volumeUnitResolution(_volume_unit_res), + volumeUnitSize(voxelSize * volumeUnitResolution), + zFirstMemOrder(_zFirstMemOrder) { truncDist = std::max(_truncDist, 2.1f * voxelSize); } HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, - float _truncDist, int _maxWeight, - float _raycastStepFactor, bool _zFirstMemOrder) - : HashTSDFVolume(_voxelSize, _volume_unit_res, _pose, _truncDist, _maxWeight, _raycastStepFactor, _zFirstMemOrder) + float _truncDist, int _maxWeight, float _raycastStepFactor, + bool _zFirstMemOrder) + : HashTSDFVolume(_voxelSize, _volume_unit_res, _pose, _truncDist, _maxWeight, + _raycastStepFactor, _zFirstMemOrder) { } @@ -55,83 +44,80 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, int _volume_unit_res, cv: void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); - volume_units_.clear(); + volumeUnits.clear(); } struct AccessedVolumeUnitsInvoker : ParallelLoopBody { AccessedVolumeUnitsInvoker(HashTSDFVolumeCPU& _volume, VolumeUnitIndexSet& _accessVolUnits, - const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose, - float _depthFactor, int _depthStride = 4) : - ParallelLoopBody(), - volume(_volume), - accessVolUnits(_accessVolUnits), - depth(_depth), - reproj(intrinsics.makeReprojector()), - cam2vol(_volume.pose.inv() * cameraPose), - dfac(1.0f/_depthFactor), - depthStride(_depthStride) + const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose, + float _depthFactor, int _depthStride = 4) + : ParallelLoopBody(), + volume(_volume), + accessVolUnits(_accessVolUnits), + depth(_depth), + reproj(intrinsics.makeReprojector()), + cam2vol(_volume.pose.inv() * cameraPose), + depthFactor(1.0f / _depthFactor), + depthStride(_depthStride) { } - virtual void operator() (const Range& range) const override + virtual void operator()(const Range& range) const override { - for(int y = range.start; y < range.end; y += depthStride) + for (int y = range.start; y < range.end; y += depthStride) { - const depthType *depthRow = depth[y]; - for(int x = 0; x < depth.cols; x += depthStride) + const depthType* depthRow = depth[y]; + for (int x = 0; x < depth.cols; x += depthStride) { - depthType z = depthRow[x]*dfac; + depthType z = depthRow[x] * depthFactor; if (z <= 0) continue; Point3f camPoint = reproj(Point3f((float)x, (float)y, z)); Point3f volPoint = cam2vol * camPoint; - /* std::cout << "volPoint" << volPoint << "\n"; */ - //! Find accessed TSDF volume unit for valid 3D vertex cv::Vec3i lower_bound = volume.volumeToVolumeUnitIdx( - volPoint - cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); + volPoint - cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); cv::Vec3i upper_bound = volume.volumeToVolumeUnitIdx( - volPoint + cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); + volPoint + cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); - //! TODO(Akash): Optimize this using differential analyzer algorithm - for(int i = lower_bound[0]; i <= upper_bound[0]; i++) - for(int j = lower_bound[1]; j <= upper_bound[1]; j++) - for(int k = lower_bound[2]; k <= lower_bound[2]; k++) + VolumeUnitIndexSet localAccessVolUnits; + for (int i = lower_bound[0]; i <= upper_bound[0]; i++) + for (int j = lower_bound[1]; j <= upper_bound[1]; j++) + for (int k = lower_bound[2]; k <= lower_bound[2]; k++) { const cv::Vec3i tsdf_idx = cv::Vec3i(i, j, k); - //! If the index does not exist AutoLock al(mutex); - if(!accessVolUnits.count(tsdf_idx)) + if (!localAccessVolUnits.count(tsdf_idx)) { - accessVolUnits.insert(tsdf_idx); - /* std::cout << "CamPoint: " << camPoint << " volPoint: " << volPoint << "\n"; */ - /* std::cout << "Inserted tsdf_idx: (" << tsdf_idx[0] << ", " << tsdf_idx[1] << ", " << tsdf_idx[2] << ")\n"; */ - //! Adds entry to unordered_map - //! and allocate memory for the volume unit - VolumeUnit volumeUnit; - /* std::cout << "Allocated volumeUnit in map" << std::endl; */ - cv::Point3i volumeDims(volume.volumeUnitResolution, - volume.volumeUnitResolution, - volume.volumeUnitResolution); - //! Translate the origin of the subvolume to the correct position in volume coordinate frame - cv::Affine3f subvolumePose = volume.pose.translate(volume.volumeUnitIdxToVolume(tsdf_idx)); - volumeUnit.pVolume = cv::makePtr(volumeDims, - volume.voxelSize, - subvolumePose, - volume.truncDist, - volume.maxWeight, - volume.raycastStepFactor); - volumeUnit.index = tsdf_idx; - volumeUnit.isActive = false; - volume.volume_units_[tsdf_idx] = volumeUnit; + localAccessVolUnits.emplace(tsdf_idx); } } + AutoLock al(mutex); + for (const auto& tsdf_idx : localAccessVolUnits) + { + if (accessVolUnits.emplace(tsdf_idx).second) + { + VolumeUnit volumeUnit; + cv::Point3i volumeDims(volume.volumeUnitResolution, + volume.volumeUnitResolution, + volume.volumeUnitResolution); + + cv::Affine3f subvolumePose = + volume.pose.translate(volume.volumeUnitIdxToVolume(tsdf_idx)); + volumeUnit.pVolume = cv::makePtr( + volumeDims, volume.voxelSize, subvolumePose, volume.truncDist, + volume.maxWeight, volume.raycastStepFactor); + //! This volume unit will definitely be required for current integration + volumeUnit.index = tsdf_idx; + volumeUnit.isActive = true; + volume.volumeUnits[tsdf_idx] = volumeUnit; + } + } } } - } HashTSDFVolumeCPU& volume; @@ -139,7 +125,7 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody const Depth& depth; const Intr::Reprojector reproj; const cv::Affine3f cam2vol; - const float dfac; + const float depthFactor; const int depthStride; mutable Mutex mutex; }; @@ -147,35 +133,35 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody struct IntegrateSubvolumeInvoker : ParallelLoopBody { IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, std::vector _totalVolUnits, - const Depth& _depth, Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : - ParallelLoopBody(), - volume(_volume), - totalVolUnits(_totalVolUnits), - depth(_depth), - depthFactor(_depthFactor), - cameraPose(_cameraPose), - intrinsics(_intrinsics) + const Depth& _depth, Intr _intrinsics, cv::Affine3f _cameraPose, + float _depthFactor) + : ParallelLoopBody(), + volume(_volume), + totalVolUnits(_totalVolUnits), + depth(_depth), + depthFactor(_depthFactor), + cameraPose(_cameraPose), + intrinsics(_intrinsics) { } - virtual void operator() (const Range& range) const override + virtual void operator()(const Range& range) const override { for (int i = range.start; i < range.end; i++) { - cv::Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitMap::iterator it = volume.volume_units_.find(tsdf_idx); - if(it == volume.volume_units_.end()) + cv::Vec3i tsdf_idx = totalVolUnits[i]; + VolumeUnitMap::iterator it = volume.volumeUnits.find(tsdf_idx); + if (it == volume.volumeUnits.end()) return; VolumeUnit& volumeUnit = it->second; - /* if(volumeUnit.isActive) */ - /* { */ - /* std::cout << "Integrating unit: " << it->first << std::endl; */ + if (volumeUnit.isActive) + { //! The volume unit should already be added into the Volume from the allocator volumeUnit.pVolume->integrate(depth, depthFactor, cameraPose, intrinsics); //! Ensure all active volumeUnits are set to inactive for next integration volumeUnit.isActive = false; - /* } */ + } } } @@ -190,42 +176,41 @@ struct IntegrateSubvolumeInvoker : ParallelLoopBody struct VolumeUnitInFrustumInvoker : ParallelLoopBody { - VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, const std::vector& _totalVolUnits, const Depth& _depth, Intr _intrinsics, - cv::Affine3f _cameraPose, float _depthFactor) : - ParallelLoopBody(), - volume(_volume), - totalVolUnits(_totalVolUnits), - depth(_depth), - proj(_intrinsics.makeProjector()), - cameraPose(_cameraPose), - depthFactor(_depthFactor), - vol2cam(cameraPose.inv() * _volume.pose) + VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, + const std::vector& _totalVolUnits, const Depth& _depth, + Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) + : ParallelLoopBody(), + volume(_volume), + totalVolUnits(_totalVolUnits), + depth(_depth), + proj(_intrinsics.makeProjector()), + depthFactor(_depthFactor), + vol2cam(_cameraPose.inv() * _volume.pose) { } - virtual void operator() (const Range& range) const override + virtual void operator()(const Range& range) const override { for (int i = range.start; i < range.end; ++i) { - cv::Vec3i tsdf_idx = totalVolUnits[i]; - - VolumeUnitMap::iterator it = volume.volume_units_.find(tsdf_idx); - if(it == volume.volume_units_.end()) + cv::Vec3i tsdf_idx = totalVolUnits[i]; + VolumeUnitMap::iterator it = volume.volumeUnits.find(tsdf_idx); + if (it == volume.volumeUnits.end()) return; - Point3f volumeUnitPos = volume.volumeUnitIdxToVolume(it->first); - if (volumeUnitPos.z < rgbd::Odometry::DEFAULT_MIN_DEPTH() || - volumeUnitPos.z > rgbd::Odometry::DEFAULT_MAX_DEPTH()) + Point3f volumeUnitPos = volume.volumeUnitIdxToVolume(it->first); + Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; + if (volUnitInCamSpace.z < rgbd::Odometry::DEFAULT_MIN_DEPTH() || + volUnitInCamSpace.z > rgbd::Odometry::DEFAULT_MAX_DEPTH()) { it->second.isActive = false; return; } - Point2f cameraPoint = proj(vol2cam * volumeUnitPos); - /* std::cout << i << " VolumeUnitPos: " << volumeUnitPos << " cameraPoint: " << cameraPoint << "\n"; */ - if(cameraPoint.x >= 0 && cameraPoint.y >= 0 && - cameraPoint.x < (depth.cols - 1) && cameraPoint.y < (depth.rows - 1)) + Point2f cameraPoint = proj(volUnitInCamSpace); + if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && + cameraPoint.y < depth.rows) { - assert(it != volume.volume_units_.end()); + assert(it != volume.volumeUnits.end()); it->second.isActive = true; } } @@ -234,13 +219,13 @@ struct VolumeUnitInFrustumInvoker : ParallelLoopBody const std::vector totalVolUnits; const Depth& depth; const Intr::Projector proj; - const Affine3f cameraPose; const float depthFactor; const Affine3f vol2cam; mutable Mutex mutex; }; -void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, Intr intrinsics) +void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, + Intr intrinsics) { CV_TRACE_FUNCTION(); @@ -248,96 +233,91 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affi Depth depth = _depth.getMat(); VolumeUnitIndexSet accessVolUnits; - //TODO(Akash): Consider reusing pyrPoints and transform the points - AccessedVolumeUnitsInvoker allocate_i(*this, accessVolUnits, depth, intrinsics, cameraPose, depthFactor); + AccessedVolumeUnitsInvoker allocate_i(*this, accessVolUnits, depth, intrinsics, cameraPose, + depthFactor); Range range(0, depth.rows); parallel_for_(range, allocate_i); + std::vector totalVolUnits; - for(const auto& keyvalue : volume_units_) + for (const auto& keyvalue : volumeUnits) { totalVolUnits.push_back(keyvalue.first); } - std::cout << "Number of VolumeUnits in map: " << totalVolUnits.size() << "\n"; - VolumeUnitInFrustumInvoker infrustum_i(*this, totalVolUnits, depth, intrinsics, cameraPose, depthFactor); - Range in_frustum_range(0, volume_units_.size()); + + VolumeUnitInFrustumInvoker infrustum_i(*this, totalVolUnits, depth, intrinsics, cameraPose, + depthFactor); + Range in_frustum_range(0, volumeUnits.size()); parallel_for_(in_frustum_range, infrustum_i); - /* std::cout << "Number of subvolumes in Frustum: " << unitsInFrustum << " Waiting:\n"; */ - /* std::cin >> input; */ - IntegrateSubvolumeInvoker integrate_i(*this, totalVolUnits, depth, intrinsics, cameraPose, depthFactor); + + IntegrateSubvolumeInvoker integrate_i(*this, totalVolUnits, depth, intrinsics, cameraPose, + depthFactor); Range accessed_units_range(0, totalVolUnits.size()); parallel_for_(accessed_units_range, integrate_i); - /* std::cout << "Integration complete \n"; */ } cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx(cv::Point3f p) const { - return cv::Vec3i(cvFloor(p.x / volumeUnitSize), - cvFloor(p.y / volumeUnitSize), + return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), cvFloor(p.z / volumeUnitSize)); } cv::Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const { - return cv::Point3f(volumeUnitIdx[0] * volumeUnitSize, - volumeUnitIdx[1] * volumeUnitSize, + return cv::Point3f(volumeUnitIdx[0] * volumeUnitSize, volumeUnitIdx[1] * volumeUnitSize, volumeUnitIdx[2] * volumeUnitSize); } cv::Point3f HashTSDFVolumeCPU::voxelCoordToVolume(cv::Vec3i voxelIdx) const { - return cv::Point3f(voxelIdx[0] * voxelSize, - voxelIdx[1] * voxelSize, - voxelIdx[2] * voxelSize); + return cv::Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); } cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const { - return cv::Vec3i(cvFloor(point.x * voxelSizeInv), - cvFloor(point.y * voxelSizeInv), + return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } -inline Voxel HashTSDFVolumeCPU::at(const cv::Vec3i &volumeIdx) const +inline Voxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { - cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), - cvFloor(volumeIdx[1] / volumeUnitResolution), - cvFloor(volumeIdx[2] / volumeUnitResolution)); + cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), + cvFloor(volumeIdx[1] / volumeUnitResolution), + cvFloor(volumeIdx[2] / volumeUnitResolution)); cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, volumeUnitIdx[2] * volumeUnitResolution); - /* std::cout << "VolumeUnitIdx: " << volumeUnitIdx << "\n"; */ - /* std::cout << "subvolumeCoords: " << subVolumeCoords << "\n"; */ - VolumeUnitMap::const_iterator it = volume_units_.find(volumeUnitIdx); - if(it == volume_units_.end()) + VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); + if (it == volumeUnits.end()) { Voxel dummy; - dummy.tsdf = 0.f; dummy.weight = 0; + dummy.tsdf = 0.f; + dummy.weight = 0; return dummy; } - cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + cv::Ptr volumeUnit = + std::dynamic_pointer_cast(it->second.pVolume); return volumeUnit->at(volUnitLocalIdx); } -inline Voxel HashTSDFVolumeCPU::at(const cv::Point3f &point) const +inline Voxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const { - cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); - cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); - cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); - VolumeUnitMap::const_iterator it = volume_units_.find(volumeUnitIdx); - if(it == volume_units_.end()) + cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); + cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); + cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); + VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); + if (it == volumeUnits.end()) { Voxel dummy; - dummy.tsdf = 0; dummy.weight = 0; + dummy.tsdf = 0; + dummy.weight = 0; return dummy; } - cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - /* std::cout << "volumeUnitIdx: " << volumeUnitIdx << "volUnitLocalIdx: " << volUnitLocalIdx << std::endl; */ - /* std::cout << volumeUnit->at(volUnitLocalIdx).tsdf << std::endl; */ + cv::Ptr volumeUnit = + std::dynamic_pointer_cast(it->second.pVolume); return volumeUnit->at(volUnitLocalIdx); } - inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(cv::Point3f p) const { int ix = cvFloor(p.x); @@ -360,33 +340,33 @@ inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(cv::Point3f p) const vx[7] = at(cv::Vec3i(1, 1, 1) + cv::Vec3i(ix, iy, iz)).tsdf; /* std::cout << "tsdf 7th: " << vx[7] << "\n"; */ - TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); - TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); - TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); - TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); + TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); - TsdfType v0 = v00 + ty*(v01 - v00); - TsdfType v1 = v10 + ty*(v11 - v10); + TsdfType v0 = v00 + ty * (v01 - v00); + TsdfType v1 = v10 + ty * (v11 - v10); - return v0 + tx*(v1 - v0); + return v0 + tx * (v1 - v0); } inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const { - Vec3f pointVec(point); Vec3f normal = Vec3f(0, 0, 0); Vec3f pointPrev = point; Vec3f pointNext = point; - for(int c = 0; c < 3; c++) + for (int c = 0; c < 3; c++) { - pointPrev[c] -= voxelSize*0.5; - pointNext[c] += voxelSize*0.5; + pointPrev[c] -= voxelSize * 0.5; + pointNext[c] += voxelSize * 0.5; - normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; - /* std::cout << "pointPrev, pointNext: " << at(Point3f(pointPrev)).tsdf << ", " << at(Point3f(pointNext)).tsdf << std::endl; */ + normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; + /* std::cout << "pointPrev, pointNext: " << at(Point3f(pointPrev)).tsdf << ", " << + * at(Point3f(pointNext)).tsdf << std::endl; */ normal[c] *= 0.5; /* std::cout << "TSDF diff: " << normal[c] << std::endl; */ @@ -414,14 +394,22 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const /* coordBase0[c] -= 1; */ /* coordBase1[c] += 1; */ - /* vx[0] = at(cv::Vec3i(0, 0, 0) + coordBase1).tsdf - at(cv::Vec3i(0, 0, 0) + coordBase0).tsdf; */ - /* vx[1] = at(cv::Vec3i(0, 0, 1) + coordBase1).tsdf - at(cv::Vec3i(0, 0, 1) + coordBase0).tsdf; */ - /* vx[2] = at(cv::Vec3i(0, 1, 0) + coordBase1).tsdf - at(cv::Vec3i(0, 1, 0) + coordBase0).tsdf; */ - /* vx[3] = at(cv::Vec3i(0, 1, 1) + coordBase1).tsdf - at(cv::Vec3i(0, 1, 1) + coordBase0).tsdf; */ - /* vx[4] = at(cv::Vec3i(1, 0, 0) + coordBase1).tsdf - at(cv::Vec3i(1, 0, 0) + coordBase0).tsdf; */ - /* vx[5] = at(cv::Vec3i(1, 0, 1) + coordBase1).tsdf - at(cv::Vec3i(1, 0, 1) + coordBase0).tsdf; */ - /* vx[6] = at(cv::Vec3i(1, 1, 0) + coordBase1).tsdf - at(cv::Vec3i(1, 1, 0) + coordBase0).tsdf; */ - /* vx[7] = at(cv::Vec3i(1, 1, 1) + coordBase1).tsdf - at(cv::Vec3i(1, 1, 1) + coordBase0).tsdf; */ + /* vx[0] = at(cv::Vec3i(0, 0, 0) + coordBase1).tsdf - at(cv::Vec3i(0, 0, 0) + + * coordBase0).tsdf; */ + /* vx[1] = at(cv::Vec3i(0, 0, 1) + coordBase1).tsdf - at(cv::Vec3i(0, 0, 1) + + * coordBase0).tsdf; */ + /* vx[2] = at(cv::Vec3i(0, 1, 0) + coordBase1).tsdf - at(cv::Vec3i(0, 1, 0) + + * coordBase0).tsdf; */ + /* vx[3] = at(cv::Vec3i(0, 1, 1) + coordBase1).tsdf - at(cv::Vec3i(0, 1, 1) + + * coordBase0).tsdf; */ + /* vx[4] = at(cv::Vec3i(1, 0, 0) + coordBase1).tsdf - at(cv::Vec3i(1, 0, 0) + + * coordBase0).tsdf; */ + /* vx[5] = at(cv::Vec3i(1, 0, 1) + coordBase1).tsdf - at(cv::Vec3i(1, 0, 1) + + * coordBase0).tsdf; */ + /* vx[6] = at(cv::Vec3i(1, 1, 0) + coordBase1).tsdf - at(cv::Vec3i(1, 1, 0) + + * coordBase0).tsdf; */ + /* vx[7] = at(cv::Vec3i(1, 1, 1) + coordBase1).tsdf - at(cv::Vec3i(1, 1, 1) + + * coordBase0).tsdf; */ /* TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); */ /* TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); */ @@ -437,21 +425,22 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const /* return normalize(an); */ } - struct RaycastInvoker : ParallelLoopBody { - RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, Intr intrinsics, const HashTSDFVolumeCPU& _volume) : - ParallelLoopBody(), - points(_points), - normals(_normals), - volume(_volume), - tstep(_volume.truncDist * _volume.raycastStepFactor), - cam2vol(volume.pose.inv() * cameraPose), - vol2cam(cameraPose.inv() * volume.pose), - reproj(intrinsics.makeReprojector()) - {} - - virtual void operator() (const Range& range) const override + RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, Intr intrinsics, + const HashTSDFVolumeCPU& _volume) + : ParallelLoopBody(), + points(_points), + normals(_normals), + volume(_volume), + tstep(_volume.truncDist * _volume.raycastStepFactor), + cam2vol(volume.pose.inv() * cameraPose), + vol2cam(cameraPose.inv() * volume.pose), + reproj(intrinsics.makeReprojector()) + { + } + + virtual void operator()(const Range& range) const override { const Point3f cam2volTrans = cam2vol.translation(); const Matx33f cam2volRot = cam2vol.rotation(); @@ -461,99 +450,78 @@ struct RaycastInvoker : ParallelLoopBody for (int y = range.start; y < range.end; y++) { - ptype* ptsRow = points[y]; ptype* nrmRow = normals[y]; - for(int x = 0; x < points.cols; x++) + for (int x = 0; x < points.cols; x++) { Point3f point = nan3, normal = nan3; - //! Ray origin in the volume coordinate frame - Point3f orig = cam2volTrans; - //! Ray direction in the volume coordinate frame + //! Ray origin and direction in the volume coordinate frame + Point3f orig = cam2volTrans; Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(x, y, 1.f)))); - float tmin = rgbd::Odometry::DEFAULT_MIN_DEPTH()/rayDirV.z; - float tmax = rgbd::Odometry::DEFAULT_MAX_DEPTH()/rayDirV.z; - /* std::cout << "tmin, tmax :" << tmin << ", " << tmax << "\n"; */ - /* std::cout << "Origin: " << orig << " rayDirection: " << rayDirV << "\n"; */ + float tmin = rgbd::Odometry::DEFAULT_MIN_DEPTH() / rayDirV.z; + float tmax = rgbd::Odometry::DEFAULT_MAX_DEPTH() / rayDirV.z; + float tprev = tmin; float tcurr = tmin; - //! Is this a reasonable initialization? + TsdfType prevTsdf = volume.truncDist; - cv::Vec3i prevVolumeUnitIdx = cv::Vec3i(std::numeric_limits::min(), - std::numeric_limits::min(), - std::numeric_limits::min()); + cv::Vec3i prevVolumeUnitIdx = + cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), + std::numeric_limits::min()); cv::Ptr currVolumeUnit; + while (tcurr < tmax) + { + Point3f currRayPos = orig + tcurr * rayDirV; + cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - /* Point3f initRayPos = orig + tmin */ - /* cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); */ - /* cv::Vec3f rayDeltaDistance(1/rayDirV.x, 1/rayDirV.y, 1/rayDirV.z); */ - + VolumeUnitMap::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); - while(tcurr < tmax) - { - Point3f currRayPos = orig + tcurr * rayDirV; - cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); - /* std::cout << "tCurr: " << tcurr << "\n"; */ - /* std::cout << "Current Ray cast position: " << currRayPos << "\n"; */ - /* std::cout << "Previous volume unit Index: " << prevVolumeUnitIdx << "\n"; */ - /* std::cout << "Current volume unit Index: " << currVolumeUnitIdx << "\n"; */ - - VolumeUnitMap::const_iterator it = volume.volume_units_.find(currVolumeUnitIdx); TsdfType currTsdf = prevTsdf; - int currWeight = 0; - float stepSize = blockSize; + int currWeight = 0; + float stepSize = tstep; cv::Vec3i volUnitLocalIdx; + //! Does the subvolume exist in hashtable - if(it != volume.volume_units_.end()) + if (it != volume.volumeUnits.end()) { - currVolumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); - /* std::cout << "currRaypos: " << currRayPos << " currVolUnitPos: " << currVolUnitPos << "\n"; */ + currVolumeUnit = + std::dynamic_pointer_cast(it->second.pVolume); + cv::Point3f currVolUnitPos = + volume.volumeUnitIdxToVolume(currVolumeUnitIdx); volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); - //! Figure out voxel interpolation + //! TODO: Figure out voxel interpolation Voxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); - currTsdf = currVoxel.tsdf; - currWeight = currVoxel.weight; - stepSize = std::max(tstep, currTsdf * volume.truncDist); - - + currTsdf = currVoxel.tsdf; + currWeight = currVoxel.weight; + /* stepSize = tstep; */ } //! Surface crossing - if(prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) + if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) { - /* std::cout << "Current pixel: (" << x << ", " << y << ")\n"; */ - /* std::cout << "subvolume coords: " << volUnitLocalIdx << "\n"; */ - /* std::cout << "current TSDF: " << currTsdf << "\n"; */ - /* std::cout << "current weight: " << currWeight << "\n"; */ - /* std::cout << "previous TSDF: " << prevTsdf << "\n"; */ - float tInterp = (tcurr * prevTsdf - tprev * currTsdf)/(prevTsdf - currTsdf); - - if(!cvIsNaN(tInterp) && !cvIsInf(tInterp)) + float tInterp = + (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); + + if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { Point3f pv = orig + tInterp * rayDirV; Point3f nv = volume.getNormalVoxel(pv); - /* std::cout << "normal: " << nv << std::endl; */ - if(!isNaN(nv)) + if (!isNaN(nv)) { normal = vol2camRot * nv; - point = vol2cam * pv; - - /* std::cout << "Point: " << point << "\n"; */ - /* std::cout << "normal: " << normal << "\n"; */ + point = vol2cam * pv; break; } } } - /* std::cout << "stepSize: " << stepSize << "\n"; */ prevVolumeUnitIdx = currVolumeUnitIdx; prevTsdf = currTsdf; tprev = tcurr; - tcurr += stepSize; + tcurr += stepSize; } ptsRow[x] = toPtype(point); nrmRow[x] = toPtype(normal); @@ -571,30 +539,31 @@ struct RaycastInvoker : ParallelLoopBody }; void HashTSDFVolumeCPU::raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, - cv::Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const + cv::Size frameSize, cv::OutputArray _points, + cv::OutputArray _normals) const { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); - _points.create (frameSize, POINT_TYPE); - _normals.create (frameSize, POINT_TYPE); + _points.create(frameSize, POINT_TYPE); + _normals.create(frameSize, POINT_TYPE); - Points points = _points.getMat(); + Points points = _points.getMat(); Normals normals = _normals.getMat(); RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); const int nstripes = -1; parallel_for_(Range(0, points.rows), ri, nstripes); - } -cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, int volumeUnitResolution) +cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, + int _maxWeight, float _raycastStepFactor, + int volumeUnitResolution) { - return cv::makePtr(_voxelSize, volumeUnitResolution, _pose, _truncDist, _maxWeight, _raycastStepFactor); + return cv::makePtr(_voxelSize, volumeUnitResolution, _pose, _truncDist, + _maxWeight, _raycastStepFactor); } -} // namespace kinfu -} // namespace cv - +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 4a5f5b8dccf..92f99621579 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -9,8 +9,8 @@ #include #include + #include "opencv2/core/affine.hpp" -#include "kinfu_frame.hpp" #include "tsdf.hpp" namespace cv { @@ -94,7 +94,11 @@ class HashTSDFVolumeCPU : public HashTSDFVolume /* virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; */ virtual void reset() override; + + //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) virtual Voxel at(const cv::Vec3i &volumeIdx) const; + + //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = 1m) virtual Voxel at(const cv::Point3f &point) const; TsdfType interpolateVoxel(cv::Point3f p) const; @@ -107,11 +111,9 @@ class HashTSDFVolumeCPU : public HashTSDFVolume cv::Point3f voxelCoordToVolume(cv::Vec3i voxelIdx) const; cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const; - -//! TODO: Make this private public: //! Hashtable of individual smaller volume units - VolumeUnitMap volume_units_; + VolumeUnitMap volumeUnits; }; cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index ef832b50745..05389271730 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -249,20 +249,11 @@ bool KinFuImpl::updateT(const T& _depth) if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement) { // use depth instead of distance - std::cout << "Starting integration: " << std::endl; volume->integrate(depth, params.depthFactor, pose, params.intr); - std::cout << "Completed integration: " << frameCounter << std::endl; } - /* int number; */ - /* std::cout << "Waiting: " << std::endl; */ - /* std::cin >> number; */ T& points = pyrPoints [0]; T& normals = pyrNormals[0]; volume->raycast(pose, params.intr, params.frameSize, points, normals); - /* std::cout << "Raycast complete \n"; */ - /* std::cout << "Waiting: " << std::endl; */ - /* std::cin >> number; */ - // build a pyramid of points and normals buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, params.pyramidLevels); std::cout << "Built Point normal pyramids\n"; diff --git a/modules/rgbd/src/utils.hpp b/modules/rgbd/src/utils.hpp index 6df1af86785..2662a973d6c 100644 --- a/modules/rgbd/src/utils.hpp +++ b/modules/rgbd/src/utils.hpp @@ -10,6 +10,7 @@ #define __OPENCV_RGBD_UTILS_HPP__ #include "opencv2/core/matx.hpp" +#include "opencv2/rgbd/depth.hpp" namespace cv { namespace rgbd @@ -23,6 +24,8 @@ namespace rgbd * @param the desired output depth (floats or double) * @param out The rescaled float depth image */ +/* void rescaleDepth(InputArray in_in, int depth, OutputArray out_out); */ + template void rescaleDepthTemplated(const Mat& in, Mat& out); From bdbafd0b611a88cd1e261bbff1c1f1c31ec30074 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 9 Jun 2020 23:47:49 -0400 Subject: [PATCH 06/36] Add Kinect Fusion backup file --- modules/rgbd/src/kinfu_back.cpp | 333 ++++++++++++++++++++++++++++++++ 1 file changed, 333 insertions(+) create mode 100644 modules/rgbd/src/kinfu_back.cpp diff --git a/modules/rgbd/src/kinfu_back.cpp b/modules/rgbd/src/kinfu_back.cpp new file mode 100644 index 00000000000..b58679c6f43 --- /dev/null +++ b/modules/rgbd/src/kinfu_back.cpp @@ -0,0 +1,333 @@ +/* // This file is part of OpenCV project. */ +/* // It is subject to the license terms in the LICENSE file found in the top-level directory */ +/* // of this distribution and at http://opencv.org/license.html */ + +/* // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory */ + +/* #include "precomp.hpp" */ +/* #include "fast_icp.hpp" */ +/* #include "tsdf.hpp" */ +/* #include "kinfu_frame.hpp" */ + +/* namespace cv { */ +/* namespace kinfu { */ + +/* void Params::setInitialVolumePose(Matx33f R, Vec3f t) */ +/* { */ +/* setInitialVolumePose(Affine3f(R,t).matrix); */ +/* } */ + +/* void Params::setInitialVolumePose(Matx44f homogen_tf) */ +/* { */ +/* Params::volumePose.matrix = homogen_tf; */ +/* } */ + +/* Ptr Params::defaultParams() */ +/* { */ +/* Params p; */ + +/* p.frameSize = Size(640, 480); */ + +/* float fx, fy, cx, cy; */ +/* fx = fy = 525.f; */ +/* cx = p.frameSize.width/2 - 0.5f; */ +/* cy = p.frameSize.height/2 - 0.5f; */ +/* p.intr = Matx33f(fx, 0, cx, */ +/* 0, fy, cy, */ +/* 0, 0, 1); */ + +/* // 5000 for the 16-bit PNG files */ +/* // 1 for the 32-bit float images in the ROS bag files */ +/* p.depthFactor = 5000; */ + +/* // sigma_depth is scaled by depthFactor when calling bilateral filter */ +/* p.bilateral_sigma_depth = 0.04f; //meter */ +/* p.bilateral_sigma_spatial = 4.5; //pixels */ +/* p.bilateral_kernel_size = 7; //pixels */ + +/* p.icpAngleThresh = (float)(30. * CV_PI / 180.); // radians */ +/* p.icpDistThresh = 0.1f; // meters */ + +/* p.icpIterations = {10, 5, 4}; */ +/* p.pyramidLevels = (int)p.icpIterations.size(); */ + +/* p.tsdf_min_camera_movement = 0.f; //meters, disabled */ + +/* p.volumeDims = Vec3i::all(512); //number of voxels */ + +/* float volSize = 3.f; */ +/* p.voxelSize = volSize/512.f; //meters */ + +/* // default pose of volume cube */ +/* p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f)); */ +/* p.tsdf_trunc_dist = 0.04f; //meters; */ +/* p.tsdf_max_weight = 64; //frames */ + +/* p.raycast_step_factor = 0.25f; //in voxel sizes */ +/* // gradient delta factor is fixed at 1.0f and is not used */ +/* //p.gradient_delta_factor = 0.5f; //in voxel sizes */ + +/* //p.lightPose = p.volume_pose.translation()/4; //meters */ +/* p.lightPose = Vec3f::all(0.f); //meters */ + +/* // depth truncation is not used by default but can be useful in some scenes */ +/* p.truncateThreshold = 0.f; //meters */ + +/* return makePtr(p); */ +/* } */ + +/* Ptr Params::coarseParams() */ +/* { */ +/* Ptr p = defaultParams(); */ + +/* p->icpIterations = {5, 3, 2}; */ +/* p->pyramidLevels = (int)p->icpIterations.size(); */ + +/* float volSize = 3.f; */ +/* p->volumeDims = Vec3i::all(128); //number of voxels */ +/* p->voxelSize = volSize/128.f; */ + +/* p->raycast_step_factor = 0.75f; //in voxel sizes */ + +/* return p; */ +/* } */ + +/* // T should be Mat or UMat */ +/* template< typename T > */ +/* class KinFuImpl : public KinFu */ +/* { */ +/* public: */ +/* KinFuImpl(const Params& _params); */ +/* virtual ~KinFuImpl(); */ + +/* const Params& getParams() const CV_OVERRIDE; */ + +/* void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE; */ + +/* void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE; */ +/* void getPoints(OutputArray points) const CV_OVERRIDE; */ +/* void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; */ + +/* void reset() CV_OVERRIDE; */ + +/* const Affine3f getPose() const CV_OVERRIDE; */ + +/* bool update(InputArray depth) CV_OVERRIDE; */ + +/* bool updateT(const T& depth); */ + +/* private: */ +/* Params params; */ + +/* cv::Ptr icp; */ +/* cv::Ptr volume; */ + +/* int frameCounter; */ +/* Affine3f pose; */ +/* std::vector pyrPoints; */ +/* std::vector pyrNormals; */ +/* }; */ + + +/* template< typename T > */ +/* KinFuImpl::KinFuImpl(const Params &_params) : */ +/* params(_params), */ +/* icp(makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)), */ +/* volume(makeTSDFVolume(params.volumeDims, params.voxelSize, params.volumePose, */ +/* params.tsdf_trunc_dist, params.tsdf_max_weight, */ +/* params.raycast_step_factor)), */ +/* pyrPoints(), pyrNormals() */ +/* { */ +/* reset(); */ +/* } */ + +/* template< typename T > */ +/* void KinFuImpl::reset() */ +/* { */ +/* frameCounter = 0; */ +/* pose = Affine3f::Identity(); */ +/* volume->reset(); */ +/* } */ + +/* template< typename T > */ +/* KinFuImpl::~KinFuImpl() */ +/* { } */ + +/* template< typename T > */ +/* const Params& KinFuImpl::getParams() const */ +/* { */ +/* return params; */ +/* } */ + +/* template< typename T > */ +/* const Affine3f KinFuImpl::getPose() const */ +/* { */ +/* return pose; */ +/* } */ + + +/* template<> */ +/* bool KinFuImpl::update(InputArray _depth) */ +/* { */ +/* CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); */ + +/* Mat depth; */ +/* if(_depth.isUMat()) */ +/* { */ +/* _depth.copyTo(depth); */ +/* return updateT(depth); */ +/* } */ +/* else */ +/* { */ +/* return updateT(_depth.getMat()); */ +/* } */ +/* } */ + + +/* template<> */ +/* bool KinFuImpl::update(InputArray _depth) */ +/* { */ +/* CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); */ + +/* UMat depth; */ +/* if(!_depth.isUMat()) */ +/* { */ +/* _depth.copyTo(depth); */ +/* return updateT(depth); */ +/* } */ +/* else */ +/* { */ +/* return updateT(_depth.getUMat()); */ +/* } */ +/* } */ + + +/* template< typename T > */ +/* bool KinFuImpl::updateT(const T& _depth) */ +/* { */ +/* CV_TRACE_FUNCTION(); */ + +/* T depth; */ +/* if(_depth.type() != DEPTH_TYPE) */ +/* _depth.convertTo(depth, DEPTH_TYPE); */ +/* else */ +/* depth = _depth; */ + +/* std::vector newPoints, newNormals; */ +/* makeFrameFromDepth(depth, newPoints, newNormals, params.intr, */ +/* params.pyramidLevels, */ +/* params.depthFactor, */ +/* params.bilateral_sigma_depth, */ +/* params.bilateral_sigma_spatial, */ +/* params.bilateral_kernel_size, */ +/* params.truncateThreshold); */ + +/* if(frameCounter == 0) */ +/* { */ +/* // use depth instead of distance */ +/* volume->integrate(depth, params.depthFactor, pose, params.intr); */ + +/* pyrPoints = newPoints; */ +/* pyrNormals = newNormals; */ +/* } */ +/* else */ +/* { */ +/* Affine3f affine; */ +/* bool success = icp->estimateTransform(affine, pyrPoints, pyrNormals, newPoints, newNormals); */ +/* if(!success) */ +/* return false; */ + +/* pose = pose * affine; */ + +/* float rnorm = (float)cv::norm(affine.rvec()); */ +/* float tnorm = (float)cv::norm(affine.translation()); */ +/* // We do not integrate volume if camera does not move */ +/* if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement) */ +/* { */ +/* // use depth instead of distance */ +/* volume->integrate(depth, params.depthFactor, pose, params.intr); */ +/* } */ + +/* T& points = pyrPoints [0]; */ +/* T& normals = pyrNormals[0]; */ +/* volume->raycast(pose, params.intr, params.frameSize, points, normals); */ +/* // build a pyramid of points and normals */ +/* buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, */ +/* params.pyramidLevels); */ +/* } */ + +/* frameCounter++; */ +/* return true; */ +/* } */ + + +/* template< typename T > */ +/* void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const */ +/* { */ +/* CV_TRACE_FUNCTION(); */ + +/* Affine3f cameraPose(_cameraPose); */ + +/* const Affine3f id = Affine3f::Identity(); */ +/* if((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) || */ +/* (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) */ +/* { */ +/* renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose); */ +/* } */ +/* else */ +/* { */ +/* T points, normals; */ +/* volume->raycast(cameraPose, params.intr, params.frameSize, points, normals); */ +/* renderPointsNormals(points, normals, image, params.lightPose); */ +/* } */ +/* } */ + + +/* template< typename T > */ +/* void KinFuImpl::getCloud(OutputArray p, OutputArray n) const */ +/* { */ +/* volume->fetchPointsNormals(p, n); */ +/* } */ + + +/* template< typename T > */ +/* void KinFuImpl::getPoints(OutputArray points) const */ +/* { */ +/* volume->fetchPointsNormals(points, noArray()); */ +/* } */ + + +/* template< typename T > */ +/* void KinFuImpl::getNormals(InputArray points, OutputArray normals) const */ +/* { */ +/* volume->fetchNormals(points, normals); */ +/* } */ + +/* // importing class */ + +/* #ifdef OPENCV_ENABLE_NONFREE */ + +/* Ptr KinFu::create(const Ptr& params) */ +/* { */ +/* CV_Assert((int)params->icpIterations.size() == params->pyramidLevels); */ +/* CV_Assert(params->intr(0,1) == 0 && params->intr(1,0) == 0 && params->intr(2,0) == 0 && params->intr(2,1) == 0 && params->intr(2,2) == 1); */ +/* #ifdef HAVE_OPENCL */ +/* if(cv::ocl::useOpenCL()) */ +/* return makePtr< KinFuImpl >(*params); */ +/* #endif */ +/* return makePtr< KinFuImpl >(*params); */ +/* } */ + +/* #else */ +/* Ptr KinFu::create(const Ptr& params) */ +/* { */ +/* CV_Error(Error::StsNotImplemented, */ +/* "This algorithm is patented and is excluded in this configuration; " */ +/* "Set OPENCV_ENABLE_NONFREE CMake option and rebuild the library"); */ +/* } */ +/* #endif */ + +/* KinFu::~KinFu() {} */ + +/* } // namespace kinfu */ +/* } // namespace cv */ From 70b1dfbd3ebbc99391a8dced6192139bd7d84e91 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Wed, 10 Jun 2020 14:00:26 -0400 Subject: [PATCH 07/36] - Add interpolation for vertices and normals (slow and unreliable!) - Format code - Delete kinfu_back.cpp --- modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 2 +- modules/rgbd/samples/kinfu_demo.cpp | 4 +- modules/rgbd/src/hash_tsdf.cpp | 337 +++++-- modules/rgbd/src/hash_tsdf.hpp | 8 +- modules/rgbd/src/kinfu.cpp | 18 +- modules/rgbd/src/kinfu_back.cpp | 333 ------- modules/rgbd/src/tsdf.cpp | 966 ++++++++++---------- modules/rgbd/src/tsdf.hpp | 1 + 8 files changed, 725 insertions(+), 944 deletions(-) delete mode 100644 modules/rgbd/src/kinfu_back.cpp diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 9589a00bf61..d91c62f91bb 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -202,7 +202,7 @@ class CV_EXPORTS_W KinFu @param points vector of points which are 4-float vectors @param normals vector of normals which are 4-float vectors */ - /* CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0; */ + CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0; /** @brief Gets points of current 3d mesh diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index 291a7e54ecc..cae21431005 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -411,7 +411,7 @@ int main(int argc, char** argv) viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05, viz::Color::gray()); window.showWidget("cloud", cloudWidget); - window.showWidget("normals", cloudNormals); + /* window.showWidget("normals", cloudNormals); */ Vec3d volSize = kf->getParams().voxelSize * Vec3d(kf->getParams().volumeDims); window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), @@ -424,7 +424,7 @@ int main(int argc, char** argv) Point())); window.spin(); window.removeWidget("text"); - window.removeWidget("cloud"); + /* window.removeWidget("cloud"); */ window.removeWidget("normals"); window.registerMouseCallback(0); } diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index aa62e9e5e38..d51138c8478 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -8,9 +8,11 @@ #include #include +#include #include #include +#include "opencv2/core/cvstd.hpp" #include "opencv2/core/utils/trace.hpp" namespace cv @@ -132,9 +134,9 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody struct IntegrateSubvolumeInvoker : ParallelLoopBody { - IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, std::vector _totalVolUnits, - const Depth& _depth, Intr _intrinsics, cv::Affine3f _cameraPose, - float _depthFactor) + IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, + const std::vector& _totalVolUnits, const Depth& _depth, + Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : ParallelLoopBody(), volume(_volume), totalVolUnits(_totalVolUnits), @@ -177,11 +179,12 @@ struct IntegrateSubvolumeInvoker : ParallelLoopBody struct VolumeUnitInFrustumInvoker : ParallelLoopBody { VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, - const std::vector& _totalVolUnits, const Depth& _depth, + const std::vector& _totalVolUnits, int& _noOfActiveVolumeUnits, const Depth& _depth, Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : ParallelLoopBody(), volume(_volume), totalVolUnits(_totalVolUnits), + noOfActiveVolumeUnits(_noOfActiveVolumeUnits), depth(_depth), proj(_intrinsics.makeProjector()), depthFactor(_depthFactor), @@ -213,10 +216,16 @@ struct VolumeUnitInFrustumInvoker : ParallelLoopBody assert(it != volume.volumeUnits.end()); it->second.isActive = true; } + AutoLock al(mutex); + if(it->second.isActive) + { + noOfActiveVolumeUnits++; + } } } HashTSDFVolumeCPU& volume; const std::vector totalVolUnits; + int& noOfActiveVolumeUnits; const Depth& depth; const Intr::Projector proj; const float depthFactor; @@ -232,7 +241,6 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affi CV_Assert(_depth.type() == DEPTH_TYPE); Depth depth = _depth.getMat(); VolumeUnitIndexSet accessVolUnits; - AccessedVolumeUnitsInvoker allocate_i(*this, accessVolUnits, depth, intrinsics, cameraPose, depthFactor); Range range(0, depth.rows); @@ -243,12 +251,16 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affi { totalVolUnits.push_back(keyvalue.first); } - - VolumeUnitInFrustumInvoker infrustum_i(*this, totalVolUnits, depth, intrinsics, cameraPose, + int noOfActiveVolumeUnits = 0; + VolumeUnitInFrustumInvoker infrustum_i(*this, totalVolUnits, noOfActiveVolumeUnits, depth, intrinsics, cameraPose, depthFactor); Range in_frustum_range(0, volumeUnits.size()); parallel_for_(in_frustum_range, infrustum_i); + std::cout << "Number of new volume units created: " << accessVolUnits.size() << "\n"; + std::cout << " Number of total units in volume: " << volumeUnits.size() << " " << totalVolUnits.size() << "\n"; + std::cout << " Number of Active volume units in frame: " << noOfActiveVolumeUnits << "\n"; + IntegrateSubvolumeInvoker integrate_i(*this, totalVolUnits, depth, intrinsics, cameraPose, depthFactor); Range accessed_units_range(0, totalVolUnits.size()); @@ -284,75 +296,102 @@ inline Voxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const cvFloor(volumeIdx[1] / volumeUnitResolution), cvFloor(volumeIdx[2] / volumeUnitResolution)); - cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, - volumeUnitIdx[1] * volumeUnitResolution, - volumeUnitIdx[2] * volumeUnitResolution); VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); if (it == volumeUnits.end()) { Voxel dummy; - dummy.tsdf = 0.f; + dummy.tsdf = 1.f; dummy.weight = 0; return dummy; } cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + + cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, + volumeUnitIdx[1] * volumeUnitResolution, + volumeUnitIdx[2] * volumeUnitResolution); + + volUnitLocalIdx = + cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + /* std::cout << "VolumeIdx: " << volumeIdx << " VolumeUnitIdx: " << volumeUnitIdx << " Volume + * Local unit idx: " << volUnitLocalIdx << "\n"; */ return volumeUnit->at(volUnitLocalIdx); } inline Voxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const { cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); - cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); - cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); if (it == volumeUnits.end()) { Voxel dummy; - dummy.tsdf = 0; + dummy.tsdf = 1.f; dummy.weight = 0; return dummy; } cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + + cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); + cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); + volUnitLocalIdx = + cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + return volumeUnit->at(volUnitLocalIdx); } -inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(cv::Point3f p) const +inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(const cv::Point3f& point) const { - int ix = cvFloor(p.x); - int iy = cvFloor(p.y); - int iz = cvFloor(p.z); - - float tx = p.x - ix; - float ty = p.y - iy; - float tz = p.z - iz; - - TsdfType vx[8]; - //! This fetches the tsdf value from the correct subvolumes - vx[0] = at(cv::Vec3i(0, 0, 0) + cv::Vec3i(ix, iy, iz)).tsdf; - vx[1] = at(cv::Vec3i(0, 0, 1) + cv::Vec3i(ix, iy, iz)).tsdf; - vx[2] = at(cv::Vec3i(0, 1, 0) + cv::Vec3i(ix, iy, iz)).tsdf; - vx[3] = at(cv::Vec3i(0, 1, 1) + cv::Vec3i(ix, iy, iz)).tsdf; - vx[4] = at(cv::Vec3i(1, 0, 0) + cv::Vec3i(ix, iy, iz)).tsdf; - vx[5] = at(cv::Vec3i(1, 0, 1) + cv::Vec3i(ix, iy, iz)).tsdf; - vx[6] = at(cv::Vec3i(0, 1, 0) + cv::Vec3i(ix, iy, iz)).tsdf; - vx[7] = at(cv::Vec3i(1, 1, 1) + cv::Vec3i(ix, iy, iz)).tsdf; - - /* std::cout << "tsdf 7th: " << vx[7] << "\n"; */ - TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); - TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); - TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); - TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); - - TsdfType v0 = v00 + ty * (v01 - v00); - TsdfType v1 = v10 + ty * (v11 - v10); - - return v0 + tx * (v1 - v0); + cv::Vec3i voxelIdx = volumeToVoxelCoord(point); + + Point3f t = point - Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); + + std::array neighbourCoords = { cv::Vec3i(0, 0, 0), cv::Vec3i(0, 0, 1), + cv::Vec3i(0, 1, 0), cv::Vec3i(0, 1, 1), + cv::Vec3i(1, 0, 0), cv::Vec3i(1, 0, 1), + cv::Vec3i(1, 1, 0), cv::Vec3i(1, 1, 1) }; + + std::array tsdf_array; + for (int i = 0; i < 8; ++i) + { + Voxel voxel = at(neighbourCoords[i] + voxelIdx); + /* std::cout << "VoxelIdx : " << voxelIdx << " voxel weight: " << voxel.weight << " voxel value: " << voxel.tsdf << "\n"; */ + /* if(voxel.weight != 0) */ + tsdf_array[i] = voxel.tsdf; + } + + TsdfType v00 = tsdf_array[0] + t.z * (tsdf_array[1] - tsdf_array[0]); + TsdfType v01 = tsdf_array[2] + t.z * (tsdf_array[3] - tsdf_array[2]); + TsdfType v10 = tsdf_array[4] + t.z * (tsdf_array[5] - tsdf_array[4]); + TsdfType v11 = tsdf_array[6] + t.z * (tsdf_array[7] - tsdf_array[6]); + + TsdfType v0 = v00 + t.y * (v01 - v00); + TsdfType v1 = v10 + t.y * (v11 - v10); + /* std::cout << " interpolation t: " << t << std::endl; */ + return v0 + t.x * (v1 - v0); } inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const { + /* cv::Vec3i voxelIdx = volumeToVoxelCoord(point); */ + + /* Vec3i prev = voxelIdx; */ + /* Vec3i next = voxelIdx; */ + /* Vec3f normal = Vec3f(0, 0, 0); */ + + /* for (int c = 0; c < 3; c++) */ + /* { */ + /* prev[c] -= 1; */ + /* next[c] += 1; */ + + /* normal[c] = at(next).tsdf - at(prev).tsdf; */ + /* normal[c] *= 0.5; */ + + /* prev[c] = voxelIdx[c]; */ + /* next[c] = voxelIdx[c]; */ + /* } */ + /* return normalize(normal); */ + Vec3f pointVec(point); Vec3f normal = Vec3f(0, 0, 0); @@ -374,57 +413,55 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const pointNext[c] = pointVec[c]; } return normalize(normal); - - /* int ix = cvFloor(p.x); */ - /* int iy = cvFloor(p.y); */ - /* int iz = cvFloor(p.z); */ - - /* float tx = p.x - ix; */ - /* float ty = p.y - iy; */ - /* float tz = p.z - iz; */ - - /* Vec3i coordBase0 = cv::Vec3i(ix, iy, iz); */ - /* Vec3i coordBase1 = cv::Vec3i(ix, iy, iz); */ - /* Vec3f an; */ - /* for(int c = 0; c < 3; c++) */ - /* { */ - /* float& nv = an[c]; */ - - /* TsdfType vx[8]; */ - /* coordBase0[c] -= 1; */ - /* coordBase1[c] += 1; */ - - /* vx[0] = at(cv::Vec3i(0, 0, 0) + coordBase1).tsdf - at(cv::Vec3i(0, 0, 0) + - * coordBase0).tsdf; */ - /* vx[1] = at(cv::Vec3i(0, 0, 1) + coordBase1).tsdf - at(cv::Vec3i(0, 0, 1) + - * coordBase0).tsdf; */ - /* vx[2] = at(cv::Vec3i(0, 1, 0) + coordBase1).tsdf - at(cv::Vec3i(0, 1, 0) + - * coordBase0).tsdf; */ - /* vx[3] = at(cv::Vec3i(0, 1, 1) + coordBase1).tsdf - at(cv::Vec3i(0, 1, 1) + - * coordBase0).tsdf; */ - /* vx[4] = at(cv::Vec3i(1, 0, 0) + coordBase1).tsdf - at(cv::Vec3i(1, 0, 0) + - * coordBase0).tsdf; */ - /* vx[5] = at(cv::Vec3i(1, 0, 1) + coordBase1).tsdf - at(cv::Vec3i(1, 0, 1) + - * coordBase0).tsdf; */ - /* vx[6] = at(cv::Vec3i(1, 1, 0) + coordBase1).tsdf - at(cv::Vec3i(1, 1, 0) + - * coordBase0).tsdf; */ - /* vx[7] = at(cv::Vec3i(1, 1, 1) + coordBase1).tsdf - at(cv::Vec3i(1, 1, 1) + - * coordBase0).tsdf; */ - - /* TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); */ - /* TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); */ - /* TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); */ - /* TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); */ - - /* TsdfType v0 = v00 + ty*(v01 - v00); */ - /* TsdfType v1 = v10 + ty*(v11 - v10); */ - - /* nv = v0 + tx*(v1 - v0); */ - /* } */ - - /* return normalize(an); */ } +/* inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const */ +/* { */ +/* cv::Vec3i voxelIdx = volumeToVoxelCoord(point); */ +/* cv::Vec3i next = voxelIdx; */ +/* cv::Vec3i prev = voxelIdx; */ +/* Point3f t = point - Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); */ + +/* std::array neighbourCoords = { */ +/* cv::Vec3i(0, 0, 0), */ +/* cv::Vec3i(0, 0, 1), */ +/* cv::Vec3i(0, 1, 0), */ +/* cv::Vec3i(0, 1, 1), */ +/* cv::Vec3i(1, 0, 0), */ +/* cv::Vec3i(1, 0, 1), */ +/* cv::Vec3i(1, 1, 0), */ +/* cv::Vec3i(1, 1, 1)}; */ + +/* Vec3f normal; */ +/* for(int c = 0; c < 3; c++) */ +/* { */ +/* float& nv = normal[c]; */ + +/* next[c] += 1; */ +/* prev[c] -= 1; */ + +/* std::array vx; */ +/* for(int i = 0; i < 8; i++) */ +/* vx[i] = at(neighbourCoords[i] + next).tsdf - */ +/* at(neighbourCoords[i] + prev).tsdf; */ + +/* TsdfType v00 = vx[0] + t.z*(vx[1] - vx[0]); */ +/* TsdfType v01 = vx[2] + t.z*(vx[3] - vx[2]); */ +/* TsdfType v10 = vx[4] + t.z*(vx[5] - vx[4]); */ +/* TsdfType v11 = vx[6] + t.z*(vx[7] - vx[6]); */ + +/* TsdfType v0 = v00 + t.y*(v01 - v00); */ +/* TsdfType v1 = v10 + t.y*(v11 - v10); */ + +/* nv = v0 + t.x*(v1 - v0); */ + +/* next[c] = voxelIdx[c]; */ +/* prev[c] = voxelIdx[c]; */ +/* } */ + +/* return normalize(normal); */ +/* } */ + struct RaycastInvoker : ParallelLoopBody { RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, Intr intrinsics, @@ -455,6 +492,7 @@ struct RaycastInvoker : ParallelLoopBody for (int x = 0; x < points.cols; x++) { + //! Initialize default value Point3f point = nan3, normal = nan3; //! Ray origin and direction in the volume coordinate frame @@ -481,7 +519,7 @@ struct RaycastInvoker : ParallelLoopBody TsdfType currTsdf = prevTsdf; int currWeight = 0; - float stepSize = tstep; + float stepSize = blockSize; cv::Vec3i volUnitLocalIdx; //! Does the subvolume exist in hashtable @@ -496,15 +534,17 @@ struct RaycastInvoker : ParallelLoopBody //! TODO: Figure out voxel interpolation Voxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); currTsdf = currVoxel.tsdf; - currWeight = currVoxel.weight; - /* stepSize = tstep; */ + /* currTsdf = volume.interpolateVoxel(currRayPos); */ + /* std::cout << "Current TSDF: " << currTsdf << " interpolated: " << interpolatedTsdf << "\n"; */ + currWeight = currVoxel.weight; + stepSize = tstep; } //! Surface crossing if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) { float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); - + /* std::cout << "tcurr: " << tcurr << " tprev: " << tprev << " tInterp: " << tInterp << "\n"; */ if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { Point3f pv = orig + tInterp * rayDirV; @@ -557,6 +597,109 @@ void HashTSDFVolumeCPU::raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrins parallel_for_(Range(0, points.rows), ri, nstripes); } +struct FetchPointsNormalsInvoker : ParallelLoopBody +{ + FetchPointsNormalsInvoker(const HashTSDFVolumeCPU& _volume, + const std::vector& _totalVolUnits, + std::vector>& _pVecs, + std::vector>& _nVecs, bool _needNormals) + : ParallelLoopBody(), + volume(_volume), + totalVolUnits(_totalVolUnits), + pVecs(_pVecs), + nVecs(_nVecs), + needNormals(_needNormals) + { + } + + virtual void operator()(const Range& range) const override + { + std::vector points, normals; + for (int i = range.start; i < range.end; i++) + { + cv::Vec3i tsdf_idx = totalVolUnits[i]; + + VolumeUnitMap::const_iterator it = volume.volumeUnits.find(tsdf_idx); + Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); + if (it != volume.volumeUnits.end()) + { + cv::Ptr volumeUnit = + std::dynamic_pointer_cast(it->second.pVolume); + std::vector localPoints; + std::vector localNormals; + for (int x = 0; x < volume.volumeUnitResolution; x++) + for (int y = 0; y < volume.volumeUnitResolution; y++) + for (int z = 0; z < volume.volumeUnitResolution; z++) + { + cv::Vec3i voxelIdx(x, y, z); + Voxel voxel = volumeUnit->at(voxelIdx); + + if (voxel.tsdf != 1.f && voxel.weight != 0) + { + Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); + localPoints.push_back(toPtype(point)); + if (needNormals) + { + Point3f normal = volume.getNormalVoxel(point); + localNormals.push_back(toPtype(normal)); + } + } + } + + AutoLock al(mutex); + pVecs.push_back(localPoints); + nVecs.push_back(localNormals); + } + } + } + + const HashTSDFVolumeCPU& volume; + std::vector totalVolUnits; + std::vector>& pVecs; + std::vector>& nVecs; + const Voxel* volDataStart; + bool needNormals; + mutable Mutex mutex; +}; + +void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const +{ + CV_TRACE_FUNCTION(); + + if (_points.needed()) + { + std::vector> pVecs, nVecs; + + std::vector totalVolUnits; + for (const auto& keyvalue : volumeUnits) + { + totalVolUnits.push_back(keyvalue.first); + } + std::cout << "Number of volumeUnits in volume(fetchPoints): " << totalVolUnits.size() << "\n"; + FetchPointsNormalsInvoker fi(*this, totalVolUnits, pVecs, nVecs, _normals.needed()); + Range range(0, totalVolUnits.size()); + const int nstripes = -1; + parallel_for_(range, fi, nstripes); + std::vector points, normals; + for (size_t i = 0; i < pVecs.size(); i++) + { + points.insert(points.end(), pVecs[i].begin(), pVecs[i].end()); + normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end()); + } + + _points.create((int)points.size(), 1, POINT_TYPE); + if (!points.empty()) + Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat()); + + if (_normals.needed()) + { + _normals.create((int)normals.size(), 1, POINT_TYPE); + if (!normals.empty()) + Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat()); + } + } +} + cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, float _raycastStepFactor, int volumeUnitResolution) diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 92f99621579..605b79cdcce 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -10,7 +10,6 @@ #include #include -#include "opencv2/core/affine.hpp" #include "tsdf.hpp" namespace cv { @@ -31,7 +30,7 @@ class HashTSDFVolume cv::OutputArray points, cv::OutputArray normals) const = 0; /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; */ - /* virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; */ + virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; virtual void reset() = 0; @@ -46,7 +45,6 @@ class HashTSDFVolume uint16_t volumeUnitResolution; float volumeUnitSize; bool zFirstMemOrder; - }; struct VolumeUnit @@ -91,7 +89,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume cv::OutputArray points, cv::OutputArray normals) const override; /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; */ - /* virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; */ + virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; virtual void reset() override; @@ -101,7 +99,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = 1m) virtual Voxel at(const cv::Point3f &point) const; - TsdfType interpolateVoxel(cv::Point3f p) const; + inline TsdfType interpolateVoxel(const cv::Point3f& point) const; Point3f getNormalVoxel(cv::Point3f p) const; //! Utility functions for coordinate transformations diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index 05389271730..be9fecb88af 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -108,7 +108,7 @@ class KinFuImpl : public KinFu void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE; //! TODO(Akash): Add back later - /* virtual void getCloud(OutputArray points, OutputArray normals, const Matx44f& cameraPose) const CV_OVERRIDE; */ + virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE; /* void getPoints(OutputArray points) const CV_OVERRIDE; */ /* void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; */ @@ -275,24 +275,24 @@ void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const if((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) || (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) { + std::cout << " Render without raycast: " << std::endl; renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose); } else { T points, normals; + std::cout << " Raycasted render: " << std::endl; volume->raycast(cameraPose, params.intr, params.frameSize, points, normals); - std::cout << "Raycasted render" << std::endl; renderPointsNormals(points, normals, image, params.lightPose); } } -/* template< typename T > */ -/* void KinFuImpl::getCloud(cv::OutputArray p, OutputArray n, const Matx44f& _cameraPose) const */ -/* { */ -/* /1* volume->fetchPointsNormals(p, n); *1/ */ -/* /1* volume->raycast(pose, params.intr, params.frameSize, p, n); *1/ */ -/* } */ +template< typename T > +void KinFuImpl::getCloud(cv::OutputArray p, OutputArray n) const +{ + volume->fetchPointsNormals(p, n); +} /* template< typename T > */ @@ -324,7 +324,7 @@ Ptr KinFu::create(const Ptr& params) } #else -Ptr KinFu::create(const Ptr& /*params*/) +Ptr KinFu::create(const Ptr& /* params */) { CV_Error(Error::StsNotImplemented, "This algorithm is patented and is excluded in this configuration; " diff --git a/modules/rgbd/src/kinfu_back.cpp b/modules/rgbd/src/kinfu_back.cpp deleted file mode 100644 index b58679c6f43..00000000000 --- a/modules/rgbd/src/kinfu_back.cpp +++ /dev/null @@ -1,333 +0,0 @@ -/* // This file is part of OpenCV project. */ -/* // It is subject to the license terms in the LICENSE file found in the top-level directory */ -/* // of this distribution and at http://opencv.org/license.html */ - -/* // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory */ - -/* #include "precomp.hpp" */ -/* #include "fast_icp.hpp" */ -/* #include "tsdf.hpp" */ -/* #include "kinfu_frame.hpp" */ - -/* namespace cv { */ -/* namespace kinfu { */ - -/* void Params::setInitialVolumePose(Matx33f R, Vec3f t) */ -/* { */ -/* setInitialVolumePose(Affine3f(R,t).matrix); */ -/* } */ - -/* void Params::setInitialVolumePose(Matx44f homogen_tf) */ -/* { */ -/* Params::volumePose.matrix = homogen_tf; */ -/* } */ - -/* Ptr Params::defaultParams() */ -/* { */ -/* Params p; */ - -/* p.frameSize = Size(640, 480); */ - -/* float fx, fy, cx, cy; */ -/* fx = fy = 525.f; */ -/* cx = p.frameSize.width/2 - 0.5f; */ -/* cy = p.frameSize.height/2 - 0.5f; */ -/* p.intr = Matx33f(fx, 0, cx, */ -/* 0, fy, cy, */ -/* 0, 0, 1); */ - -/* // 5000 for the 16-bit PNG files */ -/* // 1 for the 32-bit float images in the ROS bag files */ -/* p.depthFactor = 5000; */ - -/* // sigma_depth is scaled by depthFactor when calling bilateral filter */ -/* p.bilateral_sigma_depth = 0.04f; //meter */ -/* p.bilateral_sigma_spatial = 4.5; //pixels */ -/* p.bilateral_kernel_size = 7; //pixels */ - -/* p.icpAngleThresh = (float)(30. * CV_PI / 180.); // radians */ -/* p.icpDistThresh = 0.1f; // meters */ - -/* p.icpIterations = {10, 5, 4}; */ -/* p.pyramidLevels = (int)p.icpIterations.size(); */ - -/* p.tsdf_min_camera_movement = 0.f; //meters, disabled */ - -/* p.volumeDims = Vec3i::all(512); //number of voxels */ - -/* float volSize = 3.f; */ -/* p.voxelSize = volSize/512.f; //meters */ - -/* // default pose of volume cube */ -/* p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f)); */ -/* p.tsdf_trunc_dist = 0.04f; //meters; */ -/* p.tsdf_max_weight = 64; //frames */ - -/* p.raycast_step_factor = 0.25f; //in voxel sizes */ -/* // gradient delta factor is fixed at 1.0f and is not used */ -/* //p.gradient_delta_factor = 0.5f; //in voxel sizes */ - -/* //p.lightPose = p.volume_pose.translation()/4; //meters */ -/* p.lightPose = Vec3f::all(0.f); //meters */ - -/* // depth truncation is not used by default but can be useful in some scenes */ -/* p.truncateThreshold = 0.f; //meters */ - -/* return makePtr(p); */ -/* } */ - -/* Ptr Params::coarseParams() */ -/* { */ -/* Ptr p = defaultParams(); */ - -/* p->icpIterations = {5, 3, 2}; */ -/* p->pyramidLevels = (int)p->icpIterations.size(); */ - -/* float volSize = 3.f; */ -/* p->volumeDims = Vec3i::all(128); //number of voxels */ -/* p->voxelSize = volSize/128.f; */ - -/* p->raycast_step_factor = 0.75f; //in voxel sizes */ - -/* return p; */ -/* } */ - -/* // T should be Mat or UMat */ -/* template< typename T > */ -/* class KinFuImpl : public KinFu */ -/* { */ -/* public: */ -/* KinFuImpl(const Params& _params); */ -/* virtual ~KinFuImpl(); */ - -/* const Params& getParams() const CV_OVERRIDE; */ - -/* void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE; */ - -/* void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE; */ -/* void getPoints(OutputArray points) const CV_OVERRIDE; */ -/* void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; */ - -/* void reset() CV_OVERRIDE; */ - -/* const Affine3f getPose() const CV_OVERRIDE; */ - -/* bool update(InputArray depth) CV_OVERRIDE; */ - -/* bool updateT(const T& depth); */ - -/* private: */ -/* Params params; */ - -/* cv::Ptr icp; */ -/* cv::Ptr volume; */ - -/* int frameCounter; */ -/* Affine3f pose; */ -/* std::vector pyrPoints; */ -/* std::vector pyrNormals; */ -/* }; */ - - -/* template< typename T > */ -/* KinFuImpl::KinFuImpl(const Params &_params) : */ -/* params(_params), */ -/* icp(makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)), */ -/* volume(makeTSDFVolume(params.volumeDims, params.voxelSize, params.volumePose, */ -/* params.tsdf_trunc_dist, params.tsdf_max_weight, */ -/* params.raycast_step_factor)), */ -/* pyrPoints(), pyrNormals() */ -/* { */ -/* reset(); */ -/* } */ - -/* template< typename T > */ -/* void KinFuImpl::reset() */ -/* { */ -/* frameCounter = 0; */ -/* pose = Affine3f::Identity(); */ -/* volume->reset(); */ -/* } */ - -/* template< typename T > */ -/* KinFuImpl::~KinFuImpl() */ -/* { } */ - -/* template< typename T > */ -/* const Params& KinFuImpl::getParams() const */ -/* { */ -/* return params; */ -/* } */ - -/* template< typename T > */ -/* const Affine3f KinFuImpl::getPose() const */ -/* { */ -/* return pose; */ -/* } */ - - -/* template<> */ -/* bool KinFuImpl::update(InputArray _depth) */ -/* { */ -/* CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); */ - -/* Mat depth; */ -/* if(_depth.isUMat()) */ -/* { */ -/* _depth.copyTo(depth); */ -/* return updateT(depth); */ -/* } */ -/* else */ -/* { */ -/* return updateT(_depth.getMat()); */ -/* } */ -/* } */ - - -/* template<> */ -/* bool KinFuImpl::update(InputArray _depth) */ -/* { */ -/* CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); */ - -/* UMat depth; */ -/* if(!_depth.isUMat()) */ -/* { */ -/* _depth.copyTo(depth); */ -/* return updateT(depth); */ -/* } */ -/* else */ -/* { */ -/* return updateT(_depth.getUMat()); */ -/* } */ -/* } */ - - -/* template< typename T > */ -/* bool KinFuImpl::updateT(const T& _depth) */ -/* { */ -/* CV_TRACE_FUNCTION(); */ - -/* T depth; */ -/* if(_depth.type() != DEPTH_TYPE) */ -/* _depth.convertTo(depth, DEPTH_TYPE); */ -/* else */ -/* depth = _depth; */ - -/* std::vector newPoints, newNormals; */ -/* makeFrameFromDepth(depth, newPoints, newNormals, params.intr, */ -/* params.pyramidLevels, */ -/* params.depthFactor, */ -/* params.bilateral_sigma_depth, */ -/* params.bilateral_sigma_spatial, */ -/* params.bilateral_kernel_size, */ -/* params.truncateThreshold); */ - -/* if(frameCounter == 0) */ -/* { */ -/* // use depth instead of distance */ -/* volume->integrate(depth, params.depthFactor, pose, params.intr); */ - -/* pyrPoints = newPoints; */ -/* pyrNormals = newNormals; */ -/* } */ -/* else */ -/* { */ -/* Affine3f affine; */ -/* bool success = icp->estimateTransform(affine, pyrPoints, pyrNormals, newPoints, newNormals); */ -/* if(!success) */ -/* return false; */ - -/* pose = pose * affine; */ - -/* float rnorm = (float)cv::norm(affine.rvec()); */ -/* float tnorm = (float)cv::norm(affine.translation()); */ -/* // We do not integrate volume if camera does not move */ -/* if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement) */ -/* { */ -/* // use depth instead of distance */ -/* volume->integrate(depth, params.depthFactor, pose, params.intr); */ -/* } */ - -/* T& points = pyrPoints [0]; */ -/* T& normals = pyrNormals[0]; */ -/* volume->raycast(pose, params.intr, params.frameSize, points, normals); */ -/* // build a pyramid of points and normals */ -/* buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, */ -/* params.pyramidLevels); */ -/* } */ - -/* frameCounter++; */ -/* return true; */ -/* } */ - - -/* template< typename T > */ -/* void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const */ -/* { */ -/* CV_TRACE_FUNCTION(); */ - -/* Affine3f cameraPose(_cameraPose); */ - -/* const Affine3f id = Affine3f::Identity(); */ -/* if((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) || */ -/* (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) */ -/* { */ -/* renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose); */ -/* } */ -/* else */ -/* { */ -/* T points, normals; */ -/* volume->raycast(cameraPose, params.intr, params.frameSize, points, normals); */ -/* renderPointsNormals(points, normals, image, params.lightPose); */ -/* } */ -/* } */ - - -/* template< typename T > */ -/* void KinFuImpl::getCloud(OutputArray p, OutputArray n) const */ -/* { */ -/* volume->fetchPointsNormals(p, n); */ -/* } */ - - -/* template< typename T > */ -/* void KinFuImpl::getPoints(OutputArray points) const */ -/* { */ -/* volume->fetchPointsNormals(points, noArray()); */ -/* } */ - - -/* template< typename T > */ -/* void KinFuImpl::getNormals(InputArray points, OutputArray normals) const */ -/* { */ -/* volume->fetchNormals(points, normals); */ -/* } */ - -/* // importing class */ - -/* #ifdef OPENCV_ENABLE_NONFREE */ - -/* Ptr KinFu::create(const Ptr& params) */ -/* { */ -/* CV_Assert((int)params->icpIterations.size() == params->pyramidLevels); */ -/* CV_Assert(params->intr(0,1) == 0 && params->intr(1,0) == 0 && params->intr(2,0) == 0 && params->intr(2,1) == 0 && params->intr(2,2) == 1); */ -/* #ifdef HAVE_OPENCL */ -/* if(cv::ocl::useOpenCL()) */ -/* return makePtr< KinFuImpl >(*params); */ -/* #endif */ -/* return makePtr< KinFuImpl >(*params); */ -/* } */ - -/* #else */ -/* Ptr KinFu::create(const Ptr& params) */ -/* { */ -/* CV_Error(Error::StsNotImplemented, */ -/* "This algorithm is patented and is excluded in this configuration; " */ -/* "Set OPENCV_ENABLE_NONFREE CMake option and rebuild the library"); */ -/* } */ -/* #endif */ - -/* KinFu::~KinFu() {} */ - -/* } // namespace kinfu */ -/* } // namespace cv */ diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 22f899d67f4..4576cf8119d 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -2,24 +2,26 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory -#include "precomp.hpp" #include "tsdf.hpp" -#include "opencl_kernels_rgbd.hpp" -namespace cv { - -namespace kinfu { +#include "opencl_kernels_rgbd.hpp" +#include "precomp.hpp" -TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder) : - voxelSize(_voxelSize), - voxelSizeInv(1.f/_voxelSize), - volResolution(_res), - maxWeight(_maxWeight), - pose(_pose), - raycastStepFactor(_raycastStepFactor) +namespace cv +{ +namespace kinfu +{ +TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _truncDist, + int _maxWeight, float _raycastStepFactor, bool zFirstMemOrder) + : voxelSize(_voxelSize), + voxelSizeInv(1.f / _voxelSize), + volResolution(_res), + maxWeight(_maxWeight), + pose(_pose), + raycastStepFactor(_raycastStepFactor) { // Unlike original code, this should work with any volume size // Not only when (x,y,z % 32) == 0 @@ -33,7 +35,7 @@ TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _tr // &elem(x, y, z) = data + x*zRes*yRes + y*zRes + z; // &elem(x, y, z) = data + x + y*xRes + z*xRes*yRes; int xdim, ydim, zdim; - if(zFirstMemOrder) + if (zFirstMemOrder) { xdim = volResolution.z * volResolution.y; ydim = volResolution.z; @@ -47,22 +49,17 @@ TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _tr } volDims = Vec4i(xdim, ydim, zdim); - neighbourCoords = Vec8i( - volDims.dot(Vec4i(0, 0, 0)), - volDims.dot(Vec4i(0, 0, 1)), - volDims.dot(Vec4i(0, 1, 0)), - volDims.dot(Vec4i(0, 1, 1)), - volDims.dot(Vec4i(1, 0, 0)), - volDims.dot(Vec4i(1, 0, 1)), - volDims.dot(Vec4i(1, 1, 0)), - volDims.dot(Vec4i(1, 1, 1)) - ); + neighbourCoords = + Vec8i(volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 1, 0)), + volDims.dot(Vec4i(0, 1, 1)), volDims.dot(Vec4i(1, 0, 0)), volDims.dot(Vec4i(1, 0, 1)), + volDims.dot(Vec4i(1, 1, 0)), volDims.dot(Vec4i(1, 1, 1))); } // dimension in voxels, size in meters -TSDFVolumeCPU::TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder) : - TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, zFirstMemOrder) +TSDFVolumeCPU::TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, + int _maxWeight, float _raycastStepFactor, bool zFirstMemOrder) + : TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, + zFirstMemOrder) { volume = Mat(1, volResolution.x * volResolution.y * volResolution.z, rawType()); @@ -74,30 +71,32 @@ void TSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); - volume.forEach([](VecT& vv, const int* /* position */) - { + volume.forEach([](VecT& vv, const int* /* position */) { Voxel& v = reinterpret_cast(vv); - v.tsdf = 0; v.weight = 0; + v.tsdf = 0; + v.weight = 0; }); } -Voxel TSDFVolumeCPU::at(const cv::Vec3i &volumeIdx) const +Voxel TSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { //! Out of bounds - if (volumeIdx[0] >= volResolution.x || - volumeIdx[1] >= volResolution.y || - volumeIdx[2] >= volResolution.z) + if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || + (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || + (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) { Voxel dummy; - dummy.tsdf = 0.0; + dummy.tsdf = 1.0; dummy.weight = 0; std::cout << "Returning dummy\n"; return dummy; } const Voxel* volData = volume.ptr(); - int coordBase = volumeIdx[0]*volDims[0] + volumeIdx[1]*volDims[1] + volumeIdx[2]*volDims[2]; - /* std::cout << " Current TSDF: " << volData[coordBase].tsdf << " current weight: " << volData[coordBase].weight << "\n"; */ + int coordBase = + volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; + /* std::cout << " Current TSDF: " << volData[coordBase].tsdf << " current weight: " << + * volData[coordBase].weight << "\n"; */ return volData[coordBase]; } @@ -108,19 +107,18 @@ static const bool fixMissingData = false; static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) { const depthType defaultValue = qnan; - if(pt.x < 0 || pt.x >= m.cols-1 || - pt.y < 0 || pt.y >= m.rows-1) + if (pt.x < 0 || pt.x >= m.cols - 1 || pt.y < 0 || pt.y >= m.rows - 1) return defaultValue; int xi = cvFloor(pt.x), yi = cvFloor(pt.y); - const depthType* row0 = m[yi+0]; - const depthType* row1 = m[yi+1]; + const depthType* row0 = m[yi + 0]; + const depthType* row1 = m[yi + 1]; - depthType v00 = row0[xi+0]; - depthType v01 = row0[xi+1]; - depthType v10 = row1[xi+0]; - depthType v11 = row1[xi+1]; + depthType v00 = row0[xi + 0]; + depthType v01 = row0[xi + 1]; + depthType v10 = row1[xi + 0]; + depthType v11 = row1[xi + 1]; // assume correct depth is positive bool b00 = v00 > 0; @@ -128,115 +126,131 @@ static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) bool b10 = v10 > 0; bool b11 = v11 > 0; - if(!fixMissingData) + if (!fixMissingData) { - if(!(b00 && b01 && b10 && b11)) + if (!(b00 && b01 && b10 && b11)) return defaultValue; else { float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx*(v01 - v00); - depthType v1 = v10 + tx*(v11 - v10); - return v0 + ty*(v1 - v0); + depthType v0 = v00 + tx * (v01 - v00); + depthType v1 = v10 + tx * (v11 - v10); + return v0 + ty * (v1 - v0); } } else { int nz = b00 + b01 + b10 + b11; - if(nz == 0) + if (nz == 0) { return defaultValue; } - if(nz == 1) + if (nz == 1) { - if(b00) return v00; - if(b01) return v01; - if(b10) return v10; - if(b11) return v11; + if (b00) + return v00; + if (b01) + return v01; + if (b10) + return v10; + if (b11) + return v11; } - else if(nz == 2) + else if (nz == 2) { - if(b00 && b10) v01 = v00, v11 = v10; - if(b01 && b11) v00 = v01, v10 = v11; - if(b00 && b01) v10 = v00, v11 = v01; - if(b10 && b11) v00 = v10, v01 = v11; - if(b00 && b11) v01 = v10 = (v00 + v11)*0.5f; - if(b01 && b10) v00 = v11 = (v01 + v10)*0.5f; + if (b00 && b10) + v01 = v00, v11 = v10; + if (b01 && b11) + v00 = v01, v10 = v11; + if (b00 && b01) + v10 = v00, v11 = v01; + if (b10 && b11) + v00 = v10, v01 = v11; + if (b00 && b11) + v01 = v10 = (v00 + v11) * 0.5f; + if (b01 && b10) + v00 = v11 = (v01 + v10) * 0.5f; } - else if(nz == 3) + else if (nz == 3) { - if(!b00) v00 = v10 + v01 - v11; - if(!b01) v01 = v00 + v11 - v10; - if(!b10) v10 = v00 + v11 - v01; - if(!b11) v11 = v01 + v10 - v00; + if (!b00) + v00 = v10 + v01 - v11; + if (!b01) + v01 = v00 + v11 - v10; + if (!b10) + v10 = v00 + v11 - v01; + if (!b11) + v11 = v01 + v10 - v00; } float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx*(v01 - v00); - depthType v1 = v10 + tx*(v11 - v10); - return v0 + ty*(v1 - v0); + depthType v0 = v00 + tx * (v01 - v00); + depthType v1 = v10 + tx * (v11 - v10); + return v0 + ty * (v1 - v0); } } #endif struct IntegrateInvoker : ParallelLoopBody { - IntegrateInvoker(TSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose, - float depthFactor) : - ParallelLoopBody(), - volume(_volume), - depth(_depth), - proj(intrinsics.makeProjector()), - vol2cam(cameraPose.inv() * _volume.pose), - truncDistInv(1.f/_volume.truncDist), - dfac(1.f/depthFactor) + IntegrateInvoker(TSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, + cv::Affine3f cameraPose, float depthFactor) + : ParallelLoopBody(), + volume(_volume), + depth(_depth), + proj(intrinsics.makeProjector()), + vol2cam(cameraPose.inv() * _volume.pose), + truncDistInv(1.f / _volume.truncDist), + dfac(1.f / depthFactor) { volDataStart = volume.volume.ptr(); } #if USE_INTRINSICS - virtual void operator() (const Range& range) const override + virtual void operator()(const Range& range) const override { // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; - Point3f zStepPt = Point3f(vol2cam.matrix(0, 2), - vol2cam.matrix(1, 2), - vol2cam.matrix(2, 2))*volume.voxelSize; + Point3f zStepPt = + Point3f(vol2cam.matrix(0, 2), vol2cam.matrix(1, 2), vol2cam.matrix(2, 2)) * + volume.voxelSize; v_float32x4 zStep(zStepPt.x, zStepPt.y, zStepPt.z, 0); v_float32x4 vfxy(proj.fx, proj.fy, 0.f, 0.f), vcxy(proj.cx, proj.cy, 0.f, 0.f); - const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols-1, depth.rows-1, 0, 0)); + const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols - 1, depth.rows - 1, 0, 0)); - for(int x = range.start; x < range.end; x++) + for (int x = range.start; x < range.end; x++) { - Voxel* volDataX = volDataStart + x*volume.volDims[0]; - for(int y = 0; y < volume.volResolution.y; y++) + Voxel* volDataX = volDataStart + x * volume.volDims[0]; + for (int y = 0; y < volume.volResolution.y; y++) { - Voxel* volDataY = volDataX + y*volume.volDims[1]; - // optimization of camSpace transformation (vector addition instead of matmul at each z) - Point3f basePt = vol2cam*(Point3f((float)x, (float)y, 0)*volume.voxelSize); + Voxel* volDataY = volDataX + y * volume.volDims[1]; + // optimization of camSpace transformation (vector addition instead of matmul at + // each z) + Point3f basePt = vol2cam * (Point3f((float)x, (float)y, 0) * volume.voxelSize); v_float32x4 camSpacePt(basePt.x, basePt.y, basePt.z, 0); /* std::cout << "basePt: " << basePt << " zstep: " << zStepPt << "\n"; */ int startZ, endZ; - if(abs(zStepPt.z) > 1e-5) + if (abs(zStepPt.z) > 1e-5) { int baseZ = (int)(-basePt.z / zStepPt.z); - if(zStepPt.z > 0) + if (zStepPt.z > 0) { startZ = baseZ; - endZ = volume.volResolution.z; + endZ = volume.volResolution.z; } else { startZ = 0; - endZ = baseZ; + endZ = baseZ; } } else { - if(basePt.z > 0) + if (basePt.z > 0) { - startZ = 0; endZ = volume.volResolution.z; + startZ = 0; + endZ = volume.volResolution.z; } else { @@ -245,21 +259,23 @@ struct IntegrateInvoker : ParallelLoopBody } } startZ = max(0, startZ); - endZ = min(volume.volResolution.z, endZ); + endZ = min(volume.volResolution.z, endZ); /* std::cout << "StartZ, EndZ: " << startZ << ", " << endZ << "\n"; */ - for(int z = startZ; z < endZ; z++) + for (int z = startZ; z < endZ; z++) { // optimization of the following: - //Point3f volPt = Point3f(x, y, z)*voxelSize; - //Point3f camSpacePt = vol2cam * volPt; + // Point3f volPt = Point3f(x, y, z)*voxelSize; + // Point3f camSpacePt = vol2cam * volPt; camSpacePt += zStep; - float zCamSpace = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(camSpacePt))).get0(); + float zCamSpace = + v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(camSpacePt))) + .get0(); /* std::cout << "zCamSpace: " << zCamSpace << std::endl; */ - if(zCamSpace <= 0.f) + if (zCamSpace <= 0.f) continue; - v_float32x4 camPixVec = camSpacePt/v_setall_f32(zCamSpace); + v_float32x4 camPixVec = camSpacePt / v_setall_f32(zCamSpace); v_float32x4 projected = v_muladd(camPixVec, vfxy, vcxy); // leave only first 2 lanes projected = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & @@ -273,18 +289,18 @@ struct IntegrateInvoker : ParallelLoopBody v_uint32x4 limits = v_reinterpret_as_u32(pt < v_setzero_f32()) | v_reinterpret_as_u32(pt >= upLimits); limits = limits | v_rotate_right<1>(limits); - if(limits.get0()) + if (limits.get0()) continue; // xi, yi = floor(pt) - v_int32x4 ip = v_floor(pt); + v_int32x4 ip = v_floor(pt); v_int32x4 ipshift = ip; - int xi = ipshift.get0(); - ipshift = v_rotate_right<1>(ipshift); - int yi = ipshift.get0(); + int xi = ipshift.get0(); + ipshift = v_rotate_right<1>(ipshift); + int yi = ipshift.get0(); - const depthType* row0 = depth[yi+0]; - const depthType* row1 = depth[yi+1]; + const depthType* row0 = depth[yi + 0]; + const depthType* row1 = depth[yi + 1]; // v001 = [v(xi + 0, yi + 0), v(xi + 1, yi + 0)] v_float32x4 v001 = v_load_low(row0 + xi); @@ -295,40 +311,40 @@ struct IntegrateInvoker : ParallelLoopBody // assume correct depth is positive // don't fix missing data - if(v_check_all(vall > v_setzero_f32())) + if (v_check_all(vall > v_setzero_f32())) { v_float32x4 t = pt - v_cvt_f32(ip); - float tx = t.get0(); + float tx = t.get0(); t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); v_float32x4 ty = v_setall_f32(t.get0()); // vx is y-interpolated between rows 0 and 1 - v_float32x4 vx = v001 + ty*(v101 - v001); - float v0 = vx.get0(); + v_float32x4 vx = v001 + ty * (v101 - v001); + float v0 = vx.get0(); vx = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vx))); float v1 = vx.get0(); - v = v0 + tx*(v1 - v0); + v = v0 + tx * (v1 - v0); } else continue; } // norm(camPixVec) produces double which is too slow - float pixNorm = sqrt(v_reduce_sum(camPixVec*camPixVec)); + float pixNorm = sqrt(v_reduce_sum(camPixVec * camPixVec)); // difference between distances of point and of surface to camera - TsdfType sdf = pixNorm*(v*dfac - zCamSpace); + TsdfType sdf = pixNorm * (v * dfac - zCamSpace); // possible alternative is: // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); /* std::cout << "SDF: " << sdf << "truncDist: " << -volume.truncDist << "\n"; */ - if(sdf >= -volume.truncDist) + if (sdf >= -volume.truncDist) { TsdfType tsdf = fmin(1.f, sdf * truncDistInv); - Voxel& voxel = volDataY[z*volume.volDims[2]]; - int& weight = voxel.weight; + Voxel& voxel = volDataY[z * volume.volDims[2]]; + int& weight = voxel.weight; TsdfType& value = voxel.tsdf; // update TSDF - value = (value*weight+tsdf) / (weight + 1); + value = (value * weight + tsdf) / (weight + 1); weight = min(weight + 1, volume.maxWeight); /* std::cout << "Updated TSDF: " << value << "\n"; */ } @@ -337,43 +353,46 @@ struct IntegrateInvoker : ParallelLoopBody } } #else - virtual void operator() (const Range& range) const override + virtual void operator()(const Range& range) const override { - for(int x = range.start; x < range.end; x++) + for (int x = range.start; x < range.end; x++) { - Voxel* volDataX = volDataStart + x*volume.volDims[0]; - for(int y = 0; y < volume.volResolution.y; y++) + Voxel* volDataX = volDataStart + x * volume.volDims[0]; + for (int y = 0; y < volume.volResolution.y; y++) { - Voxel* volDataY = volDataX+y*volume.volDims[1]; - // optimization of camSpace transformation (vector addition instead of matmul at each z) - Point3f basePt = vol2cam*(Point3f(x, y, 0)*volume.voxelSize); - /* std::cout << "Base Point: " << basePt << " VoxelSize: " << volume.voxelSize << "\n"; */ + Voxel* volDataY = volDataX + y * volume.volDims[1]; + // optimization of camSpace transformation (vector addition instead of matmul at + // each z) + Point3f basePt = vol2cam * (Point3f(x, y, 0) * volume.voxelSize); + /* std::cout << "Base Point: " << basePt << " VoxelSize: " << volume.voxelSize << + * "\n"; */ Point3f camSpacePt = basePt; // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; // zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize - Point3f zStep = Point3f(vol2cam.matrix(0, 2), - vol2cam.matrix(1, 2), - vol2cam.matrix(2, 2))*volume.voxelSize; + Point3f zStep = + Point3f(vol2cam.matrix(0, 2), vol2cam.matrix(1, 2), vol2cam.matrix(2, 2)) * + volume.voxelSize; int startZ, endZ; - if(abs(zStep.z) > 1e-5) + if (abs(zStep.z) > 1e-5) { int baseZ = -basePt.z / zStep.z; - if(zStep.z > 0) + if (zStep.z > 0) { startZ = baseZ; - endZ = volume.volResolution.z; + endZ = volume.volResolution.z; } else { startZ = 0; - endZ = baseZ; + endZ = baseZ; } } else { - if(basePt.z > 0) + if (basePt.z > 0) { - startZ = 0; endZ = volume.volResolution.z; + startZ = 0; + endZ = volume.volResolution.z; } else { @@ -382,28 +401,28 @@ struct IntegrateInvoker : ParallelLoopBody } } startZ = max(0, startZ); - endZ = min(volume.volResolution.z, endZ); + endZ = min(volume.volResolution.z, endZ); /* std::cout << "startZ, endZ: (" << startZ << ", " << endZ << ")\n"; */ - for(int z = startZ; z < endZ; z++) + for (int z = startZ; z < endZ; z++) { // optimization of the following: - //Point3f volPt = Point3f(x, y, z)*volume.voxelSize; - //Point3f camSpacePt = vol2cam * volPt; + // Point3f volPt = Point3f(x, y, z)*volume.voxelSize; + // Point3f camSpacePt = vol2cam * volPt; camSpacePt += zStep; - if(camSpacePt.z <= 0) + if (camSpacePt.z <= 0) continue; Point3f camPixVec; Point2f projected = proj(camSpacePt, camPixVec); depthType v = bilinearDepth(depth, projected); - if(v == 0) + if (v == 0) continue; // norm(camPixVec) produces double which is too slow float pixNorm = sqrt(camPixVec.dot(camPixVec)); // difference between distances of point and of surface to camera - TsdfType sdf = pixNorm*(v*dfac - camSpacePt.z); + TsdfType sdf = pixNorm * (v * dfac - camSpacePt.z); /* std::cout << "camSpacePt: " << camSpacePt << "\n"; */ /* std::cout << "projected: " << projected << "\n"; */ /* std::cout << "depth at projected: " << v << "\n"; */ @@ -411,16 +430,16 @@ struct IntegrateInvoker : ParallelLoopBody // possible alternative is: // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); - if(sdf >= -volume.truncDist) + if (sdf >= -volume.truncDist) { TsdfType tsdf = fmin(1.f, sdf * truncDistInv); - Voxel& voxel = volDataY[z*volume.volDims[2]]; - int& weight = voxel.weight; + Voxel& voxel = volDataY[z * volume.volDims[2]]; + int& weight = voxel.weight; TsdfType& value = voxel.tsdf; // update TSDF - value = (value*weight+tsdf) / (weight + 1); + value = (value * weight + tsdf) / (weight + 1); weight = min(weight + 1, volume.maxWeight); /* std::cout << "Voxel coord: (" << x << ", " << y << ", " << z << ") "; */ /* std::cout << "Updated value: " << value << " Updated weight: " << weight << "\n"; */ @@ -441,7 +460,8 @@ struct IntegrateInvoker : ParallelLoopBody }; // use depth instead of distance (optimization) -void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, Intr intrinsics) +void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, + Intr intrinsics) { CV_TRACE_FUNCTION(); @@ -463,39 +483,41 @@ inline TsdfType TSDFVolumeCPU::interpolateVoxel(Point3f _p) const inline TsdfType TSDFVolumeCPU::interpolateVoxel(const v_float32x4& p) const { // tx, ty, tz = floor(p) - v_int32x4 ip = v_floor(p); + v_int32x4 ip = v_floor(p); v_float32x4 t = p - v_cvt_f32(ip); - float tx = t.get0(); - t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); - float ty = t.get0(); - t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); - float tz = t.get0(); + float tx = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float ty = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float tz = t.get0(); int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const Voxel* volData = volume.ptr(); - int ix = ip.get0(); ip = v_rotate_right<1>(ip); - int iy = ip.get0(); ip = v_rotate_right<1>(ip); + int ix = ip.get0(); + ip = v_rotate_right<1>(ip); + int iy = ip.get0(); + ip = v_rotate_right<1>(ip); int iz = ip.get0(); - int coordBase = ix*xdim + iy*ydim + iz*zdim; + int coordBase = ix * xdim + iy * ydim + iz * zdim; TsdfType vx[8]; - for(int i = 0; i < 8; i++) + for (int i = 0; i < 8; i++) vx[i] = volData[neighbourCoords[i] + coordBase].tsdf; v_float32x4 v0246(vx[0], vx[2], vx[4], vx[6]), v1357(vx[1], vx[3], vx[5], vx[7]); - v_float32x4 vxx = v0246 + v_setall_f32(tz)*(v1357 - v0246); + v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); v_float32x4 v00_10 = vxx; v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx))); - v_float32x4 v0_1 = v00_10 + v_setall_f32(ty)*(v01_11 - v00_10); - float v0 = v0_1.get0(); - v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); - float v1 = v0_1.get0(); + v_float32x4 v0_1 = v00_10 + v_setall_f32(ty) * (v01_11 - v00_10); + float v0 = v0_1.get0(); + v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); + float v1 = v0_1.get0(); - return v0 + tx*(v1 - v0); + return v0 + tx * (v1 - v0); } #else inline TsdfType TSDFVolumeCPU::interpolateVoxel(Point3f p) const @@ -510,27 +532,27 @@ inline TsdfType TSDFVolumeCPU::interpolateVoxel(Point3f p) const float ty = p.y - iy; float tz = p.z - iz; - int coordBase = ix*xdim + iy*ydim + iz*zdim; + int coordBase = ix * xdim + iy * ydim + iz * zdim; const Voxel* volData = volume.ptr(); TsdfType vx[8]; - for(int i = 0; i < 8; i++) + for (int i = 0; i < 8; i++) vx[i] = volData[neighbourCoords[i] + coordBase].tsdf; - TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); - TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); - TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); - TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); + TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); - TsdfType v0 = v00 + ty*(v01 - v00); - TsdfType v1 = v10 + ty*(v11 - v10); + TsdfType v0 = v00 + ty * (v01 - v00); + TsdfType v1 = v10 + ty * (v11 - v10); - return v0 + tx*(v1 - v0); + return v0 + tx * (v1 - v0); } #endif #if USE_INTRINSICS -//gradientDeltaFactor is fixed at 1.0 of voxel size +// gradientDeltaFactor is fixed at 1.0 of voxel size inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f _p) const { v_float32x4 p(_p.x, _p.y, _p.z, 0.f); @@ -542,59 +564,59 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f _p) const inline v_float32x4 TSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const { - if(v_check_any((p < v_float32x4(1.f, 1.f, 1.f, 0.f)) + - (p >= v_float32x4((float)(volResolution.x-2), - (float)(volResolution.y-2), - (float)(volResolution.z-2), 1.f)) - )) + if (v_check_any((p < v_float32x4(1.f, 1.f, 1.f, 0.f)) + + (p >= v_float32x4((float)(volResolution.x - 2), (float)(volResolution.y - 2), + (float)(volResolution.z - 2), 1.f)))) return nanv; - v_int32x4 ip = v_floor(p); + v_int32x4 ip = v_floor(p); v_float32x4 t = p - v_cvt_f32(ip); - float tx = t.get0(); - t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); - float ty = t.get0(); - t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); - float tz = t.get0(); + float tx = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float ty = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float tz = t.get0(); const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const Voxel* volData = volume.ptr(); - int ix = ip.get0(); ip = v_rotate_right<1>(ip); - int iy = ip.get0(); ip = v_rotate_right<1>(ip); + int ix = ip.get0(); + ip = v_rotate_right<1>(ip); + int iy = ip.get0(); + ip = v_rotate_right<1>(ip); int iz = ip.get0(); - int coordBase = ix*xdim + iy*ydim + iz*zdim; + int coordBase = ix * xdim + iy * ydim + iz * zdim; float CV_DECL_ALIGNED(16) an[4]; an[0] = an[1] = an[2] = an[3] = 0.f; - for(int c = 0; c < 3; c++) + for (int c = 0; c < 3; c++) { const int dim = volDims[c]; - float& nv = an[c]; + float& nv = an[c]; TsdfType vx[8]; - for(int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - - volData[neighbourCoords[i] + coordBase - 1*dim].tsdf; + for (int i = 0; i < 8; i++) + vx[i] = volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf - + volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf; v_float32x4 v0246(vx[0], vx[2], vx[4], vx[6]), v1357(vx[1], vx[3], vx[5], vx[7]); - v_float32x4 vxx = v0246 + v_setall_f32(tz)*(v1357 - v0246); + v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); v_float32x4 v00_10 = vxx; v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx))); - v_float32x4 v0_1 = v00_10 + v_setall_f32(ty)*(v01_11 - v00_10); - float v0 = v0_1.get0(); - v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); - float v1 = v0_1.get0(); + v_float32x4 v0_1 = v00_10 + v_setall_f32(ty) * (v01_11 - v00_10); + float v0 = v0_1.get0(); + v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); + float v1 = v0_1.get0(); - nv = v0 + tx*(v1 - v0); + nv = v0 + tx * (v1 - v0); } - v_float32x4 n = v_load_aligned(an); - v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(n*n))); - return n*invNorm; + v_float32x4 n = v_load_aligned(an); + v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(n * n))); + return n * invNorm; } #else inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const @@ -602,9 +624,8 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const Voxel* volData = volume.ptr(); - if(p.x < 1 || p.x >= volResolution.x - 2 || - p.y < 1 || p.y >= volResolution.y - 2 || - p.z < 1 || p.z >= volResolution.z - 2) + if (p.x < 1 || p.x >= volResolution.x - 2 || p.y < 1 || p.y >= volResolution.y - 2 || p.z < 1 || + p.z >= volResolution.z - 2) return nan3; int ix = cvFloor(p.x); @@ -615,87 +636,84 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const float ty = p.y - iy; float tz = p.z - iz; - int coordBase = ix*xdim + iy*ydim + iz*zdim; + int coordBase = ix * xdim + iy * ydim + iz * zdim; Vec3f an; - for(int c = 0; c < 3; c++) + for (int c = 0; c < 3; c++) { const int dim = volDims[c]; float& nv = an[c]; TsdfType vx[8]; - for(int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - - volData[neighbourCoords[i] + coordBase - 1*dim].tsdf; + for (int i = 0; i < 8; i++) + vx[i] = volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf - + volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf; - TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); - TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); - TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); - TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); + TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); - TsdfType v0 = v00 + ty*(v01 - v00); - TsdfType v1 = v10 + ty*(v11 - v10); + TsdfType v0 = v00 + ty * (v01 - v00); + TsdfType v1 = v10 + ty * (v11 - v10); - nv = v0 + tx*(v1 - v0); + nv = v0 + tx * (v1 - v0); } return normalize(an); } #endif - struct RaycastInvoker : ParallelLoopBody { - RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, - Intr intrinsics, const TSDFVolumeCPU& _volume) : - ParallelLoopBody(), - points(_points), - normals(_normals), - volume(_volume), - tstep(volume.truncDist * volume.raycastStepFactor), - // We do subtract voxel size to minimize checks after - // Note: origin of volume coordinate is placed - // in the center of voxel (0,0,0), not in the corner of the voxel! - boxMax(volume.volSize - Point3f(volume.voxelSize, - volume.voxelSize, - volume.voxelSize)), - boxMin(), - cam2vol(volume.pose.inv() * cameraPose), - vol2cam(cameraPose.inv() * volume.pose), - reproj(intrinsics.makeReprojector()) - { } + RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, Intr intrinsics, + const TSDFVolumeCPU& _volume) + : ParallelLoopBody(), + points(_points), + normals(_normals), + volume(_volume), + tstep(volume.truncDist * volume.raycastStepFactor), + // We do subtract voxel size to minimize checks after + // Note: origin of volume coordinate is placed + // in the center of voxel (0,0,0), not in the corner of the voxel! + boxMax(volume.volSize - Point3f(volume.voxelSize, volume.voxelSize, volume.voxelSize)), + boxMin(), + cam2vol(volume.pose.inv() * cameraPose), + vol2cam(cameraPose.inv() * volume.pose), + reproj(intrinsics.makeReprojector()) + { + } #if USE_INTRINSICS - virtual void operator() (const Range& range) const override + virtual void operator()(const Range& range) const override { const v_float32x4 vfxy(reproj.fxinv, reproj.fyinv, 0, 0); const v_float32x4 vcxy(reproj.cx, reproj.cy, 0, 0); - const float (&cm)[16] = cam2vol.matrix.val; - const v_float32x4 camRot0(cm[0], cm[4], cm[ 8], 0); - const v_float32x4 camRot1(cm[1], cm[5], cm[ 9], 0); + const float(&cm)[16] = cam2vol.matrix.val; + const v_float32x4 camRot0(cm[0], cm[4], cm[8], 0); + const v_float32x4 camRot1(cm[1], cm[5], cm[9], 0); const v_float32x4 camRot2(cm[2], cm[6], cm[10], 0); const v_float32x4 camTrans(cm[3], cm[7], cm[11], 0); const v_float32x4 boxDown(boxMin.x, boxMin.y, boxMin.z, 0.f); const v_float32x4 boxUp(boxMax.x, boxMax.y, boxMax.z, 0.f); - const v_float32x4 invVoxelSize = v_float32x4(volume.voxelSizeInv, - volume.voxelSizeInv, - volume.voxelSizeInv, 1.f); + const v_float32x4 invVoxelSize = + v_float32x4(volume.voxelSizeInv, volume.voxelSizeInv, volume.voxelSizeInv, 1.f); - const float (&vm)[16] = vol2cam.matrix.val; - const v_float32x4 volRot0(vm[0], vm[4], vm[ 8], 0); - const v_float32x4 volRot1(vm[1], vm[5], vm[ 9], 0); + const float(&vm)[16] = vol2cam.matrix.val; + const v_float32x4 volRot0(vm[0], vm[4], vm[8], 0); + const v_float32x4 volRot1(vm[1], vm[5], vm[9], 0); const v_float32x4 volRot2(vm[2], vm[6], vm[10], 0); const v_float32x4 volTrans(vm[3], vm[7], vm[11], 0); - for(int y = range.start; y < range.end; y++) + for (int y = range.start; y < range.end; y++) { ptype* ptsRow = points[y]; ptype* nrmRow = normals[y]; - for(int x = 0; x < points.cols; x++) + for (int x = 0; x < points.cols; x++) { v_float32x4 point = nanv, normal = nanv; @@ -704,21 +722,21 @@ struct RaycastInvoker : ParallelLoopBody // get direction through pixel in volume space: // 1. reproject (x, y) on projecting plane where z = 1.f - v_float32x4 planed = (v_float32x4((float)x, (float)y, 0.f, 0.f) - vcxy)*vfxy; - planed = v_combine_low(planed, v_float32x4(1.f, 0.f, 0.f, 0.f)); + v_float32x4 planed = (v_float32x4((float)x, (float)y, 0.f, 0.f) - vcxy) * vfxy; + planed = v_combine_low(planed, v_float32x4(1.f, 0.f, 0.f, 0.f)); // 2. rotate to volume space planed = v_matmuladd(planed, camRot0, camRot1, camRot2, v_setzero_f32()); // 3. normalize - v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(planed*planed))); - v_float32x4 dir = planed*invNorm; + v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(planed * planed))); + v_float32x4 dir = planed * invNorm; // compute intersection of ray with all six bbox planes - v_float32x4 rayinv = v_setall_f32(1.f)/dir; + v_float32x4 rayinv = v_setall_f32(1.f) / dir; // div by zero should be eliminated by these products - v_float32x4 tbottom = rayinv*(boxDown - orig); - v_float32x4 ttop = rayinv*(boxUp - orig); + v_float32x4 tbottom = rayinv * (boxDown - orig); + v_float32x4 ttop = rayinv * (boxUp - orig); // re-order intersections to find smallest and largest on each axis v_float32x4 minAx = v_min(ttop, tbottom); @@ -726,45 +744,47 @@ struct RaycastInvoker : ParallelLoopBody // near clipping plane const float clip = 0.f; - float tmin = max(v_reduce_max(minAx), clip); - float tmax = v_reduce_min(maxAx); + float tmin = max(v_reduce_max(minAx), clip); + float tmax = v_reduce_min(maxAx); // precautions against getting coordinates out of bounds tmin = tmin + tstep; tmax = tmax - tstep; - if(tmin < tmax) + if (tmin < tmax) { // interpolation optimized a little orig *= invVoxelSize; - dir *= invVoxelSize; + dir *= invVoxelSize; - int xdim = volume.volDims[0]; - int ydim = volume.volDims[1]; - int zdim = volume.volDims[2]; + int xdim = volume.volDims[0]; + int ydim = volume.volDims[1]; + int zdim = volume.volDims[2]; v_float32x4 rayStep = dir * v_setall_f32(tstep); - v_float32x4 next = (orig + dir * v_setall_f32(tmin)); + v_float32x4 next = (orig + dir * v_setall_f32(tmin)); TsdfType f = volume.interpolateVoxel(next), fnext = f; - //raymarch - int steps = 0; - int nSteps = cvFloor((tmax - tmin)/tstep); - for(; steps < nSteps; steps++) + // raymarch + int steps = 0; + int nSteps = cvFloor((tmax - tmin) / tstep); + for (; steps < nSteps; steps++) { next += rayStep; v_int32x4 ip = v_round(next); - int ix = ip.get0(); ip = v_rotate_right<1>(ip); - int iy = ip.get0(); ip = v_rotate_right<1>(ip); - int iz = ip.get0(); - int coord = ix*xdim + iy*ydim + iz*zdim; + int ix = ip.get0(); + ip = v_rotate_right<1>(ip); + int iy = ip.get0(); + ip = v_rotate_right<1>(ip); + int iz = ip.get0(); + int coord = ix * xdim + iy * ydim + iz * zdim; fnext = volume.volume.at(coord).tsdf; - if(fnext != f) + if (fnext != f) { fnext = volume.interpolateVoxel(next); // when ray crosses a surface - if(std::signbit(f) != std::signbit(fnext)) + if (std::signbit(f) != std::signbit(fnext)) break; f = fnext; @@ -773,30 +793,31 @@ struct RaycastInvoker : ParallelLoopBody // if ray penetrates a surface from outside // linearly interpolate t between two f values - if(f > 0.f && fnext < 0.f) + if (f > 0.f && fnext < 0.f) { v_float32x4 tp = next - rayStep; - TsdfType ft = volume.interpolateVoxel(tp); - TsdfType ftdt = volume.interpolateVoxel(next); + TsdfType ft = volume.interpolateVoxel(tp); + TsdfType ftdt = volume.interpolateVoxel(next); // float t = tmin + steps*tstep; // float ts = t - tstep*ft/(ftdt - ft); - float ts = tmin + tstep*(steps - ft/(ftdt - ft)); + float ts = tmin + tstep * (steps - ft / (ftdt - ft)); // avoid division by zero - if(!cvIsNaN(ts) && !cvIsInf(ts)) + if (!cvIsNaN(ts) && !cvIsInf(ts)) { - v_float32x4 pv = (orig + dir*v_setall_f32(ts)); + v_float32x4 pv = (orig + dir * v_setall_f32(ts)); v_float32x4 nv = volume.getNormalVoxel(pv); - if(!isNaN(nv)) + if (!isNaN(nv)) { - //convert pv and nv to camera space - normal = v_matmuladd(nv, volRot0, volRot1, volRot2, v_setzero_f32()); + // convert pv and nv to camera space + normal = + v_matmuladd(nv, volRot0, volRot1, volRot2, v_setzero_f32()); // interpolation optimized a little - point = v_matmuladd(pv*v_float32x4(volume.voxelSize, - volume.voxelSize, - volume.voxelSize, 1.f), - volRot0, volRot1, volRot2, volTrans); + point = + v_matmuladd(pv * v_float32x4(volume.voxelSize, volume.voxelSize, + volume.voxelSize, 1.f), + volRot0, volRot1, volRot2, volTrans); } } } @@ -808,18 +829,18 @@ struct RaycastInvoker : ParallelLoopBody } } #else - virtual void operator() (const Range& range) const override + virtual void operator()(const Range& range) const override { const Point3f camTrans = cam2vol.translation(); - const Matx33f camRot = cam2vol.rotation(); - const Matx33f volRot = vol2cam.rotation(); + const Matx33f camRot = cam2vol.rotation(); + const Matx33f volRot = vol2cam.rotation(); - for(int y = range.start; y < range.end; y++) + for (int y = range.start; y < range.end; y++) { ptype* ptsRow = points[y]; ptype* nrmRow = normals[y]; - for(int x = 0; x < points.cols; x++) + for (int x = 0; x < points.cols; x++) { Point3f point = nan3, normal = nan3; @@ -828,37 +849,39 @@ struct RaycastInvoker : ParallelLoopBody Point3f dir = normalize(Vec3f(camRot * reproj(Point3f(x, y, 1.f)))); // compute intersection of ray with all six bbox planes - Vec3f rayinv(1.f/dir.x, 1.f/dir.y, 1.f/dir.z); + Vec3f rayinv(1.f / dir.x, 1.f / dir.y, 1.f / dir.z); Point3f tbottom = rayinv.mul(boxMin - orig); - Point3f ttop = rayinv.mul(boxMax - orig); + Point3f ttop = rayinv.mul(boxMax - orig); // re-order intersections to find smallest and largest on each axis - Point3f minAx(min(ttop.x, tbottom.x), min(ttop.y, tbottom.y), min(ttop.z, tbottom.z)); - Point3f maxAx(max(ttop.x, tbottom.x), max(ttop.y, tbottom.y), max(ttop.z, tbottom.z)); + Point3f minAx(min(ttop.x, tbottom.x), min(ttop.y, tbottom.y), + min(ttop.z, tbottom.z)); + Point3f maxAx(max(ttop.x, tbottom.x), max(ttop.y, tbottom.y), + max(ttop.z, tbottom.z)); // near clipping plane const float clip = 0.f; float tmin = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip); - float tmax = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z)); + float tmax = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z)); // precautions against getting coordinates out of bounds tmin = tmin + tstep; tmax = tmax - tstep; - if(tmin < tmax) + if (tmin < tmax) { // interpolation optimized a little - orig = orig*volume.voxelSizeInv; - dir = dir*volume.voxelSizeInv; + orig = orig * volume.voxelSizeInv; + dir = dir * volume.voxelSizeInv; Point3f rayStep = dir * tstep; Point3f next = (orig + dir * tmin); TsdfType f = volume.interpolateVoxel(next), fnext = f; - //raymarch + // raymarch int steps = 0; - int nSteps = floor((tmax - tmin)/tstep); - for(; steps < nSteps; steps++) + int nSteps = floor((tmax - tmin) / tstep); + for (; steps < nSteps; steps++) { next += rayStep; int xdim = volume.volDims[0]; @@ -867,13 +890,13 @@ struct RaycastInvoker : ParallelLoopBody int ix = cvRound(next.x); int iy = cvRound(next.y); int iz = cvRound(next.z); - fnext = volume.volume.at(ix*xdim + iy*ydim + iz*zdim).tsdf; - if(fnext != f) + fnext = volume.volume.at(ix * xdim + iy * ydim + iz * zdim).tsdf; + if (fnext != f) { fnext = volume.interpolateVoxel(next); // when ray crosses a surface - if(std::signbit(f) != std::signbit(fnext)) + if (std::signbit(f) != std::signbit(fnext)) break; f = fnext; @@ -882,27 +905,27 @@ struct RaycastInvoker : ParallelLoopBody // if ray penetrates a surface from outside // linearly interpolate t between two f values - if(f > 0.f && fnext < 0.f) + if (f > 0.f && fnext < 0.f) { Point3f tp = next - rayStep; - TsdfType ft = volume.interpolateVoxel(tp); + TsdfType ft = volume.interpolateVoxel(tp); TsdfType ftdt = volume.interpolateVoxel(next); // float t = tmin + steps*tstep; // float ts = t - tstep*ft/(ftdt - ft); - float ts = tmin + tstep*(steps - ft/(ftdt - ft)); + float ts = tmin + tstep * (steps - ft / (ftdt - ft)); // avoid division by zero - if(!cvIsNaN(ts) && !cvIsInf(ts)) + if (!cvIsNaN(ts) && !cvIsInf(ts)) { - Point3f pv = (orig + dir*ts); + Point3f pv = (orig + dir * ts); Point3f nv = volume.getNormalVoxel(pv); - if(!isNaN(nv)) + if (!isNaN(nv)) { - //convert pv and nv to camera space + // convert pv and nv to camera space normal = volRot * nv; // interpolation optimized a little - point = vol2cam * (pv*volume.voxelSize); + point = vol2cam * (pv * volume.voxelSize); } } } @@ -929,7 +952,6 @@ struct RaycastInvoker : ParallelLoopBody const Intr::Reprojector reproj; }; - void TSDFVolumeCPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const { @@ -937,10 +959,10 @@ void TSDFVolumeCPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frame CV_Assert(frameSize.area() > 0); - _points.create (frameSize, POINT_TYPE); + _points.create(frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE); - Points points = _points.getMat(); + Points points = _points.getMat(); Normals normals = _normals.getMat(); RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); @@ -949,100 +971,92 @@ void TSDFVolumeCPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frame parallel_for_(Range(0, points.rows), ri, nstripes); } - struct FetchPointsNormalsInvoker : ParallelLoopBody { - FetchPointsNormalsInvoker(const TSDFVolumeCPU& _volume, - std::vector< std::vector >& _pVecs, - std::vector< std::vector >& _nVecs, - bool _needNormals) : - ParallelLoopBody(), - vol(_volume), - pVecs(_pVecs), - nVecs(_nVecs), - needNormals(_needNormals) + FetchPointsNormalsInvoker(const TSDFVolumeCPU& _volume, std::vector>& _pVecs, + std::vector>& _nVecs, bool _needNormals) + : ParallelLoopBody(), vol(_volume), pVecs(_pVecs), nVecs(_nVecs), needNormals(_needNormals) { volDataStart = vol.volume.ptr(); } - inline void coord(std::vector& points, std::vector& normals, - int x, int y, int z, Point3f V, float v0, int axis) const + inline void coord(std::vector& points, std::vector& normals, int x, int y, int z, + Point3f V, float v0, int axis) const { // 0 for x, 1 for y, 2 for z bool limits = false; Point3i shift; float Vc = 0.f; - if(axis == 0) + if (axis == 0) { - shift = Point3i(1, 0, 0); + shift = Point3i(1, 0, 0); limits = (x + 1 < vol.volResolution.x); - Vc = V.x; + Vc = V.x; } - if(axis == 1) + if (axis == 1) { - shift = Point3i(0, 1, 0); + shift = Point3i(0, 1, 0); limits = (y + 1 < vol.volResolution.y); - Vc = V.y; + Vc = V.y; } - if(axis == 2) + if (axis == 2) { - shift = Point3i(0, 0, 1); + shift = Point3i(0, 0, 1); limits = (z + 1 < vol.volResolution.z); - Vc = V.z; + Vc = V.z; } - if(limits) + if (limits) { - const Voxel& voxeld = volDataStart[(x+shift.x)*vol.volDims[0] + - (y+shift.y)*vol.volDims[1] + - (z+shift.z)*vol.volDims[2]]; + const Voxel& voxeld = + volDataStart[(x + shift.x) * vol.volDims[0] + (y + shift.y) * vol.volDims[1] + + (z + shift.z) * vol.volDims[2]]; TsdfType vd = voxeld.tsdf; - if(voxeld.weight != 0 && vd != 1.f) + if (voxeld.weight != 0 && vd != 1.f) { - if((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0)) + if ((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0)) { - //linearly interpolate coordinate - float Vn = Vc + vol.voxelSize; - float dinv = 1.f/(abs(v0)+abs(vd)); - float inter = (Vc*abs(vd) + Vn*abs(v0))*dinv; - - Point3f p(shift.x ? inter : V.x, - shift.y ? inter : V.y, - shift.z ? inter : V.z); + // linearly interpolate coordinate + float Vn = Vc + vol.voxelSize; + float dinv = 1.f / (abs(v0) + abs(vd)); + float inter = (Vc * abs(vd) + Vn * abs(v0)) * dinv; + + Point3f p(shift.x ? inter : V.x, shift.y ? inter : V.y, shift.z ? inter : V.z); { points.push_back(toPtype(vol.pose * p)); - if(needNormals) + if (needNormals) normals.push_back(toPtype(vol.pose.rotation() * - vol.getNormalVoxel(p*vol.voxelSizeInv))); + vol.getNormalVoxel(p * vol.voxelSizeInv))); } } } } } - virtual void operator() (const Range& range) const override + virtual void operator()(const Range& range) const override { std::vector points, normals; - for(int x = range.start; x < range.end; x++) + for (int x = range.start; x < range.end; x++) { - const Voxel* volDataX = volDataStart + x*vol.volDims[0]; - for(int y = 0; y < vol.volResolution.y; y++) + const Voxel* volDataX = volDataStart + x * vol.volDims[0]; + for (int y = 0; y < vol.volResolution.y; y++) { - const Voxel* volDataY = volDataX + y*vol.volDims[1]; - for(int z = 0; z < vol.volResolution.z; z++) + const Voxel* volDataY = volDataX + y * vol.volDims[1]; + for (int z = 0; z < vol.volResolution.z; z++) { - const Voxel& voxel0 = volDataY[z*vol.volDims[2]]; - TsdfType v0 = voxel0.tsdf; - if(voxel0.weight != 0 && v0 != 1.f) + const Voxel& voxel0 = volDataY[z * vol.volDims[2]]; + TsdfType v0 = voxel0.tsdf; + if (voxel0.weight != 0 && v0 != 1.f) { - Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f)*vol.voxelSize); + Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f) * + vol.voxelSize); coord(points, normals, x, y, z, V, v0, 0); coord(points, normals, x, y, z, V, v0, 1); coord(points, normals, x, y, z, V, v0, 2); - } // if voxel is not empty + } // if voxel is not empty } } } @@ -1053,8 +1067,8 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody } const TSDFVolumeCPU& vol; - std::vector< std::vector >& pVecs; - std::vector< std::vector >& nVecs; + std::vector>& pVecs; + std::vector>& nVecs; const Voxel* volDataStart; bool needNormals; mutable Mutex mutex; @@ -1064,48 +1078,48 @@ void TSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals { CV_TRACE_FUNCTION(); - if(_points.needed()) + if (_points.needed()) { - std::vector< std::vector > pVecs, nVecs; + std::vector> pVecs, nVecs; FetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed()); Range range(0, volResolution.x); const int nstripes = -1; parallel_for_(range, fi, nstripes); std::vector points, normals; - for(size_t i = 0; i < pVecs.size(); i++) + for (size_t i = 0; i < pVecs.size(); i++) { points.insert(points.end(), pVecs[i].begin(), pVecs[i].end()); normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end()); } _points.create((int)points.size(), 1, POINT_TYPE); - if(!points.empty()) + if (!points.empty()) Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat()); - if(_normals.needed()) + if (_normals.needed()) { _normals.create((int)normals.size(), 1, POINT_TYPE); - if(!normals.empty()) + if (!normals.empty()) Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat()); } } } - struct PushNormals { - PushNormals(const TSDFVolumeCPU& _vol, Mat_& _nrm) : - vol(_vol), normals(_nrm), invPose(vol.pose.inv()) - { } - void operator ()(const ptype &pp, const int * position) const + PushNormals(const TSDFVolumeCPU& _vol, Mat_& _nrm) + : vol(_vol), normals(_nrm), invPose(vol.pose.inv()) + { + } + void operator()(const ptype& pp, const int* position) const { Point3f p = fromPtype(pp); Point3f n = nan3; - if(!isNaN(p)) + if (!isNaN(p)) { Point3f voxPt = (invPose * p); - voxPt = voxPt * vol.voxelSizeInv; - n = vol.pose.rotation() * vol.getNormalVoxel(voxPt); + voxPt = voxPt * vol.voxelSizeInv; + n = vol.pose.rotation() * vol.getNormalVoxel(voxPt); } normals(position[0], position[1]) = toPtype(n); } @@ -1115,12 +1129,11 @@ struct PushNormals Affine3f invPose; }; - void TSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); - if(_normals.needed()) + if (_normals.needed()) { Points points = _points.getMat(); CV_Assert(points.type() == POINT_TYPE); @@ -1135,16 +1148,15 @@ void TSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const ///////// GPU implementation ///////// #ifdef HAVE_OPENCL -TSDFVolumeGPU::TSDFVolumeGPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor) : - TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, false) +TSDFVolumeGPU::TSDFVolumeGPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, + int _maxWeight, float _raycastStepFactor) + : TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, false) { volume = UMat(1, volResolution.x * volResolution.y * volResolution.z, CV_32FC2); reset(); } - // zero volume, leave rest params the same void TSDFVolumeGPU::reset() { @@ -1153,54 +1165,43 @@ void TSDFVolumeGPU::reset() volume.setTo(Scalar(0, 0)); } - // use depth instead of distance (optimization) -void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, - cv::Affine3f cameraPose, Intr intrinsics) +void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, + Intr intrinsics) { CV_TRACE_FUNCTION(); UMat depth = _depth.getUMat(); cv::String errorStr; - cv::String name = "integrate"; + cv::String name = "integrate"; ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; + cv::String options = "-cl-mad-enable"; ocl::Kernel k; k.create(name.c_str(), source, options, &errorStr); - if(k.empty()) + if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); cv::Affine3f vol2cam(cameraPose.inv() * pose); - float dfac = 1.f/depthFactor; + float dfac = 1.f / depthFactor; Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); // TODO: optimization possible // Use sampler for depth (mask needed) - k.args(ocl::KernelArg::ReadOnly(depth), - ocl::KernelArg::PtrReadWrite(volume), - ocl::KernelArg::Constant(vol2cam.matrix.val, - sizeof(vol2cam.matrix.val)), - voxelSize, - volResGpu.val, - volDims.val, - fxy.val, - cxy.val, - dfac, - truncDist, - maxWeight); + k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(volume), + ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)), voxelSize, + volResGpu.val, volDims.val, fxy.val, cxy.val, dfac, truncDist, maxWeight); size_t globalSize[2]; globalSize[0] = (size_t)volResolution.x; globalSize[1] = (size_t)volResolution.y; - if(!k.run(2, globalSize, NULL, true)) + if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } - void TSDFVolumeGPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const { @@ -1209,19 +1210,19 @@ void TSDFVolumeGPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frame CV_Assert(frameSize.area() > 0); cv::String errorStr; - cv::String name = "raycast"; + cv::String name = "raycast"; ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; + cv::String options = "-cl-mad-enable"; ocl::Kernel k; k.create(name.c_str(), source, options, &errorStr); - if(k.empty()) + if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); - _points.create (frameSize, CV_32FC4); + _points.create(frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); - UMat points = _points.getUMat(); + UMat points = _points.getUMat(); UMat normals = _normals.getUMat(); UMat vol2camGpu, cam2volGpu; @@ -1233,42 +1234,30 @@ void TSDFVolumeGPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frame // We do subtract voxel size to minimize checks after // Note: origin of volume coordinate is placed // in the center of voxel (0,0,0), not in the corner of the voxel! - Vec4f boxMin, boxMax(volSize.x - voxelSize, - volSize.y - voxelSize, - volSize.z - voxelSize); + Vec4f boxMin, boxMax(volSize.x - voxelSize, volSize.y - voxelSize, volSize.z - voxelSize); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); float tstep = truncDist * raycastStepFactor; Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); - k.args(ocl::KernelArg::WriteOnlyNoSize(points), - ocl::KernelArg::WriteOnlyNoSize(normals), - frameSize, - ocl::KernelArg::PtrReadOnly(volume), - ocl::KernelArg::PtrReadOnly(vol2camGpu), - ocl::KernelArg::PtrReadOnly(cam2volGpu), - finv.val, cxy.val, - boxMin.val, boxMax.val, - tstep, - voxelSize, - volResGpu.val, - volDims.val, - neighbourCoords.val); + k.args(ocl::KernelArg::WriteOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), + frameSize, ocl::KernelArg::PtrReadOnly(volume), ocl::KernelArg::PtrReadOnly(vol2camGpu), + ocl::KernelArg::PtrReadOnly(cam2volGpu), finv.val, cxy.val, boxMin.val, boxMax.val, + tstep, voxelSize, volResGpu.val, volDims.val, neighbourCoords.val); size_t globalSize[2]; globalSize[0] = (size_t)frameSize.width; globalSize[1] = (size_t)frameSize.height; - if(!k.run(2, globalSize, NULL, true)) + if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } - void TSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); - if(_normals.needed()) + if (_normals.needed()) { UMat points = _points.getUMat(); CV_Assert(points.type() == POINT_TYPE); @@ -1277,37 +1266,31 @@ void TSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const UMat normals = _normals.getUMat(); cv::String errorStr; - cv::String name = "getNormals"; + cv::String name = "getNormals"; ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; + cv::String options = "-cl-mad-enable"; ocl::Kernel k; k.create(name.c_str(), source, options, &errorStr); - if(k.empty()) + if (k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); UMat volPoseGpu, invPoseGpu; - Mat(pose .matrix).copyTo(volPoseGpu); + Mat(pose.matrix).copyTo(volPoseGpu); Mat(pose.inv().matrix).copyTo(invPoseGpu); Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); Size frameSize = points.size(); - k.args(ocl::KernelArg::ReadOnlyNoSize(points), - ocl::KernelArg::WriteOnlyNoSize(normals), - frameSize, - ocl::KernelArg::PtrReadOnly(volume), - ocl::KernelArg::PtrReadOnly(volPoseGpu), - ocl::KernelArg::PtrReadOnly(invPoseGpu), - voxelSizeInv, - volResGpu.val, - volDims.val, - neighbourCoords.val); + k.args(ocl::KernelArg::ReadOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), + frameSize, ocl::KernelArg::PtrReadOnly(volume), + ocl::KernelArg::PtrReadOnly(volPoseGpu), ocl::KernelArg::PtrReadOnly(invPoseGpu), + voxelSizeInv, volResGpu.val, volDims.val, neighbourCoords.val); size_t globalSize[2]; globalSize[0] = (size_t)points.cols; globalSize[1] = (size_t)points.rows; - if(!k.run(2, globalSize, NULL, true)) + if (!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } } @@ -1316,7 +1299,7 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) { CV_TRACE_FUNCTION(); - if(points.needed()) + if (points.needed()) { bool needNormals = normals.needed(); @@ -1326,11 +1309,11 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) cv::String errorStr; ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; + cv::String options = "-cl-mad-enable"; kscan.create("scanSize", source, options, &errorStr); - if(kscan.empty()) + if (kscan.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); size_t globalSize[3]; @@ -1339,45 +1322,40 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) globalSize[2] = (size_t)volResolution.z; const ocl::Device& device = ocl::Device::getDefault(); - size_t wgsLimit = device.maxWorkGroupSize(); - size_t memSize = device.localMemSize(); + size_t wgsLimit = device.maxWorkGroupSize(); + size_t memSize = device.localMemSize(); // local mem should keep a point (and a normal) for each thread in a group // use 4 float per each point and normal - size_t elemSize = (sizeof(float)*4)*(needNormals ? 2 : 1); - const size_t lcols = 8; - const size_t lrows = 8; - size_t lplanes = min(memSize/elemSize, wgsLimit)/lcols/lrows; - lplanes = roundDownPow2(lplanes); - size_t localSize[3] = {lcols, lrows, lplanes}; + size_t elemSize = (sizeof(float) * 4) * (needNormals ? 2 : 1); + const size_t lcols = 8; + const size_t lrows = 8; + size_t lplanes = min(memSize / elemSize, wgsLimit) / lcols / lrows; + lplanes = roundDownPow2(lplanes); + size_t localSize[3] = { lcols, lrows, lplanes }; Vec3i ngroups((int)divUp(globalSize[0], (unsigned int)localSize[0]), (int)divUp(globalSize[1], (unsigned int)localSize[1]), (int)divUp(globalSize[2], (unsigned int)localSize[2])); const size_t counterSize = sizeof(int); - size_t lszscan = localSize[0]*localSize[1]*localSize[2]*counterSize; + size_t lszscan = localSize[0] * localSize[1] * localSize[2] * counterSize; - const int gsz[3] = {ngroups[2], ngroups[1], ngroups[0]}; + const int gsz[3] = { ngroups[2], ngroups[1], ngroups[0] }; UMat groupedSum(3, gsz, CV_32S, Scalar(0)); UMat volPoseGpu; Mat(pose.matrix).copyTo(volPoseGpu); Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); - kscan.args(ocl::KernelArg::PtrReadOnly(volume), - volResGpu.val, - volDims.val, - neighbourCoords.val, - ocl::KernelArg::PtrReadOnly(volPoseGpu), - voxelSize, - voxelSizeInv, - ocl::KernelArg::Local(lszscan), + kscan.args(ocl::KernelArg::PtrReadOnly(volume), volResGpu.val, volDims.val, + neighbourCoords.val, ocl::KernelArg::PtrReadOnly(volPoseGpu), voxelSize, + voxelSizeInv, ocl::KernelArg::Local(lszscan), ocl::KernelArg::WriteOnlyNoSize(groupedSum)); - if(!kscan.run(3, globalSize, localSize, true)) + if (!kscan.run(3, globalSize, localSize, true)) throw std::runtime_error("Failed to run kernel"); Mat groupedSumCpu = groupedSum.getMat(ACCESS_READ); - int gpuSum = (int)cv::sum(groupedSumCpu)[0]; + int gpuSum = (int)cv::sum(groupedSumCpu)[0]; // should be no CPU copies when new kernel is executing groupedSumCpu.release(); @@ -1386,7 +1364,7 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) points.create(gpuSum, 1, POINT_TYPE); UMat pts = points.getUMat(); UMat nrm; - if(needNormals) + if (needNormals) { normals.create(gpuSum, 1, POINT_TYPE); nrm = normals.getUMat(); @@ -1402,30 +1380,22 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) ocl::Kernel kfill; kfill.create("fillPtsNrm", source, options, &errorStr); - if(kfill.empty()) + if (kfill.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); UMat atomicCtr(1, 1, CV_32S, Scalar(0)); // mem size to keep pts (and normals optionally) for all work-items in a group - size_t lszfill = localSize[0]*localSize[1]*localSize[2]*elemSize; - - kfill.args(ocl::KernelArg::PtrReadOnly(volume), - volResGpu.val, - volDims.val, - neighbourCoords.val, - ocl::KernelArg::PtrReadOnly(volPoseGpu), - voxelSize, - voxelSizeInv, - ((int)needNormals), - ocl::KernelArg::Local(lszfill), + size_t lszfill = localSize[0] * localSize[1] * localSize[2] * elemSize; + + kfill.args(ocl::KernelArg::PtrReadOnly(volume), volResGpu.val, volDims.val, + neighbourCoords.val, ocl::KernelArg::PtrReadOnly(volPoseGpu), voxelSize, + voxelSizeInv, ((int)needNormals), ocl::KernelArg::Local(lszfill), ocl::KernelArg::PtrReadWrite(atomicCtr), ocl::KernelArg::ReadOnlyNoSize(groupedSum), - ocl::KernelArg::WriteOnlyNoSize(pts), - ocl::KernelArg::WriteOnlyNoSize(nrm) - ); + ocl::KernelArg::WriteOnlyNoSize(pts), ocl::KernelArg::WriteOnlyNoSize(nrm)); - if(!kfill.run(3, globalSize, localSize, true)) + if (!kfill.run(3, globalSize, localSize, true)) throw std::runtime_error("Failed to run kernel"); } } @@ -1433,15 +1403,17 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) #endif -cv::Ptr makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor) +cv::Ptr makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, + float _truncDist, int _maxWeight, float _raycastStepFactor) { #ifdef HAVE_OPENCL - if(cv::ocl::useOpenCL()) - return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor); + if (cv::ocl::useOpenCL()) + return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, + _raycastStepFactor); #endif - return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor); + return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, + _raycastStepFactor); } -} // namespace kinfu -} // namespace cv +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index b6de802fb48..f302a33aa91 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -7,6 +7,7 @@ #ifndef __OPENCV_KINFU_TSDF_H__ #define __OPENCV_KINFU_TSDF_H__ +#include "opencv2/core/affine.hpp" #include "kinfu_frame.hpp" namespace cv { From fcfd6ddb00811aba1b893cb3c5aa6db84affd318 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Thu, 11 Jun 2020 20:55:59 -0400 Subject: [PATCH 08/36] Bug fix for integration and noisy odometry --- modules/rgbd/samples/kinfu_demo.cpp | 4 +- modules/rgbd/src/hash_tsdf.cpp | 175 ++++++++++++++++++---------- 2 files changed, 114 insertions(+), 65 deletions(-) diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index cae21431005..291a7e54ecc 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -411,7 +411,7 @@ int main(int argc, char** argv) viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05, viz::Color::gray()); window.showWidget("cloud", cloudWidget); - /* window.showWidget("normals", cloudNormals); */ + window.showWidget("normals", cloudNormals); Vec3d volSize = kf->getParams().voxelSize * Vec3d(kf->getParams().volumeDims); window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), @@ -424,7 +424,7 @@ int main(int argc, char** argv) Point())); window.spin(); window.removeWidget("text"); - /* window.removeWidget("cloud"); */ + window.removeWidget("cloud"); window.removeWidget("normals"); window.registerMouseCallback(0); } diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d51138c8478..e95a9e497f5 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -19,7 +19,7 @@ namespace cv { namespace kinfu { -HashTSDFVolume::HashTSDFVolume(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, +HashTSDFVolume::HashTSDFVolume(float _voxelSize, int _volumeUnitRes, cv::Affine3f _pose, float _truncDist, int _maxWeight, float _raycastStepFactor, bool _zFirstMemOrder) : voxelSize(_voxelSize), @@ -27,18 +27,18 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, int _volume_unit_res, cv::Affin pose(_pose), maxWeight(_maxWeight), raycastStepFactor(_raycastStepFactor), - volumeUnitResolution(_volume_unit_res), + volumeUnitResolution(_volumeUnitRes), volumeUnitSize(voxelSize * volumeUnitResolution), zFirstMemOrder(_zFirstMemOrder) { truncDist = std::max(_truncDist, 2.1f * voxelSize); } -HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, +HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, int _volumeUnitRes, cv::Affine3f _pose, float _truncDist, int _maxWeight, float _raycastStepFactor, bool _zFirstMemOrder) - : HashTSDFVolume(_voxelSize, _volume_unit_res, _pose, _truncDist, _maxWeight, - _raycastStepFactor, _zFirstMemOrder) + : HashTSDFVolume(_voxelSize, _volumeUnitRes, _pose, _truncDist, _maxWeight, _raycastStepFactor, + _zFirstMemOrder) { } @@ -51,12 +51,10 @@ void HashTSDFVolumeCPU::reset() struct AccessedVolumeUnitsInvoker : ParallelLoopBody { - AccessedVolumeUnitsInvoker(HashTSDFVolumeCPU& _volume, VolumeUnitIndexSet& _accessVolUnits, - const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose, - float _depthFactor, int _depthStride = 4) + AccessedVolumeUnitsInvoker(HashTSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, + cv::Affine3f cameraPose, float _depthFactor, int _depthStride = 4) : ParallelLoopBody(), volume(_volume), - accessVolUnits(_accessVolUnits), depth(_depth), reproj(intrinsics.makeReprojector()), cam2vol(_volume.pose.inv() * cameraPose), @@ -84,14 +82,12 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody volPoint - cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); cv::Vec3i upper_bound = volume.volumeToVolumeUnitIdx( volPoint + cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); - VolumeUnitIndexSet localAccessVolUnits; for (int i = lower_bound[0]; i <= upper_bound[0]; i++) for (int j = lower_bound[1]; j <= upper_bound[1]; j++) for (int k = lower_bound[2]; k <= lower_bound[2]; k++) { const cv::Vec3i tsdf_idx = cv::Vec3i(i, j, k); - AutoLock al(mutex); if (!localAccessVolUnits.count(tsdf_idx)) { localAccessVolUnits.emplace(tsdf_idx); @@ -100,7 +96,8 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody AutoLock al(mutex); for (const auto& tsdf_idx : localAccessVolUnits) { - if (accessVolUnits.emplace(tsdf_idx).second) + //! If the insert into the global set passes + if (!volume.volumeUnits.count(tsdf_idx)) { VolumeUnit volumeUnit; cv::Point3i volumeDims(volume.volumeUnitResolution, @@ -113,8 +110,8 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody volumeDims, volume.voxelSize, subvolumePose, volume.truncDist, volume.maxWeight, volume.raycastStepFactor); //! This volume unit will definitely be required for current integration - volumeUnit.index = tsdf_idx; - volumeUnit.isActive = true; + volumeUnit.index = tsdf_idx; + volumeUnit.isActive = true; volume.volumeUnits[tsdf_idx] = volumeUnit; } } @@ -123,7 +120,6 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody } HashTSDFVolumeCPU& volume; - VolumeUnitIndexSet& accessVolUnits; const Depth& depth; const Intr::Reprojector reproj; const cv::Affine3f cam2vol; @@ -179,12 +175,11 @@ struct IntegrateSubvolumeInvoker : ParallelLoopBody struct VolumeUnitInFrustumInvoker : ParallelLoopBody { VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, - const std::vector& _totalVolUnits, int& _noOfActiveVolumeUnits, const Depth& _depth, + const std::vector& _totalVolUnits, const Depth& _depth, Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : ParallelLoopBody(), volume(_volume), totalVolUnits(_totalVolUnits), - noOfActiveVolumeUnits(_noOfActiveVolumeUnits), depth(_depth), proj(_intrinsics.makeProjector()), depthFactor(_depthFactor), @@ -216,16 +211,10 @@ struct VolumeUnitInFrustumInvoker : ParallelLoopBody assert(it != volume.volumeUnits.end()); it->second.isActive = true; } - AutoLock al(mutex); - if(it->second.isActive) - { - noOfActiveVolumeUnits++; - } } } HashTSDFVolumeCPU& volume; const std::vector totalVolUnits; - int& noOfActiveVolumeUnits; const Depth& depth; const Intr::Projector proj; const float depthFactor; @@ -240,26 +229,53 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affi CV_Assert(_depth.type() == DEPTH_TYPE); Depth depth = _depth.getMat(); - VolumeUnitIndexSet accessVolUnits; - AccessedVolumeUnitsInvoker allocate_i(*this, accessVolUnits, depth, intrinsics, cameraPose, - depthFactor); + + int activeVolUnits = 0; + for (const auto& keyvalue : volumeUnits) + { + const VolumeUnit& volUnit = keyvalue.second; + if (volUnit.isActive) + { + activeVolUnits++; + } + } + std::cout << " Active volume Units before integration: " << activeVolUnits << "\n"; + AccessedVolumeUnitsInvoker allocate_i(*this, depth, intrinsics, cameraPose, depthFactor); Range range(0, depth.rows); parallel_for_(range, allocate_i); + activeVolUnits = 0; + for (const auto& keyvalue : volumeUnits) + { + const VolumeUnit& volUnit = keyvalue.second; + if (volUnit.isActive) + { + activeVolUnits++; + } + } + std::cout << "Active volumes after allocation: " << activeVolUnits << "\n"; std::vector totalVolUnits; for (const auto& keyvalue : volumeUnits) { totalVolUnits.push_back(keyvalue.first); } - int noOfActiveVolumeUnits = 0; - VolumeUnitInFrustumInvoker infrustum_i(*this, totalVolUnits, noOfActiveVolumeUnits, depth, intrinsics, cameraPose, + VolumeUnitInFrustumInvoker infrustum_i(*this, totalVolUnits, depth, intrinsics, cameraPose, depthFactor); Range in_frustum_range(0, volumeUnits.size()); parallel_for_(in_frustum_range, infrustum_i); - std::cout << "Number of new volume units created: " << accessVolUnits.size() << "\n"; - std::cout << " Number of total units in volume: " << volumeUnits.size() << " " << totalVolUnits.size() << "\n"; - std::cout << " Number of Active volume units in frame: " << noOfActiveVolumeUnits << "\n"; + std::cout << " Number of total units in volume: " << volumeUnits.size() << " " + << totalVolUnits.size() << "\n"; + activeVolUnits = 0; + for (const auto& keyvalue : volumeUnits) + { + const VolumeUnit& volUnit = keyvalue.second; + if (volUnit.isActive) + { + activeVolUnits++; + } + } + std::cout << " Number of Active volume units in frame: " << activeVolUnits << "\n"; IntegrateSubvolumeInvoker integrate_i(*this, totalVolUnits, depth, intrinsics, cameraPose, depthFactor); @@ -344,7 +360,8 @@ inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(const cv::Point3f& point) co { cv::Vec3i voxelIdx = volumeToVoxelCoord(point); - Point3f t = point - Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); + Point3f t = + point - Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); std::array neighbourCoords = { cv::Vec3i(0, 0, 0), cv::Vec3i(0, 0, 1), cv::Vec3i(0, 1, 0), cv::Vec3i(0, 1, 1), @@ -355,9 +372,10 @@ inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(const cv::Point3f& point) co for (int i = 0; i < 8; ++i) { Voxel voxel = at(neighbourCoords[i] + voxelIdx); - /* std::cout << "VoxelIdx : " << voxelIdx << " voxel weight: " << voxel.weight << " voxel value: " << voxel.tsdf << "\n"; */ + /* std::cout << "VoxelIdx : " << voxelIdx << " voxel weight: " << voxel.weight << " voxel + * value: " << voxel.tsdf << "\n"; */ /* if(voxel.weight != 0) */ - tsdf_array[i] = voxel.tsdf; + tsdf_array[i] = voxel.tsdf; } TsdfType v00 = tsdf_array[0] + t.z * (tsdf_array[1] - tsdf_array[0]); @@ -373,24 +391,6 @@ inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(const cv::Point3f& point) co inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const { - /* cv::Vec3i voxelIdx = volumeToVoxelCoord(point); */ - - /* Vec3i prev = voxelIdx; */ - /* Vec3i next = voxelIdx; */ - /* Vec3f normal = Vec3f(0, 0, 0); */ - - /* for (int c = 0; c < 3; c++) */ - /* { */ - /* prev[c] -= 1; */ - /* next[c] += 1; */ - - /* normal[c] = at(next).tsdf - at(prev).tsdf; */ - /* normal[c] *= 0.5; */ - - /* prev[c] = voxelIdx[c]; */ - /* next[c] = voxelIdx[c]; */ - /* } */ - /* return normalize(normal); */ Vec3f pointVec(point); Vec3f normal = Vec3f(0, 0, 0); @@ -404,10 +404,7 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const pointNext[c] += voxelSize * 0.5; normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; - /* std::cout << "pointPrev, pointNext: " << at(Point3f(pointPrev)).tsdf << ", " << - * at(Point3f(pointNext)).tsdf << std::endl; */ normal[c] *= 0.5; - /* std::cout << "TSDF diff: " << normal[c] << std::endl; */ pointPrev[c] = pointVec[c]; pointNext[c] = pointVec[c]; @@ -420,7 +417,7 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const /* cv::Vec3i voxelIdx = volumeToVoxelCoord(point); */ /* cv::Vec3i next = voxelIdx; */ /* cv::Vec3i prev = voxelIdx; */ -/* Point3f t = point - Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); */ +/* Point3f t = point - Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2]* voxelSize); */ /* std::array neighbourCoords = { */ /* cv::Vec3i(0, 0, 0), */ @@ -496,19 +493,65 @@ struct RaycastInvoker : ParallelLoopBody Point3f point = nan3, normal = nan3; //! Ray origin and direction in the volume coordinate frame - Point3f orig = cam2volTrans; - Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(x, y, 1.f)))); + Point3f orig = cam2volTrans; + Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(x, y, 1.f)))); + Vec3f rayDirInvV = cv::Vec3f(1.f / rayDirV.x, 1.f / rayDirV.y, 1.f / rayDirV.z); float tmin = rgbd::Odometry::DEFAULT_MIN_DEPTH() / rayDirV.z; float tmax = rgbd::Odometry::DEFAULT_MAX_DEPTH() / rayDirV.z; - float tprev = tmin; float tcurr = tmin; - TsdfType prevTsdf = volume.truncDist; cv::Vec3i prevVolumeUnitIdx = cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), std::numeric_limits::min()); + + Point3f startRayPos = orig + tmin * rayDirV; + + cv::Vec3i startVolumeUnitIdx = volume.volumeToVolumeUnitIdx(startRayPos); + + //! Find the intersection of the ray with the current volume Unit and advance the + //! rayposition + Point3f firstVolumeUnitPos = volume.volumeUnitIdxToVolume(startVolumeUnitIdx); + Point3f firstVolUnitEndPos = + firstVolumeUnitPos + Point3f(volume.volumeUnitSize - volume.voxelSize, + volume.volumeUnitSize - volume.voxelSize, + volume.volumeUnitSize - volume.voxelSize); + + Point3f tbottom = rayDirInvV.mul(firstVolumeUnitPos - orig); + Point3f ttop = rayDirInvV.mul(firstVolUnitEndPos - orig); + + Point3f minAx(min(ttop.x, tbottom.x), min(ttop.y, tbottom.y), + min(ttop.z, tbottom.z)); + Point3f maxAx(max(ttop.x, tbottom.x), max(ttop.y, tbottom.y), + max(ttop.z, tbottom.z)); + + float clip = 0.f; + float tstart = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip); + float tend = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z)); + // precautions against getting coordinates out of bounds + /* std::cout << " tstart : " << tstart << " tend: " << tend << "\n"; */ + { + Point3f startRayPos = orig + tstart * rayDirV; + Point3f endRayPos = orig + tend * rayDirV; + cv::Vec3i volUnitLocalStartIdx = volume.volumeToVoxelCoord( + startRayPos - + volume.volumeUnitIdxToVolume(volume.volumeToVolumeUnitIdx(startRayPos))); + cv::Vec3i volUnitLocalEndIdx = volume.volumeToVoxelCoord( + endRayPos - + volume.volumeUnitIdxToVolume(volume.volumeToVolumeUnitIdx(endRayPos))); + + /* std::cout << "minAx: " << minAx << " maxAx: " << maxAx << "\n"; */ + /* std::cout << " StartVolIdx: " << volume.volumeToVolumeUnitIdx(startRayPos) << + * " VolUnitLocalStartIdx : " << volUnitLocalStartIdx */ + /* << " EndVolIdx: " << volume.volumeToVolumeUnitIdx(endRayPos) << " + * VolUnitLocalEndIdx : " << volUnitLocalEndIdx << "\n"; */ + } + + tcurr = tend - tstep; + float tprev = tcurr; + + TsdfType prevTsdf = volume.truncDist; cv::Ptr currVolumeUnit; while (tcurr < tmax) { @@ -534,8 +577,11 @@ struct RaycastInvoker : ParallelLoopBody //! TODO: Figure out voxel interpolation Voxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); currTsdf = currVoxel.tsdf; + /* std::cout << " volumeUnitLocalIdx : " << volUnitLocalIdx << " Current + * TSDF: " << currTsdf << "\n"; */ /* currTsdf = volume.interpolateVoxel(currRayPos); */ - /* std::cout << "Current TSDF: " << currTsdf << " interpolated: " << interpolatedTsdf << "\n"; */ + /* std::cout << "Current TSDF: " << currTsdf << " interpolated: " << + * interpolatedTsdf << "\n"; */ currWeight = currVoxel.weight; stepSize = tstep; } @@ -544,7 +590,8 @@ struct RaycastInvoker : ParallelLoopBody { float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); - /* std::cout << "tcurr: " << tcurr << " tprev: " << tprev << " tInterp: " << tInterp << "\n"; */ + /* std::cout << "tcurr: " << tcurr << " tprev: " << tprev << " tInterp: " << + * tInterp << "\n"; */ if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { Point3f pv = orig + tInterp * rayDirV; @@ -576,6 +623,7 @@ struct RaycastInvoker : ParallelLoopBody const Affine3f cam2vol; const Affine3f vol2cam; const Intr::Reprojector reproj; + mutable Mutex mutex; }; void HashTSDFVolumeCPU::raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, @@ -675,7 +723,8 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { totalVolUnits.push_back(keyvalue.first); } - std::cout << "Number of volumeUnits in volume(fetchPoints): " << totalVolUnits.size() << "\n"; + std::cout << "Number of volumeUnits in volume(fetchPoints): " << totalVolUnits.size() + << "\n"; FetchPointsNormalsInvoker fi(*this, totalVolUnits, pVecs, nVecs, _normals.needed()); Range range(0, totalVolUnits.size()); const int nstripes = -1; From ddf5c18590d62f9e5f28c39a258f55b87410cb75 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Sun, 14 Jun 2020 23:41:15 -0400 Subject: [PATCH 09/36] - Create volume abstract class - Address Review comments --- modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 14 +- modules/rgbd/samples/kinfu_demo.cpp | 12 +- modules/rgbd/src/hash_tsdf.cpp | 371 ++++++-------------- modules/rgbd/src/hash_tsdf.hpp | 94 +++-- modules/rgbd/src/kinfu.cpp | 102 +++--- modules/rgbd/src/kinfu_frame.cpp | 7 +- modules/rgbd/src/kinfu_frame.hpp | 1 + modules/rgbd/src/tsdf.cpp | 133 +++---- modules/rgbd/src/tsdf.hpp | 77 ++-- modules/rgbd/src/utils.hpp | 5 +- modules/rgbd/src/volume.hpp | 59 ++++ 11 files changed, 406 insertions(+), 469 deletions(-) create mode 100644 modules/rgbd/src/volume.hpp diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index d91c62f91bb..17bebe33fa5 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -17,6 +17,11 @@ namespace kinfu { struct CV_EXPORTS_W Params { + enum VolumeType + { + TSDF = 0, + HASHTSDF = 1 + }; CV_WRAP Params(){} @@ -68,9 +73,16 @@ struct CV_EXPORTS_W Params */ CV_WRAP static Ptr coarseParams(); + /** @brief HashTSDF parameters + A set of parameters suitable for use with HashTSDFVolume + */ + CV_WRAP static Ptr hashTSDFParams(bool isCoarse); + /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; + CV_PROP_RW VolumeType volumeType; + /** @brief camera intrinsics */ CV_PROP_RW Matx33f intr; @@ -126,7 +138,7 @@ struct CV_EXPORTS_W Params How much voxel sizes we skip each raycast step */ - CV_PROP_RW float raycast_step_factor; + CV_PROP_RW float raycastStepFactor; // gradient delta in voxel sizes // fixed at 1.0f diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index 291a7e54ecc..00ba0ccea20 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -297,6 +297,7 @@ static const char* keys = { "{camera |0| Index of depth camera to be used as a depth source }" "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," " in coarse mode points and normals are displayed }" + "{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}" "{idle | | Do not run KinFu, just display depth frames }" "{record | | Write depth frames to specified file list" " (the same format as for the 'depth' key) }" @@ -311,6 +312,7 @@ int main(int argc, char** argv) { bool coarse = false; bool idle = false; + bool useHashTSDF = false; string recordPath; CommandLineParser parser(argc, argv, keys); @@ -336,11 +338,16 @@ int main(int argc, char** argv) { recordPath = parser.get("record"); } + if (parser.has("useHashTSDF")) + { + useHashTSDF = true; + } if (parser.has("idle")) { idle = true; } + Ptr ds; if (parser.has("depth")) ds = makePtr(parser.get("depth")); @@ -366,11 +373,14 @@ int main(int argc, char** argv) else params = Params::defaultParams(); + if(useHashTSDF) + params = Params::hashTSDFParams(coarse); + // These params can be different for each depth sensor ds->updateParams(*params); // Enables OpenCL explicitly (by default can be switched-off) - cv::setUseOptimized(false); + cv::setUseOptimized(true); // Scene-specific params should be tuned for each scene individually // float cubeSize = 1.f; diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index e95a9e497f5..e87b3ded9c1 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -19,26 +19,24 @@ namespace cv { namespace kinfu { -HashTSDFVolume::HashTSDFVolume(float _voxelSize, int _volumeUnitRes, cv::Affine3f _pose, - float _truncDist, int _maxWeight, float _raycastStepFactor, - bool _zFirstMemOrder) - : voxelSize(_voxelSize), - voxelSizeInv(1.0f / _voxelSize), - pose(_pose), +HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, float _truncateThreshold, + int _volumeUnitRes, bool _zFirstMemOrder) + : Volume(_voxelSize, _pose, _raycastStepFactor), maxWeight(_maxWeight), - raycastStepFactor(_raycastStepFactor), + truncateThreshold(_truncateThreshold), volumeUnitResolution(_volumeUnitRes), volumeUnitSize(voxelSize * volumeUnitResolution), zFirstMemOrder(_zFirstMemOrder) { - truncDist = std::max(_truncDist, 2.1f * voxelSize); + truncDist = std::max(_truncDist, 4.0f * voxelSize); } -HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, int _volumeUnitRes, cv::Affine3f _pose, - float _truncDist, int _maxWeight, float _raycastStepFactor, - bool _zFirstMemOrder) - : HashTSDFVolume(_voxelSize, _volumeUnitRes, _pose, _truncDist, _maxWeight, _raycastStepFactor, - _zFirstMemOrder) +HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, float _truncateThreshold, + int _volumeUnitRes, bool _zFirstMemOrder) + : HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { } @@ -49,9 +47,9 @@ void HashTSDFVolumeCPU::reset() volumeUnits.clear(); } -struct AccessedVolumeUnitsInvoker : ParallelLoopBody +struct AllocateVolumeUnitsInvoker : ParallelLoopBody { - AccessedVolumeUnitsInvoker(HashTSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, + AllocateVolumeUnitsInvoker(HashTSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose, float _depthFactor, int _depthStride = 4) : ParallelLoopBody(), volume(_volume), @@ -107,11 +105,11 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody cv::Affine3f subvolumePose = volume.pose.translate(volume.volumeUnitIdxToVolume(tsdf_idx)); volumeUnit.pVolume = cv::makePtr( - volumeDims, volume.voxelSize, subvolumePose, volume.truncDist, - volume.maxWeight, volume.raycastStepFactor); + volume.voxelSize, subvolumePose, volume.raycastStepFactor, + volume.truncDist, volume.maxWeight, volumeDims); //! This volume unit will definitely be required for current integration - volumeUnit.index = tsdf_idx; - volumeUnit.isActive = true; + volumeUnit.index = tsdf_idx; + volumeUnit.isActive = true; volume.volumeUnits[tsdf_idx] = volumeUnit; } } @@ -128,97 +126,96 @@ struct AccessedVolumeUnitsInvoker : ParallelLoopBody mutable Mutex mutex; }; -struct IntegrateSubvolumeInvoker : ParallelLoopBody +struct VolumeUnitInFrustumInvoker : ParallelLoopBody { - IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, - const std::vector& _totalVolUnits, const Depth& _depth, - Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) + VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, + const std::vector& _totalVolUnits, const Depth& _depth, + Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : ParallelLoopBody(), volume(_volume), totalVolUnits(_totalVolUnits), depth(_depth), + proj(_intrinsics.makeProjector()), depthFactor(_depthFactor), - cameraPose(_cameraPose), - intrinsics(_intrinsics) + vol2cam(_cameraPose.inv() * _volume.pose) { } virtual void operator()(const Range& range) const override { - for (int i = range.start; i < range.end; i++) + for (int i = range.start; i < range.end; ++i) { cv::Vec3i tsdf_idx = totalVolUnits[i]; VolumeUnitMap::iterator it = volume.volumeUnits.find(tsdf_idx); if (it == volume.volumeUnits.end()) return; - VolumeUnit& volumeUnit = it->second; - if (volumeUnit.isActive) + Point3f volumeUnitPos = volume.volumeUnitIdxToVolume(it->first); + Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; + if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > volume.truncateThreshold) { - //! The volume unit should already be added into the Volume from the allocator - volumeUnit.pVolume->integrate(depth, depthFactor, cameraPose, intrinsics); - //! Ensure all active volumeUnits are set to inactive for next integration - volumeUnit.isActive = false; + it->second.isActive = false; + return; + } + Point2f cameraPoint = proj(volUnitInCamSpace); + if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && + cameraPoint.y < depth.rows) + { + assert(it != volume.volumeUnits.end()); + it->second.isActive = true; } } } - HashTSDFVolumeCPU& volume; - std::vector totalVolUnits; + const std::vector totalVolUnits; const Depth& depth; - float depthFactor; - cv::Affine3f cameraPose; - Intr intrinsics; + const Intr::Projector proj; + const float depthFactor; + const Affine3f vol2cam; mutable Mutex mutex; }; -struct VolumeUnitInFrustumInvoker : ParallelLoopBody +struct IntegrateSubvolumeInvoker : ParallelLoopBody { - VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, - const std::vector& _totalVolUnits, const Depth& _depth, - Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) + IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, + const std::vector& _totalVolUnits, const Depth& _depth, + Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) : ParallelLoopBody(), volume(_volume), totalVolUnits(_totalVolUnits), depth(_depth), - proj(_intrinsics.makeProjector()), depthFactor(_depthFactor), - vol2cam(_cameraPose.inv() * _volume.pose) + cameraPose(_cameraPose), + intrinsics(_intrinsics) { } virtual void operator()(const Range& range) const override { - for (int i = range.start; i < range.end; ++i) + for (int i = range.start; i < range.end; i++) { cv::Vec3i tsdf_idx = totalVolUnits[i]; VolumeUnitMap::iterator it = volume.volumeUnits.find(tsdf_idx); if (it == volume.volumeUnits.end()) return; - Point3f volumeUnitPos = volume.volumeUnitIdxToVolume(it->first); - Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; - if (volUnitInCamSpace.z < rgbd::Odometry::DEFAULT_MIN_DEPTH() || - volUnitInCamSpace.z > rgbd::Odometry::DEFAULT_MAX_DEPTH()) - { - it->second.isActive = false; - return; - } - Point2f cameraPoint = proj(volUnitInCamSpace); - if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && - cameraPoint.y < depth.rows) + VolumeUnit& volumeUnit = it->second; + if (volumeUnit.isActive) { - assert(it != volume.volumeUnits.end()); - it->second.isActive = true; + //! The volume unit should already be added into the Volume from the allocator + volumeUnit.pVolume->integrate(depth, depthFactor, cameraPose, intrinsics); + //! Ensure all active volumeUnits are set to inactive for next integration + volumeUnit.isActive = false; } } } + HashTSDFVolumeCPU& volume; - const std::vector totalVolUnits; + std::vector totalVolUnits; const Depth& depth; - const Intr::Projector proj; - const float depthFactor; - const Affine3f vol2cam; + float depthFactor; + cv::Affine3f cameraPose; + Intr intrinsics; mutable Mutex mutex; }; @@ -229,31 +226,32 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affi CV_Assert(_depth.type() == DEPTH_TYPE); Depth depth = _depth.getMat(); - - int activeVolUnits = 0; - for (const auto& keyvalue : volumeUnits) - { - const VolumeUnit& volUnit = keyvalue.second; - if (volUnit.isActive) - { - activeVolUnits++; - } - } - std::cout << " Active volume Units before integration: " << activeVolUnits << "\n"; - AccessedVolumeUnitsInvoker allocate_i(*this, depth, intrinsics, cameraPose, depthFactor); + /* int activeVolUnits = 0; */ + /* for (const auto& keyvalue : volumeUnits) */ + /* { */ + /* const VolumeUnit& volUnit = keyvalue.second; */ + /* if (volUnit.isActive) */ + /* { */ + /* activeVolUnits++; */ + /* } */ + /* } */ + /* std::cout << " Active volume Units before integration: " << activeVolUnits << "\n"; */ + //! Compute volumes to be allocated + AllocateVolumeUnitsInvoker allocate_i(*this, depth, intrinsics, cameraPose, depthFactor); Range range(0, depth.rows); parallel_for_(range, allocate_i); - activeVolUnits = 0; - for (const auto& keyvalue : volumeUnits) - { - const VolumeUnit& volUnit = keyvalue.second; - if (volUnit.isActive) - { - activeVolUnits++; - } - } - std::cout << "Active volumes after allocation: " << activeVolUnits << "\n"; + /* activeVolUnits = 0; */ + /* for (const auto& keyvalue : volumeUnits) */ + /* { */ + /* const VolumeUnit& volUnit = keyvalue.second; */ + /* if (volUnit.isActive) */ + /* { */ + /* activeVolUnits++; */ + /* } */ + /* } */ + /* std::cout << "Active volumes after allocation: " << activeVolUnits << "\n"; */ + //! Get volumes that are in the current camera frame std::vector totalVolUnits; for (const auto& keyvalue : volumeUnits) { @@ -263,20 +261,19 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affi depthFactor); Range in_frustum_range(0, volumeUnits.size()); parallel_for_(in_frustum_range, infrustum_i); - - std::cout << " Number of total units in volume: " << volumeUnits.size() << " " - << totalVolUnits.size() << "\n"; - activeVolUnits = 0; - for (const auto& keyvalue : volumeUnits) - { - const VolumeUnit& volUnit = keyvalue.second; - if (volUnit.isActive) - { - activeVolUnits++; - } - } - std::cout << " Number of Active volume units in frame: " << activeVolUnits << "\n"; - + /* std::cout << " Number of total units in volume: " << volumeUnits.size() << " " */ + /* << totalVolUnits.size() << "\n"; */ + /* activeVolUnits = 0; */ + /* for (const auto& keyvalue : volumeUnits) */ + /* { */ + /* const VolumeUnit& volUnit = keyvalue.second; */ + /* if (volUnit.isActive) */ + /* { */ + /* activeVolUnits++; */ + /* } */ + /* } */ + /* std::cout << " Number of Active volume units in frame: " << activeVolUnits << "\n"; */ + //! Integrate the correct volumeUnits IntegrateSubvolumeInvoker integrate_i(*this, totalVolUnits, depth, intrinsics, cameraPose, depthFactor); Range accessed_units_range(0, totalVolUnits.size()); @@ -306,7 +303,7 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const cvFloor(point.z * voxelSizeInv)); } -inline Voxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const +inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), @@ -315,8 +312,8 @@ inline Voxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); if (it == volumeUnits.end()) { - Voxel dummy; - dummy.tsdf = 1.f; + TsdfVoxel dummy; + dummy.tsdf = float16_t(1.f); dummy.weight = 0; return dummy; } @@ -329,19 +326,17 @@ inline Voxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - /* std::cout << "VolumeIdx: " << volumeIdx << " VolumeUnitIdx: " << volumeUnitIdx << " Volume - * Local unit idx: " << volUnitLocalIdx << "\n"; */ return volumeUnit->at(volUnitLocalIdx); } -inline Voxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const +inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const { cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); if (it == volumeUnits.end()) { - Voxel dummy; - dummy.tsdf = 1.f; + TsdfVoxel dummy; + dummy.tsdf = float16_t(1.f); dummy.weight = 0; return dummy; } @@ -352,46 +347,11 @@ inline Voxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return volumeUnit->at(volUnitLocalIdx); } -inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(const cv::Point3f& point) const -{ - cv::Vec3i voxelIdx = volumeToVoxelCoord(point); - - Point3f t = - point - Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); - - std::array neighbourCoords = { cv::Vec3i(0, 0, 0), cv::Vec3i(0, 0, 1), - cv::Vec3i(0, 1, 0), cv::Vec3i(0, 1, 1), - cv::Vec3i(1, 0, 0), cv::Vec3i(1, 0, 1), - cv::Vec3i(1, 1, 0), cv::Vec3i(1, 1, 1) }; - - std::array tsdf_array; - for (int i = 0; i < 8; ++i) - { - Voxel voxel = at(neighbourCoords[i] + voxelIdx); - /* std::cout << "VoxelIdx : " << voxelIdx << " voxel weight: " << voxel.weight << " voxel - * value: " << voxel.tsdf << "\n"; */ - /* if(voxel.weight != 0) */ - tsdf_array[i] = voxel.tsdf; - } - - TsdfType v00 = tsdf_array[0] + t.z * (tsdf_array[1] - tsdf_array[0]); - TsdfType v01 = tsdf_array[2] + t.z * (tsdf_array[3] - tsdf_array[2]); - TsdfType v10 = tsdf_array[4] + t.z * (tsdf_array[5] - tsdf_array[4]); - TsdfType v11 = tsdf_array[6] + t.z * (tsdf_array[7] - tsdf_array[6]); - - TsdfType v0 = v00 + t.y * (v01 - v00); - TsdfType v1 = v10 + t.y * (v11 - v10); - /* std::cout << " interpolation t: " << t << std::endl; */ - return v0 + t.x * (v1 - v0); -} - inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const { - Vec3f pointVec(point); Vec3f normal = Vec3f(0, 0, 0); @@ -412,53 +372,6 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const return normalize(normal); } -/* inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const */ -/* { */ -/* cv::Vec3i voxelIdx = volumeToVoxelCoord(point); */ -/* cv::Vec3i next = voxelIdx; */ -/* cv::Vec3i prev = voxelIdx; */ -/* Point3f t = point - Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2]* voxelSize); */ - -/* std::array neighbourCoords = { */ -/* cv::Vec3i(0, 0, 0), */ -/* cv::Vec3i(0, 0, 1), */ -/* cv::Vec3i(0, 1, 0), */ -/* cv::Vec3i(0, 1, 1), */ -/* cv::Vec3i(1, 0, 0), */ -/* cv::Vec3i(1, 0, 1), */ -/* cv::Vec3i(1, 1, 0), */ -/* cv::Vec3i(1, 1, 1)}; */ - -/* Vec3f normal; */ -/* for(int c = 0; c < 3; c++) */ -/* { */ -/* float& nv = normal[c]; */ - -/* next[c] += 1; */ -/* prev[c] -= 1; */ - -/* std::array vx; */ -/* for(int i = 0; i < 8; i++) */ -/* vx[i] = at(neighbourCoords[i] + next).tsdf - */ -/* at(neighbourCoords[i] + prev).tsdf; */ - -/* TsdfType v00 = vx[0] + t.z*(vx[1] - vx[0]); */ -/* TsdfType v01 = vx[2] + t.z*(vx[3] - vx[2]); */ -/* TsdfType v10 = vx[4] + t.z*(vx[5] - vx[4]); */ -/* TsdfType v11 = vx[6] + t.z*(vx[7] - vx[6]); */ - -/* TsdfType v0 = v00 + t.y*(v01 - v00); */ -/* TsdfType v1 = v10 + t.y*(v11 - v10); */ - -/* nv = v0 + t.x*(v1 - v0); */ - -/* next[c] = voxelIdx[c]; */ -/* prev[c] = voxelIdx[c]; */ -/* } */ - -/* return normalize(normal); */ -/* } */ - struct RaycastInvoker : ParallelLoopBody { RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, Intr intrinsics, @@ -497,61 +410,16 @@ struct RaycastInvoker : ParallelLoopBody Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(x, y, 1.f)))); Vec3f rayDirInvV = cv::Vec3f(1.f / rayDirV.x, 1.f / rayDirV.y, 1.f / rayDirV.z); - float tmin = rgbd::Odometry::DEFAULT_MIN_DEPTH() / rayDirV.z; - float tmax = rgbd::Odometry::DEFAULT_MAX_DEPTH() / rayDirV.z; - + float tmin = 0; + float tmax = volume.truncateThreshold / rayDirV.z; float tcurr = tmin; cv::Vec3i prevVolumeUnitIdx = cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), std::numeric_limits::min()); - Point3f startRayPos = orig + tmin * rayDirV; - - cv::Vec3i startVolumeUnitIdx = volume.volumeToVolumeUnitIdx(startRayPos); - - //! Find the intersection of the ray with the current volume Unit and advance the - //! rayposition - Point3f firstVolumeUnitPos = volume.volumeUnitIdxToVolume(startVolumeUnitIdx); - Point3f firstVolUnitEndPos = - firstVolumeUnitPos + Point3f(volume.volumeUnitSize - volume.voxelSize, - volume.volumeUnitSize - volume.voxelSize, - volume.volumeUnitSize - volume.voxelSize); - - Point3f tbottom = rayDirInvV.mul(firstVolumeUnitPos - orig); - Point3f ttop = rayDirInvV.mul(firstVolUnitEndPos - orig); - - Point3f minAx(min(ttop.x, tbottom.x), min(ttop.y, tbottom.y), - min(ttop.z, tbottom.z)); - Point3f maxAx(max(ttop.x, tbottom.x), max(ttop.y, tbottom.y), - max(ttop.z, tbottom.z)); - - float clip = 0.f; - float tstart = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip); - float tend = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z)); - // precautions against getting coordinates out of bounds - /* std::cout << " tstart : " << tstart << " tend: " << tend << "\n"; */ - { - Point3f startRayPos = orig + tstart * rayDirV; - Point3f endRayPos = orig + tend * rayDirV; - cv::Vec3i volUnitLocalStartIdx = volume.volumeToVoxelCoord( - startRayPos - - volume.volumeUnitIdxToVolume(volume.volumeToVolumeUnitIdx(startRayPos))); - cv::Vec3i volUnitLocalEndIdx = volume.volumeToVoxelCoord( - endRayPos - - volume.volumeUnitIdxToVolume(volume.volumeToVolumeUnitIdx(endRayPos))); - - /* std::cout << "minAx: " << minAx << " maxAx: " << maxAx << "\n"; */ - /* std::cout << " StartVolIdx: " << volume.volumeToVolumeUnitIdx(startRayPos) << - * " VolUnitLocalStartIdx : " << volUnitLocalStartIdx */ - /* << " EndVolIdx: " << volume.volumeToVolumeUnitIdx(endRayPos) << " - * VolUnitLocalEndIdx : " << volUnitLocalEndIdx << "\n"; */ - } - - tcurr = tend - tstep; float tprev = tcurr; - - TsdfType prevTsdf = volume.truncDist; + TsdfType prevTsdf = float16_t(volume.truncDist); cv::Ptr currVolumeUnit; while (tcurr < tmax) { @@ -562,7 +430,7 @@ struct RaycastInvoker : ParallelLoopBody TsdfType currTsdf = prevTsdf; int currWeight = 0; - float stepSize = blockSize; + float stepSize = 0.5 * blockSize; cv::Vec3i volUnitLocalIdx; //! Does the subvolume exist in hashtable @@ -575,23 +443,16 @@ struct RaycastInvoker : ParallelLoopBody volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation - Voxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); - currTsdf = currVoxel.tsdf; - /* std::cout << " volumeUnitLocalIdx : " << volUnitLocalIdx << " Current - * TSDF: " << currTsdf << "\n"; */ - /* currTsdf = volume.interpolateVoxel(currRayPos); */ - /* std::cout << "Current TSDF: " << currTsdf << " interpolated: " << - * interpolatedTsdf << "\n"; */ - currWeight = currVoxel.weight; - stepSize = tstep; + TsdfVoxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); + currTsdf = 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); - /* std::cout << "tcurr: " << tcurr << " tprev: " << tprev << " tInterp: " << - * tInterp << "\n"; */ if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { Point3f pv = orig + tInterp * rayDirV; @@ -623,7 +484,6 @@ struct RaycastInvoker : ParallelLoopBody const Affine3f cam2vol; const Affine3f vol2cam; const Intr::Reprojector reproj; - mutable Mutex mutex; }; void HashTSDFVolumeCPU::raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, @@ -680,7 +540,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody for (int z = 0; z < volume.volumeUnitResolution; z++) { cv::Vec3i voxelIdx(x, y, z); - Voxel voxel = volumeUnit->at(voxelIdx); + TsdfVoxel voxel = volumeUnit->at(voxelIdx); if (voxel.tsdf != 1.f && voxel.weight != 0) { @@ -705,7 +565,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody std::vector totalVolUnits; std::vector>& pVecs; std::vector>& nVecs; - const Voxel* volDataStart; + const TsdfVoxel* volDataStart; bool needNormals; mutable Mutex mutex; }; @@ -749,12 +609,13 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor } } -cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, - int _maxWeight, float _raycastStepFactor, - int volumeUnitResolution) +cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, + float _raycastStepFactor, float _truncDist, + int _maxWeight, float _truncateThreshold, + int _volumeUnitResolution) { - return cv::makePtr(_voxelSize, volumeUnitResolution, _pose, _truncDist, - _maxWeight, _raycastStepFactor); + return cv::makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, + _maxWeight, _truncateThreshold, _volumeUnitResolution); } } // namespace kinfu diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 605b79cdcce..2a9b25cdcdd 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -2,7 +2,8 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory #ifndef __OPENCV_HASH_TSDF_H__ #define __OPENCV_HASH_TSDF_H__ @@ -11,60 +12,52 @@ #include #include "tsdf.hpp" +#include "volume.hpp" -namespace cv { -namespace kinfu { - -class HashTSDFVolume +namespace cv +{ +namespace kinfu +{ +class HashTSDFVolume : public Volume { -public: + public: // dimension in voxels, size in meters //! Use fixed volume cuboid - explicit HashTSDFVolume(float _voxelSize, int _voxel_unit_res, cv::Affine3f _pose, - float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder = true); + explicit HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, float _truncateThreshold, + int _volumeUnitRes, bool zFirstMemOrder = true); virtual ~HashTSDFVolume() = default; - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) = 0; - virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, - cv::OutputArray points, cv::OutputArray normals) const = 0; /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; */ - virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; - - virtual void reset() = 0; - - -public: - float voxelSize; - float voxelSizeInv; - cv::Affine3f pose; - int maxWeight; - float raycastStepFactor; - float truncDist; - uint16_t volumeUnitResolution; - float volumeUnitSize; - bool zFirstMemOrder; + public: + int maxWeight; + float truncDist; + float truncateThreshold; + uint16_t volumeUnitResolution; + float volumeUnitSize; + bool zFirstMemOrder; }; struct VolumeUnit { - explicit VolumeUnit() : pVolume(nullptr) {}; + explicit VolumeUnit() : pVolume(nullptr){}; ~VolumeUnit() = default; cv::Ptr pVolume; - cv::Vec3i index; + cv::Vec3i index; bool isActive; }; //! Spatial hashing struct tsdf_hash { - size_t operator()(const cv::Vec3i & x) const noexcept + size_t operator()(const cv::Vec3i &x) const noexcept { - size_t seed = 0; + size_t seed = 0; constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; - for (uint16_t i = 0; i < 3; i++) { + for (uint16_t i = 0; i < 3; i++) + { seed ^= std::hash()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); } return seed; @@ -74,17 +67,16 @@ struct tsdf_hash typedef std::unordered_set VolumeUnitIndexSet; typedef std::unordered_map VolumeUnitMap; - class HashTSDFVolumeCPU : public HashTSDFVolume { -public: + public: // dimension in voxels, size in meters - HashTSDFVolumeCPU(float _voxelSize, int _volume_unit_res, cv::Affine3f _pose, - float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder = true); + explicit HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, float _truncateThreshold, + int _volumeUnitRes, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, float depthFactor, - cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; + virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, + cv::kinfu::Intr intrinsics) override; virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, cv::OutputArray points, cv::OutputArray normals) const override; @@ -94,12 +86,13 @@ class HashTSDFVolumeCPU : public HashTSDFVolume virtual void reset() override; //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) - virtual Voxel at(const cv::Vec3i &volumeIdx) const; + virtual TsdfVoxel at(const cv::Vec3i &volumeIdx) const; - //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = 1m) - virtual Voxel at(const cv::Point3f &point) const; + //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = + //! 1m) + virtual TsdfVoxel at(const cv::Point3f &point) const; - inline TsdfType interpolateVoxel(const cv::Point3f& point) const; + inline TsdfType interpolateVoxel(const cv::Point3f &point) const; Point3f getNormalVoxel(cv::Point3f p) const; //! Utility functions for coordinate transformations @@ -107,16 +100,17 @@ class HashTSDFVolumeCPU : public HashTSDFVolume cv::Point3f volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const; cv::Point3f voxelCoordToVolume(cv::Vec3i voxelIdx) const; - cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const; + cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const; -public: + public: //! Hashtable of individual smaller volume units VolumeUnitMap volumeUnits; }; -cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, int volumeUnitResolution = 16); -} // namespace kinfu -} // namespace cv +cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, + float _raycastStepFactor, float _truncDist, + int _maxWeight, float truncateThreshold, + int volumeUnitResolution = 16); +} // namespace kinfu +} // namespace cv #endif - diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index be9fecb88af..77fa5115a14 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -29,6 +29,8 @@ Ptr Params::defaultParams() p.frameSize = Size(640, 480); + p.volumeType = VolumeType::TSDF; + float fx, fy, cx, cy; fx = fy = 525.f; cx = p.frameSize.width/2 - 0.5f; @@ -61,11 +63,10 @@ Ptr Params::defaultParams() // default pose of volume cube p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f)); - /* p.volumePose = Affine3f(); */ p.tsdf_trunc_dist = 5 * p.voxelSize; //meters; p.tsdf_max_weight = 64; //frames - p.raycast_step_factor = 0.25f; //in voxel sizes + p.raycastStepFactor = 0.25f; //in voxel sizes // gradient delta factor is fixed at 1.0f and is not used //p.gradient_delta_factor = 0.5f; //in voxel sizes @@ -90,13 +91,24 @@ Ptr Params::coarseParams() p->voxelSize = volSize/128.f; p->tsdf_trunc_dist = 5 * p->voxelSize; - p->raycast_step_factor = 0.25f; //in voxel sizes + p->raycastStepFactor = 0.75f; //in voxel sizes return p; } +Ptr Params::hashTSDFParams(bool isCoarse) +{ + Ptr p; + if(isCoarse) + p = coarseParams(); + else + p = defaultParams(); + p->volumeType = VolumeType::HASHTSDF; + p->truncateThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); + return p; +} -// T should be Mat or UMat -template< typename T > +// MatType should be Mat or UMat +template< typename MatType> class KinFuImpl : public KinFu { public: @@ -118,53 +130,56 @@ class KinFuImpl : public KinFu bool update(InputArray depth) CV_OVERRIDE; - bool updateT(const T& depth); + bool updateT(const MatType& depth); private: Params params; cv::Ptr icp; - cv::Ptr volume; + cv::Ptr volume; int frameCounter; Affine3f pose; - std::vector pyrPoints; - std::vector pyrNormals; + std::vector pyrPoints; + std::vector pyrNormals; }; -template< typename T > -KinFuImpl::KinFuImpl(const Params &_params) : +template< typename MatType > +KinFuImpl::KinFuImpl(const Params &_params) : params(_params), icp(makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)), - volume(makeHashTSDFVolume(params.voxelSize, params.volumePose, - params.tsdf_trunc_dist, params.tsdf_max_weight, - params.raycast_step_factor)), pyrPoints(), pyrNormals() { + if(params.volumeType == Params::VolumeType::TSDF) + volume = makeTSDFVolume(params.voxelSize, params.volumePose, params.raycastStepFactor, + params.tsdf_trunc_dist, params.tsdf_max_weight, params.volumeDims); + else + volume = makeHashTSDFVolume(params.voxelSize, params.volumePose, params.raycastStepFactor, + params.tsdf_trunc_dist, params.tsdf_max_weight, params.truncateThreshold); reset(); } -template< typename T > -void KinFuImpl::reset() +template< typename MatType > +void KinFuImpl::reset() { frameCounter = 0; pose = Affine3f::Identity(); volume->reset(); } -template< typename T > -KinFuImpl::~KinFuImpl() +template< typename MatType > +KinFuImpl::~KinFuImpl() { } -template< typename T > -const Params& KinFuImpl::getParams() const +template< typename MatType > +const Params& KinFuImpl::getParams() const { return params; } -template< typename T > -const Affine3f KinFuImpl::getPose() const +template< typename MatType > +const Affine3f KinFuImpl::getPose() const { return pose; } @@ -206,18 +221,19 @@ bool KinFuImpl::update(InputArray _depth) } -template< typename T > -bool KinFuImpl::updateT(const T& _depth) +template< typename MatType > +bool KinFuImpl::updateT(const MatType& _depth) { CV_TRACE_FUNCTION(); - T depth; + MatType depth; if(_depth.type() != DEPTH_TYPE) _depth.convertTo(depth, DEPTH_TYPE); else depth = _depth; - std::vector newPoints, newNormals; + + std::vector newPoints, newNormals; makeFrameFromDepth(depth, newPoints, newNormals, params.intr, params.pyramidLevels, params.depthFactor, @@ -225,7 +241,7 @@ bool KinFuImpl::updateT(const T& _depth) params.bilateral_sigma_spatial, params.bilateral_kernel_size, params.truncateThreshold); - std::cout << "Created Frame from depth FrameID: " << frameCounter << "\n"; + /* std::cout << "Created Frame from depth FrameID: " << frameCounter << "\n"; */ if(frameCounter == 0) { // use depth instead of distance @@ -241,7 +257,7 @@ bool KinFuImpl::updateT(const T& _depth) return false; pose = pose * affine; - std::cout << "Obtained pose:\n " << pose.matrix << "\n"; + /* std::cout << "Obtained pose:\n " << pose.matrix << "\n"; */ float rnorm = (float)cv::norm(affine.rvec()); float tnorm = (float)cv::norm(affine.translation()); @@ -251,12 +267,12 @@ bool KinFuImpl::updateT(const T& _depth) // use depth instead of distance volume->integrate(depth, params.depthFactor, pose, params.intr); } - T& points = pyrPoints [0]; - T& normals = pyrNormals[0]; + MatType& points = pyrPoints [0]; + MatType& normals = pyrNormals[0]; volume->raycast(pose, params.intr, params.frameSize, points, normals); buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, params.pyramidLevels); - std::cout << "Built Point normal pyramids\n"; + /* std::cout << "Built Point normal pyramids\n"; */ } frameCounter++; @@ -264,8 +280,8 @@ bool KinFuImpl::updateT(const T& _depth) } -template< typename T > -void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const +template< typename MatType > +void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const { CV_TRACE_FUNCTION(); @@ -275,35 +291,35 @@ void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const if((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) || (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) { - std::cout << " Render without raycast: " << std::endl; + /* std::cout << " Render without raycast: " << std::endl; */ renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose); } else { - T points, normals; - std::cout << " Raycasted render: " << std::endl; + MatType points, normals; + /* std::cout << " Raycasted render: " << std::endl; */ volume->raycast(cameraPose, params.intr, params.frameSize, points, normals); renderPointsNormals(points, normals, image, params.lightPose); } } -template< typename T > -void KinFuImpl::getCloud(cv::OutputArray p, OutputArray n) const +template< typename MatType > +void KinFuImpl::getCloud(cv::OutputArray p, OutputArray n) const { volume->fetchPointsNormals(p, n); } -/* template< typename T > */ -/* void KinFuImpl::getPoints(OutputArray points) const */ +/* template< typename MatType > */ +/* void KinFuImpl::getPoints(OutputArray points) const */ /* { */ /* volume->fetchPointsNormals(points, noArray()); */ /* } */ -/* template< typename T > */ -/* void KinFuImpl::getNormals(InputArray points, OutputArray normals) const */ +/* template< typename MatType > */ +/* void KinFuImpl::getNormals(InputArray points, OutputArray normals) const */ /* { */ /* volume->fetchNormals(points, normals); */ /* } */ @@ -320,7 +336,7 @@ Ptr KinFu::create(const Ptr& params) if(cv::ocl::useOpenCL()) return makePtr< KinFuImpl >(*params); #endif - return makePtr< KinFuImpl >(*params); + return makePtr< KinFuImpl >(*params); } #else diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index 3a65a7a0727..863d6a9c77c 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -624,13 +624,16 @@ void makeFrameFromDepth(InputArray _depth, bilateralFilter(depth, smooth, kernelSize, sigmaDepth*depthFactor, sigmaSpatial); // depth truncation can be used in some scenes + Depth depthThreshold; if(truncateThreshold > 0.f) - threshold(depth, depth, truncateThreshold, 0.0, THRESH_TOZERO_INV); + threshold(smooth, depthThreshold, truncateThreshold * depthFactor, 0.0, THRESH_TOZERO_INV); + else + depthThreshold = smooth; // we don't need depth pyramid outside this method // if we do, the code is to be refactored - Depth scaled = smooth; + Depth scaled = depthThreshold; Size sz = smooth.size(); pyrPoints.create(levels, 1, POINT_TYPE); pyrNormals.create(levels, 1, POINT_TYPE); diff --git a/modules/rgbd/src/kinfu_frame.hpp b/modules/rgbd/src/kinfu_frame.hpp index 8f2d6f82936..16327d36aa5 100644 --- a/modules/rgbd/src/kinfu_frame.hpp +++ b/modules/rgbd/src/kinfu_frame.hpp @@ -7,6 +7,7 @@ #ifndef __OPENCV_KINFU_FRAME_H__ #define __OPENCV_KINFU_FRAME_H__ +#include #include "utils.hpp" namespace cv { diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 4576cf8119d..c16a33f5942 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -14,21 +14,16 @@ namespace cv { namespace kinfu { -TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _truncDist, - int _maxWeight, float _raycastStepFactor, bool zFirstMemOrder) - : voxelSize(_voxelSize), - voxelSizeInv(1.f / _voxelSize), - volResolution(_res), - maxWeight(_maxWeight), - pose(_pose), - raycastStepFactor(_raycastStepFactor) +TSDFVolume::TSDFVolume(float _voxelSize, Affine3f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, Point3i _resolution, bool zFirstMemOrder) + : Volume(_voxelSize, _pose, _raycastStepFactor), + volResolution(_resolution), + maxWeight(_maxWeight) { // Unlike original code, this should work with any volume size // Not only when (x,y,z % 32) == 0 - - volSize = Point3f(volResolution) * voxelSize; - - truncDist = std::max(_truncDist, 2.1f * voxelSize); + volSize = Point3f(volResolution) * voxelSize; + truncDist = std::max(_truncDist, 4.0f * voxelSize); // (xRes*yRes*zRes) array // Depending on zFirstMemOrder arg: @@ -56,12 +51,13 @@ TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _tr } // dimension in voxels, size in meters -TSDFVolumeCPU::TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, - int _maxWeight, float _raycastStepFactor, bool zFirstMemOrder) - : TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, +TSDFVolumeCPU::TSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, Point3i _resolution, + bool zFirstMemOrder) + : TSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution, zFirstMemOrder) { - volume = Mat(1, volResolution.x * volResolution.y * volResolution.z, rawType()); + volume = Mat(1, volResolution.x * volResolution.y * volResolution.z, rawType()); reset(); } @@ -71,32 +67,30 @@ void TSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); - volume.forEach([](VecT& vv, const int* /* position */) { - Voxel& v = reinterpret_cast(vv); - v.tsdf = 0; - v.weight = 0; + volume.forEach([](VecTsdfVoxel& vv, const int* /* position */) { + TsdfVoxel& v = reinterpret_cast(vv); + v.tsdf = 0.0f; + v.weight = 0; }); } -Voxel TSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const +TsdfVoxel TSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const { //! Out of bounds if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) { - Voxel dummy; - dummy.tsdf = 1.0; + TsdfVoxel dummy; + dummy.tsdf = 1.0f; dummy.weight = 0; std::cout << "Returning dummy\n"; return dummy; } - const Voxel* volData = volume.ptr(); + const TsdfVoxel* volData = volume.ptr(); int coordBase = volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; - /* std::cout << " Current TSDF: " << volData[coordBase].tsdf << " current weight: " << - * volData[coordBase].weight << "\n"; */ return volData[coordBase]; } @@ -203,7 +197,7 @@ struct IntegrateInvoker : ParallelLoopBody truncDistInv(1.f / _volume.truncDist), dfac(1.f / depthFactor) { - volDataStart = volume.volume.ptr(); + volDataStart = volume.volume.ptr(); } #if USE_INTRINSICS @@ -220,10 +214,10 @@ struct IntegrateInvoker : ParallelLoopBody for (int x = range.start; x < range.end; x++) { - Voxel* volDataX = volDataStart + x * volume.volDims[0]; + TsdfVoxel* volDataX = volDataStart + x * volume.volDims[0]; for (int y = 0; y < volume.volResolution.y; y++) { - Voxel* volDataY = volDataX + y * volume.volDims[1]; + TsdfVoxel* volDataY = volDataX + y * volume.volDims[1]; // optimization of camSpace transformation (vector addition instead of matmul at // each z) Point3f basePt = vol2cam * (Point3f((float)x, (float)y, 0) * volume.voxelSize); @@ -339,9 +333,9 @@ struct IntegrateInvoker : ParallelLoopBody { TsdfType tsdf = fmin(1.f, sdf * truncDistInv); - Voxel& voxel = volDataY[z * volume.volDims[2]]; - int& weight = voxel.weight; - TsdfType& value = voxel.tsdf; + TsdfVoxel& voxel = volDataY[z * volume.volDims[2]]; + int& weight = voxel.weight; + TsdfType& value = voxel.tsdf; // update TSDF value = (value * weight + tsdf) / (weight + 1); @@ -357,14 +351,15 @@ struct IntegrateInvoker : ParallelLoopBody { for (int x = range.start; x < range.end; x++) { - Voxel* volDataX = volDataStart + x * volume.volDims[0]; + TsdfVoxel* volDataX = volDataStart + x * volume.volDims[0]; for (int y = 0; y < volume.volResolution.y; y++) { - Voxel* volDataY = volDataX + y * volume.volDims[1]; + TsdfVoxel* volDataY = volDataX + y * volume.volDims[1]; // optimization of camSpace transformation (vector addition instead of matmul at // each z) Point3f basePt = vol2cam * (Point3f(x, y, 0) * volume.voxelSize); - /* std::cout << "Base Point: " << basePt << " VoxelSize: " << volume.voxelSize << + /* std::cout << "Base Point: " << basePt << " TsdfVoxelSize: " << volume.voxelSize + * << * "\n"; */ Point3f camSpacePt = basePt; // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; @@ -434,15 +429,17 @@ struct IntegrateInvoker : ParallelLoopBody { TsdfType tsdf = fmin(1.f, sdf * truncDistInv); - Voxel& voxel = volDataY[z * volume.volDims[2]]; - int& weight = voxel.weight; - TsdfType& value = voxel.tsdf; + TsdfVoxel& voxel = volDataY[z * volume.volDims[2]]; + int& weight = voxel.weight; + TsdfType& value = voxel.tsdf; // update TSDF value = (value * weight + tsdf) / (weight + 1); weight = min(weight + 1, volume.maxWeight); - /* std::cout << "Voxel coord: (" << x << ", " << y << ", " << z << ") "; */ - /* std::cout << "Updated value: " << value << " Updated weight: " << weight << "\n"; */ + /* std::cout << "TsdfVoxel coord: (" << x << ", " << y << ", " << z << ") "; + */ + /* std::cout << "Updated value: " << value << " Updated weight: " << weight + * << "\n"; */ } } } @@ -456,7 +453,7 @@ struct IntegrateInvoker : ParallelLoopBody const cv::Affine3f vol2cam; const float truncDistInv; const float dfac; - Voxel* volDataStart; + TsdfVoxel* volDataStart; }; // use depth instead of distance (optimization) @@ -492,7 +489,7 @@ inline TsdfType TSDFVolumeCPU::interpolateVoxel(const v_float32x4& p) const float tz = t.get0(); int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; - const Voxel* volData = volume.ptr(); + const TsdfVoxel* volData = volume.ptr(); int ix = ip.get0(); ip = v_rotate_right<1>(ip); @@ -533,7 +530,7 @@ inline TsdfType TSDFVolumeCPU::interpolateVoxel(Point3f p) const float tz = p.z - iz; int coordBase = ix * xdim + iy * ydim + iz * zdim; - const Voxel* volData = volume.ptr(); + const TsdfVoxel* volData = volume.ptr(); TsdfType vx[8]; for (int i = 0; i < 8; i++) @@ -578,7 +575,7 @@ inline v_float32x4 TSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const float tz = t.get0(); const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; - const Voxel* volData = volume.ptr(); + const TsdfVoxel* volData = volume.ptr(); int ix = ip.get0(); ip = v_rotate_right<1>(ip); @@ -622,7 +619,7 @@ inline v_float32x4 TSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const { const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; - const Voxel* volData = volume.ptr(); + const TsdfVoxel* volData = volume.ptr(); if (p.x < 1 || p.x >= volResolution.x - 2 || p.y < 1 || p.y >= volResolution.y - 2 || p.z < 1 || p.z >= volResolution.z - 2) @@ -682,11 +679,13 @@ struct RaycastInvoker : ParallelLoopBody vol2cam(cameraPose.inv() * volume.pose), reproj(intrinsics.makeReprojector()) { + std::cout << "Entered RaycastInvoker:" << std::endl; } #if USE_INTRINSICS virtual void operator()(const Range& range) const override { + std::cout << "Entered raycastInvoker" << std::endl; const v_float32x4 vfxy(reproj.fxinv, reproj.fyinv, 0, 0); const v_float32x4 vcxy(reproj.cx, reproj.cy, 0, 0); @@ -778,7 +777,7 @@ struct RaycastInvoker : ParallelLoopBody int iz = ip.get0(); int coord = ix * xdim + iy * ydim + iz * zdim; - fnext = volume.volume.at(coord).tsdf; + fnext = volume.volume.at(coord).tsdf; if (fnext != f) { fnext = volume.interpolateVoxel(next); @@ -831,10 +830,11 @@ struct RaycastInvoker : ParallelLoopBody #else virtual void operator()(const Range& range) const override { + std::cout << " Entered raycastInvoker" << std::endl; const Point3f camTrans = cam2vol.translation(); const Matx33f camRot = cam2vol.rotation(); const Matx33f volRot = vol2cam.rotation(); - + std::cout << "range.start: " << range.start << " range.end: " << range.end << "\n"; for (int y = range.start; y < range.end; y++) { ptype* ptsRow = points[y]; @@ -848,6 +848,7 @@ struct RaycastInvoker : ParallelLoopBody // direction through pixel in volume space Point3f dir = normalize(Vec3f(camRot * reproj(Point3f(x, y, 1.f)))); + std::cout << "origin: " << orig << " direction: " << dir << "\n"; // compute intersection of ray with all six bbox planes Vec3f rayinv(1.f / dir.x, 1.f / dir.y, 1.f / dir.z); Point3f tbottom = rayinv.mul(boxMin - orig); @@ -867,7 +868,7 @@ struct RaycastInvoker : ParallelLoopBody // precautions against getting coordinates out of bounds tmin = tmin + tstep; tmax = tmax - tstep; - + std::cout << "Tmin: " << tmin << "tmax: " << tmax << "\n"; if (tmin < tmax) { // interpolation optimized a little @@ -890,7 +891,7 @@ struct RaycastInvoker : ParallelLoopBody int ix = cvRound(next.x); int iy = cvRound(next.y); int iz = cvRound(next.z); - fnext = volume.volume.at(ix * xdim + iy * ydim + iz * zdim).tsdf; + fnext = volume.volume.at(ix * xdim + iy * ydim + iz * zdim).tsdf; if (fnext != f) { fnext = volume.interpolateVoxel(next); @@ -964,7 +965,6 @@ void TSDFVolumeCPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frame Points points = _points.getMat(); Normals normals = _normals.getMat(); - RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); const int nstripes = -1; @@ -977,7 +977,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody std::vector>& _nVecs, bool _needNormals) : ParallelLoopBody(), vol(_volume), pVecs(_pVecs), nVecs(_nVecs), needNormals(_needNormals) { - volDataStart = vol.volume.ptr(); + volDataStart = vol.volume.ptr(); } inline void coord(std::vector& points, std::vector& normals, int x, int y, int z, @@ -1008,7 +1008,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody if (limits) { - const Voxel& voxeld = + const TsdfVoxel& voxeld = volDataStart[(x + shift.x) * vol.volDims[0] + (y + shift.y) * vol.volDims[1] + (z + shift.z) * vol.volDims[2]]; TsdfType vd = voxeld.tsdf; @@ -1039,14 +1039,14 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody std::vector points, normals; for (int x = range.start; x < range.end; x++) { - const Voxel* volDataX = volDataStart + x * vol.volDims[0]; + const TsdfVoxel* volDataX = volDataStart + x * vol.volDims[0]; for (int y = 0; y < vol.volResolution.y; y++) { - const Voxel* volDataY = volDataX + y * vol.volDims[1]; + const TsdfVoxel* volDataY = volDataX + y * vol.volDims[1]; for (int z = 0; z < vol.volResolution.z; z++) { - const Voxel& voxel0 = volDataY[z * vol.volDims[2]]; - TsdfType v0 = voxel0.tsdf; + const TsdfVoxel& voxel0 = volDataY[z * vol.volDims[2]]; + TsdfType v0 = voxel0.tsdf; if (voxel0.weight != 0 && v0 != 1.f) { Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f) * @@ -1069,7 +1069,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody const TSDFVolumeCPU& vol; std::vector>& pVecs; std::vector>& nVecs; - const Voxel* volDataStart; + const TsdfVoxel* volDataStart; bool needNormals; mutable Mutex mutex; }; @@ -1148,10 +1148,11 @@ void TSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const ///////// GPU implementation ///////// #ifdef HAVE_OPENCL -TSDFVolumeGPU::TSDFVolumeGPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, - int _maxWeight, float _raycastStepFactor) - : TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, false) +TSDFVolumeGPU::TSDFVolumeGPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, Point3i _resolution) + : TSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution, false) { + //! TODO: Optimize this to use half precision floats volume = UMat(1, volResolution.x * volResolution.y * volResolution.z, CV_32FC2); reset(); @@ -1403,16 +1404,16 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) #endif -cv::Ptr makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, - float _truncDist, int _maxWeight, float _raycastStepFactor) +cv::Ptr makeTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, Point3i _resolution) { #ifdef HAVE_OPENCL if (cv::ocl::useOpenCL()) - return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, - _raycastStepFactor); + return cv::makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + _resolution); #endif - return cv::makePtr(_res, _voxelSize, _pose, _truncDist, _maxWeight, - _raycastStepFactor); + return cv::makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + _resolution); } } // namespace kinfu diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index f302a33aa91..9ff4b2cf58b 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -2,42 +2,32 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory #ifndef __OPENCV_KINFU_TSDF_H__ #define __OPENCV_KINFU_TSDF_H__ -#include "opencv2/core/affine.hpp" -#include "kinfu_frame.hpp" +#include "volume.hpp" -namespace cv { -namespace kinfu { - - -class TSDFVolume +namespace cv +{ +namespace kinfu { -public: +class TSDFVolume : public Volume +{ + public: // dimension in voxels, size in meters - TSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder = true); - - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) = 0; - virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, - cv::OutputArray points, cv::OutputArray normals) const = 0; + explicit TSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, Point3i _resolution, + bool zFirstMemOrder = true); + virtual ~TSDFVolume() = default; - virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; - virtual void reset() = 0; - - virtual ~TSDFVolume() { } - - float voxelSize; - float voxelSizeInv; + public: Point3i volResolution; int maxWeight; - cv::Affine3f pose; - float raycastStepFactor; Point3f volSize; float truncDist; @@ -45,25 +35,15 @@ class TSDFVolume Vec8i neighbourCoords; }; -// TODO: Optimization possible: -// * TsdfType can be FP16 -// * weight can be int16 -typedef float TsdfType; -struct Voxel -{ - TsdfType tsdf; - int weight; -}; -typedef Vec VecT; - class TSDFVolumeCPU : public TSDFVolume { -public: + public: // dimension in voxels, size in meters - TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor, bool zFirstMemOrder = true); + TSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; + virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, + cv::kinfu::Intr intrinsics) override; virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, cv::OutputArray points, cv::OutputArray normals) const override; @@ -71,7 +51,7 @@ class TSDFVolumeCPU : public TSDFVolume virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; virtual void reset() override; - virtual Voxel at(const cv::Vec3i &volumeIdx) const; + virtual TsdfVoxel at(const cv::Vec3i& volumeIdx) const; TsdfType interpolateVoxel(cv::Point3f p) const; Point3f getNormalVoxel(cv::Point3f p) const; @@ -90,12 +70,13 @@ class TSDFVolumeCPU : public TSDFVolume #ifdef HAVE_OPENCL class TSDFVolumeGPU : public TSDFVolume { -public: + public: // dimension in voxels, size in meters - TSDFVolumeGPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor); + TSDFVolumeGPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, Point3i _resolution); - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics) override; + virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, + cv::kinfu::Intr intrinsics) override; virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const override; @@ -111,9 +92,9 @@ class TSDFVolumeGPU : public TSDFVolume UMat volume; }; #endif -cv::Ptr makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight, - float _raycastStepFactor); +cv::Ptr makeTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, Point3i _resolution); -} // namespace kinfu -} // namespace cv +} // namespace kinfu +} // namespace cv #endif diff --git a/modules/rgbd/src/utils.hpp b/modules/rgbd/src/utils.hpp index 2662a973d6c..278cfbb977a 100644 --- a/modules/rgbd/src/utils.hpp +++ b/modules/rgbd/src/utils.hpp @@ -9,8 +9,8 @@ #ifndef __OPENCV_RGBD_UTILS_HPP__ #define __OPENCV_RGBD_UTILS_HPP__ -#include "opencv2/core/matx.hpp" -#include "opencv2/rgbd/depth.hpp" +#include "precomp.hpp" + namespace cv { namespace rgbd @@ -51,7 +51,6 @@ namespace kinfu { // One place to turn intrinsics on and off #define USE_INTRINSICS CV_SIMD128 -#undef USE_INTRINSICS typedef float depthType; diff --git a/modules/rgbd/src/volume.hpp b/modules/rgbd/src/volume.hpp new file mode 100644 index 00000000000..c458b55edd0 --- /dev/null +++ b/modules/rgbd/src/volume.hpp @@ -0,0 +1,59 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory + +#ifndef __OPENCV_VOLUME_H__ +#define __OPENCV_VOLUME_H__ + +#include "precomp.hpp" +#include "kinfu_frame.hpp" +#include "opencv2/core/affine.hpp" + +namespace cv +{ +namespace kinfu +{ +class Volume +{ + public: + explicit Volume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor) + : voxelSize(_voxelSize), + voxelSizeInv(1.0f / voxelSize), + pose(_pose), + raycastStepFactor(_raycastStepFactor) + { + } + + virtual ~Volume(){}; + + virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, + cv::kinfu::Intr intrinsics) = 0; + virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, + cv::OutputArray points, cv::OutputArray normals) const = 0; + virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; + virtual void reset() = 0; + + public: + const float voxelSize; + const float voxelSizeInv; + const cv::Affine3f pose; + const float raycastStepFactor; +}; + +// TODO: Optimization possible: +// * TsdfType can be FP16 +// * weight can be uint16 +typedef float TsdfType; +struct TsdfVoxel +{ + TsdfType tsdf; + int weight; +}; +typedef Vec VecTsdfVoxel; + +} // namespace kinfu +} // namespace cv +#endif From a00d7ebf3d0b65c564541a88bae1e97d03c64515 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Thu, 18 Jun 2020 19:22:53 -0400 Subject: [PATCH 10/36] - Add getPoints and getNormals function - Fix formatting according to comments - Move volume abstract class to include/opencv2/rgbd/ - Write factory method for creating TSDFVolumes - Small bug fixes - Minor fixes according to comments --- .../rgbd/include/opencv2/rgbd/intrinsics.hpp | 76 ++ modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 14 +- .../{src => include/opencv2/rgbd}/volume.hpp | 21 +- modules/rgbd/samples/kinfu_demo.cpp | 4 +- modules/rgbd/src/hash_tsdf.cpp | 51 +- modules/rgbd/src/hash_tsdf.hpp | 6 +- modules/rgbd/src/kinfu.cpp | 48 +- modules/rgbd/src/kinfu_frame.cpp | 7 +- modules/rgbd/src/tsdf.cpp | 783 +++++++++--------- modules/rgbd/src/tsdf.hpp | 19 +- modules/rgbd/src/utils.cpp | 2 +- modules/rgbd/src/utils.hpp | 62 -- modules/rgbd/src/volume.cpp | 25 + 13 files changed, 602 insertions(+), 516 deletions(-) create mode 100644 modules/rgbd/include/opencv2/rgbd/intrinsics.hpp rename modules/rgbd/{src => include/opencv2/rgbd}/volume.hpp (76%) create mode 100644 modules/rgbd/src/volume.cpp diff --git a/modules/rgbd/include/opencv2/rgbd/intrinsics.hpp b/modules/rgbd/include/opencv2/rgbd/intrinsics.hpp new file mode 100644 index 00000000000..a68cf3fb444 --- /dev/null +++ b/modules/rgbd/include/opencv2/rgbd/intrinsics.hpp @@ -0,0 +1,76 @@ +#ifndef __OPENCV_RGBD_INTRINSICS_HPP__ +#define __OPENCV_RGBD_INTRINSICS_HPP__ + +#include "opencv2/core/matx.hpp" + +namespace cv +{ + namespace kinfu + { +struct Intr +{ + /** @brief Camera intrinsics */ + /** Reprojects screen point to camera space given z coord. */ + struct Reprojector + { + Reprojector() {} + inline Reprojector(Intr intr) + { + fxinv = 1.f/intr.fx, fyinv = 1.f/intr.fy; + cx = intr.cx, cy = intr.cy; + } + template + inline cv::Point3_ operator()(cv::Point3_ p) const + { + T x = p.z * (p.x - cx) * fxinv; + T y = p.z * (p.y - cy) * fyinv; + return cv::Point3_(x, y, p.z); + } + + float fxinv, fyinv, cx, cy; + }; + + /** Projects camera space vector onto screen */ + struct Projector + { + inline Projector(Intr intr) : fx(intr.fx), fy(intr.fy), cx(intr.cx), cy(intr.cy) { } + template + inline cv::Point_ operator()(cv::Point3_ p) const + { + T invz = T(1)/p.z; + T x = fx*(p.x*invz) + cx; + T y = fy*(p.y*invz) + cy; + return cv::Point_(x, y); + } + template + inline cv::Point_ operator()(cv::Point3_ p, cv::Point3_& pixVec) const + { + T invz = T(1)/p.z; + pixVec = cv::Point3_(p.x*invz, p.y*invz, 1); + T x = fx*pixVec.x + cx; + T y = fy*pixVec.y + cy; + return cv::Point_(x, y); + } + float fx, fy, cx, cy; + }; + Intr() : fx(), fy(), cx(), cy() { } + Intr(float _fx, float _fy, float _cx, float _cy) : fx(_fx), fy(_fy), cx(_cx), cy(_cy) { } + Intr(cv::Matx33f m) : fx(m(0, 0)), fy(m(1, 1)), cx(m(0, 2)), cy(m(1, 2)) { } + // scale intrinsics to pyramid level + inline Intr scale(int pyr) const + { + float factor = (1.f /(1 << pyr)); + return Intr(fx*factor, fy*factor, cx*factor, cy*factor); + } + inline Reprojector makeReprojector() const { return Reprojector(*this); } + inline Projector makeProjector() const { return Projector(*this); } + + inline cv::Matx33f getMat() const { return Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); } + + float fx, fy, cx, cy; +}; + +} // namespace rgbd +} // namespace cv + +#endif diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 17bebe33fa5..8fcc3189ac3 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -9,6 +9,7 @@ #include "opencv2/core.hpp" #include "opencv2/core/affine.hpp" +#include namespace cv { namespace kinfu { @@ -17,11 +18,6 @@ namespace kinfu { struct CV_EXPORTS_W Params { - enum VolumeType - { - TSDF = 0, - HASHTSDF = 1 - }; CV_WRAP Params(){} @@ -81,7 +77,7 @@ struct CV_EXPORTS_W Params /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; - CV_PROP_RW VolumeType volumeType; + CV_PROP_RW cv::kinfu::VolumeType volumeType; /** @brief camera intrinsics */ CV_PROP_RW Matx33f intr; @@ -138,7 +134,7 @@ struct CV_EXPORTS_W Params How much voxel sizes we skip each raycast step */ - CV_PROP_RW float raycastStepFactor; + CV_PROP_RW float raycast_step_factor; // gradient delta in voxel sizes // fixed at 1.0f @@ -222,13 +218,13 @@ class CV_EXPORTS_W KinFu @param points vector of points which are 4-float vectors */ - /* CV_WRAP virtual void getPoints(OutputArray points) const = 0; */ + CV_WRAP virtual void getPoints(OutputArray points) const = 0; /** @brief Calculates normals for given points @param points input vector of points which are 4-float vectors @param normals output vector of corresponding normals which are 4-float vectors */ - /* CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0; */ + CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0; /** @brief Resets the algorithm diff --git a/modules/rgbd/src/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp similarity index 76% rename from modules/rgbd/src/volume.hpp rename to modules/rgbd/include/opencv2/rgbd/volume.hpp index c458b55edd0..61db94d3c44 100644 --- a/modules/rgbd/src/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -8,14 +8,14 @@ #ifndef __OPENCV_VOLUME_H__ #define __OPENCV_VOLUME_H__ -#include "precomp.hpp" -#include "kinfu_frame.hpp" +#include "intrinsics.hpp" #include "opencv2/core/affine.hpp" namespace cv { namespace kinfu { + class Volume { public: @@ -33,6 +33,7 @@ class Volume cv::kinfu::Intr intrinsics) = 0; virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, cv::OutputArray points, cv::OutputArray normals) const = 0; + virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; virtual void reset() = 0; @@ -43,17 +44,15 @@ class Volume const float raycastStepFactor; }; -// TODO: Optimization possible: -// * TsdfType can be FP16 -// * weight can be uint16 -typedef float TsdfType; -struct TsdfVoxel +enum class VolumeType { - TsdfType tsdf; - int weight; + TSDF = 0, + HASHTSDF = 1 }; -typedef Vec VecTsdfVoxel; -} // namespace kinfu +cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, + float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, Point3i _resolution); +} // namespace rgbd } // namespace cv #endif diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index 00ba0ccea20..bec1727390f 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -387,8 +387,8 @@ int main(int argc, char** argv) // params->voxelSize = cubeSize/params->volumeDims[0]; //meters // params->tsdf_trunc_dist = 0.01f; //meters // params->icpDistThresh = 0.01f; //meters - // params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); - // //meters params->tsdf_max_weight = 16; + // params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters + // params->tsdf_max_weight = 16; if (!idle) kf = KinFu::create(params); diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index e87b3ded9c1..6306c0c9f9e 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -12,8 +12,10 @@ #include #include +#include "kinfu_frame.hpp" #include "opencv2/core/cvstd.hpp" #include "opencv2/core/utils/trace.hpp" +#include "utils.hpp" namespace cv { @@ -313,7 +315,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const if (it == volumeUnits.end()) { TsdfVoxel dummy; - dummy.tsdf = float16_t(1.f); + dummy.tsdf = 1.f; dummy.weight = 0; return dummy; } @@ -336,7 +338,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const if (it == volumeUnits.end()) { TsdfVoxel dummy; - dummy.tsdf = float16_t(1.f); + dummy.tsdf = 1.f; dummy.weight = 0; return dummy; } @@ -408,7 +410,6 @@ struct RaycastInvoker : ParallelLoopBody //! Ray origin and direction in the volume coordinate frame Point3f orig = cam2volTrans; Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(x, y, 1.f)))); - Vec3f rayDirInvV = cv::Vec3f(1.f / rayDirV.x, 1.f / rayDirV.y, 1.f / rayDirV.z); float tmin = 0; float tmax = volume.truncateThreshold / rayDirV.z; @@ -419,7 +420,7 @@ struct RaycastInvoker : ParallelLoopBody std::numeric_limits::min()); float tprev = tcurr; - TsdfType prevTsdf = float16_t(volume.truncDist); + TsdfType prevTsdf = volume.truncDist; cv::Ptr currVolumeUnit; while (tcurr < tmax) { @@ -462,9 +463,9 @@ struct RaycastInvoker : ParallelLoopBody { normal = vol2camRot * nv; point = vol2cam * pv; - break; } } + break; } prevVolumeUnitIdx = currVolumeUnitIdx; prevTsdf = currTsdf; @@ -583,8 +584,6 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor { totalVolUnits.push_back(keyvalue.first); } - std::cout << "Number of volumeUnits in volume(fetchPoints): " << totalVolUnits.size() - << "\n"; FetchPointsNormalsInvoker fi(*this, totalVolUnits, pVecs, nVecs, _normals.needed()); Range range(0, totalVolUnits.size()); const int nstripes = -1; @@ -609,6 +608,44 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor } } +struct PushNormals +{ + PushNormals(const HashTSDFVolumeCPU& _volume, Normals& _normals) : + volume(_volume), normals(_normals), invPose(volume.pose.inv()) + {} + + void operator ()(const ptype &point, const int* position) const + { + 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); + } + const HashTSDFVolumeCPU& volume; + Normals& normals; + Affine3f invPose; +}; + +void HashTSDFVolumeCPU::fetchNormals(cv::InputArray _points, cv::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(); + + points.forEach(PushNormals(*this, normals)); + } +} + cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 2a9b25cdcdd..bf9e9887488 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -11,8 +11,8 @@ #include #include +#include #include "tsdf.hpp" -#include "volume.hpp" namespace cv { @@ -29,7 +29,6 @@ class HashTSDFVolume : public Volume virtual ~HashTSDFVolume() = default; - /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; */ public: int maxWeight; float truncDist; @@ -80,7 +79,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, cv::OutputArray points, cv::OutputArray normals) const override; - /* virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; */ + virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; virtual void reset() override; @@ -106,7 +105,6 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Hashtable of individual smaller volume units VolumeUnitMap volumeUnits; }; - cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float truncateThreshold, diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index 77fa5115a14..f484c2861d7 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -63,10 +63,10 @@ Ptr Params::defaultParams() // default pose of volume cube p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f)); - p.tsdf_trunc_dist = 5 * p.voxelSize; //meters; + p.tsdf_trunc_dist = 7 * p.voxelSize; // about 0.04f in meters p.tsdf_max_weight = 64; //frames - p.raycastStepFactor = 0.25f; //in voxel sizes + p.raycast_step_factor = 0.25f; //in voxel sizes // gradient delta factor is fixed at 1.0f and is not used //p.gradient_delta_factor = 0.5f; //in voxel sizes @@ -89,9 +89,9 @@ Ptr Params::coarseParams() float volSize = 3.f; p->volumeDims = Vec3i::all(128); //number of voxels p->voxelSize = volSize/128.f; - p->tsdf_trunc_dist = 5 * p->voxelSize; + p->tsdf_trunc_dist = 2 * p->voxelSize; // 0.04f in meters - p->raycastStepFactor = 0.75f; //in voxel sizes + p->raycast_step_factor = 0.75f; //in voxel sizes return p; } @@ -121,8 +121,8 @@ class KinFuImpl : public KinFu //! TODO(Akash): Add back later virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE; - /* void getPoints(OutputArray points) const CV_OVERRIDE; */ - /* void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; */ + void getPoints(OutputArray points) const CV_OVERRIDE; + void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; void reset() CV_OVERRIDE; @@ -151,12 +151,8 @@ KinFuImpl::KinFuImpl(const Params &_params) : icp(makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)), pyrPoints(), pyrNormals() { - if(params.volumeType == Params::VolumeType::TSDF) - volume = makeTSDFVolume(params.voxelSize, params.volumePose, params.raycastStepFactor, - params.tsdf_trunc_dist, params.tsdf_max_weight, params.volumeDims); - else - volume = makeHashTSDFVolume(params.voxelSize, params.volumePose, params.raycastStepFactor, - params.tsdf_trunc_dist, params.tsdf_max_weight, params.truncateThreshold); + volume = makeVolume(params.volumeType, params.voxelSize, params.volumePose, params.raycast_step_factor, + params.tsdf_trunc_dist, params.tsdf_max_weight, params.truncateThreshold, params.volumeDims); reset(); } @@ -241,7 +237,6 @@ bool KinFuImpl::updateT(const MatType& _depth) params.bilateral_sigma_spatial, params.bilateral_kernel_size, params.truncateThreshold); - /* std::cout << "Created Frame from depth FrameID: " << frameCounter << "\n"; */ if(frameCounter == 0) { // use depth instead of distance @@ -257,7 +252,6 @@ bool KinFuImpl::updateT(const MatType& _depth) return false; pose = pose * affine; - /* std::cout << "Obtained pose:\n " << pose.matrix << "\n"; */ float rnorm = (float)cv::norm(affine.rvec()); float tnorm = (float)cv::norm(affine.translation()); @@ -272,7 +266,6 @@ bool KinFuImpl::updateT(const MatType& _depth) volume->raycast(pose, params.intr, params.frameSize, points, normals); buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, params.pyramidLevels); - /* std::cout << "Built Point normal pyramids\n"; */ } frameCounter++; @@ -291,13 +284,11 @@ void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) c if((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) || (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) { - /* std::cout << " Render without raycast: " << std::endl; */ renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose); } else { MatType points, normals; - /* std::cout << " Raycasted render: " << std::endl; */ volume->raycast(cameraPose, params.intr, params.frameSize, points, normals); renderPointsNormals(points, normals, image, params.lightPose); } @@ -305,24 +296,24 @@ void KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) c template< typename MatType > -void KinFuImpl::getCloud(cv::OutputArray p, OutputArray n) const +void KinFuImpl::getCloud(OutputArray p, OutputArray n) const { volume->fetchPointsNormals(p, n); } -/* template< typename MatType > */ -/* void KinFuImpl::getPoints(OutputArray points) const */ -/* { */ -/* volume->fetchPointsNormals(points, noArray()); */ -/* } */ +template< typename MatType > +void KinFuImpl::getPoints(OutputArray points) const +{ + volume->fetchPointsNormals(points, noArray()); +} -/* template< typename MatType > */ -/* void KinFuImpl::getNormals(InputArray points, OutputArray normals) const */ -/* { */ -/* volume->fetchNormals(points, normals); */ -/* } */ +template< typename MatType > +void KinFuImpl::getNormals(InputArray points, OutputArray normals) const +{ + volume->fetchNormals(points, normals); +} // importing class @@ -352,4 +343,3 @@ KinFu::~KinFu() {} } // namespace kinfu } // namespace cv - diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index 863d6a9c77c..ce160dd5f3c 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -505,10 +505,13 @@ static bool ocl_makeFrameFromDepth(const UMat depth, OutputArrayOfArrays points, return false; // depth truncation can be used in some scenes + UMat depthThreshold; if(truncateThreshold > 0.f) - threshold(depth, depth, truncateThreshold*depthFactor, 0.0, THRESH_TOZERO_INV); + threshold(smooth, depthThreshold, truncateThreshold * depthFactor, 0.0, THRESH_TOZERO_INV); + else + depthThreshold = smooth; - UMat scaled = smooth; + UMat scaled = depthThreshold; Size sz = smooth.size(); points.create(levels, 1, POINT_TYPE); normals.create(levels, 1, POINT_TYPE); diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index c16a33f5942..cd77914c7af 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -2,18 +2,16 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this -// module's directory +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +#include "precomp.hpp" #include "tsdf.hpp" - #include "opencl_kernels_rgbd.hpp" -#include "precomp.hpp" -namespace cv -{ -namespace kinfu -{ +namespace cv { + +namespace kinfu { + TSDFVolume::TSDFVolume(float _voxelSize, Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution, bool zFirstMemOrder) : Volume(_voxelSize, _pose, _raycastStepFactor), @@ -23,14 +21,14 @@ TSDFVolume::TSDFVolume(float _voxelSize, Affine3f _pose, float _raycastStepFacto // Unlike original code, this should work with any volume size // Not only when (x,y,z % 32) == 0 volSize = Point3f(volResolution) * voxelSize; - truncDist = std::max(_truncDist, 4.0f * voxelSize); + truncDist = std::max(_truncDist, 2.1f * voxelSize); // (xRes*yRes*zRes) array // Depending on zFirstMemOrder arg: // &elem(x, y, z) = data + x*zRes*yRes + y*zRes + z; // &elem(x, y, z) = data + x + y*xRes + z*xRes*yRes; int xdim, ydim, zdim; - if (zFirstMemOrder) + if(zFirstMemOrder) { xdim = volResolution.z * volResolution.y; ydim = volResolution.z; @@ -44,10 +42,16 @@ TSDFVolume::TSDFVolume(float _voxelSize, Affine3f _pose, float _raycastStepFacto } volDims = Vec4i(xdim, ydim, zdim); - neighbourCoords = - Vec8i(volDims.dot(Vec4i(0, 0, 0)), volDims.dot(Vec4i(0, 0, 1)), volDims.dot(Vec4i(0, 1, 0)), - volDims.dot(Vec4i(0, 1, 1)), volDims.dot(Vec4i(1, 0, 0)), volDims.dot(Vec4i(1, 0, 1)), - volDims.dot(Vec4i(1, 1, 0)), volDims.dot(Vec4i(1, 1, 1))); + neighbourCoords = Vec8i( + volDims.dot(Vec4i(0, 0, 0)), + volDims.dot(Vec4i(0, 0, 1)), + volDims.dot(Vec4i(0, 1, 0)), + volDims.dot(Vec4i(0, 1, 1)), + volDims.dot(Vec4i(1, 0, 0)), + volDims.dot(Vec4i(1, 0, 1)), + volDims.dot(Vec4i(1, 1, 0)), + volDims.dot(Vec4i(1, 1, 1)) + ); } // dimension in voxels, size in meters @@ -67,10 +71,10 @@ void TSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); - volume.forEach([](VecTsdfVoxel& vv, const int* /* position */) { + volume.forEach([](VecTsdfVoxel& vv, const int* /* position */) + { TsdfVoxel& v = reinterpret_cast(vv); - v.tsdf = 0.0f; - v.weight = 0; + v.tsdf = 0.0f; v.weight = 0; }); } @@ -84,7 +88,6 @@ TsdfVoxel TSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const TsdfVoxel dummy; dummy.tsdf = 1.0f; dummy.weight = 0; - std::cout << "Returning dummy\n"; return dummy; } @@ -101,18 +104,19 @@ static const bool fixMissingData = false; static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) { const depthType defaultValue = qnan; - if (pt.x < 0 || pt.x >= m.cols - 1 || pt.y < 0 || pt.y >= m.rows - 1) + if(pt.x < 0 || pt.x >= m.cols-1 || + pt.y < 0 || pt.y >= m.rows-1) return defaultValue; int xi = cvFloor(pt.x), yi = cvFloor(pt.y); - const depthType* row0 = m[yi + 0]; - const depthType* row1 = m[yi + 1]; + const depthType* row0 = m[yi+0]; + const depthType* row1 = m[yi+1]; - depthType v00 = row0[xi + 0]; - depthType v01 = row0[xi + 1]; - depthType v10 = row1[xi + 0]; - depthType v11 = row1[xi + 1]; + depthType v00 = row0[xi+0]; + depthType v01 = row0[xi+1]; + depthType v10 = row1[xi+0]; + depthType v11 = row1[xi+1]; // assume correct depth is positive bool b00 = v00 > 0; @@ -120,115 +124,99 @@ static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) bool b10 = v10 > 0; bool b11 = v11 > 0; - if (!fixMissingData) + if(!fixMissingData) { - if (!(b00 && b01 && b10 && b11)) + if(!(b00 && b01 && b10 && b11)) return defaultValue; else { float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx * (v01 - v00); - depthType v1 = v10 + tx * (v11 - v10); - return v0 + ty * (v1 - v0); + depthType v0 = v00 + tx*(v01 - v00); + depthType v1 = v10 + tx*(v11 - v10); + return v0 + ty*(v1 - v0); } } else { int nz = b00 + b01 + b10 + b11; - if (nz == 0) + if(nz == 0) { return defaultValue; } - if (nz == 1) + if(nz == 1) { - if (b00) - return v00; - if (b01) - return v01; - if (b10) - return v10; - if (b11) - return v11; + if(b00) return v00; + if(b01) return v01; + if(b10) return v10; + if(b11) return v11; } - else if (nz == 2) + else if(nz == 2) { - if (b00 && b10) - v01 = v00, v11 = v10; - if (b01 && b11) - v00 = v01, v10 = v11; - if (b00 && b01) - v10 = v00, v11 = v01; - if (b10 && b11) - v00 = v10, v01 = v11; - if (b00 && b11) - v01 = v10 = (v00 + v11) * 0.5f; - if (b01 && b10) - v00 = v11 = (v01 + v10) * 0.5f; + if(b00 && b10) v01 = v00, v11 = v10; + if(b01 && b11) v00 = v01, v10 = v11; + if(b00 && b01) v10 = v00, v11 = v01; + if(b10 && b11) v00 = v10, v01 = v11; + if(b00 && b11) v01 = v10 = (v00 + v11)*0.5f; + if(b01 && b10) v00 = v11 = (v01 + v10)*0.5f; } - else if (nz == 3) + else if(nz == 3) { - if (!b00) - v00 = v10 + v01 - v11; - if (!b01) - v01 = v00 + v11 - v10; - if (!b10) - v10 = v00 + v11 - v01; - if (!b11) - v11 = v01 + v10 - v00; + if(!b00) v00 = v10 + v01 - v11; + if(!b01) v01 = v00 + v11 - v10; + if(!b10) v10 = v00 + v11 - v01; + if(!b11) v11 = v01 + v10 - v00; } float tx = pt.x - xi, ty = pt.y - yi; - depthType v0 = v00 + tx * (v01 - v00); - depthType v1 = v10 + tx * (v11 - v10); - return v0 + ty * (v1 - v0); + depthType v0 = v00 + tx*(v01 - v00); + depthType v1 = v10 + tx*(v11 - v10); + return v0 + ty*(v1 - v0); } } #endif struct IntegrateInvoker : ParallelLoopBody { - IntegrateInvoker(TSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, - cv::Affine3f cameraPose, float depthFactor) - : ParallelLoopBody(), - volume(_volume), - depth(_depth), - proj(intrinsics.makeProjector()), - vol2cam(cameraPose.inv() * _volume.pose), - truncDistInv(1.f / _volume.truncDist), - dfac(1.f / depthFactor) + IntegrateInvoker(TSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose, + float depthFactor) : + ParallelLoopBody(), + volume(_volume), + depth(_depth), + proj(intrinsics.makeProjector()), + vol2cam(cameraPose.inv() * _volume.pose), + truncDistInv(1.f/_volume.truncDist), + dfac(1.f/depthFactor) { volDataStart = volume.volume.ptr(); } #if USE_INTRINSICS - virtual void operator()(const Range& range) const override + virtual void operator() (const Range& range) const override { // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; - Point3f zStepPt = - Point3f(vol2cam.matrix(0, 2), vol2cam.matrix(1, 2), vol2cam.matrix(2, 2)) * - volume.voxelSize; + Point3f zStepPt = Point3f(vol2cam.matrix(0, 2), + vol2cam.matrix(1, 2), + vol2cam.matrix(2, 2))*volume.voxelSize; v_float32x4 zStep(zStepPt.x, zStepPt.y, zStepPt.z, 0); v_float32x4 vfxy(proj.fx, proj.fy, 0.f, 0.f), vcxy(proj.cx, proj.cy, 0.f, 0.f); - const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols - 1, depth.rows - 1, 0, 0)); + const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols-1, depth.rows-1, 0, 0)); - for (int x = range.start; x < range.end; x++) + for(int x = range.start; x < range.end; x++) { - TsdfVoxel* volDataX = volDataStart + x * volume.volDims[0]; - for (int y = 0; y < volume.volResolution.y; y++) + TsdfVoxel* volDataX = volDataStart + x*volume.volDims[0]; + for(int y = 0; y < volume.volResolution.y; y++) { - TsdfVoxel* volDataY = volDataX + y * volume.volDims[1]; - // optimization of camSpace transformation (vector addition instead of matmul at - // each z) - Point3f basePt = vol2cam * (Point3f((float)x, (float)y, 0) * volume.voxelSize); + TsdfVoxel* volDataY = volDataX + y*volume.volDims[1]; + // optimization of camSpace transformation (vector addition instead of matmul at each z) + Point3f basePt = vol2cam*(Point3f((float)x, (float)y, 0)*volume.voxelSize); v_float32x4 camSpacePt(basePt.x, basePt.y, basePt.z, 0); - /* std::cout << "basePt: " << basePt << " zstep: " << zStepPt << "\n"; */ int startZ, endZ; - if (abs(zStepPt.z) > 1e-5) + if(abs(zStepPt.z) > 1e-5) { int baseZ = (int)(-basePt.z / zStepPt.z); - if (zStepPt.z > 0) + if(zStepPt.z > 0) { startZ = baseZ; endZ = volume.volResolution.z; @@ -254,22 +242,18 @@ struct IntegrateInvoker : ParallelLoopBody } startZ = max(0, startZ); endZ = min(volume.volResolution.z, endZ); - /* std::cout << "StartZ, EndZ: " << startZ << ", " << endZ << "\n"; */ - for (int z = startZ; z < endZ; z++) + for(int z = startZ; z < endZ; z++) { // optimization of the following: - // Point3f volPt = Point3f(x, y, z)*voxelSize; - // Point3f camSpacePt = vol2cam * volPt; + //Point3f volPt = Point3f(x, y, z)*voxelSize; + //Point3f camSpacePt = vol2cam * volPt; camSpacePt += zStep; - float zCamSpace = - v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(camSpacePt))) - .get0(); - /* std::cout << "zCamSpace: " << zCamSpace << std::endl; */ - if (zCamSpace <= 0.f) + float zCamSpace = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(camSpacePt))).get0(); + if(zCamSpace <= 0.f) continue; - v_float32x4 camPixVec = camSpacePt / v_setall_f32(zCamSpace); + v_float32x4 camPixVec = camSpacePt/v_setall_f32(zCamSpace); v_float32x4 projected = v_muladd(camPixVec, vfxy, vcxy); // leave only first 2 lanes projected = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & @@ -283,7 +267,7 @@ struct IntegrateInvoker : ParallelLoopBody v_uint32x4 limits = v_reinterpret_as_u32(pt < v_setzero_f32()) | v_reinterpret_as_u32(pt >= upLimits); limits = limits | v_rotate_right<1>(limits); - if (limits.get0()) + if(limits.get0()) continue; // xi, yi = floor(pt) @@ -293,8 +277,8 @@ struct IntegrateInvoker : ParallelLoopBody ipshift = v_rotate_right<1>(ipshift); int yi = ipshift.get0(); - const depthType* row0 = depth[yi + 0]; - const depthType* row1 = depth[yi + 1]; + const depthType* row0 = depth[yi+0]; + const depthType* row1 = depth[yi+1]; // v001 = [v(xi + 0, yi + 0), v(xi + 1, yi + 0)] v_float32x4 v001 = v_load_low(row0 + xi); @@ -305,73 +289,67 @@ struct IntegrateInvoker : ParallelLoopBody // assume correct depth is positive // don't fix missing data - if (v_check_all(vall > v_setzero_f32())) + if(v_check_all(vall > v_setzero_f32())) { v_float32x4 t = pt - v_cvt_f32(ip); float tx = t.get0(); t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); v_float32x4 ty = v_setall_f32(t.get0()); // vx is y-interpolated between rows 0 and 1 - v_float32x4 vx = v001 + ty * (v101 - v001); + v_float32x4 vx = v001 + ty*(v101 - v001); float v0 = vx.get0(); vx = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vx))); float v1 = vx.get0(); - v = v0 + tx * (v1 - v0); + v = v0 + tx*(v1 - v0); } else continue; } // norm(camPixVec) produces double which is too slow - float pixNorm = sqrt(v_reduce_sum(camPixVec * camPixVec)); + float pixNorm = sqrt(v_reduce_sum(camPixVec*camPixVec)); // difference between distances of point and of surface to camera - TsdfType sdf = pixNorm * (v * dfac - zCamSpace); + TsdfType sdf = pixNorm*(v*dfac - zCamSpace); // possible alternative is: // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); - /* std::cout << "SDF: " << sdf << "truncDist: " << -volume.truncDist << "\n"; */ - if (sdf >= -volume.truncDist) + if(sdf >= -volume.truncDist) { TsdfType tsdf = fmin(1.f, sdf * truncDistInv); - TsdfVoxel& voxel = volDataY[z * volume.volDims[2]]; + TsdfVoxel& voxel = volDataY[z*volume.volDims[2]]; int& weight = voxel.weight; TsdfType& value = voxel.tsdf; // update TSDF - value = (value * weight + tsdf) / (weight + 1); + value = (value*weight+tsdf) / (weight + 1); weight = min(weight + 1, volume.maxWeight); - /* std::cout << "Updated TSDF: " << value << "\n"; */ } } } } } #else - virtual void operator()(const Range& range) const override + virtual void operator() (const Range& range) const override { - for (int x = range.start; x < range.end; x++) + for(int x = range.start; x < range.end; x++) { - TsdfVoxel* volDataX = volDataStart + x * volume.volDims[0]; - for (int y = 0; y < volume.volResolution.y; y++) + TsdfVoxel* volDataX = volDataStart + x*volume.volDims[0]; + for(int y = 0; y < volume.volResolution.y; y++) { - TsdfVoxel* volDataY = volDataX + y * volume.volDims[1]; - // optimization of camSpace transformation (vector addition instead of matmul at - // each z) - Point3f basePt = vol2cam * (Point3f(x, y, 0) * volume.voxelSize); - /* std::cout << "Base Point: " << basePt << " TsdfVoxelSize: " << volume.voxelSize - * << - * "\n"; */ + TsdfVoxel* volDataY = volDataX+y*volume.volDims[1]; + // optimization of camSpace transformation (vector addition instead of matmul at each z) + Point3f basePt = vol2cam*(Point3f(x, y, 0)*volume.voxelSize); Point3f camSpacePt = basePt; // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; // zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize - Point3f zStep = - Point3f(vol2cam.matrix(0, 2), vol2cam.matrix(1, 2), vol2cam.matrix(2, 2)) * - volume.voxelSize; + Point3f zStep = Point3f(vol2cam.matrix(0, 2), + vol2cam.matrix(1, 2), + vol2cam.matrix(2, 2))*volume.voxelSize; int startZ, endZ; - if (abs(zStep.z) > 1e-5) + if(abs(zStep.z) > 1e-5) { int baseZ = -basePt.z / zStep.z; - if (zStep.z > 0) + if(zStep.z > 0) { startZ = baseZ; endZ = volume.volResolution.z; @@ -384,7 +362,7 @@ struct IntegrateInvoker : ParallelLoopBody } else { - if (basePt.z > 0) + if(basePt.z > 0) { startZ = 0; endZ = volume.volResolution.z; @@ -397,49 +375,40 @@ struct IntegrateInvoker : ParallelLoopBody } startZ = max(0, startZ); endZ = min(volume.volResolution.z, endZ); - /* std::cout << "startZ, endZ: (" << startZ << ", " << endZ << ")\n"; */ - for (int z = startZ; z < endZ; z++) + for(int z = startZ; z < endZ; z++) { // optimization of the following: - // Point3f volPt = Point3f(x, y, z)*volume.voxelSize; - // Point3f camSpacePt = vol2cam * volPt; + //Point3f volPt = Point3f(x, y, z)*volume.voxelSize; + //Point3f camSpacePt = vol2cam * volPt; camSpacePt += zStep; - if (camSpacePt.z <= 0) + if(camSpacePt.z <= 0) continue; Point3f camPixVec; Point2f projected = proj(camSpacePt, camPixVec); depthType v = bilinearDepth(depth, projected); - if (v == 0) + if(v == 0) continue; // norm(camPixVec) produces double which is too slow float pixNorm = sqrt(camPixVec.dot(camPixVec)); // difference between distances of point and of surface to camera - TsdfType sdf = pixNorm * (v * dfac - camSpacePt.z); - /* std::cout << "camSpacePt: " << camSpacePt << "\n"; */ - /* std::cout << "projected: " << projected << "\n"; */ - /* std::cout << "depth at projected: " << v << "\n"; */ - /* std::cout << "SDF: " << sdf << "\n"; */ + TsdfType sdf = pixNorm*(v*dfac - camSpacePt.z); // possible alternative is: // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); - if (sdf >= -volume.truncDist) + if(sdf >= -volume.truncDist) { TsdfType tsdf = fmin(1.f, sdf * truncDistInv); - TsdfVoxel& voxel = volDataY[z * volume.volDims[2]]; + TsdfVoxel& voxel = volDataY[z*volume.volDims[2]]; int& weight = voxel.weight; TsdfType& value = voxel.tsdf; // update TSDF - value = (value * weight + tsdf) / (weight + 1); + value = (value*weight+tsdf) / (weight + 1); weight = min(weight + 1, volume.maxWeight); - /* std::cout << "TsdfVoxel coord: (" << x << ", " << y << ", " << z << ") "; - */ - /* std::cout << "Updated value: " << value << " Updated weight: " << weight - * << "\n"; */ } } } @@ -497,24 +466,24 @@ inline TsdfType TSDFVolumeCPU::interpolateVoxel(const v_float32x4& p) const ip = v_rotate_right<1>(ip); int iz = ip.get0(); - int coordBase = ix * xdim + iy * ydim + iz * zdim; + int coordBase = ix*xdim + iy*ydim + iz*zdim; TsdfType vx[8]; - for (int i = 0; i < 8; i++) + for(int i = 0; i < 8; i++) vx[i] = volData[neighbourCoords[i] + coordBase].tsdf; v_float32x4 v0246(vx[0], vx[2], vx[4], vx[6]), v1357(vx[1], vx[3], vx[5], vx[7]); - v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); + v_float32x4 vxx = v0246 + v_setall_f32(tz)*(v1357 - v0246); v_float32x4 v00_10 = vxx; v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx))); - v_float32x4 v0_1 = v00_10 + v_setall_f32(ty) * (v01_11 - v00_10); + v_float32x4 v0_1 = v00_10 + v_setall_f32(ty)*(v01_11 - v00_10); float v0 = v0_1.get0(); v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); float v1 = v0_1.get0(); - return v0 + tx * (v1 - v0); + return v0 + tx*(v1 - v0); } #else inline TsdfType TSDFVolumeCPU::interpolateVoxel(Point3f p) const @@ -529,27 +498,27 @@ inline TsdfType TSDFVolumeCPU::interpolateVoxel(Point3f p) const float ty = p.y - iy; float tz = p.z - iz; - int coordBase = ix * xdim + iy * ydim + iz * zdim; + int coordBase = ix*xdim + iy*ydim + iz*zdim; const TsdfVoxel* volData = volume.ptr(); TsdfType vx[8]; - for (int i = 0; i < 8; i++) + for(int i = 0; i < 8; i++) vx[i] = volData[neighbourCoords[i] + coordBase].tsdf; - TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); - TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); - TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); - TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); + TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); - TsdfType v0 = v00 + ty * (v01 - v00); - TsdfType v1 = v10 + ty * (v11 - v10); + TsdfType v0 = v00 + ty*(v01 - v00); + TsdfType v1 = v10 + ty*(v11 - v10); - return v0 + tx * (v1 - v0); + return v0 + tx*(v1 - v0); } #endif #if USE_INTRINSICS -// gradientDeltaFactor is fixed at 1.0 of voxel size +//gradientDeltaFactor is fixed at 1.0 of voxel size inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f _p) const { v_float32x4 p(_p.x, _p.y, _p.z, 0.f); @@ -561,9 +530,11 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f _p) const inline v_float32x4 TSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const { - if (v_check_any((p < v_float32x4(1.f, 1.f, 1.f, 0.f)) + - (p >= v_float32x4((float)(volResolution.x - 2), (float)(volResolution.y - 2), - (float)(volResolution.z - 2), 1.f)))) + if(v_check_any((p < v_float32x4(1.f, 1.f, 1.f, 0.f)) + + (p >= v_float32x4((float)(volResolution.x-2), + (float)(volResolution.y-2), + (float)(volResolution.z-2), 1.f)) + )) return nanv; v_int32x4 ip = v_floor(p); @@ -577,43 +548,41 @@ inline v_float32x4 TSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const TsdfVoxel* volData = volume.ptr(); - int ix = ip.get0(); - ip = v_rotate_right<1>(ip); - int iy = ip.get0(); - ip = v_rotate_right<1>(ip); + int ix = ip.get0(); ip = v_rotate_right<1>(ip); + int iy = ip.get0(); ip = v_rotate_right<1>(ip); int iz = ip.get0(); - int coordBase = ix * xdim + iy * ydim + iz * zdim; + int coordBase = ix*xdim + iy*ydim + iz*zdim; float CV_DECL_ALIGNED(16) an[4]; an[0] = an[1] = an[2] = an[3] = 0.f; - for (int c = 0; c < 3; c++) + for(int c = 0; c < 3; c++) { const int dim = volDims[c]; float& nv = an[c]; TsdfType vx[8]; - for (int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf - - volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf; + for(int i = 0; i < 8; i++) + vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - + volData[neighbourCoords[i] + coordBase - 1*dim].tsdf; v_float32x4 v0246(vx[0], vx[2], vx[4], vx[6]), v1357(vx[1], vx[3], vx[5], vx[7]); - v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); + v_float32x4 vxx = v0246 + v_setall_f32(tz)*(v1357 - v0246); v_float32x4 v00_10 = vxx; v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx))); - v_float32x4 v0_1 = v00_10 + v_setall_f32(ty) * (v01_11 - v00_10); + v_float32x4 v0_1 = v00_10 + v_setall_f32(ty)*(v01_11 - v00_10); float v0 = v0_1.get0(); v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); float v1 = v0_1.get0(); - nv = v0 + tx * (v1 - v0); + nv = v0 + tx*(v1 - v0); } v_float32x4 n = v_load_aligned(an); - v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(n * n))); - return n * invNorm; + v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(n*n))); + return n*invNorm; } #else inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const @@ -621,8 +590,9 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const TsdfVoxel* volData = volume.ptr(); - if (p.x < 1 || p.x >= volResolution.x - 2 || p.y < 1 || p.y >= volResolution.y - 2 || p.z < 1 || - p.z >= volResolution.z - 2) + if(p.x < 1 || p.x >= volResolution.x - 2 || + p.y < 1 || p.y >= volResolution.y - 2 || + p.z < 1 || p.z >= volResolution.z - 2) return nan3; int ix = cvFloor(p.x); @@ -633,28 +603,28 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const float ty = p.y - iy; float tz = p.z - iz; - int coordBase = ix * xdim + iy * ydim + iz * zdim; + int coordBase = ix*xdim + iy*ydim + iz*zdim; Vec3f an; - for (int c = 0; c < 3; c++) + for(int c = 0; c < 3; c++) { const int dim = volDims[c]; float& nv = an[c]; TsdfType vx[8]; - for (int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf - - volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf; + for(int i = 0; i < 8; i++) + vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - + volData[neighbourCoords[i] + coordBase - 1*dim].tsdf; - TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); - TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); - TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); - TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); + TsdfType v00 = vx[0] + tz*(vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz*(vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz*(vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz*(vx[7] - vx[6]); - TsdfType v0 = v00 + ty * (v01 - v00); - TsdfType v1 = v10 + ty * (v11 - v10); + TsdfType v0 = v00 + ty*(v01 - v00); + TsdfType v1 = v10 + ty*(v11 - v10); - nv = v0 + tx * (v1 - v0); + nv = v0 + tx*(v1 - v0); } return normalize(an); @@ -663,33 +633,32 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const struct RaycastInvoker : ParallelLoopBody { - RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, Intr intrinsics, - const TSDFVolumeCPU& _volume) - : ParallelLoopBody(), - points(_points), - normals(_normals), - volume(_volume), - tstep(volume.truncDist * volume.raycastStepFactor), - // We do subtract voxel size to minimize checks after - // Note: origin of volume coordinate is placed - // in the center of voxel (0,0,0), not in the corner of the voxel! - boxMax(volume.volSize - Point3f(volume.voxelSize, volume.voxelSize, volume.voxelSize)), - boxMin(), - cam2vol(volume.pose.inv() * cameraPose), - vol2cam(cameraPose.inv() * volume.pose), - reproj(intrinsics.makeReprojector()) - { - std::cout << "Entered RaycastInvoker:" << std::endl; - } + RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, + Intr intrinsics, const TSDFVolumeCPU& _volume) : + ParallelLoopBody(), + points(_points), + normals(_normals), + volume(_volume), + tstep(volume.truncDist * volume.raycastStepFactor), + // We do subtract voxel size to minimize checks after + // Note: origin of volume coordinate is placed + // in the center of voxel (0,0,0), not in the corner of the voxel! + boxMax(volume.volSize - Point3f(volume.voxelSize, + volume.voxelSize, + volume.voxelSize)), + boxMin(), + cam2vol(volume.pose.inv() * cameraPose), + vol2cam(cameraPose.inv() * volume.pose), + reproj(intrinsics.makeReprojector()) + { } #if USE_INTRINSICS - virtual void operator()(const Range& range) const override + virtual void operator() (const Range& range) const override { - std::cout << "Entered raycastInvoker" << std::endl; const v_float32x4 vfxy(reproj.fxinv, reproj.fyinv, 0, 0); const v_float32x4 vcxy(reproj.cx, reproj.cy, 0, 0); - const float(&cm)[16] = cam2vol.matrix.val; + const float (&cm)[16] = cam2vol.matrix.val; const v_float32x4 camRot0(cm[0], cm[4], cm[8], 0); const v_float32x4 camRot1(cm[1], cm[5], cm[9], 0); const v_float32x4 camRot2(cm[2], cm[6], cm[10], 0); @@ -698,21 +667,22 @@ struct RaycastInvoker : ParallelLoopBody const v_float32x4 boxDown(boxMin.x, boxMin.y, boxMin.z, 0.f); const v_float32x4 boxUp(boxMax.x, boxMax.y, boxMax.z, 0.f); - const v_float32x4 invVoxelSize = - v_float32x4(volume.voxelSizeInv, volume.voxelSizeInv, volume.voxelSizeInv, 1.f); + const v_float32x4 invVoxelSize = v_float32x4(volume.voxelSizeInv, + volume.voxelSizeInv, + volume.voxelSizeInv, 1.f); - const float(&vm)[16] = vol2cam.matrix.val; + const float (&vm)[16] = vol2cam.matrix.val; const v_float32x4 volRot0(vm[0], vm[4], vm[8], 0); const v_float32x4 volRot1(vm[1], vm[5], vm[9], 0); const v_float32x4 volRot2(vm[2], vm[6], vm[10], 0); const v_float32x4 volTrans(vm[3], vm[7], vm[11], 0); - for (int y = range.start; y < range.end; y++) + for(int y = range.start; y < range.end; y++) { ptype* ptsRow = points[y]; ptype* nrmRow = normals[y]; - for (int x = 0; x < points.cols; x++) + for(int x = 0; x < points.cols; x++) { v_float32x4 point = nanv, normal = nanv; @@ -721,21 +691,21 @@ struct RaycastInvoker : ParallelLoopBody // get direction through pixel in volume space: // 1. reproject (x, y) on projecting plane where z = 1.f - v_float32x4 planed = (v_float32x4((float)x, (float)y, 0.f, 0.f) - vcxy) * vfxy; + v_float32x4 planed = (v_float32x4((float)x, (float)y, 0.f, 0.f) - vcxy)*vfxy; planed = v_combine_low(planed, v_float32x4(1.f, 0.f, 0.f, 0.f)); // 2. rotate to volume space planed = v_matmuladd(planed, camRot0, camRot1, camRot2, v_setzero_f32()); // 3. normalize - v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(planed * planed))); - v_float32x4 dir = planed * invNorm; + v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(planed*planed))); + v_float32x4 dir = planed*invNorm; // compute intersection of ray with all six bbox planes - v_float32x4 rayinv = v_setall_f32(1.f) / dir; + v_float32x4 rayinv = v_setall_f32(1.f)/dir; // div by zero should be eliminated by these products - v_float32x4 tbottom = rayinv * (boxDown - orig); - v_float32x4 ttop = rayinv * (boxUp - orig); + v_float32x4 tbottom = rayinv*(boxDown - orig); + v_float32x4 ttop = rayinv*(boxUp - orig); // re-order intersections to find smallest and largest on each axis v_float32x4 minAx = v_min(ttop, tbottom); @@ -750,11 +720,11 @@ struct RaycastInvoker : ParallelLoopBody tmin = tmin + tstep; tmax = tmax - tstep; - if (tmin < tmax) + if(tmin < tmax) { // interpolation optimized a little orig *= invVoxelSize; - dir *= invVoxelSize; + dir *= invVoxelSize; int xdim = volume.volDims[0]; int ydim = volume.volDims[1]; @@ -763,27 +733,25 @@ struct RaycastInvoker : ParallelLoopBody v_float32x4 next = (orig + dir * v_setall_f32(tmin)); TsdfType f = volume.interpolateVoxel(next), fnext = f; - // raymarch - int steps = 0; - int nSteps = cvFloor((tmax - tmin) / tstep); - for (; steps < nSteps; steps++) + //raymarch + int steps = 0; + int nSteps = cvFloor((tmax - tmin)/tstep); + for(; steps < nSteps; steps++) { next += rayStep; v_int32x4 ip = v_round(next); - int ix = ip.get0(); - ip = v_rotate_right<1>(ip); - int iy = ip.get0(); - ip = v_rotate_right<1>(ip); - int iz = ip.get0(); - int coord = ix * xdim + iy * ydim + iz * zdim; + int ix = ip.get0(); ip = v_rotate_right<1>(ip); + int iy = ip.get0(); ip = v_rotate_right<1>(ip); + int iz = ip.get0(); + int coord = ix*xdim + iy*ydim + iz*zdim; fnext = volume.volume.at(coord).tsdf; - if (fnext != f) + if(fnext != f) { fnext = volume.interpolateVoxel(next); // when ray crosses a surface - if (std::signbit(f) != std::signbit(fnext)) + if(std::signbit(f) != std::signbit(fnext)) break; f = fnext; @@ -792,31 +760,28 @@ struct RaycastInvoker : ParallelLoopBody // if ray penetrates a surface from outside // linearly interpolate t between two f values - if (f > 0.f && fnext < 0.f) + if(f > 0.f && fnext < 0.f) { v_float32x4 tp = next - rayStep; TsdfType ft = volume.interpolateVoxel(tp); TsdfType ftdt = volume.interpolateVoxel(next); - // float t = tmin + steps*tstep; - // float ts = t - tstep*ft/(ftdt - ft); - float ts = tmin + tstep * (steps - ft / (ftdt - ft)); + float ts = tmin + tstep*(steps - ft/(ftdt - ft)); // avoid division by zero - if (!cvIsNaN(ts) && !cvIsInf(ts)) + if(!cvIsNaN(ts) && !cvIsInf(ts)) { - v_float32x4 pv = (orig + dir * v_setall_f32(ts)); + v_float32x4 pv = (orig + dir*v_setall_f32(ts)); v_float32x4 nv = volume.getNormalVoxel(pv); - if (!isNaN(nv)) + if(!isNaN(nv)) { - // convert pv and nv to camera space - normal = - v_matmuladd(nv, volRot0, volRot1, volRot2, v_setzero_f32()); + //convert pv and nv to camera space + normal = v_matmuladd(nv, volRot0, volRot1, volRot2, v_setzero_f32()); // interpolation optimized a little - point = - v_matmuladd(pv * v_float32x4(volume.voxelSize, volume.voxelSize, - volume.voxelSize, 1.f), - volRot0, volRot1, volRot2, volTrans); + point = v_matmuladd(pv*v_float32x4(volume.voxelSize, + volume.voxelSize, + volume.voxelSize, 1.f), + volRot0, volRot1, volRot2, volTrans); } } } @@ -828,19 +793,18 @@ struct RaycastInvoker : ParallelLoopBody } } #else - virtual void operator()(const Range& range) const override + virtual void operator() (const Range& range) const override { - std::cout << " Entered raycastInvoker" << std::endl; const Point3f camTrans = cam2vol.translation(); - const Matx33f camRot = cam2vol.rotation(); - const Matx33f volRot = vol2cam.rotation(); - std::cout << "range.start: " << range.start << " range.end: " << range.end << "\n"; - for (int y = range.start; y < range.end; y++) + const Matx33f camRot = cam2vol.rotation(); + const Matx33f volRot = vol2cam.rotation(); + + for(int y = range.start; y < range.end; y++) { ptype* ptsRow = points[y]; ptype* nrmRow = normals[y]; - for (int x = 0; x < points.cols; x++) + for(int x = 0; x < points.cols; x++) { Point3f point = nan3, normal = nan3; @@ -848,41 +812,38 @@ struct RaycastInvoker : ParallelLoopBody // direction through pixel in volume space Point3f dir = normalize(Vec3f(camRot * reproj(Point3f(x, y, 1.f)))); - std::cout << "origin: " << orig << " direction: " << dir << "\n"; // compute intersection of ray with all six bbox planes - Vec3f rayinv(1.f / dir.x, 1.f / dir.y, 1.f / dir.z); + Vec3f rayinv(1.f/dir.x, 1.f/dir.y, 1.f/dir.z); Point3f tbottom = rayinv.mul(boxMin - orig); - Point3f ttop = rayinv.mul(boxMax - orig); + Point3f ttop = rayinv.mul(boxMax - orig); // re-order intersections to find smallest and largest on each axis - Point3f minAx(min(ttop.x, tbottom.x), min(ttop.y, tbottom.y), - min(ttop.z, tbottom.z)); - Point3f maxAx(max(ttop.x, tbottom.x), max(ttop.y, tbottom.y), - max(ttop.z, tbottom.z)); + Point3f minAx(min(ttop.x, tbottom.x), min(ttop.y, tbottom.y), min(ttop.z, tbottom.z)); + Point3f maxAx(max(ttop.x, tbottom.x), max(ttop.y, tbottom.y), max(ttop.z, tbottom.z)); // near clipping plane const float clip = 0.f; float tmin = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip); - float tmax = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z)); + float tmax = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z)); // precautions against getting coordinates out of bounds tmin = tmin + tstep; tmax = tmax - tstep; - std::cout << "Tmin: " << tmin << "tmax: " << tmax << "\n"; - if (tmin < tmax) + + if(tmin < tmax) { // interpolation optimized a little - orig = orig * volume.voxelSizeInv; - dir = dir * volume.voxelSizeInv; + orig = orig*volume.voxelSizeInv; + dir = dir*volume.voxelSizeInv; Point3f rayStep = dir * tstep; Point3f next = (orig + dir * tmin); TsdfType f = volume.interpolateVoxel(next), fnext = f; - // raymarch + //raymarch int steps = 0; - int nSteps = floor((tmax - tmin) / tstep); - for (; steps < nSteps; steps++) + int nSteps = floor((tmax - tmin)/tstep); + for(; steps < nSteps; steps++) { next += rayStep; int xdim = volume.volDims[0]; @@ -891,13 +852,13 @@ struct RaycastInvoker : ParallelLoopBody int ix = cvRound(next.x); int iy = cvRound(next.y); int iz = cvRound(next.z); - fnext = volume.volume.at(ix * xdim + iy * ydim + iz * zdim).tsdf; - if (fnext != f) + fnext = volume.volume.at(ix*xdim + iy*ydim + iz*zdim).tsdf; + if(fnext != f) { fnext = volume.interpolateVoxel(next); // when ray crosses a surface - if (std::signbit(f) != std::signbit(fnext)) + if(std::signbit(f) != std::signbit(fnext)) break; f = fnext; @@ -906,27 +867,27 @@ struct RaycastInvoker : ParallelLoopBody // if ray penetrates a surface from outside // linearly interpolate t between two f values - if (f > 0.f && fnext < 0.f) + if(f > 0.f && fnext < 0.f) { - Point3f tp = next - rayStep; - TsdfType ft = volume.interpolateVoxel(tp); + Point3f tp = next - rayStep; + TsdfType ft = volume.interpolateVoxel(tp); TsdfType ftdt = volume.interpolateVoxel(next); // float t = tmin + steps*tstep; // float ts = t - tstep*ft/(ftdt - ft); - float ts = tmin + tstep * (steps - ft / (ftdt - ft)); + float ts = tmin + tstep*(steps - ft/(ftdt - ft)); // avoid division by zero - if (!cvIsNaN(ts) && !cvIsInf(ts)) + if(!cvIsNaN(ts) && !cvIsInf(ts)) { - Point3f pv = (orig + dir * ts); + Point3f pv = (orig + dir*ts); Point3f nv = volume.getNormalVoxel(pv); - if (!isNaN(nv)) + if(!isNaN(nv)) { - // convert pv and nv to camera space + //convert pv and nv to camera space normal = volRot * nv; // interpolation optimized a little - point = vol2cam * (pv * volume.voxelSize); + point = vol2cam * (pv*volume.voxelSize); } } } @@ -953,6 +914,7 @@ struct RaycastInvoker : ParallelLoopBody const Intr::Reprojector reproj; }; + void TSDFVolumeCPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const { @@ -960,103 +922,112 @@ void TSDFVolumeCPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frame CV_Assert(frameSize.area() > 0); - _points.create(frameSize, POINT_TYPE); + _points.create (frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE); - Points points = _points.getMat(); + Points points = _points.getMat(); Normals normals = _normals.getMat(); + RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); const int nstripes = -1; parallel_for_(Range(0, points.rows), ri, nstripes); } + struct FetchPointsNormalsInvoker : ParallelLoopBody { - FetchPointsNormalsInvoker(const TSDFVolumeCPU& _volume, std::vector>& _pVecs, - std::vector>& _nVecs, bool _needNormals) - : ParallelLoopBody(), vol(_volume), pVecs(_pVecs), nVecs(_nVecs), needNormals(_needNormals) + FetchPointsNormalsInvoker(const TSDFVolumeCPU& _volume, + std::vector>& _pVecs, + std::vector>& _nVecs, + bool _needNormals) : + ParallelLoopBody(), + vol(_volume), + pVecs(_pVecs), + nVecs(_nVecs), + needNormals(_needNormals) { volDataStart = vol.volume.ptr(); } - inline void coord(std::vector& points, std::vector& normals, int x, int y, int z, - Point3f V, float v0, int axis) const + inline void coord(std::vector& points, std::vector& normals, + int x, int y, int z, Point3f V, float v0, int axis) const { // 0 for x, 1 for y, 2 for z bool limits = false; Point3i shift; float Vc = 0.f; - if (axis == 0) + if(axis == 0) { shift = Point3i(1, 0, 0); limits = (x + 1 < vol.volResolution.x); Vc = V.x; } - if (axis == 1) + if(axis == 1) { shift = Point3i(0, 1, 0); limits = (y + 1 < vol.volResolution.y); Vc = V.y; } - if (axis == 2) + if(axis == 2) { shift = Point3i(0, 0, 1); limits = (z + 1 < vol.volResolution.z); Vc = V.z; } - if (limits) + if(limits) { - const TsdfVoxel& voxeld = - volDataStart[(x + shift.x) * vol.volDims[0] + (y + shift.y) * vol.volDims[1] + - (z + shift.z) * vol.volDims[2]]; + const TsdfVoxel& voxeld = volDataStart[(x+shift.x)*vol.volDims[0] + + (y+shift.y)*vol.volDims[1] + + (z+shift.z)*vol.volDims[2]]; TsdfType vd = voxeld.tsdf; - if (voxeld.weight != 0 && vd != 1.f) + if(voxeld.weight != 0 && vd != 1.f) { - if ((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0)) + if((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0)) { - // linearly interpolate coordinate + //linearly interpolate coordinate float Vn = Vc + vol.voxelSize; - float dinv = 1.f / (abs(v0) + abs(vd)); - float inter = (Vc * abs(vd) + Vn * abs(v0)) * dinv; + float dinv = 1.f/(abs(v0)+abs(vd)); + float inter = (Vc*abs(vd) + Vn*abs(v0))*dinv; - Point3f p(shift.x ? inter : V.x, shift.y ? inter : V.y, shift.z ? inter : V.z); + Point3f p(shift.x ? inter : V.x, + shift.y ? inter : V.y, + shift.z ? inter : V.z); { points.push_back(toPtype(vol.pose * p)); - if (needNormals) + if(needNormals) normals.push_back(toPtype(vol.pose.rotation() * - vol.getNormalVoxel(p * vol.voxelSizeInv))); + vol.getNormalVoxel(p*vol.voxelSizeInv))); } } } } } - virtual void operator()(const Range& range) const override + virtual void operator() (const Range& range) const override { std::vector points, normals; - for (int x = range.start; x < range.end; x++) + for(int x = range.start; x < range.end; x++) { - const TsdfVoxel* volDataX = volDataStart + x * vol.volDims[0]; - for (int y = 0; y < vol.volResolution.y; y++) + const TsdfVoxel* volDataX = volDataStart + x*vol.volDims[0]; + for(int y = 0; y < vol.volResolution.y; y++) { - const TsdfVoxel* volDataY = volDataX + y * vol.volDims[1]; - for (int z = 0; z < vol.volResolution.z; z++) + const TsdfVoxel* volDataY = volDataX + y*vol.volDims[1]; + for(int z = 0; z < vol.volResolution.z; z++) { - const TsdfVoxel& voxel0 = volDataY[z * vol.volDims[2]]; + const TsdfVoxel& voxel0 = volDataY[z*vol.volDims[2]]; TsdfType v0 = voxel0.tsdf; - if (voxel0.weight != 0 && v0 != 1.f) + if(voxel0.weight != 0 && v0 != 1.f) { - Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f) * - vol.voxelSize); + Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f)*vol.voxelSize); coord(points, normals, x, y, z, V, v0, 0); coord(points, normals, x, y, z, V, v0, 1); coord(points, normals, x, y, z, V, v0, 2); - } // if voxel is not empty + } // if voxel is not empty } } } @@ -1078,7 +1049,7 @@ void TSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals { CV_TRACE_FUNCTION(); - if (_points.needed()) + if(_points.needed()) { std::vector> pVecs, nVecs; FetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed()); @@ -1086,20 +1057,20 @@ void TSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals const int nstripes = -1; parallel_for_(range, fi, nstripes); std::vector points, normals; - for (size_t i = 0; i < pVecs.size(); i++) + for(size_t i = 0; i < pVecs.size(); i++) { points.insert(points.end(), pVecs[i].begin(), pVecs[i].end()); normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end()); } _points.create((int)points.size(), 1, POINT_TYPE); - if (!points.empty()) + if(!points.empty()) Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat()); - if (_normals.needed()) + if(_normals.needed()) { _normals.create((int)normals.size(), 1, POINT_TYPE); - if (!normals.empty()) + if(!normals.empty()) Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat()); } } @@ -1107,15 +1078,14 @@ void TSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals struct PushNormals { - PushNormals(const TSDFVolumeCPU& _vol, Mat_& _nrm) - : vol(_vol), normals(_nrm), invPose(vol.pose.inv()) - { - } - void operator()(const ptype& pp, const int* position) const + PushNormals(const TSDFVolumeCPU& _vol, Normals& _nrm) : + vol(_vol), normals(_nrm), invPose(vol.pose.inv()) + { } + void operator ()(const ptype &pp, const int * position) const { Point3f p = fromPtype(pp); Point3f n = nan3; - if (!isNaN(p)) + if(!isNaN(p)) { Point3f voxPt = (invPose * p); voxPt = voxPt * vol.voxelSizeInv; @@ -1124,22 +1094,23 @@ struct PushNormals normals(position[0], position[1]) = toPtype(n); } const TSDFVolumeCPU& vol; - Mat_& normals; + Normals& normals; Affine3f invPose; }; + void TSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); - if (_normals.needed()) + if(_normals.needed()) { Points points = _points.getMat(); CV_Assert(points.type() == POINT_TYPE); _normals.createSameSize(_points, _points.type()); - Mat_ normals = _normals.getMat(); + Normals normals = _normals.getMat(); points.forEach(PushNormals(*this, normals)); } @@ -1148,16 +1119,16 @@ void TSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const ///////// GPU implementation ///////// #ifdef HAVE_OPENCL -TSDFVolumeGPU::TSDFVolumeGPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, Point3i _resolution) - : TSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution, false) +TSDFVolumeGPU::TSDFVolumeGPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, + Point3i _resolution) : + TSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution, false) { - //! TODO: Optimize this to use half precision floats volume = UMat(1, volResolution.x * volResolution.y * volResolution.z, CV_32FC2); reset(); } + // zero volume, leave rest params the same void TSDFVolumeGPU::reset() { @@ -1166,9 +1137,10 @@ void TSDFVolumeGPU::reset() volume.setTo(Scalar(0, 0)); } + // use depth instead of distance (optimization) -void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, - Intr intrinsics) +void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, + cv::Affine3f cameraPose, Intr intrinsics) { CV_TRACE_FUNCTION(); @@ -1181,28 +1153,38 @@ void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f ocl::Kernel k; k.create(name.c_str(), source, options, &errorStr); - if (k.empty()) + if(k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); cv::Affine3f vol2cam(cameraPose.inv() * pose); - float dfac = 1.f / depthFactor; + float dfac = 1.f/depthFactor; Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); // TODO: optimization possible // Use sampler for depth (mask needed) - k.args(ocl::KernelArg::ReadOnly(depth), ocl::KernelArg::PtrReadWrite(volume), - ocl::KernelArg::Constant(vol2cam.matrix.val, sizeof(vol2cam.matrix.val)), voxelSize, - volResGpu.val, volDims.val, fxy.val, cxy.val, dfac, truncDist, maxWeight); + k.args(ocl::KernelArg::ReadOnly(depth), + ocl::KernelArg::PtrReadWrite(volume), + ocl::KernelArg::Constant(vol2cam.matrix.val, + sizeof(vol2cam.matrix.val)), + voxelSize, + volResGpu.val, + volDims.val, + fxy.val, + cxy.val, + dfac, + truncDist, + maxWeight); size_t globalSize[2]; globalSize[0] = (size_t)volResolution.x; globalSize[1] = (size_t)volResolution.y; - if (!k.run(2, globalSize, NULL, true)) + if(!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } + void TSDFVolumeGPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const { @@ -1217,13 +1199,13 @@ void TSDFVolumeGPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frame ocl::Kernel k; k.create(name.c_str(), source, options, &errorStr); - if (k.empty()) + if(k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); - _points.create(frameSize, CV_32FC4); + _points.create (frameSize, CV_32FC4); _normals.create(frameSize, CV_32FC4); - UMat points = _points.getUMat(); + UMat points = _points.getUMat(); UMat normals = _normals.getUMat(); UMat vol2camGpu, cam2volGpu; @@ -1235,30 +1217,42 @@ void TSDFVolumeGPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frame // We do subtract voxel size to minimize checks after // Note: origin of volume coordinate is placed // in the center of voxel (0,0,0), not in the corner of the voxel! - Vec4f boxMin, boxMax(volSize.x - voxelSize, volSize.y - voxelSize, volSize.z - voxelSize); + Vec4f boxMin, boxMax(volSize.x - voxelSize, + volSize.y - voxelSize, + volSize.z - voxelSize); Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy); float tstep = truncDist * raycastStepFactor; Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); - k.args(ocl::KernelArg::WriteOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), - frameSize, ocl::KernelArg::PtrReadOnly(volume), ocl::KernelArg::PtrReadOnly(vol2camGpu), - ocl::KernelArg::PtrReadOnly(cam2volGpu), finv.val, cxy.val, boxMin.val, boxMax.val, - tstep, voxelSize, volResGpu.val, volDims.val, neighbourCoords.val); + k.args(ocl::KernelArg::WriteOnlyNoSize(points), + ocl::KernelArg::WriteOnlyNoSize(normals), + frameSize, + ocl::KernelArg::PtrReadOnly(volume), + ocl::KernelArg::PtrReadOnly(vol2camGpu), + ocl::KernelArg::PtrReadOnly(cam2volGpu), + finv.val, cxy.val, + boxMin.val, boxMax.val, + tstep, + voxelSize, + volResGpu.val, + volDims.val, + neighbourCoords.val); size_t globalSize[2]; globalSize[0] = (size_t)frameSize.width; globalSize[1] = (size_t)frameSize.height; - if (!k.run(2, globalSize, NULL, true)) + if(!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } + void TSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); - if (_normals.needed()) + if(_normals.needed()) { UMat points = _points.getUMat(); CV_Assert(points.type() == POINT_TYPE); @@ -1273,25 +1267,31 @@ void TSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const ocl::Kernel k; k.create(name.c_str(), source, options, &errorStr); - if (k.empty()) + if(k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); UMat volPoseGpu, invPoseGpu; - Mat(pose.matrix).copyTo(volPoseGpu); + Mat(pose .matrix).copyTo(volPoseGpu); Mat(pose.inv().matrix).copyTo(invPoseGpu); Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); Size frameSize = points.size(); - k.args(ocl::KernelArg::ReadOnlyNoSize(points), ocl::KernelArg::WriteOnlyNoSize(normals), - frameSize, ocl::KernelArg::PtrReadOnly(volume), - ocl::KernelArg::PtrReadOnly(volPoseGpu), ocl::KernelArg::PtrReadOnly(invPoseGpu), - voxelSizeInv, volResGpu.val, volDims.val, neighbourCoords.val); + k.args(ocl::KernelArg::ReadOnlyNoSize(points), + ocl::KernelArg::WriteOnlyNoSize(normals), + frameSize, + ocl::KernelArg::PtrReadOnly(volume), + ocl::KernelArg::PtrReadOnly(volPoseGpu), + ocl::KernelArg::PtrReadOnly(invPoseGpu), + voxelSizeInv, + volResGpu.val, + volDims.val, + neighbourCoords.val); size_t globalSize[2]; globalSize[0] = (size_t)points.cols; globalSize[1] = (size_t)points.rows; - if (!k.run(2, globalSize, NULL, true)) + if(!k.run(2, globalSize, NULL, true)) throw std::runtime_error("Failed to run kernel"); } } @@ -1300,7 +1300,7 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) { CV_TRACE_FUNCTION(); - if (points.needed()) + if(points.needed()) { bool needNormals = normals.needed(); @@ -1314,7 +1314,7 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) kscan.create("scanSize", source, options, &errorStr); - if (kscan.empty()) + if(kscan.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); size_t globalSize[3]; @@ -1327,32 +1327,37 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) size_t memSize = device.localMemSize(); // local mem should keep a point (and a normal) for each thread in a group // use 4 float per each point and normal - size_t elemSize = (sizeof(float) * 4) * (needNormals ? 2 : 1); + size_t elemSize = (sizeof(float)*4)*(needNormals ? 2 : 1); const size_t lcols = 8; const size_t lrows = 8; - size_t lplanes = min(memSize / elemSize, wgsLimit) / lcols / lrows; + size_t lplanes = min(memSize/elemSize, wgsLimit)/lcols/lrows; lplanes = roundDownPow2(lplanes); - size_t localSize[3] = { lcols, lrows, lplanes }; + size_t localSize[3] = {lcols, lrows, lplanes}; Vec3i ngroups((int)divUp(globalSize[0], (unsigned int)localSize[0]), (int)divUp(globalSize[1], (unsigned int)localSize[1]), (int)divUp(globalSize[2], (unsigned int)localSize[2])); const size_t counterSize = sizeof(int); - size_t lszscan = localSize[0] * localSize[1] * localSize[2] * counterSize; + size_t lszscan = localSize[0]*localSize[1]*localSize[2]*counterSize; - const int gsz[3] = { ngroups[2], ngroups[1], ngroups[0] }; + const int gsz[3] = {ngroups[2], ngroups[1], ngroups[0]}; UMat groupedSum(3, gsz, CV_32S, Scalar(0)); UMat volPoseGpu; Mat(pose.matrix).copyTo(volPoseGpu); Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); - kscan.args(ocl::KernelArg::PtrReadOnly(volume), volResGpu.val, volDims.val, - neighbourCoords.val, ocl::KernelArg::PtrReadOnly(volPoseGpu), voxelSize, - voxelSizeInv, ocl::KernelArg::Local(lszscan), + kscan.args(ocl::KernelArg::PtrReadOnly(volume), + volResGpu.val, + volDims.val, + neighbourCoords.val, + ocl::KernelArg::PtrReadOnly(volPoseGpu), + voxelSize, + voxelSizeInv, + ocl::KernelArg::Local(lszscan), ocl::KernelArg::WriteOnlyNoSize(groupedSum)); - if (!kscan.run(3, globalSize, localSize, true)) + if(!kscan.run(3, globalSize, localSize, true)) throw std::runtime_error("Failed to run kernel"); Mat groupedSumCpu = groupedSum.getMat(ACCESS_READ); @@ -1365,7 +1370,7 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) points.create(gpuSum, 1, POINT_TYPE); UMat pts = points.getUMat(); UMat nrm; - if (needNormals) + if(needNormals) { normals.create(gpuSum, 1, POINT_TYPE); nrm = normals.getUMat(); @@ -1381,22 +1386,30 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) ocl::Kernel kfill; kfill.create("fillPtsNrm", source, options, &errorStr); - if (kfill.empty()) + if(kfill.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); UMat atomicCtr(1, 1, CV_32S, Scalar(0)); // mem size to keep pts (and normals optionally) for all work-items in a group - size_t lszfill = localSize[0] * localSize[1] * localSize[2] * elemSize; - - kfill.args(ocl::KernelArg::PtrReadOnly(volume), volResGpu.val, volDims.val, - neighbourCoords.val, ocl::KernelArg::PtrReadOnly(volPoseGpu), voxelSize, - voxelSizeInv, ((int)needNormals), ocl::KernelArg::Local(lszfill), + size_t lszfill = localSize[0]*localSize[1]*localSize[2]*elemSize; + + kfill.args(ocl::KernelArg::PtrReadOnly(volume), + volResGpu.val, + volDims.val, + neighbourCoords.val, + ocl::KernelArg::PtrReadOnly(volPoseGpu), + voxelSize, + voxelSizeInv, + ((int)needNormals), + ocl::KernelArg::Local(lszfill), ocl::KernelArg::PtrReadWrite(atomicCtr), ocl::KernelArg::ReadOnlyNoSize(groupedSum), - ocl::KernelArg::WriteOnlyNoSize(pts), ocl::KernelArg::WriteOnlyNoSize(nrm)); + ocl::KernelArg::WriteOnlyNoSize(pts), + ocl::KernelArg::WriteOnlyNoSize(nrm) + ); - if (!kfill.run(3, globalSize, localSize, true)) + if(!kfill.run(3, globalSize, localSize, true)) throw std::runtime_error("Failed to run kernel"); } } @@ -1416,5 +1429,5 @@ cv::Ptr makeTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _ _resolution); } -} // namespace kinfu -} // namespace cv +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 9ff4b2cf58b..1843cbb2a41 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -8,12 +8,26 @@ #ifndef __OPENCV_KINFU_TSDF_H__ #define __OPENCV_KINFU_TSDF_H__ -#include "volume.hpp" +#include +#include "kinfu_frame.hpp" +#include "utils.hpp" namespace cv { namespace kinfu { + +// TODO: Optimization possible: +// * TsdfType can be FP16 +// * weight can be uint16 +typedef float TsdfType; +struct TsdfVoxel +{ + TsdfType tsdf; + int weight; +}; +typedef Vec VecTsdfVoxel; + class TSDFVolume : public Volume { public: @@ -23,8 +37,6 @@ class TSDFVolume : public Volume bool zFirstMemOrder = true); virtual ~TSDFVolume() = default; - virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; - public: Point3i volResolution; int maxWeight; @@ -94,7 +106,6 @@ class TSDFVolumeGPU : public TSDFVolume #endif cv::Ptr makeTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution); - } // namespace kinfu } // namespace cv #endif diff --git a/modules/rgbd/src/utils.cpp b/modules/rgbd/src/utils.cpp index d2188a77c23..30c8aa153f2 100644 --- a/modules/rgbd/src/utils.cpp +++ b/modules/rgbd/src/utils.cpp @@ -12,7 +12,7 @@ namespace cv { namespace rgbd -{ +{ /** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided * by 1000 to get a depth in meters, and the values 0 are converted to std::numeric_limits::quiet_NaN() * Otherwise, the image is simply converted to floats diff --git a/modules/rgbd/src/utils.hpp b/modules/rgbd/src/utils.hpp index 278cfbb977a..0b963675390 100644 --- a/modules/rgbd/src/utils.hpp +++ b/modules/rgbd/src/utils.hpp @@ -72,68 +72,6 @@ static inline bool isNaN(const cv::v_float32x4& p) } #endif -/** @brief Camera intrinsics */ -struct Intr -{ - /** Reprojects screen point to camera space given z coord. */ - struct Reprojector - { - Reprojector() {} - inline Reprojector(Intr intr) - { - fxinv = 1.f/intr.fx, fyinv = 1.f/intr.fy; - cx = intr.cx, cy = intr.cy; - } - template - inline cv::Point3_ operator()(cv::Point3_ p) const - { - T x = p.z * (p.x - cx) * fxinv; - T y = p.z * (p.y - cy) * fyinv; - return cv::Point3_(x, y, p.z); - } - - float fxinv, fyinv, cx, cy; - }; - /** Projects camera space vector onto screen */ - struct Projector - { - inline Projector(Intr intr) : fx(intr.fx), fy(intr.fy), cx(intr.cx), cy(intr.cy) { } - template - inline cv::Point_ operator()(cv::Point3_ p) const - { - T invz = T(1)/p.z; - T x = fx*(p.x*invz) + cx; - T y = fy*(p.y*invz) + cy; - return cv::Point_(x, y); - } - template - inline cv::Point_ operator()(cv::Point3_ p, cv::Point3_& pixVec) const - { - T invz = T(1)/p.z; - pixVec = cv::Point3_(p.x*invz, p.y*invz, 1); - T x = fx*pixVec.x + cx; - T y = fy*pixVec.y + cy; - return cv::Point_(x, y); - } - float fx, fy, cx, cy; - }; - Intr() : fx(), fy(), cx(), cy() { } - Intr(float _fx, float _fy, float _cx, float _cy) : fx(_fx), fy(_fy), cx(_cx), cy(_cy) { } - Intr(cv::Matx33f m) : fx(m(0, 0)), fy(m(1, 1)), cx(m(0, 2)), cy(m(1, 2)) { } - // scale intrinsics to pyramid level - inline Intr scale(int pyr) const - { - float factor = (1.f /(1 << pyr)); - return Intr(fx*factor, fy*factor, cx*factor, cy*factor); - } - inline Reprojector makeReprojector() const { return Reprojector(*this); } - inline Projector makeProjector() const { return Projector(*this); } - - inline cv::Matx33f getMat() const { return Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); } - - float fx, fy, cx, cy; -}; - inline size_t roundDownPow2(size_t x) { size_t shift = 0; diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp new file mode 100644 index 00000000000..b02557beb28 --- /dev/null +++ b/modules/rgbd/src/volume.cpp @@ -0,0 +1,25 @@ +#include +#include "tsdf.hpp" +#include "hash_tsdf.hpp" + +namespace cv +{ +namespace kinfu +{ + +cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, + float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, Point3i _resolution) +{ + if (_volumeType == VolumeType::TSDF) + return makeTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + _resolution); + else if (_volumeType == VolumeType::HASHTSDF) + return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + _truncateThreshold); + else + return nullptr; +} + +} // namespace kinfu +} // namespace cv From 75656bd2d2aafc1d7b986da8d12bc1c32ee669ec Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 23 Jun 2020 16:04:45 -0400 Subject: [PATCH 11/36] - Add tests for hashTSDF - Fix raycasting bug causing to loop forever - Suppress warnings by explicit conversion - Disable hashTsdf test until we figure out memory leak - style changes - Add missing license in a few files, correct precomp.hpp usage --- .../rgbd/include/opencv2/rgbd/intrinsics.hpp | 9 +- modules/rgbd/include/opencv2/rgbd/volume.hpp | 18 +- modules/rgbd/samples/kinfu_demo.cpp | 282 +++++++++--------- modules/rgbd/src/hash_tsdf.cpp | 201 ++++--------- modules/rgbd/src/hash_tsdf.hpp | 38 ++- modules/rgbd/src/tsdf.cpp | 16 +- modules/rgbd/src/tsdf.hpp | 28 +- modules/rgbd/src/utils.hpp | 2 - modules/rgbd/src/volume.cpp | 11 +- modules/rgbd/test/test_kinfu.cpp | 11 +- 10 files changed, 284 insertions(+), 332 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/intrinsics.hpp b/modules/rgbd/include/opencv2/rgbd/intrinsics.hpp index a68cf3fb444..b1ed780222e 100644 --- a/modules/rgbd/include/opencv2/rgbd/intrinsics.hpp +++ b/modules/rgbd/include/opencv2/rgbd/intrinsics.hpp @@ -1,3 +1,7 @@ +// 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_RGBD_INTRINSICS_HPP__ #define __OPENCV_RGBD_INTRINSICS_HPP__ @@ -5,8 +9,9 @@ namespace cv { - namespace kinfu - { +namespace kinfu +{ + struct Intr { /** @brief Camera intrinsics */ diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 61db94d3c44..f057a0a55c0 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -5,8 +5,8 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this // module's directory -#ifndef __OPENCV_VOLUME_H__ -#define __OPENCV_VOLUME_H__ +#ifndef __OPENCV_RGBD_VOLUME_H__ +#define __OPENCV_RGBD_VOLUME_H__ #include "intrinsics.hpp" #include "opencv2/core/affine.hpp" @@ -15,11 +15,10 @@ namespace cv { namespace kinfu { - class Volume { public: - explicit Volume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor) + Volume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor) : voxelSize(_voxelSize), voxelSizeInv(1.0f / voxelSize), pose(_pose), @@ -29,10 +28,11 @@ class Volume virtual ~Volume(){}; - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, - cv::kinfu::Intr intrinsics) = 0; - virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, - cv::OutputArray points, cv::OutputArray normals) const = 0; + virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, + const cv::kinfu::Intr& intrinsics) = 0; + virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, + cv::Size frameSize, cv::OutputArray points, + cv::OutputArray normals) const = 0; virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; virtual void reset() = 0; @@ -53,6 +53,6 @@ enum class VolumeType cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, Point3i _resolution); -} // namespace rgbd +} // namespace kinfu } // namespace cv #endif diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index bec1727390f..147ffaa2d8e 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -2,14 +2,13 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this -// module's directory +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory -#include #include +#include +#include #include #include -#include #include using namespace cv; @@ -27,25 +26,24 @@ static vector readDepth(std::string fileList) vector v; fstream file(fileList); - if (!file.is_open()) + if(!file.is_open()) throw std::runtime_error("Failed to read depth list"); std::string dir; size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); - while (!file.eof()) + while(!file.eof()) { std::string s, imgPath; std::getline(file, s); - if (s.empty() || s[0] == '#') - continue; + if(s.empty() || s[0] == '#') continue; std::stringstream ss; ss << s; double thumb; ss >> thumb >> imgPath; - v.push_back(dir + '/' + imgPath); + v.push_back(dir+'/'+imgPath); } return v; @@ -53,13 +51,14 @@ static vector readDepth(std::string fileList) struct DepthWriter { - DepthWriter(string fileList) : file(fileList, ios::out), count(0), dir() + DepthWriter(string fileList) : + file(fileList, ios::out), count(0), dir() { size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); - if (!file.is_open()) + if(!file.is_open()) throw std::runtime_error("Failed to write depth list"); file << "# depth maps saved from device" << endl; @@ -68,10 +67,10 @@ struct DepthWriter void append(InputArray _depth) { - Mat depth = _depth.getMat(); - string depthFname = cv::format("%04d.png", count); + Mat depth = _depth.getMat(); + string depthFname = cv::format("%04d.png", count); string fullDepthFname = dir + '/' + depthFname; - if (!imwrite(fullDepthFname, depth)) + if(!imwrite(fullDepthFname, depth)) throw std::runtime_error("Failed to write depth to file " + fullDepthFname); file << count++ << " " << depthFname << endl; } @@ -83,19 +82,19 @@ struct DepthWriter namespace Kinect2Params { -static const Size frameSize = Size(512, 424); -// approximate values, no guarantee to be correct -static const float focal = 366.1f; -static const float cx = 258.2f; -static const float cy = 204.f; -static const float k1 = 0.12f; -static const float k2 = -0.34f; -static const float k3 = 0.12f; -}; // namespace Kinect2Params + static const Size frameSize = Size(512, 424); + // approximate values, no guarantee to be correct + static const float focal = 366.1f; + static const float cx = 258.2f; + static const float cy = 204.f; + static const float k1 = 0.12f; + static const float k2 = -0.34f; + static const float k3 = 0.12f; +}; struct DepthSource { - public: +public: enum Type { DEPTH_LIST, @@ -104,27 +103,31 @@ struct DepthSource DEPTH_REALSENSE }; - DepthSource(int cam) : DepthSource("", cam) {} + DepthSource(int cam) : + DepthSource("", cam) + { } - DepthSource(String fileListName) : DepthSource(fileListName, -1) {} + DepthSource(String fileListName) : + DepthSource(fileListName, -1) + { } - DepthSource(String fileListName, int cam) - : depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), - frameIdx(0), - undistortMap1(), - undistortMap2() + DepthSource(String fileListName, int cam) : + depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), + frameIdx(0), + undistortMap1(), + undistortMap2() { - if (cam >= 0) + if(cam >= 0) { vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); - if (vc.isOpened()) + if(vc.isOpened()) { sourceType = Type::DEPTH_KINECT2; } else { vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); - if (vc.isOpened()) + if(vc.isOpened()) { sourceType = Type::DEPTH_REALSENSE; } @@ -132,7 +135,7 @@ struct DepthSource } else { - vc = VideoCapture(); + vc = VideoCapture(); sourceType = Type::DEPTH_KINECT2_LIST; } } @@ -157,15 +160,19 @@ struct DepthSource vc.grab(); switch (sourceType) { - case Type::DEPTH_KINECT2: vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); break; - case Type::DEPTH_REALSENSE: vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); break; - default: - // unknown depth source - vc.retrieve(out); + case Type::DEPTH_KINECT2: + vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); + break; + case Type::DEPTH_REALSENSE: + vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); + break; + default: + // unknown depth source + vc.retrieve(out); } // workaround for Kinect 2 - if (sourceType == Type::DEPTH_KINECT2) + if(sourceType == Type::DEPTH_KINECT2) { out = out(Rect(Point(), Kinect2Params::frameSize)); @@ -182,7 +189,10 @@ struct DepthSource return out; } - bool empty() { return depthFileList.empty() && !(vc.isOpened()); } + bool empty() + { + return depthFileList.empty() && !(vc.isOpened()); + } void updateParams(Params& params) { @@ -196,63 +206,66 @@ struct DepthSource float fx, fy, cx, cy; float depthFactor = 1000.f; Size frameSize; - if (sourceType == Type::DEPTH_KINECT2) + if(sourceType == Type::DEPTH_KINECT2) { fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; + cx = Kinect2Params::cx; + cy = Kinect2Params::cy; frameSize = Kinect2Params::frameSize; } else { - if (sourceType == Type::DEPTH_REALSENSE) + if(sourceType == Type::DEPTH_REALSENSE) { - fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); - fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); - depthFactor = 1.f / (float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); + fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); + fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); + depthFactor = 1.f/(float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); } else { - fx = fy = - (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); + fx = fy = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); } - cx = w / 2 - 0.5f; - cy = h / 2 - 0.5f; + cx = w/2 - 0.5f; + cy = h/2 - 0.5f; frameSize = Size(w, h); } - Matx33f camMatrix = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); + Matx33f camMatrix = Matx33f(fx, 0, cx, + 0, fy, cy, + 0, 0, 1); - params.frameSize = frameSize; - params.intr = camMatrix; + params.frameSize = frameSize; + params.intr = camMatrix; params.depthFactor = depthFactor; // RealSense has shorter depth range, some params should be tuned - if (sourceType == Type::DEPTH_REALSENSE) + if(sourceType == Type::DEPTH_REALSENSE) { // all sizes in meters - float cubeSize = 1.f; - params.voxelSize = cubeSize / params.volumeDims[0]; + float cubeSize = 1.f; + params.voxelSize = cubeSize/params.volumeDims[0]; params.tsdf_trunc_dist = 0.01f; - params.icpDistThresh = 0.01f; - params.volumePose = - Affine3f().translate(Vec3f(-cubeSize / 2.f, -cubeSize / 2.f, 0.05f)); - params.truncateThreshold = 2.5f; + params.icpDistThresh = 0.01f; + params.volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, + -cubeSize/2.f, + 0.05f)); + params.truncateThreshold = 2.5f; params.bilateral_sigma_depth = 0.01f; } - if (sourceType == Type::DEPTH_KINECT2) + if(sourceType == Type::DEPTH_KINECT2) { Matx distCoeffs; distCoeffs(0) = Kinect2Params::k1; distCoeffs(1) = Kinect2Params::k2; distCoeffs(4) = Kinect2Params::k3; - initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), camMatrix, frameSize, - CV_16SC2, undistortMap1, undistortMap2); + initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), + camMatrix, frameSize, CV_16SC2, + undistortMap1, undistortMap2); } } } @@ -269,7 +282,8 @@ const std::string vizWindowName = "cloud"; struct PauseCallbackArgs { - PauseCallbackArgs(KinFu& _kf) : kf(_kf) {} + PauseCallbackArgs(KinFu& _kf) : kf(_kf) + { } KinFu& kf; }; @@ -277,9 +291,9 @@ struct PauseCallbackArgs void pauseCallback(const viz::MouseEvent& me, void* args); void pauseCallback(const viz::MouseEvent& me, void* args) { - if (me.type == viz::MouseEvent::Type::MouseMove || - me.type == viz::MouseEvent::Type::MouseScrollDown || - me.type == viz::MouseEvent::Type::MouseScrollUp) + if(me.type == viz::MouseEvent::Type::MouseMove || + me.type == viz::MouseEvent::Type::MouseScrollDown || + me.type == viz::MouseEvent::Type::MouseScrollUp) { PauseCallbackArgs pca = *((PauseCallbackArgs*)(args)); viz::Viz3d window(vizWindowName); @@ -291,63 +305,64 @@ void pauseCallback(const viz::MouseEvent& me, void* args) } #endif -static const char* keys = { +static const char* keys = +{ "{help h usage ? | | print this message }" "{depth | | Path to depth.txt file listing a set of depth images }" "{camera |0| Index of depth camera to be used as a depth source }" "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," - " in coarse mode points and normals are displayed }" + " in coarse mode points and normals are displayed }" "{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}" "{idle | | Do not run KinFu, just display depth frames }" "{record | | Write depth frames to specified file list" - " (the same format as for the 'depth' key) }" + " (the same format as for the 'depth' key) }" }; static const std::string message = - "\nThis demo uses live depth input or RGB-D dataset taken from" - "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" - "\nto demonstrate KinectFusion implementation \n"; + "\nThis demo uses live depth input or RGB-D dataset taken from" + "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" + "\nto demonstrate KinectFusion implementation \n"; + -int main(int argc, char** argv) +int main(int argc, char **argv) { bool coarse = false; - bool idle = false; + bool idle = false; bool useHashTSDF = false; string recordPath; CommandLineParser parser(argc, argv, keys); parser.about(message); - if (!parser.check()) + if(!parser.check()) { parser.printMessage(); parser.printErrors(); return -1; } - if (parser.has("help")) + if(parser.has("help")) { parser.printMessage(); return 0; } - if (parser.has("coarse")) + if(parser.has("coarse")) { coarse = true; } - if (parser.has("record")) + if(parser.has("record")) { recordPath = parser.get("record"); } - if (parser.has("useHashTSDF")) + if(parser.has("useHashTSDF")) { useHashTSDF = true; } - if (parser.has("idle")) + if(parser.has("idle")) { idle = true; } - Ptr ds; if (parser.has("depth")) ds = makePtr(parser.get("depth")); @@ -362,13 +377,13 @@ int main(int argc, char** argv) } Ptr depthWriter; - if (!recordPath.empty()) + if(!recordPath.empty()) depthWriter = makePtr(recordPath); Ptr params; Ptr kf; - if (coarse) + if(coarse) params = Params::coarseParams(); else params = Params::defaultParams(); @@ -383,14 +398,14 @@ int main(int argc, char** argv) cv::setUseOptimized(true); // Scene-specific params should be tuned for each scene individually - // float cubeSize = 1.f; - // params->voxelSize = cubeSize/params->volumeDims[0]; //meters - // params->tsdf_trunc_dist = 0.01f; //meters - // params->icpDistThresh = 0.01f; //meters - // params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters - // params->tsdf_max_weight = 16; - - if (!idle) + //float cubeSize = 1.f; + //params->voxelSize = cubeSize/params->volumeDims[0]; //meters + //params->tsdf_trunc_dist = 0.01f; //meters + //params->icpDistThresh = 0.01f; //meters + //params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters + //params->tsdf_max_weight = 16; + + if(!idle) kf = KinFu::create(params); #ifdef HAVE_OPENCV_VIZ @@ -405,33 +420,31 @@ int main(int argc, char** argv) int64 prevTime = getTickCount(); - for (UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) + for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) { - if (depthWriter) + if(depthWriter) depthWriter->append(frame); #ifdef HAVE_OPENCV_VIZ - if (pause) + if(pause) { // doesn't happen in idle mode kf->getCloud(points, normals); - if (!points.empty() && !normals.empty()) + if(!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); - viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05, - viz::Color::gray()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); window.showWidget("cloud", cloudWidget); window.showWidget("normals", cloudNormals); - Vec3d volSize = kf->getParams().voxelSize * Vec3d(kf->getParams().volumeDims); - window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), + Vec3d volSize = kf->getParams().voxelSize*Vec3d(kf->getParams().volumeDims); + window.showWidget("cube", viz::WCube(Vec3d::all(0), + volSize), kf->getParams().volumePose); PauseCallbackArgs pca(*kf); window.registerMouseCallback(pauseCallback, (void*)&pca); - window.showWidget("text", - viz::WText(cv::String("Move camera in this window. " - "Close the window or press Q to resume"), - Point())); + window.showWidget("text", viz::WText(cv::String("Move camera in this window. " + "Close the window or press Q to resume"), Point())); window.spin(); window.removeWidget("text"); window.removeWidget("cloud"); @@ -446,12 +459,12 @@ int main(int argc, char** argv) { UMat cvt8; float depthFactor = params->depthFactor; - convertScaleAbs(frame, cvt8, 0.25 * 256. / depthFactor); - if (!idle) + convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor); + if(!idle) { imshow("depth", cvt8); - if (!kf->update(frame)) + if(!kf->update(frame)) { kf->reset(); std::cout << "reset" << std::endl; @@ -459,22 +472,22 @@ int main(int argc, char** argv) #ifdef HAVE_OPENCV_VIZ else { - if (coarse) + if(coarse) { kf->getCloud(points, normals); - if (!points.empty() && !normals.empty()) + if(!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); - viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, - /*scale*/ 0.05, viz::Color::gray()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); window.showWidget("cloud", cloudWidget); window.showWidget("normals", cloudNormals); } } - // window.showWidget("worldAxes", viz::WCoordinateSystem()); - Vec3d volSize = kf->getParams().voxelSize * kf->getParams().volumeDims; - window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), + //window.showWidget("worldAxes", viz::WCoordinateSystem()); + Vec3d volSize = kf->getParams().voxelSize*kf->getParams().volumeDims; + window.showWidget("cube", viz::WCube(Vec3d::all(0), + volSize), kf->getParams().volumePose); window.setViewerPose(kf->getPose()); window.spinOnce(1, true); @@ -490,10 +503,9 @@ int main(int argc, char** argv) } int64 newTime = getTickCount(); - putText(rendered, - cv::format("FPS: %2d press R to reset, P to pause, Q to quit", - (int)(getTickFrequency() / (newTime - prevTime))), - Point(0, rendered.rows - 1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); + putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit", + (int)(getTickFrequency()/(newTime - prevTime))), + Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); prevTime = newTime; imshow("render", rendered); @@ -501,17 +513,19 @@ int main(int argc, char** argv) int c = waitKey(1); switch (c) { - case 'r': - if (!idle) - kf->reset(); - break; - case 'q': return 0; + case 'r': + if(!idle) + kf->reset(); + break; + case 'q': + return 0; #ifdef HAVE_OPENCV_VIZ - case 'p': - if (!idle) - pause = true; + case 'p': + if(!idle) + pause = true; #endif - default: break; + default: + break; } } diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 6306c0c9f9e..161197b12bb 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1,9 +1,7 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html - -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this -// module's directory +#include "precomp.hpp" #include "hash_tsdf.hpp" #include @@ -14,6 +12,7 @@ #include "kinfu_frame.hpp" #include "opencv2/core/cvstd.hpp" +#include "opencv2/core/utility.hpp" #include "opencv2/core/utils/trace.hpp" #include "utils.hpp" @@ -128,33 +127,43 @@ struct AllocateVolumeUnitsInvoker : ParallelLoopBody mutable Mutex mutex; }; -struct VolumeUnitInFrustumInvoker : ParallelLoopBody + +void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, + const cv::Affine3f& cameraPose, const Intr& intrinsics) { - VolumeUnitInFrustumInvoker(HashTSDFVolumeCPU& _volume, - const std::vector& _totalVolUnits, const Depth& _depth, - Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) - : ParallelLoopBody(), - volume(_volume), - totalVolUnits(_totalVolUnits), - depth(_depth), - proj(_intrinsics.makeProjector()), - depthFactor(_depthFactor), - vol2cam(_cameraPose.inv() * _volume.pose) + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + Depth depth = _depth.getMat(); + + //! Compute volumes to be allocated + AllocateVolumeUnitsInvoker allocate_i(*this, depth, intrinsics, cameraPose, depthFactor); + Range allocateRange(0, depth.rows); + parallel_for_(allocateRange, allocate_i); + + //! Get volumes that are in the current camera frame + std::vector totalVolUnits; + for (const auto& keyvalue : volumeUnits) { + totalVolUnits.push_back(keyvalue.first); } - virtual void operator()(const Range& range) const override - { + //! Mark volumes in the camera frustum as active + Range inFrustumRange(0, (int)volumeUnits.size()); + parallel_for_(inFrustumRange, [&](const Range& range) { + const Affine3f vol2cam(cameraPose.inv() * pose); + const Intr::Projector proj(intrinsics.makeProjector()); + for (int i = range.start; i < range.end; ++i) { cv::Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitMap::iterator it = volume.volumeUnits.find(tsdf_idx); - if (it == volume.volumeUnits.end()) + VolumeUnitMap::iterator it = volumeUnits.find(tsdf_idx); + if (it == volumeUnits.end()) return; - Point3f volumeUnitPos = volume.volumeUnitIdxToVolume(it->first); + Point3f volumeUnitPos = volumeUnitIdxToVolume(it->first); Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; - if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > volume.truncateThreshold) + if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) { it->second.isActive = false; return; @@ -163,42 +172,19 @@ struct VolumeUnitInFrustumInvoker : ParallelLoopBody if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) { - assert(it != volume.volumeUnits.end()); + assert(it != volumeUnits.end()); it->second.isActive = true; } } - } - HashTSDFVolumeCPU& volume; - const std::vector totalVolUnits; - const Depth& depth; - const Intr::Projector proj; - const float depthFactor; - const Affine3f vol2cam; - mutable Mutex mutex; -}; + }); -struct IntegrateSubvolumeInvoker : ParallelLoopBody -{ - IntegrateSubvolumeInvoker(HashTSDFVolumeCPU& _volume, - const std::vector& _totalVolUnits, const Depth& _depth, - Intr _intrinsics, cv::Affine3f _cameraPose, float _depthFactor) - : ParallelLoopBody(), - volume(_volume), - totalVolUnits(_totalVolUnits), - depth(_depth), - depthFactor(_depthFactor), - cameraPose(_cameraPose), - intrinsics(_intrinsics) - { - } - - virtual void operator()(const Range& range) const override - { + //! Integrate the correct volumeUnits + parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) { for (int i = range.start; i < range.end; i++) { cv::Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitMap::iterator it = volume.volumeUnits.find(tsdf_idx); - if (it == volume.volumeUnits.end()) + VolumeUnitMap::iterator it = volumeUnits.find(tsdf_idx); + if (it == volumeUnits.end()) return; VolumeUnit& volumeUnit = it->second; @@ -210,76 +196,7 @@ struct IntegrateSubvolumeInvoker : ParallelLoopBody volumeUnit.isActive = false; } } - } - - HashTSDFVolumeCPU& volume; - std::vector totalVolUnits; - const Depth& depth; - float depthFactor; - cv::Affine3f cameraPose; - Intr intrinsics; - mutable Mutex mutex; -}; - -void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, - Intr intrinsics) -{ - CV_TRACE_FUNCTION(); - - CV_Assert(_depth.type() == DEPTH_TYPE); - Depth depth = _depth.getMat(); - /* int activeVolUnits = 0; */ - /* for (const auto& keyvalue : volumeUnits) */ - /* { */ - /* const VolumeUnit& volUnit = keyvalue.second; */ - /* if (volUnit.isActive) */ - /* { */ - /* activeVolUnits++; */ - /* } */ - /* } */ - /* std::cout << " Active volume Units before integration: " << activeVolUnits << "\n"; */ - //! Compute volumes to be allocated - AllocateVolumeUnitsInvoker allocate_i(*this, depth, intrinsics, cameraPose, depthFactor); - Range range(0, depth.rows); - parallel_for_(range, allocate_i); - - /* activeVolUnits = 0; */ - /* for (const auto& keyvalue : volumeUnits) */ - /* { */ - /* const VolumeUnit& volUnit = keyvalue.second; */ - /* if (volUnit.isActive) */ - /* { */ - /* activeVolUnits++; */ - /* } */ - /* } */ - /* std::cout << "Active volumes after allocation: " << activeVolUnits << "\n"; */ - //! Get volumes that are in the current camera frame - std::vector totalVolUnits; - for (const auto& keyvalue : volumeUnits) - { - totalVolUnits.push_back(keyvalue.first); - } - VolumeUnitInFrustumInvoker infrustum_i(*this, totalVolUnits, depth, intrinsics, cameraPose, - depthFactor); - Range in_frustum_range(0, volumeUnits.size()); - parallel_for_(in_frustum_range, infrustum_i); - /* std::cout << " Number of total units in volume: " << volumeUnits.size() << " " */ - /* << totalVolUnits.size() << "\n"; */ - /* activeVolUnits = 0; */ - /* for (const auto& keyvalue : volumeUnits) */ - /* { */ - /* const VolumeUnit& volUnit = keyvalue.second; */ - /* if (volUnit.isActive) */ - /* { */ - /* activeVolUnits++; */ - /* } */ - /* } */ - /* std::cout << " Number of Active volume units in frame: " << activeVolUnits << "\n"; */ - //! Integrate the correct volumeUnits - IntegrateSubvolumeInvoker integrate_i(*this, totalVolUnits, depth, intrinsics, cameraPose, - depthFactor); - Range accessed_units_range(0, totalVolUnits.size()); - parallel_for_(accessed_units_range, integrate_i); + }); } cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx(cv::Point3f p) const @@ -362,11 +279,11 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const for (int c = 0; c < 3; c++) { - pointPrev[c] -= voxelSize * 0.5; - pointNext[c] += voxelSize * 0.5; + pointPrev[c] -= voxelSize * 0.5f; + pointNext[c] += voxelSize * 0.5f; normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; - normal[c] *= 0.5; + normal[c] *= 0.5f; pointPrev[c] = pointVec[c]; pointNext[c] = pointVec[c]; @@ -374,10 +291,10 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const return normalize(normal); } -struct RaycastInvoker : ParallelLoopBody +struct HashRaycastInvoker : ParallelLoopBody { - RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, Intr intrinsics, - const HashTSDFVolumeCPU& _volume) + HashRaycastInvoker(Points& _points, Normals& _normals, const Affine3f& cameraPose, + const Intr& intrinsics, const HashTSDFVolumeCPU& _volume) : ParallelLoopBody(), points(_points), normals(_normals), @@ -408,18 +325,19 @@ struct RaycastInvoker : ParallelLoopBody Point3f point = nan3, normal = nan3; //! Ray origin and direction in the volume coordinate frame - Point3f orig = cam2volTrans; - Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(x, y, 1.f)))); + Point3f orig = cam2volTrans; + Point3f rayDirV = + normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); - float tmin = 0; - float tmax = volume.truncateThreshold / rayDirV.z; + float tmin = 0; + float tmax = volume.truncateThreshold; float tcurr = tmin; cv::Vec3i prevVolumeUnitIdx = cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), std::numeric_limits::min()); - float tprev = tcurr; + float tprev = tcurr; TsdfType prevTsdf = volume.truncDist; cv::Ptr currVolumeUnit; while (tcurr < tmax) @@ -431,7 +349,7 @@ struct RaycastInvoker : ParallelLoopBody TsdfType currTsdf = prevTsdf; int currWeight = 0; - float stepSize = 0.5 * blockSize; + float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; //! Does the subvolume exist in hashtable @@ -487,7 +405,7 @@ struct RaycastInvoker : ParallelLoopBody const Intr::Reprojector reproj; }; -void HashTSDFVolumeCPU::raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, +void HashTSDFVolumeCPU::raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const { @@ -500,7 +418,7 @@ void HashTSDFVolumeCPU::raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrins Points points = _points.getMat(); Normals normals = _normals.getMat(); - RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); + HashRaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); const int nstripes = -1; parallel_for_(Range(0, points.rows), ri, nstripes); @@ -585,7 +503,7 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor totalVolUnits.push_back(keyvalue.first); } FetchPointsNormalsInvoker fi(*this, totalVolUnits, pVecs, nVecs, _normals.needed()); - Range range(0, totalVolUnits.size()); + Range range(0, (int)totalVolUnits.size()); const int nstripes = -1; parallel_for_(range, fi, nstripes); std::vector points, normals; @@ -610,18 +528,19 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor struct PushNormals { - PushNormals(const HashTSDFVolumeCPU& _volume, Normals& _normals) : - volume(_volume), normals(_normals), invPose(volume.pose.inv()) - {} + PushNormals(const HashTSDFVolumeCPU& _volume, Normals& _normals) + : volume(_volume), normals(_normals), invPose(volume.pose.inv()) + { + } - void operator ()(const ptype &point, const int* position) const + void operator()(const ptype& point, const int* position) const { Point3f p = fromPtype(point); Point3f n = nan3; - if(!isNaN(p)) + if (!isNaN(p)) { Point3f voxelPoint = invPose * p; - n = volume.pose.rotation() * volume.getNormalVoxel(voxelPoint); + n = volume.pose.rotation() * volume.getNormalVoxel(voxelPoint); } normals(position[0], position[1]) = toPtype(n); } @@ -634,7 +553,7 @@ void HashTSDFVolumeCPU::fetchNormals(cv::InputArray _points, cv::OutputArray _no { CV_TRACE_FUNCTION(); - if(_normals.needed()) + if (_normals.needed()) { Points points = _points.getMat(); CV_Assert(points.type() == POINT_TYPE); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index bf9e9887488..bba2cd5672a 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -2,16 +2,13 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this -// module's directory - #ifndef __OPENCV_HASH_TSDF_H__ #define __OPENCV_HASH_TSDF_H__ +#include #include #include -#include #include "tsdf.hpp" namespace cv @@ -23,9 +20,9 @@ class HashTSDFVolume : public Volume public: // dimension in voxels, size in meters //! Use fixed volume cuboid - explicit HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, float _truncateThreshold, - int _volumeUnitRes, bool zFirstMemOrder = true); + HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float _truncateThreshold, int _volumeUnitRes, + bool zFirstMemOrder = true); virtual ~HashTSDFVolume() = default; @@ -33,14 +30,14 @@ class HashTSDFVolume : public Volume int maxWeight; float truncDist; float truncateThreshold; - uint16_t volumeUnitResolution; + int volumeUnitResolution; float volumeUnitSize; bool zFirstMemOrder; }; struct VolumeUnit { - explicit VolumeUnit() : pVolume(nullptr){}; + VolumeUnit() : pVolume(nullptr){}; ~VolumeUnit() = default; cv::Ptr pVolume; @@ -51,7 +48,7 @@ struct VolumeUnit //! Spatial hashing struct tsdf_hash { - size_t operator()(const cv::Vec3i &x) const noexcept + size_t operator()(const cv::Vec3i& x) const noexcept { size_t seed = 0; constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; @@ -70,14 +67,15 @@ class HashTSDFVolumeCPU : public HashTSDFVolume { public: // dimension in voxels, size in meters - explicit HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, float _truncateThreshold, - int _volumeUnitRes, bool zFirstMemOrder = true); + HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, float _truncateThreshold, + int _volumeUnitRes, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, - cv::kinfu::Intr intrinsics) override; - virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, - cv::OutputArray points, cv::OutputArray normals) const override; + virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, + const cv::kinfu::Intr& intrinsics) override; + virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, + cv::Size frameSize, cv::OutputArray points, + cv::OutputArray normals) const override; virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; @@ -85,13 +83,13 @@ class HashTSDFVolumeCPU : public HashTSDFVolume virtual void reset() override; //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) - virtual TsdfVoxel at(const cv::Vec3i &volumeIdx) const; + virtual TsdfVoxel at(const cv::Vec3i& volumeIdx) const; //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) - virtual TsdfVoxel at(const cv::Point3f &point) const; + virtual TsdfVoxel at(const cv::Point3f& point) const; - inline TsdfType interpolateVoxel(const cv::Point3f &point) const; + inline TsdfType interpolateVoxel(const cv::Point3f& point) const; Point3f getNormalVoxel(cv::Point3f p) const; //! Utility functions for coordinate transformations diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index cd77914c7af..a650fd27bcb 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -177,7 +177,7 @@ static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) struct IntegrateInvoker : ParallelLoopBody { - IntegrateInvoker(TSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose, + IntegrateInvoker(TSDFVolumeCPU& _volume, const Depth& _depth, const Intr& intrinsics, const cv::Affine3f& cameraPose, float depthFactor) : ParallelLoopBody(), volume(_volume), @@ -426,8 +426,8 @@ struct IntegrateInvoker : ParallelLoopBody }; // use depth instead of distance (optimization) -void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, - Intr intrinsics) +void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, + const Intr& intrinsics) { CV_TRACE_FUNCTION(); @@ -633,8 +633,8 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const struct RaycastInvoker : ParallelLoopBody { - RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose, - Intr intrinsics, const TSDFVolumeCPU& _volume) : + RaycastInvoker(Points& _points, Normals& _normals, const Affine3f& cameraPose, + const Intr& intrinsics, const TSDFVolumeCPU& _volume) : ParallelLoopBody(), points(_points), normals(_normals), @@ -915,7 +915,7 @@ struct RaycastInvoker : ParallelLoopBody }; -void TSDFVolumeCPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frameSize, +void TSDFVolumeCPU::raycast(const cv::Affine3f& cameraPose, const Intr& intrinsics, Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const { CV_TRACE_FUNCTION(); @@ -1140,7 +1140,7 @@ void TSDFVolumeGPU::reset() // use depth instead of distance (optimization) void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, - cv::Affine3f cameraPose, Intr intrinsics) + const cv::Affine3f& cameraPose, const Intr& intrinsics) { CV_TRACE_FUNCTION(); @@ -1185,7 +1185,7 @@ void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, } -void TSDFVolumeGPU::raycast(cv::Affine3f cameraPose, Intr intrinsics, Size frameSize, +void TSDFVolumeGPU::raycast(const cv::Affine3f& cameraPose, const Intr& intrinsics, Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const { CV_TRACE_FUNCTION(); diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 1843cbb2a41..89f49963885 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -2,13 +2,13 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this -// module's directory +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory #ifndef __OPENCV_KINFU_TSDF_H__ #define __OPENCV_KINFU_TSDF_H__ #include + #include "kinfu_frame.hpp" #include "utils.hpp" @@ -16,7 +16,6 @@ namespace cv { namespace kinfu { - // TODO: Optimization possible: // * TsdfType can be FP16 // * weight can be uint16 @@ -32,9 +31,8 @@ class TSDFVolume : public Volume { public: // dimension in voxels, size in meters - explicit TSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, Point3i _resolution, - bool zFirstMemOrder = true); + TSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true); virtual ~TSDFVolume() = default; public: @@ -54,10 +52,11 @@ class TSDFVolumeCPU : public TSDFVolume TSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, - cv::kinfu::Intr intrinsics) override; - virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, - cv::OutputArray points, cv::OutputArray normals) const override; + virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, + const cv::kinfu::Intr& intrinsics) override; + virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, + cv::Size frameSize, cv::OutputArray points, + cv::OutputArray normals) const override; virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; @@ -87,10 +86,11 @@ class TSDFVolumeGPU : public TSDFVolume TSDFVolumeGPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution); - virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, - cv::kinfu::Intr intrinsics) override; - virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, cv::Size frameSize, - cv::OutputArray _points, cv::OutputArray _normals) const override; + virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, + const cv::kinfu::Intr& intrinsics) override; + virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, + cv::Size frameSize, cv::OutputArray _points, + cv::OutputArray _normals) const override; virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; virtual void fetchNormals(cv::InputArray points, cv::OutputArray normals) const override; diff --git a/modules/rgbd/src/utils.hpp b/modules/rgbd/src/utils.hpp index 0b963675390..b7febed57a7 100644 --- a/modules/rgbd/src/utils.hpp +++ b/modules/rgbd/src/utils.hpp @@ -9,8 +9,6 @@ #ifndef __OPENCV_RGBD_UTILS_HPP__ #define __OPENCV_RGBD_UTILS_HPP__ -#include "precomp.hpp" - namespace cv { namespace rgbd diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index b02557beb28..a018ab723d2 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -1,4 +1,10 @@ +// 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 + #include "tsdf.hpp" #include "hash_tsdf.hpp" @@ -6,17 +12,20 @@ namespace cv { namespace kinfu { - cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, Point3i _resolution) { if (_volumeType == VolumeType::TSDF) + { return makeTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution); + } else if (_volumeType == VolumeType::HASHTSDF) + { return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); + } else return nullptr; } diff --git a/modules/rgbd/test/test_kinfu.cpp b/modules/rgbd/test/test_kinfu.cpp index 19196121e0a..18059013ee7 100644 --- a/modules/rgbd/test/test_kinfu.cpp +++ b/modules/rgbd/test/test_kinfu.cpp @@ -276,7 +276,7 @@ Ptr Scene::create(int nScene, Size sz, Matx33f _intr, float _depthFactor) static const bool display = false; -void flyTest(bool hiDense, bool inequal) +void flyTest(bool hiDense, bool inequal, bool hashTsdf = false) { Ptr params; if(hiDense) @@ -284,6 +284,9 @@ void flyTest(bool hiDense, bool inequal) else params = kinfu::Params::coarseParams(); + if(hashTsdf) + params = kinfu::Params::hashTSDFParams(!hiDense); + if(inequal) { params->volumeDims[0] += 32; @@ -369,4 +372,10 @@ TEST(KinectFusion, DISABLED_OCL) } #endif +TEST( KinectFusion, DISABLED_hashTsdf ) +{ + flyTest(false, false, true); + //! hashTSDF does not support non-equal volumeDims + flyTest(true, false, true); +} }} // namespace From d5ccfdefcc895ca98dbe15f7bd0857665d7a212a Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Sat, 4 Jul 2020 00:00:44 -0400 Subject: [PATCH 12/36] - Use CRTP based static polymorphism to choose between CPU and GPU for HashTSDF volume --- modules/rgbd/include/opencv2/rgbd/volume.hpp | 24 +++- modules/rgbd/src/hash_tsdf.cpp | 60 ++++---- modules/rgbd/src/hash_tsdf.hpp | 144 +++++++++++++++---- modules/rgbd/src/kinfu.cpp | 1 - modules/rgbd/src/tsdf.hpp | 1 + modules/rgbd/src/utils.hpp | 2 + modules/rgbd/src/volume.cpp | 74 +++++++++- 7 files changed, 244 insertions(+), 62 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index f057a0a55c0..89465ec1a5a 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -29,7 +29,7 @@ class Volume virtual ~Volume(){}; virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, - const cv::kinfu::Intr& intrinsics) = 0; + const cv::kinfu::Intr& intrinsics) = 0; virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, cv::OutputArray points, cv::OutputArray normals) const = 0; @@ -50,6 +50,28 @@ enum class VolumeType HASHTSDF = 1 }; +//! TODO: Tagged union for volumeParams +struct VolumeParams +{ + VolumeType volumeType; + cv::Affine3f volumePose; + float voxelSize; + float truncDist; + int maxWeight; + float depthTruncThreshold; + float raycastStepFactor; + + float volumeSize; + Point3i volumeResolution; + int volumeUnitResolution; + + static Ptr defaultParams(VolumeType _volumeType); + + static Ptr coarseParams(VolumeType _volumeType); +}; + +cv::Ptr makeVolume(const VolumeParams& _volumeParams); + cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, Point3i _resolution); diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 161197b12bb..bbc01012e76 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1,7 +1,6 @@ // 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 "hash_tsdf.hpp" #include @@ -14,15 +13,18 @@ #include "opencv2/core/cvstd.hpp" #include "opencv2/core/utility.hpp" #include "opencv2/core/utils/trace.hpp" +#include "precomp.hpp" #include "utils.hpp" namespace cv { namespace kinfu { -HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, float _truncateThreshold, - int _volumeUnitRes, bool _zFirstMemOrder) +template +HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, + float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, int _volumeUnitRes, + bool _zFirstMemOrder) : Volume(_voxelSize, _pose, _raycastStepFactor), maxWeight(_maxWeight), truncateThreshold(_truncateThreshold), @@ -36,13 +38,19 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _rayc HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) - : HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, - _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) + : Base(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, + _volumeUnitRes, _zFirstMemOrder) { } +HashTSDFVolumeCPU::HashTSDFVolumeCPU(const VolumeParams& _params, bool _zFirstMemOrder) + : Base(_params.voxelSize, _params.volumePose, _params.raycastStepFactor, _params.truncDist, + _params.maxWeight, _params.depthTruncThreshold, _params.volumeUnitResolution, + _zFirstMemOrder) +{ +} // zero volume, leave rest params the same -void HashTSDFVolumeCPU::reset() +void HashTSDFVolumeCPU::reset_() { CV_TRACE_FUNCTION(); volumeUnits.clear(); @@ -127,9 +135,8 @@ struct AllocateVolumeUnitsInvoker : ParallelLoopBody mutable Mutex mutex; }; - -void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, - const cv::Affine3f& cameraPose, const Intr& intrinsics) +void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, + const cv::Affine3f& cameraPose, const Intr& intrinsics) { CV_TRACE_FUNCTION(); @@ -199,30 +206,30 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, }); } -cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx(cv::Point3f p) const +cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx_(cv::Point3f p) const { return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), cvFloor(p.z / volumeUnitSize)); } -cv::Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const +cv::Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume_(cv::Vec3i volumeUnitIdx) const { return cv::Point3f(volumeUnitIdx[0] * volumeUnitSize, volumeUnitIdx[1] * volumeUnitSize, volumeUnitIdx[2] * volumeUnitSize); } -cv::Point3f HashTSDFVolumeCPU::voxelCoordToVolume(cv::Vec3i voxelIdx) const +cv::Point3f HashTSDFVolumeCPU::voxelCoordToVolume_(cv::Vec3i voxelIdx) const { return cv::Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); } -cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const +cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord_(cv::Point3f point) const { return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } -inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const +inline TsdfVoxel HashTSDFVolumeCPU::at_(const cv::Vec3i& volumeIdx) const { cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), @@ -248,7 +255,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const return volumeUnit->at(volUnitLocalIdx); } -inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const +inline TsdfVoxel HashTSDFVolumeCPU::at_(const cv::Point3f& point) const { cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); @@ -269,7 +276,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const return volumeUnit->at(volUnitLocalIdx); } -inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const +inline Point3f HashTSDFVolumeCPU::getNormalVoxel_(Point3f point) const { Vec3f pointVec(point); Vec3f normal = Vec3f(0, 0, 0); @@ -405,9 +412,9 @@ struct HashRaycastInvoker : ParallelLoopBody const Intr::Reprojector reproj; }; -void HashTSDFVolumeCPU::raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray _points, - cv::OutputArray _normals) const +void HashTSDFVolumeCPU::raycast_(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, + cv::Size frameSize, cv::OutputArray _points, + cv::OutputArray _normals) const { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); @@ -489,7 +496,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody mutable Mutex mutex; }; -void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const +void HashTSDFVolumeCPU::fetchPointsNormals_(OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); @@ -549,7 +556,7 @@ struct PushNormals Affine3f invPose; }; -void HashTSDFVolumeCPU::fetchNormals(cv::InputArray _points, cv::OutputArray _normals) const +void HashTSDFVolumeCPU::fetchNormals_(cv::InputArray _points, cv::OutputArray _normals) const { CV_TRACE_FUNCTION(); @@ -565,14 +572,5 @@ void HashTSDFVolumeCPU::fetchNormals(cv::InputArray _points, cv::OutputArray _no } } -cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, - float _raycastStepFactor, float _truncDist, - int _maxWeight, float _truncateThreshold, - int _volumeUnitResolution) -{ - return cv::makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, - _maxWeight, _truncateThreshold, _volumeUnitResolution); -} - } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index bba2cd5672a..9978b3c073a 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -15,17 +15,89 @@ namespace cv { namespace kinfu { +template class HashTSDFVolume : public Volume { public: - // dimension in voxels, size in meters - //! Use fixed volume cuboid - HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, - int _maxWeight, float _truncateThreshold, int _volumeUnitRes, - bool zFirstMemOrder = true); - virtual ~HashTSDFVolume() = default; + virtual void reset() override + { + Derived* derived = static_cast(this); + derived->reset_(); + } + + virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, + const cv::kinfu::Intr& intrinsics) override + { + Derived* derived = static_cast(this); + derived->integrate_(_depth, depthFactor, cameraPose, intrinsics); + } + virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, + cv::Size frameSize, cv::OutputArray points, + cv::OutputArray normals) const override + { + const Derived* derived = static_cast(this); + derived->raycast_(cameraPose, intrinsics, frameSize, points, normals); + } + virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override + { + const Derived* derived = static_cast(this); + derived->fetchNormals_(points, _normals); + } + virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override + { + const Derived* derived = static_cast(this); + derived->fetchPointsNormals_(points, normals); + } + size_t getTotalVolumeUnits() const + { + return static_cast(this)->getTotalVolumeUnits(); + } + + TsdfVoxel at(const cv::Vec3i& volumeIdx) const + { + const Derived* derived = static_cast(this); + return derived->at_(volumeIdx); + } + //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = + //! 1m) + virtual TsdfVoxel at(const cv::Point3f& point) const + { + const Derived* derived = static_cast(this); + return derived->at_(point); + } + + inline TsdfType interpolateVoxel(const cv::Point3f& point) const + { + const Derived* derived = static_cast(this); + return derived->interpolateVoxel_(point); + } + Point3f getNormalVoxel(cv::Point3f p) const + { + const Derived* derived = static_cast(this); + return derived->getNormalVoxel_(p); + } + + //! Utility functions for coordinate transformations + cv::Vec3i volumeToVolumeUnitIdx(cv::Point3f point) const + { + return static_cast(this)->volumeToVolumeUnitIdx_(point); + } + cv::Point3f volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const + { + return static_cast(this)->volumeUnitIdxToVolume_(volumeUnitIdx); + } + + cv::Point3f voxelCoordToVolume(cv::Vec3i voxelIdx) const + { + return static_cast(this)->voxelCoordToVolume_(voxelIdx); + } + cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const + { + return static_cast(this)->volumeToVoxelCoord_(point); + } + public: int maxWeight; float truncDist; @@ -33,6 +105,15 @@ class HashTSDFVolume : public Volume int volumeUnitResolution; float volumeUnitSize; bool zFirstMemOrder; + + protected: + //! dimension in voxels, size in meters + //! Use fixed volume cuboid + //! Can be only called by derived class + HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float _truncateThreshold, int _volumeUnitRes, + bool zFirstMemOrder = true); + friend Derived; }; struct VolumeUnit @@ -63,50 +144,59 @@ struct tsdf_hash typedef std::unordered_set VolumeUnitIndexSet; typedef std::unordered_map VolumeUnitMap; -class HashTSDFVolumeCPU : public HashTSDFVolume +class HashTSDFVolumeCPU : public HashTSDFVolume { + typedef HashTSDFVolume Base; + public: // dimension in voxels, size in meters HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, - const cv::kinfu::Intr& intrinsics) override; - virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray points, - cv::OutputArray normals) const override; + HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); + + void integrate_(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, + const cv::kinfu::Intr& intrinsics); + void raycast_(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, + cv::Size frameSize, cv::OutputArray points, cv::OutputArray normals) const; - virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; - virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; + void fetchNormals_(cv::InputArray points, cv::OutputArray _normals) const; + void fetchPointsNormals_(cv::OutputArray points, cv::OutputArray normals) const; - virtual void reset() override; + void reset_(); + size_t getTotalVolumeUnits_() const { return volumeUnits.size(); } //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) - virtual TsdfVoxel at(const cv::Vec3i& volumeIdx) const; + TsdfVoxel at_(const cv::Vec3i& volumeIdx) const; //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) - virtual TsdfVoxel at(const cv::Point3f& point) const; + TsdfVoxel at_(const cv::Point3f& point) const; - inline TsdfType interpolateVoxel(const cv::Point3f& point) const; - Point3f getNormalVoxel(cv::Point3f p) const; + inline TsdfType interpolateVoxel_(const cv::Point3f& point) const; + Point3f getNormalVoxel_(cv::Point3f p) const; //! Utility functions for coordinate transformations - cv::Vec3i volumeToVolumeUnitIdx(cv::Point3f point) const; - cv::Point3f volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const; + cv::Vec3i volumeToVolumeUnitIdx_(cv::Point3f point) const; + cv::Point3f volumeUnitIdxToVolume_(cv::Vec3i volumeUnitIdx) const; - cv::Point3f voxelCoordToVolume(cv::Vec3i voxelIdx) const; - cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const; + cv::Point3f voxelCoordToVolume_(cv::Vec3i voxelIdx) const; + cv::Vec3i volumeToVoxelCoord_(cv::Point3f point) const; public: //! Hashtable of individual smaller volume units VolumeUnitMap volumeUnits; }; -cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, - float _raycastStepFactor, float _truncDist, - int _maxWeight, float truncateThreshold, - int volumeUnitResolution = 16); +template +cv::Ptr> makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, + float _raycastStepFactor, float _truncDist, + int _maxWeight, float truncateThreshold, + int volumeUnitResolution = 16) +{ + return cv::makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + truncateThreshold, volumeUnitResolution); +} } // namespace kinfu } // namespace cv #endif diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index f484c2861d7..aa25284109d 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -119,7 +119,6 @@ class KinFuImpl : public KinFu void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE; - //! TODO(Akash): Add back later virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE; void getPoints(OutputArray points) const CV_OVERRIDE; void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 89f49963885..4829d67e598 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -106,6 +106,7 @@ class TSDFVolumeGPU : public TSDFVolume #endif cv::Ptr makeTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution); +cv::Ptr makeTSDFVolume(const VolumeParams& _params); } // namespace kinfu } // namespace cv #endif diff --git a/modules/rgbd/src/utils.hpp b/modules/rgbd/src/utils.hpp index b7febed57a7..0b963675390 100644 --- a/modules/rgbd/src/utils.hpp +++ b/modules/rgbd/src/utils.hpp @@ -9,6 +9,8 @@ #ifndef __OPENCV_RGBD_UTILS_HPP__ #define __OPENCV_RGBD_UTILS_HPP__ +#include "precomp.hpp" + namespace cv { namespace rgbd diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index a018ab723d2..066374252c9 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -5,13 +5,78 @@ #include "precomp.hpp" #include -#include "tsdf.hpp" #include "hash_tsdf.hpp" +#include "tsdf.hpp" namespace cv { namespace kinfu { +cv::Ptr VolumeParams::defaultParams(VolumeType _volumeType) +{ + VolumeParams params; + params.volumeType = _volumeType; + params.maxWeight = 64; + params.raycastStepFactor = 0.25f; + float volumeSize = 3.0f; + params.volumePose = Affine3f().translate(Vec3f(-volumeSize/2.f, -volumeSize/2.f, 0.5f)); + switch (params.volumeType) + { + case VolumeType::TSDF: + params.volumeSize = volumeSize; + params.volumeResolution = Vec3i::all(512); + params.voxelSize = volumeSize / 512.f; + params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF + break; + case VolumeType::HASHTSDF: + params.volumeSize = 3.0f; // VolumeSize not required for HASHTSDF + params.volumeUnitResolution = 16; + params.voxelSize = volumeSize / 512.f; + params.depthTruncThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); + default: + //! TODO: Should throw some exception or error + break; + } + params.truncDist = 7 * params.voxelSize; //! About 0.04f in meters + + return makePtr(params); +} + +cv::Ptr VolumeParams::coarseParams(VolumeType _volumeType) +{ + Ptr params = defaultParams(_volumeType); + + params->raycastStepFactor = 0.75f; + switch (params->volumeType) + { + case VolumeType::TSDF: + params->volumeResolution = Vec3i::all(128); + params->voxelSize = params->volumeSize/128.f; + break; + case VolumeType::HASHTSDF: + params->voxelSize = params->volumeSize/128.f; + break; + default: + break; + } + params->truncDist = 2 * params->voxelSize; //! About 0.04f in meters + return params; +} + +cv::Ptr makeVolume(const VolumeParams& _volumeParams) +{ + switch(_volumeParams.volumeType) + { + case VolumeType::TSDF: + return makeTSDFVolume(_volumeParams); + break; + case VolumeType::HASHTSDF: + return cv::makePtr(_volumeParams); + break; + default: + return nullptr; + } +} cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, Point3i _resolution) @@ -23,7 +88,12 @@ cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3 } else if (_volumeType == VolumeType::HASHTSDF) { - return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, +/* #ifdef HAVE_OPENCL */ +/* if (cv::ocl::useOpenCL()) */ +/* return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, */ +/* _truncateThreshold); */ +/* #endif */ + return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); } else From 8eaa125ca3c915470e133366bdda72f2754cd997 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Mon, 6 Jul 2020 19:14:27 -0400 Subject: [PATCH 13/36] Create submap and submapMgr Implement overlap_ratio check to create new submaps --- modules/rgbd/include/opencv2/rgbd.hpp | 1 + modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 2 +- .../rgbd/include/opencv2/rgbd/large_kinfu.hpp | 137 +++++ modules/rgbd/include/opencv2/rgbd/volume.hpp | 68 ++- modules/rgbd/samples/large_kinfu_demo.cpp | 529 ++++++++++++++++++ modules/rgbd/src/hash_tsdf.cpp | 148 ++--- modules/rgbd/src/hash_tsdf.hpp | 14 +- modules/rgbd/src/large_kinfu.cpp | 333 +++++++++++ modules/rgbd/src/submap.cpp | 69 +++ modules/rgbd/src/submap.hpp | 83 +++ modules/rgbd/src/tsdf.cpp | 16 +- modules/rgbd/src/tsdf.hpp | 4 +- modules/rgbd/src/volume.cpp | 35 +- 13 files changed, 1326 insertions(+), 113 deletions(-) create mode 100644 modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp create mode 100644 modules/rgbd/samples/large_kinfu_demo.cpp create mode 100644 modules/rgbd/src/large_kinfu.cpp create mode 100644 modules/rgbd/src/submap.cpp create mode 100644 modules/rgbd/src/submap.hpp diff --git a/modules/rgbd/include/opencv2/rgbd.hpp b/modules/rgbd/include/opencv2/rgbd.hpp index 37b2927cdcf..d4ac749c2a5 100755 --- a/modules/rgbd/include/opencv2/rgbd.hpp +++ b/modules/rgbd/include/opencv2/rgbd.hpp @@ -13,6 +13,7 @@ #include "opencv2/rgbd/depth.hpp" #include "opencv2/rgbd/kinfu.hpp" #include "opencv2/rgbd/dynafu.hpp" +#include "opencv2/rgbd/large_kinfu.hpp" /** @defgroup rgbd RGB-Depth Processing diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 8fcc3189ac3..49f7fc6a885 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -77,7 +77,7 @@ struct CV_EXPORTS_W Params /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; - CV_PROP_RW cv::kinfu::VolumeType volumeType; + CV_PROP_RW kinfu::VolumeType volumeType; /** @brief camera intrinsics */ CV_PROP_RW Matx33f intr; diff --git a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp new file mode 100644 index 00000000000..c9842e1b82e --- /dev/null +++ b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp @@ -0,0 +1,137 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory + +#ifndef __OPENCV_RGBD_LARGEKINFU_HPP__ +#define __OPENCV_RGBD_LARGEKINFU_HPP__ + +#include + +#include "opencv2/core.hpp" +#include "opencv2/core/affine.hpp" + +namespace cv +{ +namespace large_kinfu +{ +struct CV_EXPORTS_W Params +{ + /** @brief Default parameters + A set of parameters which provides better model quality, can be very slow. + */ + CV_WRAP static Ptr defaultParams(); + + /** @brief Coarse parameters + A set of parameters which provides better speed, can fail to match frames + in case of rapid sensor motion. + */ + CV_WRAP static Ptr coarseParams(); + + /** @brief HashTSDF parameters + A set of parameters suitable for use with HashTSDFVolume + */ + CV_WRAP static Ptr hashTSDFParams(bool isCoarse); + + /** @brief frame size in pixels */ + CV_PROP_RW Size frameSize; + + /** @brief camera intrinsics */ + CV_PROP_RW Matx33f intr; + + /** @brief pre-scale per 1 meter for input values + Typical values are: + * 5000 per 1 meter for the 16-bit PNG files of TUM database + * 1000 per 1 meter for Kinect 2 device + * 1 per 1 meter for the 32-bit float images in the ROS bag files + */ + CV_PROP_RW float depthFactor; + + /** @brief Depth sigma in meters for bilateral smooth */ + CV_PROP_RW float bilateral_sigma_depth; + /** @brief Spatial sigma in pixels for bilateral smooth */ + CV_PROP_RW float bilateral_sigma_spatial; + /** @brief Kernel size in pixels for bilateral smooth */ + CV_PROP_RW int bilateral_kernel_size; + + /** @brief Number of pyramid levels for ICP */ + CV_PROP_RW int pyramidLevels; + + /** @brief Minimal camera movement in meters + Integrate new depth frame only if camera movement exceeds this value. + */ + CV_PROP_RW float tsdf_min_camera_movement; + + /** @brief light pose for rendering in meters */ + CV_PROP_RW Vec3f lightPose; + + /** @brief distance theshold for ICP in meters */ + CV_PROP_RW float icpDistThresh; + /** @brief angle threshold for ICP in radians */ + CV_PROP_RW float icpAngleThresh; + /** @brief number of ICP iterations for each pyramid level */ + CV_PROP_RW std::vector icpIterations; + + /** @brief Threshold for depth truncation in meters + All depth values beyond this threshold will be set to zero + */ + CV_PROP_RW float truncateThreshold; + + /** @brief Volume parameters + */ + kinfu::VolumeParams volumeParams; +}; + +/** @brief KinectFusion implementation + + This class implements a 3d reconstruction algorithm described in + @cite kinectfusion paper. + + It takes a sequence of depth images taken from depth sensor + (or any depth images source such as stereo camera matching algorithm or even raymarching + renderer). The output can be obtained as a vector of points and their normals or can be + Phong-rendered from given camera pose. + + An internal representation of a model is a voxel cuboid that keeps TSDF values + which are a sort of distances to the surface (for details read the @cite kinectfusion article + about TSDF). There is no interface to that representation yet. + + LargeKinfu uses OpenCL acceleration automatically if available. + To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL(). + + This implementation is based on [kinfu-remake](https://github.com/Nerei/kinfu_remake). + + Note that the KinectFusion algorithm was patented and its use may be restricted by + the list of patents mentioned in README.md file in this module directory. + + That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion. +*/ +class CV_EXPORTS_W LargeKinfu +{ + public: + CV_WRAP static Ptr create(const Ptr& _params); + virtual ~LargeKinfu() = default; + + virtual const Params& getParams() const = 0; + + CV_WRAP virtual void render(OutputArray image, + const Matx44f& cameraPose = Matx44f::eye()) const = 0; + + CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0; + + CV_WRAP virtual void getPoints(OutputArray points) const = 0; + + CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0; + + CV_WRAP virtual void reset() = 0; + + virtual const Affine3f getPose() const = 0; + + CV_WRAP virtual bool update(InputArray depth) = 0; +}; + +} // namespace large_kinfu +} // namespace cv +#endif diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 89465ec1a5a..0680eee0af6 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -19,20 +19,16 @@ class Volume { public: Volume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor) - : voxelSize(_voxelSize), - voxelSizeInv(1.0f / voxelSize), - pose(_pose), - raycastStepFactor(_raycastStepFactor) + : voxelSize(_voxelSize), voxelSizeInv(1.0f / voxelSize), pose(_pose), raycastStepFactor(_raycastStepFactor) { } virtual ~Volume(){}; virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, - const cv::kinfu::Intr& intrinsics) = 0; - virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray points, - cv::OutputArray normals) const = 0; + const cv::kinfu::Intr& intrinsics, const int frameId = 0) = 0; + virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, + cv::OutputArray points, cv::OutputArray normals) const = 0; virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; virtual void reset() = 0; @@ -50,31 +46,65 @@ enum class VolumeType HASHTSDF = 1 }; -//! TODO: Tagged union for volumeParams struct VolumeParams { - VolumeType volumeType; - cv::Affine3f volumePose; + /** @brief Type of Volume + Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units) + */ + VolumeType type; + + /** @brief Resolution of voxel space + Number of voxels in each dimension. + */ + Vec3i resolution; + + /** @brief Resolution of volumeUnit in voxel space + Number of voxels in each dimension for volumeUnit + Applicable only for hashTSDF. + */ + int unitResolution; + + /** @brief Initial pose of the volume in meters */ + cv::Affine3f pose; + + /** @brief Length of voxels in meters */ float voxelSize; - float truncDist; + + /** @brief TSDF truncation distance + Distances greater than value from surface will be truncated to 1.0 + */ + float tsdfTruncDist; + + /** @brief Max number of frames to integrate per voxel + Each voxel stops integration after the maxWeight is crossed + */ int maxWeight; + + /** @brief Threshold for depth truncation in meters + Truncates the depth greater than threshold to 0 + */ float depthTruncThreshold; - float raycastStepFactor; - float volumeSize; - Point3i volumeResolution; - int volumeUnitResolution; + /** @brief Length of single raycast step + Describes the percentage of voxel length that is skipped per march + */ + float raycastStepFactor; + /** @brief Default set of parameters that provide higher quality reconstruction + at the cost of slow performance. + */ static Ptr defaultParams(VolumeType _volumeType); + /** @brief Coarse set of parameters that provides relatively higher performance + at the cost of reconstrution quality. + */ static Ptr coarseParams(VolumeType _volumeType); }; cv::Ptr makeVolume(const VolumeParams& _volumeParams); -cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, - float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, Point3i _resolution); +cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, float _truncateThreshold, Point3i _resolution); } // namespace kinfu } // namespace cv #endif diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp new file mode 100644 index 00000000000..8378a120283 --- /dev/null +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -0,0 +1,529 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory + +#include +#include +#include +#include +#include +#include + +using namespace cv; +using namespace cv::kinfu; +using namespace cv::large_kinfu; +using namespace std; + +#ifdef HAVE_OPENCV_VIZ +#include +#endif + +static vector readDepth(std::string fileList); + +static vector readDepth(std::string fileList) +{ + vector v; + + fstream file(fileList); + if(!file.is_open()) + throw std::runtime_error("Failed to read depth list"); + + std::string dir; + size_t slashIdx = fileList.rfind('/'); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); + + while(!file.eof()) + { + std::string s, imgPath; + std::getline(file, s); + if(s.empty() || s[0] == '#') continue; + std::stringstream ss; + ss << s; + double thumb; + ss >> thumb >> imgPath; + v.push_back(dir+'/'+imgPath); + } + + return v; +} + +struct DepthWriter +{ + DepthWriter(string fileList) : + file(fileList, ios::out), count(0), dir() + { + size_t slashIdx = fileList.rfind('/'); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); + + if(!file.is_open()) + throw std::runtime_error("Failed to write depth list"); + + file << "# depth maps saved from device" << endl; + file << "# useless_number filename" << endl; + } + + void append(InputArray _depth) + { + Mat depth = _depth.getMat(); + string depthFname = cv::format("%04d.png", count); + string fullDepthFname = dir + '/' + depthFname; + if(!imwrite(fullDepthFname, depth)) + throw std::runtime_error("Failed to write depth to file " + fullDepthFname); + file << count++ << " " << depthFname << endl; + } + + fstream file; + int count; + string dir; +}; + +namespace Kinect2Params +{ + static const Size frameSize = Size(512, 424); + // approximate values, no guarantee to be correct + static const float focal = 366.1f; + static const float cx = 258.2f; + static const float cy = 204.f; + static const float k1 = 0.12f; + static const float k2 = -0.34f; + static const float k3 = 0.12f; +}; + +struct DepthSource +{ +public: + enum Type + { + DEPTH_LIST, + DEPTH_KINECT2_LIST, + DEPTH_KINECT2, + DEPTH_REALSENSE + }; + + DepthSource(int cam) : + DepthSource("", cam) + { } + + DepthSource(String fileListName) : + DepthSource(fileListName, -1) + { } + + DepthSource(String fileListName, int cam) : + depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), + frameIdx(0), + undistortMap1(), + undistortMap2() + { + if(cam >= 0) + { + vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); + if(vc.isOpened()) + { + sourceType = Type::DEPTH_KINECT2; + } + else + { + vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); + if(vc.isOpened()) + { + sourceType = Type::DEPTH_REALSENSE; + } + } + } + else + { + vc = VideoCapture(); + sourceType = Type::DEPTH_KINECT2_LIST; + } + } + + UMat getDepth() + { + UMat out; + if (!vc.isOpened()) + { + if (frameIdx < depthFileList.size()) + { + Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH); + f.copyTo(out); + } + else + { + return UMat(); + } + } + else + { + vc.grab(); + switch (sourceType) + { + case Type::DEPTH_KINECT2: + vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); + break; + case Type::DEPTH_REALSENSE: + vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); + break; + default: + // unknown depth source + vc.retrieve(out); + } + + // workaround for Kinect 2 + if(sourceType == Type::DEPTH_KINECT2) + { + out = out(Rect(Point(), Kinect2Params::frameSize)); + + UMat outCopy; + // linear remap adds gradient between valid and invalid pixels + // which causes garbage, use nearest instead + remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST); + + cv::flip(outCopy, out, 1); + } + } + if (out.empty()) + throw std::runtime_error("Matrix is empty"); + return out; + } + + bool empty() + { + return depthFileList.empty() && !(vc.isOpened()); + } + + void updateParams(large_kinfu::Params& params) + { + if (vc.isOpened()) + { + // this should be set in according to user's depth sensor + int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH); + int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT); + + // it's recommended to calibrate sensor to obtain its intrinsics + float fx, fy, cx, cy; + float depthFactor = 1000.f; + Size frameSize; + if(sourceType == Type::DEPTH_KINECT2) + { + fx = fy = Kinect2Params::focal; + cx = Kinect2Params::cx; + cy = Kinect2Params::cy; + + frameSize = Kinect2Params::frameSize; + } + else + { + if(sourceType == Type::DEPTH_REALSENSE) + { + fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); + fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); + depthFactor = 1.f/(float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); + } + else + { + fx = fy = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); + } + + cx = w/2 - 0.5f; + cy = h/2 - 0.5f; + + frameSize = Size(w, h); + } + + Matx33f camMatrix = Matx33f(fx, 0, cx, + 0, fy, cy, + 0, 0, 1); + + params.frameSize = frameSize; + params.intr = camMatrix; + params.depthFactor = depthFactor; + + // RealSense has shorter depth range, some params should be tuned + if(sourceType == Type::DEPTH_REALSENSE) + { + // all sizes in meters + float cubeSize = 1.f; + params.volumeParams.voxelSize = cubeSize/params.volumeParams.resolution[0]; + params.volumeParams.tsdfTruncDist = 0.01f; + params.icpDistThresh = 0.01f; + params.volumeParams.pose = Affine3f().translate(Vec3f(-cubeSize/2.f, + -cubeSize/2.f, + 0.05f)); + params.truncateThreshold = 2.5f; + params.bilateral_sigma_depth = 0.01f; + } + + if(sourceType == Type::DEPTH_KINECT2) + { + Matx distCoeffs; + distCoeffs(0) = Kinect2Params::k1; + distCoeffs(1) = Kinect2Params::k2; + distCoeffs(4) = Kinect2Params::k3; + + initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), + camMatrix, frameSize, CV_16SC2, + undistortMap1, undistortMap2); + } + } + } + + vector depthFileList; + size_t frameIdx; + VideoCapture vc; + UMat undistortMap1, undistortMap2; + Type sourceType; +}; + +#ifdef HAVE_OPENCV_VIZ +const std::string vizWindowName = "cloud"; + +struct PauseCallbackArgs +{ + PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) + { } + + LargeKinfu& largeKinfu; +}; + +void pauseCallback(const viz::MouseEvent& me, void* args); +void pauseCallback(const viz::MouseEvent& me, void* args) +{ + if(me.type == viz::MouseEvent::Type::MouseMove || + me.type == viz::MouseEvent::Type::MouseScrollDown || + me.type == viz::MouseEvent::Type::MouseScrollUp) + { + PauseCallbackArgs pca = *((PauseCallbackArgs*)(args)); + viz::Viz3d window(vizWindowName); + UMat rendered; + pca.largeKinfu.render(rendered, window.getViewerPose().matrix); + imshow("render", rendered); + waitKey(1); + } +} +#endif + +static const char* keys = +{ + "{help h usage ? | | print this message }" + "{depth | | Path to depth.txt file listing a set of depth images }" + "{camera |0| Index of depth camera to be used as a depth source }" + "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," + " in coarse mode points and normals are displayed }" + "{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}" + "{idle | | Do not run KinFu, just display depth frames }" + "{record | | Write depth frames to specified file list" + " (the same format as for the 'depth' key) }" +}; + +static const std::string message = + "\nThis demo uses live depth input or RGB-D dataset taken from" + "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" + "\nto demonstrate KinectFusion implementation \n"; + + +int main(int argc, char **argv) +{ + bool coarse = false; + bool idle = false; + bool useHashTSDF = false; + string recordPath; + + CommandLineParser parser(argc, argv, keys); + parser.about(message); + + if(!parser.check()) + { + parser.printMessage(); + parser.printErrors(); + return -1; + } + + if(parser.has("help")) + { + parser.printMessage(); + return 0; + } + if(parser.has("coarse")) + { + coarse = true; + } + if(parser.has("record")) + { + recordPath = parser.get("record"); + } + if(parser.has("useHashTSDF")) + { + useHashTSDF = true; + } + if(parser.has("idle")) + { + idle = true; + } + + Ptr ds; + if (parser.has("depth")) + ds = makePtr(parser.get("depth")); + else + ds = makePtr(parser.get("camera")); + + if (ds->empty()) + { + std::cerr << "Failed to open depth source" << std::endl; + parser.printMessage(); + return -1; + } + + Ptr depthWriter; + if(!recordPath.empty()) + depthWriter = makePtr(recordPath); + + Ptr params; + Ptr largeKinfu; + + if(coarse) + params = Params::coarseParams(); + else + params = Params::defaultParams(); + + if(useHashTSDF) + params = Params::hashTSDFParams(coarse); + + // These params can be different for each depth sensor + ds->updateParams(*params); + + // Enables OpenCL explicitly (by default can be switched-off) + cv::setUseOptimized(true); + + if(!idle) + largeKinfu = LargeKinfu::create(params); + +#ifdef HAVE_OPENCV_VIZ + cv::viz::Viz3d window(vizWindowName); + window.setViewerPose(Affine3f::Identity()); + bool pause = false; +#endif + + UMat rendered; + UMat points; + UMat normals; + + int64 prevTime = getTickCount(); + + for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) + { + if(depthWriter) + depthWriter->append(frame); + +#ifdef HAVE_OPENCV_VIZ + if(pause) + { + // doesn't happen in idle mode + largeKinfu->getCloud(points, normals); + if(!points.empty() && !normals.empty()) + { + viz::WCloud cloudWidget(points, viz::Color::white()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); + window.showWidget("cloud", cloudWidget); + window.showWidget("normals", cloudNormals); + + Vec3d volSize = largeKinfu->getParams().volumeParams.voxelSize * + Vec3d(largeKinfu->getParams().volumeParams.resolution); + window.showWidget("cube", viz::WCube(Vec3d::all(0), + volSize), + largeKinfu->getParams().volumeParams.pose); + PauseCallbackArgs pca(*largeKinfu); + window.registerMouseCallback(pauseCallback, (void*)&pca); + window.showWidget("text", viz::WText(cv::String("Move camera in this window. " + "Close the window or press Q to resume"), Point())); + window.spin(); + window.removeWidget("text"); + window.removeWidget("cloud"); + window.removeWidget("normals"); + window.registerMouseCallback(0); + } + + pause = false; + } + else +#endif + { + UMat cvt8; + float depthFactor = params->depthFactor; + convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor); + if(!idle) + { + imshow("depth", cvt8); + + if(!largeKinfu->update(frame)) + { + largeKinfu->reset(); + std::cout << "reset" << std::endl; + } +#ifdef HAVE_OPENCV_VIZ + else + { + if(coarse) + { + largeKinfu->getCloud(points, normals); + if(!points.empty() && !normals.empty()) + { + viz::WCloud cloudWidget(points, viz::Color::white()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); + window.showWidget("cloud", cloudWidget); + window.showWidget("normals", cloudNormals); + } + } + + //window.showWidget("worldAxes", viz::WCoordinateSystem()); + Vec3d volSize = largeKinfu->getParams().volumeParams.voxelSize * + largeKinfu->getParams().volumeParams.resolution; + window.showWidget("cube", viz::WCube(Vec3d::all(0), + volSize), + largeKinfu->getParams().volumeParams.pose); + window.setViewerPose(largeKinfu->getPose()); + window.spinOnce(1, true); + } +#endif + + largeKinfu->render(rendered); + } + else + { + rendered = cvt8; + } + } + + int64 newTime = getTickCount(); + putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit", + (int)(getTickFrequency()/(newTime - prevTime))), + Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); + prevTime = newTime; + + imshow("render", rendered); + + int c = waitKey(1); + switch (c) + { + case 'r': + if(!idle) + largeKinfu->reset(); + break; + case 'q': + return 0; +#ifdef HAVE_OPENCV_VIZ + case 'p': + if(!idle) + pause = true; +#endif + default: + break; + } + } + + return 0; +} + diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index bbc01012e76..4b768713902 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -21,10 +21,8 @@ namespace cv namespace kinfu { template -HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, - float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, int _volumeUnitRes, - bool _zFirstMemOrder) +HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) : Volume(_voxelSize, _pose, _raycastStepFactor), maxWeight(_maxWeight), truncateThreshold(_truncateThreshold), @@ -35,18 +33,16 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, truncDist = std::max(_truncDist, 4.0f * voxelSize); } -HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, float _truncateThreshold, - int _volumeUnitRes, bool _zFirstMemOrder) - : Base(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, - _volumeUnitRes, _zFirstMemOrder) +HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) + : Base(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, + _zFirstMemOrder) { } HashTSDFVolumeCPU::HashTSDFVolumeCPU(const VolumeParams& _params, bool _zFirstMemOrder) - : Base(_params.voxelSize, _params.volumePose, _params.raycastStepFactor, _params.truncDist, - _params.maxWeight, _params.depthTruncThreshold, _params.volumeUnitResolution, - _zFirstMemOrder) + : Base(_params.voxelSize, _params.pose, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) { } // zero volume, leave rest params the same @@ -58,10 +54,11 @@ void HashTSDFVolumeCPU::reset_() struct AllocateVolumeUnitsInvoker : ParallelLoopBody { - AllocateVolumeUnitsInvoker(HashTSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, + AllocateVolumeUnitsInvoker(HashTSDFVolumeCPU& _volume, const int _frameId, const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose, float _depthFactor, int _depthStride = 4) : ParallelLoopBody(), volume(_volume), + frameId(_frameId), depth(_depth), reproj(intrinsics.makeReprojector()), cam2vol(_volume.pose.inv() * cameraPose), @@ -107,18 +104,17 @@ struct AllocateVolumeUnitsInvoker : ParallelLoopBody if (!volume.volumeUnits.count(tsdf_idx)) { VolumeUnit volumeUnit; - cv::Point3i volumeDims(volume.volumeUnitResolution, - volume.volumeUnitResolution, + cv::Point3i volumeDims(volume.volumeUnitResolution, volume.volumeUnitResolution, volume.volumeUnitResolution); - cv::Affine3f subvolumePose = - volume.pose.translate(volume.volumeUnitIdxToVolume(tsdf_idx)); - volumeUnit.pVolume = cv::makePtr( - volume.voxelSize, subvolumePose, volume.raycastStepFactor, - volume.truncDist, volume.maxWeight, volumeDims); + cv::Affine3f subvolumePose = volume.pose.translate(volume.volumeUnitIdxToVolume(tsdf_idx)); + volumeUnit.pVolume = + cv::makePtr(volume.voxelSize, subvolumePose, volume.raycastStepFactor, + volume.truncDist, volume.maxWeight, volumeDims); //! This volume unit will definitely be required for current integration volumeUnit.index = tsdf_idx; volumeUnit.isActive = true; + volumeUnit.lastVisibleIndex = frameId; volume.volumeUnits[tsdf_idx] = volumeUnit; } } @@ -127,6 +123,7 @@ struct AllocateVolumeUnitsInvoker : ParallelLoopBody } HashTSDFVolumeCPU& volume; + const int frameId; const Depth& depth; const Intr::Reprojector reproj; const cv::Affine3f cam2vol; @@ -135,8 +132,8 @@ struct AllocateVolumeUnitsInvoker : ParallelLoopBody mutable Mutex mutex; }; -void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, - const cv::Affine3f& cameraPose, const Intr& intrinsics) +void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, + const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); @@ -144,7 +141,7 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, Depth depth = _depth.getMat(); //! Compute volumes to be allocated - AllocateVolumeUnitsInvoker allocate_i(*this, depth, intrinsics, cameraPose, depthFactor); + AllocateVolumeUnitsInvoker allocate_i(*this, frameId, depth, intrinsics, cameraPose, depthFactor); Range allocateRange(0, depth.rows); parallel_for_(allocateRange, allocate_i); @@ -176,8 +173,7 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, return; } Point2f cameraPoint = proj(volUnitInCamSpace); - if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && - cameraPoint.y < depth.rows) + if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) { assert(it != volumeUnits.end()); it->second.isActive = true; @@ -208,8 +204,7 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx_(cv::Point3f p) const { - return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), - cvFloor(p.z / volumeUnitSize)); + return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), cvFloor(p.z / volumeUnitSize)); } cv::Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume_(cv::Vec3i volumeUnitIdx) const @@ -225,15 +220,14 @@ cv::Point3f HashTSDFVolumeCPU::voxelCoordToVolume_(cv::Vec3i voxelIdx) const cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord_(cv::Point3f point) const { - return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), - cvFloor(point.z * voxelSizeInv)); + return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } inline TsdfVoxel HashTSDFVolumeCPU::at_(const cv::Vec3i& volumeIdx) const { - cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), - cvFloor(volumeIdx[1] / volumeUnitResolution), - cvFloor(volumeIdx[2] / volumeUnitResolution)); + cv::Vec3i volumeUnitIdx = + cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), + cvFloor(volumeIdx[2] / volumeUnitResolution)); VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); if (it == volumeUnits.end()) @@ -243,15 +237,13 @@ inline TsdfVoxel HashTSDFVolumeCPU::at_(const cv::Vec3i& volumeIdx) const dummy.weight = 0; return dummy; } - cv::Ptr volumeUnit = - std::dynamic_pointer_cast(it->second.pVolume); + cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, - volumeUnitIdx[1] * volumeUnitResolution, - volumeUnitIdx[2] * volumeUnitResolution); + cv::Vec3i volUnitLocalIdx = + volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, + volumeUnitIdx[2] * volumeUnitResolution); - volUnitLocalIdx = - cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); return volumeUnit->at(volUnitLocalIdx); } @@ -266,13 +258,11 @@ inline TsdfVoxel HashTSDFVolumeCPU::at_(const cv::Point3f& point) const dummy.weight = 0; return dummy; } - cv::Ptr volumeUnit = - std::dynamic_pointer_cast(it->second.pVolume); + cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); - volUnitLocalIdx = - cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + volUnitLocalIdx = cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); return volumeUnit->at(volUnitLocalIdx); } @@ -300,8 +290,8 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel_(Point3f point) const struct HashRaycastInvoker : ParallelLoopBody { - HashRaycastInvoker(Points& _points, Normals& _normals, const Affine3f& cameraPose, - const Intr& intrinsics, const HashTSDFVolumeCPU& _volume) + HashRaycastInvoker(Points& _points, Normals& _normals, const Affine3f& cameraPose, const Intr& intrinsics, + const HashTSDFVolumeCPU& _volume) : ParallelLoopBody(), points(_points), normals(_normals), @@ -332,17 +322,15 @@ struct HashRaycastInvoker : ParallelLoopBody Point3f point = nan3, normal = nan3; //! Ray origin and direction in the volume coordinate frame - Point3f orig = cam2volTrans; - Point3f rayDirV = - normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f)))); + 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()); + cv::Vec3i prevVolumeUnitIdx = cv::Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), + std::numeric_limits::min()); float tprev = tcurr; TsdfType prevTsdf = volume.truncDist; @@ -362,11 +350,9 @@ struct HashRaycastInvoker : ParallelLoopBody //! Does the subvolume exist in hashtable if (it != volume.volumeUnits.end()) { - currVolumeUnit = - std::dynamic_pointer_cast(it->second.pVolume); - cv::Point3f currVolUnitPos = - volume.volumeUnitIdxToVolume(currVolumeUnitIdx); - volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); + currVolumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + cv::Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); + volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation TsdfVoxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); @@ -377,8 +363,7 @@ struct HashRaycastInvoker : ParallelLoopBody //! Surface crossing if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) { - float tInterp = - (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); + float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) { Point3f pv = orig + tInterp * rayDirV; @@ -412,9 +397,8 @@ struct HashRaycastInvoker : ParallelLoopBody const Intr::Reprojector reproj; }; -void HashTSDFVolumeCPU::raycast_(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray _points, - cv::OutputArray _normals) const +void HashTSDFVolumeCPU::raycast_(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, + cv::OutputArray _points, cv::OutputArray _normals) const { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); @@ -433,10 +417,9 @@ void HashTSDFVolumeCPU::raycast_(const cv::Affine3f& cameraPose, const cv::kinfu struct FetchPointsNormalsInvoker : ParallelLoopBody { - FetchPointsNormalsInvoker(const HashTSDFVolumeCPU& _volume, - const std::vector& _totalVolUnits, - std::vector>& _pVecs, - std::vector>& _nVecs, bool _needNormals) + FetchPointsNormalsInvoker(const HashTSDFVolumeCPU& _volume, const std::vector& _totalVolUnits, + std::vector>& _pVecs, std::vector>& _nVecs, + bool _needNormals) : ParallelLoopBody(), volume(_volume), totalVolUnits(_totalVolUnits), @@ -457,8 +440,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); if (it != volume.volumeUnits.end()) { - cv::Ptr volumeUnit = - std::dynamic_pointer_cast(it->second.pVolume); + cv::Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); std::vector localPoints; std::vector localNormals; for (int x = 0; x < volume.volumeUnitResolution; x++) @@ -572,5 +554,37 @@ void HashTSDFVolumeCPU::fetchNormals_(cv::InputArray _points, cv::OutputArray _n } } +int HashTSDFVolumeCPU::getVisibleBlocks_(int currFrameId, int frameThreshold) const +{ + int numVisibleBlocks = 0; + std::vector totalVolUnits; + for (const auto& keyvalue : volumeUnits) + { + totalVolUnits.push_back(keyvalue.first); + } + + Range checkVisibleRange(0, volumeUnits.size()); + //! TODO: Sum up via reduction + /* parallel_for_(checkVisibleRange, [&](const Range& range) { */ + + for (int i = checkVisibleRange.start; i < checkVisibleRange.end; ++i) + { + cv::Vec3i tsdf_idx = totalVolUnits[i]; + VolumeUnitMap::const_iterator it = volumeUnits.find(tsdf_idx); + if (it == volumeUnits.end()) + continue; + + if(it->second.lastVisibleIndex > (currFrameId - frameThreshold)) + { + //! Add count (parallel?) + numVisibleBlocks++; + } + } + + return numVisibleBlocks; + /* }); */ + +} + } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 9978b3c073a..013f1f45b1e 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -28,10 +28,10 @@ class HashTSDFVolume : public Volume } virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, - const cv::kinfu::Intr& intrinsics) override + const cv::kinfu::Intr& intrinsics, const int frameId = 0) override { Derived* derived = static_cast(this); - derived->integrate_(_depth, depthFactor, cameraPose, intrinsics); + derived->integrate_(_depth, depthFactor, cameraPose, intrinsics, frameId); } virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, cv::OutputArray points, @@ -52,7 +52,11 @@ class HashTSDFVolume : public Volume } size_t getTotalVolumeUnits() const { - return static_cast(this)->getTotalVolumeUnits(); + return static_cast(this)->getTotalVolumeUnits_(); + } + int getVisibleBlocks(int currFrameId, int frameThreshold) const + { + return static_cast(this)->getVisibleBlocks_(currFrameId, frameThreshold); } TsdfVoxel at(const cv::Vec3i& volumeIdx) const @@ -123,6 +127,7 @@ struct VolumeUnit cv::Ptr pVolume; cv::Vec3i index; + int lastVisibleIndex = 0; bool isActive; }; @@ -157,7 +162,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); void integrate_(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, - const cv::kinfu::Intr& intrinsics); + const cv::kinfu::Intr& intrinsics, const int frameId = 0); void raycast_(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, cv::OutputArray points, cv::OutputArray normals) const; @@ -166,6 +171,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume void reset_(); size_t getTotalVolumeUnits_() const { return volumeUnits.size(); } + int getVisibleBlocks_(int currFrameId, int frameThreshold) const; //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) TsdfVoxel at_(const cv::Vec3i& volumeIdx) const; diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp new file mode 100644 index 00000000000..2fc46bd3b89 --- /dev/null +++ b/modules/rgbd/src/large_kinfu.cpp @@ -0,0 +1,333 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory + +#include "fast_icp.hpp" +#include "hash_tsdf.hpp" +#include "kinfu_frame.hpp" +#include "precomp.hpp" +#include "tsdf.hpp" +#include "submap.hpp" + +namespace cv +{ +namespace large_kinfu +{ +using namespace kinfu; + +Ptr Params::defaultParams() +{ + Params p; + + //! Frame parameters + { + p.frameSize = Size(640, 480); + + float fx, fy, cx, cy; + fx = fy = 525.f; + cx = p.frameSize.width / 2 - 0.5f; + cy = p.frameSize.height / 2 - 0.5f; + p.intr = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); + + // 5000 for the 16-bit PNG files + // 1 for the 32-bit float images in the ROS bag files + p.depthFactor = 5000; + + // sigma_depth is scaled by depthFactor when calling bilateral filter + p.bilateral_sigma_depth = 0.04f; // meter + p.bilateral_sigma_spatial = 4.5; // pixels + p.bilateral_kernel_size = 7; // pixels + p.truncateThreshold = 0.f; // meters + } + //! ICP parameters + { + p.icpAngleThresh = (float)(30. * CV_PI / 180.); // radians + p.icpDistThresh = 0.1f; // meters + + p.icpIterations = { 10, 5, 4 }; + p.pyramidLevels = (int)p.icpIterations.size(); + } + //! Volume parameters + { + float volumeSize = 3.0f; + p.volumeParams.type = VolumeType::TSDF; + p.volumeParams.resolution = Vec3i::all(512); + p.volumeParams.pose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)); + p.volumeParams.voxelSize = volumeSize / 512.f; // meters + p.volumeParams.tsdfTruncDist = 7 * p.volumeParams.voxelSize; // about 0.04f in meters + p.volumeParams.maxWeight = 64; // frames + p.volumeParams.raycastStepFactor = 0.25f; // in voxel sizes + p.volumeParams.depthTruncThreshold = p.truncateThreshold; + } + //! Unused parameters + p.tsdf_min_camera_movement = 0.f; // meters, disabled + p.lightPose = Vec3f::all(0.f); // meters + + return makePtr(p); +} + +Ptr Params::coarseParams() +{ + Ptr p = defaultParams(); + + //! Reduce ICP iterations and pyramid levels + { + p->icpIterations = { 5, 3, 2 }; + p->pyramidLevels = (int)p->icpIterations.size(); + } + //! Make the volume coarse + { + float volumeSize = 3.f; + p->volumeParams.resolution = Vec3i::all(128); // number of voxels + p->volumeParams.voxelSize = volumeSize / 128.f; + p->volumeParams.tsdfTruncDist = 2 * p->volumeParams.voxelSize; // 0.04f in meters + p->volumeParams.raycastStepFactor = 0.75f; // in voxel sizes + } + return p; +} +Ptr Params::hashTSDFParams(bool isCoarse) +{ + Ptr p; + if (isCoarse) + p = coarseParams(); + else + p = defaultParams(); + + p->volumeParams.type = VolumeType::HASHTSDF; + p->volumeParams.depthTruncThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); + p->volumeParams.unitResolution = 16; + return p; +} + +// MatType should be Mat or UMat +template +class LargeKinfuImpl : public LargeKinfu +{ + public: + LargeKinfuImpl(const Params& _params); + virtual ~LargeKinfuImpl(); + + const Params& getParams() const CV_OVERRIDE; + + void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE; + + virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE; + void getPoints(OutputArray points) const CV_OVERRIDE; + void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE; + + void reset() CV_OVERRIDE; + + const Affine3f getPose() const CV_OVERRIDE; + + bool update(InputArray depth) CV_OVERRIDE; + + bool updateT(const MatType& depth); + + private: + Params params; + + cv::Ptr icp; + //! TODO: Submap manager and Pose graph optimizer + cv::Ptr submapMgr; + cv::Ptr currSubmap; + + int frameCounter; + Affine3f pose; + std::vector pyrPoints; + std::vector pyrNormals; +}; + +template +LargeKinfuImpl::LargeKinfuImpl(const Params& _params) + : params(_params), + currSubmap(nullptr), + pyrPoints(), + pyrNormals() +{ + icp = makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh); + + submapMgr = cv::makePtr(params.volumeParams); + reset(); +} + +template +void LargeKinfuImpl::reset() +{ + frameCounter = 0; + pose = Affine3f::Identity(); + submapMgr->reset(); + submapMgr->createNewSubmap(true); +} + +template +LargeKinfuImpl::~LargeKinfuImpl() +{ +} + +template +const Params& LargeKinfuImpl::getParams() const +{ + return params; +} + +template +const Affine3f LargeKinfuImpl::getPose() const +{ + return pose; +} + +template<> +bool LargeKinfuImpl::update(InputArray _depth) +{ + CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); + + Mat depth; + if (_depth.isUMat()) + { + _depth.copyTo(depth); + return updateT(depth); + } + else + { + return updateT(_depth.getMat()); + } +} + +template<> +bool LargeKinfuImpl::update(InputArray _depth) +{ + CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); + + UMat depth; + if (!_depth.isUMat()) + { + _depth.copyTo(depth); + return updateT(depth); + } + else + { + return updateT(_depth.getUMat()); + } +} + +template +bool LargeKinfuImpl::updateT(const MatType& _depth) +{ + CV_TRACE_FUNCTION(); + + MatType depth; + if (_depth.type() != DEPTH_TYPE) + _depth.convertTo(depth, DEPTH_TYPE); + else + depth = _depth; + + currSubmap = submapMgr->getCurrentSubmap(); + std::vector newPoints, newNormals; + makeFrameFromDepth(depth, newPoints, newNormals, params.intr, params.pyramidLevels, params.depthFactor, + params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size, + params.truncateThreshold); + if (frameCounter == 0) + { + // use depth instead of distance + currSubmap->volume->integrate(depth, params.depthFactor, pose, params.intr, frameCounter); + pyrPoints = newPoints; + pyrNormals = newNormals; + } + else + { + Affine3f affine; + bool success = icp->estimateTransform(affine, pyrPoints, pyrNormals, newPoints, newNormals); + if (!success) + return false; + + pose = pose * affine; + + float rnorm = (float)cv::norm(affine.rvec()); + float tnorm = (float)cv::norm(affine.translation()); + // We do not integrate volume if camera does not move + if ((rnorm + tnorm) / 2 >= params.tsdf_min_camera_movement) + { + // use depth instead of distance + currSubmap->volume->integrate(depth, params.depthFactor, pose, params.intr, frameCounter); + } + MatType& points = pyrPoints[0]; + MatType& normals = pyrNormals[0]; + currSubmap->volume->raycast(pose, params.intr, params.frameSize, points, normals); + buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, params.pyramidLevels); + + std::cout << "Total allocated blocks: " << currSubmap->getTotalAllocatedBlocks() << "\n"; + std::cout << "Visible blocks: " << currSubmap->getVisibleBlocks(frameCounter) << "\n"; + submapMgr->shouldCreateSubmap(frameCounter); + } + frameCounter++; + return true; +} + +template +void LargeKinfuImpl::render(OutputArray image, const Matx44f& _cameraPose) const +{ + CV_TRACE_FUNCTION(); + + Affine3f cameraPose(_cameraPose); + + const Affine3f id = Affine3f::Identity(); + if ((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) || + (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) + { + renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose); + } + else + { + MatType points, normals; + currSubmap->volume->raycast(cameraPose, params.intr, params.frameSize, points, normals); + renderPointsNormals(points, normals, image, params.lightPose); + } +} + +template +void LargeKinfuImpl::getCloud(OutputArray p, OutputArray n) const +{ + currSubmap->volume->fetchPointsNormals(p, n); +} + +template +void LargeKinfuImpl::getPoints(OutputArray points) const +{ + currSubmap->volume->fetchPointsNormals(points, noArray()); +} + +template +void LargeKinfuImpl::getNormals(InputArray points, OutputArray normals) const +{ + currSubmap->volume->fetchNormals(points, normals); +} + +// importing class + +#ifdef OPENCV_ENABLE_NONFREE + +Ptr LargeKinfu::create(const Ptr& params) +{ + CV_Assert((int)params->icpIterations.size() == params->pyramidLevels); + CV_Assert(params->intr(0, 1) == 0 && params->intr(1, 0) == 0 && params->intr(2, 0) == 0 && params->intr(2, 1) == 0 && + params->intr(2, 2) == 1); +#ifdef HAVE_OPENCL + if (cv::ocl::useOpenCL()) + return makePtr>(*params); +#endif + return makePtr>(*params); +} + +#else +Ptr LargeKinfu::create(const Ptr& /* params */) +{ + CV_Error(Error::StsNotImplemented, + "This algorithm is patented and is excluded in this configuration; " + "Set OPENCV_ENABLE_NONFREE CMake option and rebuild the library"); +} +#endif +} // namespace large_kinfu +} // namespace cv diff --git a/modules/rgbd/src/submap.cpp b/modules/rgbd/src/submap.cpp new file mode 100644 index 00000000000..1da4c71317f --- /dev/null +++ b/modules/rgbd/src/submap.cpp @@ -0,0 +1,69 @@ +// 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 "submap.hpp" + +#include "hash_tsdf.hpp" +namespace cv +{ +namespace kinfu +{ +Submap::Submap(SubmapId _submapId, const VolumeParams& volumeParams, const cv::Affine3f& _pose) + : submapId(_submapId), pose(_pose) +{ + volume = cv::makePtr(volumeParams); + std::cout << "Created volume\n"; +} + +SubmapManager::SubmapManager(const VolumeParams& _volumeParams) + : volumeParams(_volumeParams) +{} + +SubmapId SubmapManager::createNewSubmap(bool isCurrentActiveMap, const Affine3f& pose) +{ + size_t idx = submaps.size(); + submaps.push_back(cv::makePtr(idx, volumeParams, pose)); + /* Constraint newConstraint; */ + /* newConstraint.idx = idx; */ + /* newConstraint.type = isCurrentActiveMap ? CURRENT_ACTIVE : ACTIVE; */ + std::cout << "Created new submap\n"; + return idx; +} + +cv::Ptr SubmapManager::getCurrentSubmap(void) +{ + if(submaps.size() > 0) + return submaps.at(submaps.size() - 1); + else + return nullptr; +} + +void SubmapManager::reset() +{ + //! Technically should delete all the submaps; + for(const auto& submap : submaps) + { + submap->volume->reset(); + } + submaps.clear(); +} + +bool SubmapManager::shouldCreateSubmap(int currFrameId) +{ + cv::Ptr curr_submap = getCurrentSubmap(); + int allocate_blocks = curr_submap->getTotalAllocatedBlocks(); + int visible_blocks = curr_submap->getVisibleBlocks(currFrameId); + float ratio = float(visible_blocks) / float(allocate_blocks); + std::cout << "Ratio: " << ratio << "\n"; + + if(ratio < 0.2f) + return true; + return false; +} + + + +} // namespace rgbd +} // namespace cv diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp new file mode 100644 index 00000000000..8c80b709414 --- /dev/null +++ b/modules/rgbd/src/submap.hpp @@ -0,0 +1,83 @@ +// 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_RGBD_SUBMAP_H__ +#define __OPENCV_RGBD_SUBMAP_H__ + +#include + +#include +#include + +#include "hash_tsdf.hpp" + +namespace cv +{ +namespace kinfu +{ +typedef uint16_t SubmapId; +typedef uint16_t FrameId; + +//! T is either HashTSDFVolumeCPU or HashTSDFVolumeGPU +class Submap +{ + public: + Submap(SubmapId _submapId, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity()); + virtual ~Submap() = default; + + virtual size_t getTotalAllocatedBlocks() const { return volume->getTotalVolumeUnits(); }; + virtual size_t getVisibleBlocks(int currFrameId) const { return volume->getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD); }; + + virtual void setStartFrameId(FrameId _startFrameId) { startFrameId = _startFrameId; }; + virtual void setStopFrameId(FrameId _stopFrameId) { stopFrameId = _stopFrameId; }; + + public: + //! TODO: Should we support submaps for regular volumes? + static constexpr int FRAME_VISIBILITY_THRESHOLD = 5; + cv::Ptr> volume; + private: + const SubmapId submapId; + cv::Affine3f pose; + + FrameId startFrameId; + FrameId stopFrameId; +}; + +struct Constraint +{ + enum Type + { + CURRENT_ACTIVE, + ACTIVE, + }; + SubmapId idx; + Type type; +}; + +class SubmapManager +{ + public: + SubmapManager(const VolumeParams& _volumeParams); + virtual ~SubmapManager() = default; + + void reset(); + SubmapId createNewSubmap(bool isCurrentActiveMap, const Affine3f& pose = cv::Affine3f::Identity()); + void removeSubmap(SubmapId _submapId); + size_t getTotalSubmaps(void) const { return submaps.size(); }; + Submap getSubmap(SubmapId _submapId) const; + cv::Ptr getCurrentSubmap(void); + + bool shouldCreateSubmap(int frameId); + + void setPose(SubmapId _submapId); + + protected: + VolumeParams volumeParams; + std::vector> submaps; + std::vector constraints; +}; + +} // namespace kinfu +} // namespace cv +#endif /* ifndef __OPENCV_RGBD_SUBMAP_H__ */ diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index a650fd27bcb..ca36f2e2c4c 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -427,7 +427,7 @@ struct IntegrateInvoker : ParallelLoopBody // use depth instead of distance (optimization) void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, - const Intr& intrinsics) + const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); @@ -1140,7 +1140,7 @@ void TSDFVolumeGPU::reset() // use depth instead of distance (optimization) void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, - const cv::Affine3f& cameraPose, const Intr& intrinsics) + const cv::Affine3f& cameraPose, const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); @@ -1429,5 +1429,17 @@ cv::Ptr makeTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _ _resolution); } +cv::Ptr makeTSDFVolume(const VolumeParams& _params) +{ +#ifdef HAVE_OPENCL + if (cv::ocl::useOpenCL()) + return cv::makePtr(_params.voxelSize, _params.pose, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + _params.resolution); +#endif + return cv::makePtr(_params.voxelSize, _params.pose, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + _params.resolution); + +} + } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 4829d67e598..7965dee1a27 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -53,7 +53,7 @@ class TSDFVolumeCPU : public TSDFVolume int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true); virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, - const cv::kinfu::Intr& intrinsics) override; + const cv::kinfu::Intr& intrinsics, const int frameId = 0) override; virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, cv::OutputArray points, cv::OutputArray normals) const override; @@ -87,7 +87,7 @@ class TSDFVolumeGPU : public TSDFVolume int _maxWeight, Point3i _resolution); virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, - const cv::kinfu::Intr& intrinsics) override; + const cv::kinfu::Intr& intrinsics, const int frameId = 0) override; virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, cv::OutputArray _points, cv::OutputArray _normals) const override; diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index 066374252c9..67a405e3ed0 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -15,29 +15,27 @@ namespace kinfu cv::Ptr VolumeParams::defaultParams(VolumeType _volumeType) { VolumeParams params; - params.volumeType = _volumeType; + params.type = _volumeType; params.maxWeight = 64; params.raycastStepFactor = 0.25f; float volumeSize = 3.0f; - params.volumePose = Affine3f().translate(Vec3f(-volumeSize/2.f, -volumeSize/2.f, 0.5f)); - switch (params.volumeType) + params.pose = Affine3f().translate(Vec3f(-volumeSize/2.f, -volumeSize/2.f, 0.5f)); + switch (params.type) { case VolumeType::TSDF: - params.volumeSize = volumeSize; - params.volumeResolution = Vec3i::all(512); + params.resolution = Vec3i::all(512); params.voxelSize = volumeSize / 512.f; params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF break; case VolumeType::HASHTSDF: - params.volumeSize = 3.0f; // VolumeSize not required for HASHTSDF - params.volumeUnitResolution = 16; + params.unitResolution = 16; params.voxelSize = volumeSize / 512.f; params.depthTruncThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); default: //! TODO: Should throw some exception or error break; } - params.truncDist = 7 * params.voxelSize; //! About 0.04f in meters + params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters return makePtr(params); } @@ -47,31 +45,32 @@ cv::Ptr VolumeParams::coarseParams(VolumeType _volumeType) Ptr params = defaultParams(_volumeType); params->raycastStepFactor = 0.75f; - switch (params->volumeType) + float volumeSize = 3.0f; + switch (params->type) { case VolumeType::TSDF: - params->volumeResolution = Vec3i::all(128); - params->voxelSize = params->volumeSize/128.f; + params->resolution = Vec3i::all(128); + params->voxelSize = volumeSize/128.f; break; case VolumeType::HASHTSDF: - params->voxelSize = params->volumeSize/128.f; + params->voxelSize = volumeSize/128.f; break; default: break; } - params->truncDist = 2 * params->voxelSize; //! About 0.04f in meters + params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters return params; } cv::Ptr makeVolume(const VolumeParams& _volumeParams) { - switch(_volumeParams.volumeType) + switch(_volumeParams.type) { case VolumeType::TSDF: - return makeTSDFVolume(_volumeParams); + return kinfu::makeTSDFVolume(_volumeParams); break; case VolumeType::HASHTSDF: - return cv::makePtr(_volumeParams); + return cv::makePtr(_volumeParams); break; default: return nullptr; @@ -83,7 +82,7 @@ cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3 { if (_volumeType == VolumeType::TSDF) { - return makeTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + return kinfu::makeTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution); } else if (_volumeType == VolumeType::HASHTSDF) @@ -93,7 +92,7 @@ cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3 /* return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, */ /* _truncateThreshold); */ /* #endif */ - return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + return kinfu::makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); } else From 362ec1aaf67d7ddbd4abbffdff648700b8a3bbe1 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 14 Jul 2020 20:21:04 -0400 Subject: [PATCH 14/36] Early draft of posegraph and submaps (Doesn't even compile) --- modules/rgbd/src/large_kinfu.cpp | 12 +- modules/rgbd/src/pose_graph.cpp | 314 +++++++++++++++++++++++++++++++ modules/rgbd/src/pose_graph.h | 157 ++++++++++++++++ modules/rgbd/src/submap.cpp | 69 +++++-- modules/rgbd/src/submap.hpp | 8 + 5 files changed, 539 insertions(+), 21 deletions(-) create mode 100644 modules/rgbd/src/pose_graph.cpp create mode 100644 modules/rgbd/src/pose_graph.h diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 2fc46bd3b89..ddf17bfe81e 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -159,7 +159,6 @@ void LargeKinfuImpl::reset() frameCounter = 0; pose = Affine3f::Identity(); submapMgr->reset(); - submapMgr->createNewSubmap(true); } template @@ -224,7 +223,6 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) else depth = _depth; - currSubmap = submapMgr->getCurrentSubmap(); std::vector newPoints, newNormals; makeFrameFromDepth(depth, newPoints, newNormals, params.intr, params.pyramidLevels, params.depthFactor, params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size, @@ -232,12 +230,16 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) if (frameCounter == 0) { // use depth instead of distance + submapMgr->createNewSubmap(true); + currSubmap = submapMgr->getCurrentSubmap(); currSubmap->volume->integrate(depth, params.depthFactor, pose, params.intr, frameCounter); pyrPoints = newPoints; pyrNormals = newNormals; + } else { + currSubmap = submapMgr->getCurrentSubmap(); Affine3f affine; bool success = icp->estimateTransform(affine, pyrPoints, pyrNormals, newPoints, newNormals); if (!success) @@ -260,7 +262,11 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) std::cout << "Total allocated blocks: " << currSubmap->getTotalAllocatedBlocks() << "\n"; std::cout << "Visible blocks: " << currSubmap->getVisibleBlocks(frameCounter) << "\n"; - submapMgr->shouldCreateSubmap(frameCounter); + if(submapMgr->shouldCreateSubmap(frameCounter)) + { + int newIdx = submapMgr->createNewSubmap(true, pose, newPoints, newNormals); + } + std::cout << "Number of submaps: " << submapMgr->getTotalSubmaps() << "\n"; } frameCounter++; return true; diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp new file mode 100644 index 00000000000..329af7a5649 --- /dev/null +++ b/modules/rgbd/src/pose_graph.cpp @@ -0,0 +1,314 @@ +// 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 "pose_graph.h" +#include + +#include +#include + +#include "opencv2/core/eigen.hpp" +#include "opencv2/core/mat.hpp" +#include "opencv2/core/matx.hpp" +#if defined(HAVE_EIGEN) +#include +#include +#endif + +namespace cv +{ +namespace kinfu +{ + +//! Function to solve a sparse linear system of equations HX = B +//! Requires Eigen +static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& X) +{ + const float matValThreshold = 0.001f; + + bool result = false; + +#if defined(HAVE_EIGEN) + + std::cout << "starting eigen-insertion..." << std::endl; + + std::vector> tripletList; + tripletList.reserve(H.ijValue.size() * H.blockSize * H.blockSize); + for (auto ijValue : H.ijValue) + { + int xb = ijValue.first.x, yb = ijValue.first.y; + Matx66f vblock = ijValue.second; + for (int i = 0; i < H.blockSize; i++) + { + for (int j = 0; j < H.blockSize; j++) + { + float val = vblock(i, j); + if (abs(val) >= matValThreshold) + { + tripletList.push_back(Eigen::Triplet(H.blockSize * xb + i, H.blockSize * yb + j, val)); + } + } + } + } + + Eigen::SparseMatrix bigA(H.blockSize * H.nBlocks, H.blockSize * H.nBlocks); + bigA.setFromTriplets(tripletList.begin(), tripletList.end()); + + // TODO: do we need this? + bigA.makeCompressed(); + + Eigen::VectorXf bigB; + cv2eigen(B, bigB); + + if (!bigA.isApprox(bigA.transpose()) + { + CV_Error(Error::StsBadArg, "Sparse Matrix is not symmetric"); + return result; + } + + //!TODO: Check determinant of bigA + + // TODO: try this, LLT and Cholesky + // Eigen::SimplicialLDLT> solver; + Eigen::SimplicialCholesky, Eigen::NaturalOrdering> solver; + + std::cout << "starting eigen-compute..." << std::endl; + solver.compute(bigA); + + if (solver.info() != Eigen::Success) + { + std::cout << "failed to eigen-decompose" << std::endl; + result = false; + } + else + { + std::cout << "starting eigen-solve..." << std::endl; + + Eigen::VectorXf sx = solver.solve(bigB); + if (solver.info() != Eigen::Success) + { + std::cout << "failed to eigen-solve" << std::endl; + result = false; + } + else + { + x.resize(jtb.size); + eigen2cv(sx, x); + result = true; + } + } + +#else + std::cout << "no eigen library" << std::endl; + + CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); +#endif + + return result; +} + +void PoseGraph::addNode(const PoseGraphNode& node) { nodes.push_back(node); } + +void PoseGraph::addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } + +bool PoseGraph::nodeExists(int nodeId) { return (nodes.find(nodeId) != nodes.end()); } + +bool PoseGraph::isValid() +{ + size_t numNodes = nodes.size(); + size_t numEdges = edges.size(); + + if (numNodes <= 0 || numEdges <= 0) + return false; + + std::unordered_set nodeSet; + std::vector nodeList; + + nodeList.push_back(nodes.at(0)->getId()); + nodeSet.insert(nodes.at(0)->getId()); + + bool isGraphConnected = false; + if (!nodeList.empty()) + { + int currNodeId = nodeList.back(); + nodeList.pop_back(); + + for (size_t i = 0; i < numEdges; i++) + { + const PoseGraphEdge& potentialEdge = *edges.at(i); + int nextNodeId = -1; + + if (potentialEdge.getSourceNodeId() == currNodeId) + { + nextNodeId = potentialEdge.getTargetNodeId(); + } + else if (potentialEdge.getTargetNodeId() == currNodeId) + { + nextNodeId = potentialEdge.getSourceNodeId(); + } + + if (nextNodeId != -1) + { + nodeList.push_back(nextNodeId); + nodeSet.insert(nextNodeId); + } + } + + isGraphConnected = (nodeSet.size() == numNodes); + + bool invalidEdgeNode = false; + for (size_t i = 0; i < numEdges; i++) + { + const PoseGraphEdge& edge = *edges.at(i); + if ((nodeSet.count(edge.getSourceNodeId()) != 1) || (nodeSet.count(edge.getTargetNodeId()) != 1)) + { + invalidEdgeNode = true; + break; + } + } + return isGraphConnected && !invalidEdgeNode; + } +} + +//! TODO: Reference: Open3D and linearizeOplus in g2o +void calcJacobian(Matx66f& jacSource, Matx66f& jacTarget, const Affine3f& relativePoseMeas, const Affine3f& sourcePose, + const Affine3f& targetPose) +{ + const Affine3f relativePoseMeas_inv = relativePoseMeas.inv(); + const Affine3f targetPose_inv = targetPose.inv(); + for (int i = 0; i < 6; i++) + { + Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * generatorJacobian[i] * sourcePose; + Vec3f rot = dError_dSource.rvec(); + Vec3f trans = dError_dSource.translation(); + jacSource.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); + } + for (int i = 0; i < 6; i++) + { + Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * -generatorJacobian[i] * sourcePose; + Vec3f rot = dError_dSource.rvec(); + Vec3f trans = dError_dSource.translation(); + jacTarget.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); + } +} + +float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) +{ + size_t numNodes = nodes.size(); + size_t numEdges = edges.size(); + + float residual = 0; + //! Compute linear system of equations + for (int currEdgeNum = 0; currEdgeNum < numEdges; currEdgeNum++) + { + const PoseGraphEdge& currEdge = *edges.at(currEdgeNum); + int sourceNodeId = currEdge.getSourceNodeId(); + int targetNodeId = currEdge.getTargetNodeId(); + const Affine3f& sourcePose = nodes.at(sourceNodeId)->getPose(); + const Affine3f& targetPose = nodes.at(targetNodeId)->getPose(); + + Affine3f relativeSourceToTarget = targetPose.inv() * sourcePose; + //! Edge transformation is source to target. (relativeConstraint should be identity if no error) + Affine3f relativeConstraint = currEdge.transformation.inv() * relativeSourceToTarget; + + Vec6f error; + Vec3f rot = relativeConstraint.rvec(); + Vec3f trans = relativeConstraint.translation(); + + error[0] = rot[0]; + error[1] = rot[1]; + error[2] = rot[2]; + error[3] = trans[0]; + error[4] = trans[1]; + error[5] = trans[2]; + + Matx66f jacSource, jacTarget; + calcErrorJacobian(currEdge.transformation, sourcePose, targetPose, jacSource, jacTarget); + + Vec6f errorTransposeInfo = error.t() * currEdge.transformation; + Matx66f jacSourceTransposeInfo = jacSource.t() * currEdge.information; + Matx66f jacTargetTransposeInfo = jacTarget.t() * currEdge.information; + + residual += errorTransposeInfo * error; + + ApproxH.refBlock(sourceNodeId, sourceNodeId) += jacSourceTransposeInfo * jacSource; + ApproxH.refBlock(targetNodeId, targetNodeId) += jacTargetTransposeInfo * jacTarget; + ApproxH.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget; + ApproxH.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource; + + B.rowRange(6*sourceNodeId, 6*(sourceNodeId+1)) += errorTransposeInfo * jacSource; + B.rowRange(6*targetNodeId, 6*(targetNodeId+1)) += errorTransposeInfo * jacTarget; + } + + return residual; +} + +PoseGraph PoseGraph::updatePoseGraph(const PoseGraph& poseGraphPrev, const Mat& delta) +{ + size_t numNodes = poseGraphPrev.nodes.size(); + size_t numEdges = poseGraphPrev.edges.size(); + + //!Check: This should create a copy of the posegraph + PoseGraph updatedPoseGraph(poseGraphPrev); + + for(int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) + { + Vec6f delta = delta.rowRange(6*currentNodeId, 6*(currentNodeId+1)); + Affine3f pose = poseGraphPrev.nodes.at(currentNodeId)->getPose(); + Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val+3)); + Affine3f updatedPose = currDelta * pose; + + updatePoseGraph.nodes.at(currentNodeId)->setPose(updatedPose); + } + + return updatePoseGraph; +} + +//! NOTE: We follow the left-composition for the infinitesimal pose update +void PoseGraph::optimize(PoseGraph& poseGraph, int numIter, int minResidual) +{ + PoseGraph poseGraphOriginal = poseGraph; + //! Check if posegraph is valid + if (!isValid()) + //! Should print some error + return; + + size_t numNodes = nodes.size(); + size_t numEdges = edges.size(); + + //! ApproxH = Approximate Hessian = J^T * information * J + int iter = 0; + //! converged is set to true when error/residual is within some limit + bool converged = false; + float prevResidual = std::numeric_limits::max(); + + //! TODO: Try LM instead of GN + do { + BlockSparseMat ApproxH(numNodes); + Mat B(6 * numNodes, 1, float); + + float currentResidual = poseGraph.createLinearSystem(ApproxH, B); + + Mat delta(6*numNodes, 1, float); + bool success = sparseSolve(ApproxH, B, delta); + if(!success) + { + CV_Error(Error::StsNoConv, "Sparse solve failed"); + return; + } + //! Check delta + poseGraph = updatePoseGraph(poseGraph, delta); + + std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; + + //!TODO: Improve criterion and allow small increments in residual + if(currentResidual - prevResidual > 0f) + break; + if((currentResidual - minResidual) < 0.00001f) + converged = true; + + } while (iter < numIter && !converged) +} +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/pose_graph.h b/modules/rgbd/src/pose_graph.h new file mode 100644 index 00000000000..6310f4d7634 --- /dev/null +++ b/modules/rgbd/src/pose_graph.h @@ -0,0 +1,157 @@ +#ifndef OPENCV_RGBD_GRAPH_NODE_H +#define OPENCV_RGBD_GRAPH_NODE_H + +#include "opencv2/core/affine.hpp" +#include +#include +namespace cv +{ +namespace kinfu +{ +/*! + * \class BlockSparseMat + * Naive implementation of Sparse Block Matrix + */ +template +struct BlockSparseMat +{ + typedef std::unordered_map> IDtoBlockValueMap; + static constexpr int blockSize = blockM * blockN; + BlockSparseMat(int _nBlocks) : + nBlocks(_nBlocks), ijValue() + { } + + Matx66f& refBlock(int i, int j) + { + Point2i p(i, j); + auto it = ijValue.find(p); + if (it == ijValue.end()) + { + it = ijValue.insert({ p, Matx<_Tp, blockM, blockN>()}).first; + } + return it->second; + } + + float& refElem(int i, int j) + { + Point2i ib(i / blockSize, j / blockSize), iv(i % blockSize, j % blockSize); + return refBlock(ib.x, ib.y)(iv.x, iv.y); + } + + size_t nonZeroBlocks() const + { + return ijValue.size(); + } + + int nBlocks; + IDtoBlockValueMap ijValue; +}; + + +/*! \class GraphNode + * \brief Defines a node/variable that is optimizable in a posegraph + * + * Detailed description + */ +struct PoseGraphNode +{ + public: + explicit PoseGraphNode(int nodeId, const Affine3f& _pose) : pose(_pose) {} + virtual ~PoseGraphNode(); + + int getId() const { return nodeId; } + Affine3f getPose() const { return pose; } + void setPose(const Affine3f& _pose) { pose = _pose; } + private: + int nodeId; + Affine3f pose; +}; + +/*! \class PoseGraphEdge + * \brief Defines the constraints between two PoseGraphNodes + * + * Detailed description + */ +struct PoseGraphEdge +{ + public: + PoseGraphEdge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, + const Matx66f& _information = Matx66f::eye()) + : sourceNodeId(_sourceNodeId), targetNodeId(_targetNodeId), transformation(_transformation), information(_information) + {} + virtual ~PoseGraphEdge() = default; + + int getSourceNodeId() const { return sourceNodeId; } + int getTargetNodeId() const { return targetNodeId; } + + public: + int sourceNodeId; + int targetNodeId; + Affine3f transformation; + Matx66f information; +}; + +//! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold optimization +//! Jose Luis Blanco +//! Compactly represents the jacobian of the SE3 generator +static const std::array generatorJacobian = +{ //alpha + Affine3f(Matx44f(0, 0, 0, 0, + 0, 0, -1, 0, + 0, 1, 0, 0, + 0, 0, 0, 0)), + //beta + Affine3f(Matx44f( 0, 0, 1, 0, + 0, 0, 0, 0, + -1, 0, 0, 0, + 0, 0, 0, 0)), + //gamma + Affine3f(Matx44f(0, -1, 0, 0, + 1, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0)), + //x + Affine3f(Matx44f(0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0)), + //y + Affine3f(Matx44f(0, 0, 0, 0, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 0)), + //z + Affine3f(Matx44f(0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 1, + 0, 0, 0, 0)) +}; + +class PoseGraph +{ + public: + typedef std::vector NodeVector; + typedef std::vector EdgeVector; + + PoseGraph() {}; + virtual ~PoseGraph() = default; + + void addNode(const PoseGraphNode& node); + void addEdge(const PoseGraphEdge& edge); + + bool nodeExists(int nodeId); + + bool isValid(); + static cv::Ptr updatePoseGraph(const PoseGraph& poseGraphPrevious, const Mat& delta) + static void optimize(PoseGraph& poseGraph, int numIters = 10, float min_residual = 1e-6); + + private: + //! @brief: Constructs a linear system and returns the residual of the current system + float createLinearSystem(BlockSparseMat& H, Mat& B); + + NodeVector nodes; + EdgeVector edges; + +}; // namespace large_kinfu +} // namespace cv +#endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */ diff --git a/modules/rgbd/src/submap.cpp b/modules/rgbd/src/submap.cpp index 1da4c71317f..51c787e72fd 100644 --- a/modules/rgbd/src/submap.cpp +++ b/modules/rgbd/src/submap.cpp @@ -2,10 +2,12 @@ // 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 "submap.hpp" #include "hash_tsdf.hpp" +#include "opencv2/core/cvstd_wrapper.hpp" +#include "pose_graph.h" +#include "precomp.hpp" namespace cv { namespace kinfu @@ -17,24 +19,57 @@ Submap::Submap(SubmapId _submapId, const VolumeParams& volumeParams, const cv::A std::cout << "Created volume\n"; } -SubmapManager::SubmapManager(const VolumeParams& _volumeParams) - : volumeParams(_volumeParams) -{} +SubmapManager::SubmapManager(const VolumeParams& _volumeParams) : volumeParams(_volumeParams) {} SubmapId SubmapManager::createNewSubmap(bool isCurrentActiveMap, const Affine3f& pose) { - size_t idx = submaps.size(); - submaps.push_back(cv::makePtr(idx, volumeParams, pose)); - /* Constraint newConstraint; */ - /* newConstraint.idx = idx; */ - /* newConstraint.type = isCurrentActiveMap ? CURRENT_ACTIVE : ACTIVE; */ + SubmapId newSubmapId = submaps.size(); + cv::Ptr prevSubmap = getCurrentSubmap(); + + cv::Ptr newSubmap = cv::makePtr(newSubmapId, volumeParams, pose); + + //! If first submap being added. Add prior constraint to PoseGraph + if (submaps.size() == 0 || !prevSubmap) + { + poseGraph.addNode(newSubmapId, cv::makePtr(pose)); + } + else + { + addCameraCameraConstraint(prevSubmap->getId(), newSubmap->getId(), prevSubmap->getPose(), newSubmap->getPose()); + } + submaps.push_back(newSubmap); std::cout << "Created new submap\n"; + return idx; } +void SubmapManager::addPriorConstraint(SubmapId currId, const Affine3f& currPose) +{ + poseGraph.addNode(currId, cv::makePtr(currPose)); + poseGraph.addEdge +} + +void SubmapManager::addCameraCameraConstraint(SubmapId prevId, SubmapId currId, const Affine3f& prevPose, + const Affine3f& currPose) +{ + //! 1. Add new posegraph node + //! 2. Add new posegraph constraint + + //! TODO: Attempt registration between submaps + Affine3f Tprev2curr = currPose.inv() * currPose; + //! Constant information matrix for all odometry constraints + Matx66f information = Matx66f::eye() * 1000; + + //! Inserts only if the element does not exist already + poseGraph.addNode(prevId, cv::makePtr(prevPose)); + poseGraph.addNode(currId, cv::makePtr(currPose)); + + poseGraph.addEdge(cv::makePtr(prevId, currId, Tprev2curr, information)); +} + cv::Ptr SubmapManager::getCurrentSubmap(void) { - if(submaps.size() > 0) + if (submaps.size() > 0) return submaps.at(submaps.size() - 1); else return nullptr; @@ -43,7 +78,7 @@ cv::Ptr SubmapManager::getCurrentSubmap(void) void SubmapManager::reset() { //! Technically should delete all the submaps; - for(const auto& submap : submaps) + for (const auto& submap : submaps) { submap->volume->reset(); } @@ -53,17 +88,15 @@ void SubmapManager::reset() bool SubmapManager::shouldCreateSubmap(int currFrameId) { cv::Ptr curr_submap = getCurrentSubmap(); - int allocate_blocks = curr_submap->getTotalAllocatedBlocks(); - int visible_blocks = curr_submap->getVisibleBlocks(currFrameId); - float ratio = float(visible_blocks) / float(allocate_blocks); + int allocate_blocks = curr_submap->getTotalAllocatedBlocks(); + int visible_blocks = curr_submap->getVisibleBlocks(currFrameId); + float ratio = float(visible_blocks) / float(allocate_blocks); std::cout << "Ratio: " << ratio << "\n"; - if(ratio < 0.2f) + if (ratio < 0.2f) return true; return false; } - - -} // namespace rgbd +} // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 8c80b709414..2ac9d8a290a 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -11,6 +11,7 @@ #include #include "hash_tsdf.hpp" +#include "pose_graph.h" namespace cv { @@ -32,6 +33,9 @@ class Submap virtual void setStartFrameId(FrameId _startFrameId) { startFrameId = _startFrameId; }; virtual void setStopFrameId(FrameId _stopFrameId) { stopFrameId = _stopFrameId; }; + virtual SubmapId getId() const { return submapId; } + virtual cv::Affine3f getPose() const { return pose; } + public: //! TODO: Should we support submaps for regular volumes? static constexpr int FRAME_VISIBILITY_THRESHOLD = 5; @@ -73,9 +77,13 @@ class SubmapManager void setPose(SubmapId _submapId); protected: + + void addCameraCameraConstraint(SubmapId prevId, SubmapId currId, const Affine3f& prevPose, const Affine3f& currPose); VolumeParams volumeParams; std::vector> submaps; std::vector constraints; + + PoseGraph poseGraph; }; } // namespace kinfu From 7264be257637d317eb7b61cff8dfda24cc512aab Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Wed, 15 Jul 2020 16:03:24 -0400 Subject: [PATCH 15/36] Minor cleanup (no compilation) --- modules/rgbd/src/pose_graph.cpp | 234 ++++++------------ .../rgbd/src/{pose_graph.h => pose_graph.hpp} | 126 +++++----- modules/rgbd/src/sparse_block_matrix.h | 137 ++++++++++ modules/rgbd/src/submap.cpp | 34 +-- modules/rgbd/src/submap.hpp | 68 ++--- 5 files changed, 316 insertions(+), 283 deletions(-) rename modules/rgbd/src/{pose_graph.h => pose_graph.hpp} (54%) create mode 100644 modules/rgbd/src/sparse_block_matrix.h diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 329af7a5649..d0b51b533ca 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -3,7 +3,6 @@ // of this distribution and at http://opencv.org/license.html #include "pose_graph.h" -#include #include #include @@ -11,109 +10,11 @@ #include "opencv2/core/eigen.hpp" #include "opencv2/core/mat.hpp" #include "opencv2/core/matx.hpp" -#if defined(HAVE_EIGEN) -#include -#include -#endif namespace cv { namespace kinfu { - -//! Function to solve a sparse linear system of equations HX = B -//! Requires Eigen -static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& X) -{ - const float matValThreshold = 0.001f; - - bool result = false; - -#if defined(HAVE_EIGEN) - - std::cout << "starting eigen-insertion..." << std::endl; - - std::vector> tripletList; - tripletList.reserve(H.ijValue.size() * H.blockSize * H.blockSize); - for (auto ijValue : H.ijValue) - { - int xb = ijValue.first.x, yb = ijValue.first.y; - Matx66f vblock = ijValue.second; - for (int i = 0; i < H.blockSize; i++) - { - for (int j = 0; j < H.blockSize; j++) - { - float val = vblock(i, j); - if (abs(val) >= matValThreshold) - { - tripletList.push_back(Eigen::Triplet(H.blockSize * xb + i, H.blockSize * yb + j, val)); - } - } - } - } - - Eigen::SparseMatrix bigA(H.blockSize * H.nBlocks, H.blockSize * H.nBlocks); - bigA.setFromTriplets(tripletList.begin(), tripletList.end()); - - // TODO: do we need this? - bigA.makeCompressed(); - - Eigen::VectorXf bigB; - cv2eigen(B, bigB); - - if (!bigA.isApprox(bigA.transpose()) - { - CV_Error(Error::StsBadArg, "Sparse Matrix is not symmetric"); - return result; - } - - //!TODO: Check determinant of bigA - - // TODO: try this, LLT and Cholesky - // Eigen::SimplicialLDLT> solver; - Eigen::SimplicialCholesky, Eigen::NaturalOrdering> solver; - - std::cout << "starting eigen-compute..." << std::endl; - solver.compute(bigA); - - if (solver.info() != Eigen::Success) - { - std::cout << "failed to eigen-decompose" << std::endl; - result = false; - } - else - { - std::cout << "starting eigen-solve..." << std::endl; - - Eigen::VectorXf sx = solver.solve(bigB); - if (solver.info() != Eigen::Success) - { - std::cout << "failed to eigen-solve" << std::endl; - result = false; - } - else - { - x.resize(jtb.size); - eigen2cv(sx, x); - result = true; - } - } - -#else - std::cout << "no eigen library" << std::endl; - - CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); -#endif - - return result; -} - -void PoseGraph::addNode(const PoseGraphNode& node) { nodes.push_back(node); } - -void PoseGraph::addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } - -bool PoseGraph::nodeExists(int nodeId) { return (nodes.find(nodeId) != nodes.end()); } - bool PoseGraph::isValid() { size_t numNodes = nodes.size(); @@ -172,37 +73,40 @@ bool PoseGraph::isValid() } //! TODO: Reference: Open3D and linearizeOplus in g2o -void calcJacobian(Matx66f& jacSource, Matx66f& jacTarget, const Affine3f& relativePoseMeas, const Affine3f& sourcePose, - const Affine3f& targetPose) -{ - const Affine3f relativePoseMeas_inv = relativePoseMeas.inv(); - const Affine3f targetPose_inv = targetPose.inv(); - for (int i = 0; i < 6; i++) - { - Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * generatorJacobian[i] * sourcePose; - Vec3f rot = dError_dSource.rvec(); - Vec3f trans = dError_dSource.translation(); - jacSource.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); - } - for (int i = 0; i < 6; i++) - { - Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * -generatorJacobian[i] * sourcePose; - Vec3f rot = dError_dSource.rvec(); - Vec3f trans = dError_dSource.translation(); - jacTarget.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); - } -} +void calcErrorJacobian() {} float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) { - size_t numNodes = nodes.size(); - size_t numEdges = edges.size(); + int numNodes = int(nodes.size()); + int numEdges = int(edges.size()); float residual = 0; + + //! Lmabda to calculate error jacobian for a particular constraint + auto calcErrorJacobian = [](Matx66f& jacSource, Matx66f& jacTarget, const Affine3f& relativePoseMeas, + const Affine3f& sourcePose, const Affine3f& targetPose) -> void { + const Affine3f relativePoseMeas_inv = relativePoseMeas.inv(); + const Affine3f targetPose_inv = targetPose.inv(); + for (int i = 0; i < 6; i++) + { + Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * generatorJacobian[i] * sourcePose; + Vec3f rot = dError_dSource.rvec(); + Vec3f trans = dError_dSource.translation(); + jacSource.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); + } + for (int i = 0; i < 6; i++) + { + Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * -generatorJacobian[i] * sourcePose; + Vec3f rot = dError_dSource.rvec(); + Vec3f trans = dError_dSource.translation(); + jacTarget.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); + } + }; + //! Compute linear system of equations for (int currEdgeNum = 0; currEdgeNum < numEdges; currEdgeNum++) { - const PoseGraphEdge& currEdge = *edges.at(currEdgeNum); + const PoseGraphEdge& currEdge = edges.at(currEdgeNum); int sourceNodeId = currEdge.getSourceNodeId(); int targetNodeId = currEdge.getTargetNodeId(); const Affine3f& sourcePose = nodes.at(sourceNodeId)->getPose(); @@ -210,11 +114,11 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) Affine3f relativeSourceToTarget = targetPose.inv() * sourcePose; //! Edge transformation is source to target. (relativeConstraint should be identity if no error) - Affine3f relativeConstraint = currEdge.transformation.inv() * relativeSourceToTarget; + Affine3f poseConstraint = currEdge.transformation.inv() * relativeSourceToTarget; Vec6f error; - Vec3f rot = relativeConstraint.rvec(); - Vec3f trans = relativeConstraint.translation(); + Vec3f rot = poseConstraint.rvec(); + Vec3f trans = poseConstraint.translation(); error[0] = rot[0]; error[1] = rot[1]; @@ -226,89 +130,95 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) Matx66f jacSource, jacTarget; calcErrorJacobian(currEdge.transformation, sourcePose, targetPose, jacSource, jacTarget); - Vec6f errorTransposeInfo = error.t() * currEdge.transformation; + Matx16f errorTransposeInfo = error.t() * currEdge.information; Matx66f jacSourceTransposeInfo = jacSource.t() * currEdge.information; Matx66f jacTargetTransposeInfo = jacTarget.t() * currEdge.information; - residual += errorTransposeInfo * error; + residual += (errorTransposeInfo * error)[0]; - ApproxH.refBlock(sourceNodeId, sourceNodeId) += jacSourceTransposeInfo * jacSource; - ApproxH.refBlock(targetNodeId, targetNodeId) += jacTargetTransposeInfo * jacTarget; - ApproxH.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget; - ApproxH.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource; + H.refBlock(sourceNodeId, sourceNodeId) += jacSourceTransposeInfo * jacSource; + H.refBlock(targetNodeId, targetNodeId) += jacTargetTransposeInfo * jacTarget; + H.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget; + H.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource; - B.rowRange(6*sourceNodeId, 6*(sourceNodeId+1)) += errorTransposeInfo * jacSource; - B.rowRange(6*targetNodeId, 6*(targetNodeId+1)) += errorTransposeInfo * jacTarget; + B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += errorTransposeInfo * jacSource; + B.rowRange(6 * targetNodeId, 6 * (targetNodeId + 1)) += errorTransposeInfo * jacTarget; } return residual; } -PoseGraph PoseGraph::updatePoseGraph(const PoseGraph& poseGraphPrev, const Mat& delta) +PoseGraph PoseGraph::update(const Mat& deltaVec) { - size_t numNodes = poseGraphPrev.nodes.size(); - size_t numEdges = poseGraphPrev.edges.size(); + int numNodes = int(nodes.size()); + int numEdges = int(edges.size()); - //!Check: This should create a copy of the posegraph - PoseGraph updatedPoseGraph(poseGraphPrev); + //! Check: This should create a copy of the posegraph + PoseGraph updatedPoseGraph(*this); - for(int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) + for (int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) { - Vec6f delta = delta.rowRange(6*currentNodeId, 6*(currentNodeId+1)); - Affine3f pose = poseGraphPrev.nodes.at(currentNodeId)->getPose(); - Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val+3)); + Vec6f delta = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)); + Affine3f pose = nodes.at(currentNodeId)->getPose(); + Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3)); Affine3f updatedPose = currDelta * pose; - updatePoseGraph.nodes.at(currentNodeId)->setPose(updatedPose); + updatedPoseGraph.nodes.at(currentNodeId)->setPose(updatedPose); } - return updatePoseGraph; + return updatedPoseGraph; } //! NOTE: We follow the left-composition for the infinitesimal pose update -void PoseGraph::optimize(PoseGraph& poseGraph, int numIter, int minResidual) +void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& poseGraph) { PoseGraph poseGraphOriginal = poseGraph; //! Check if posegraph is valid - if (!isValid()) - //! Should print some error + if (!poseGraphOriginal.isValid()) + //! Should print some error + { + CV_Error(Error::StsBadArg, "Invalid PoseGraph that is either not connected or has invalid nodes"); return; + } - size_t numNodes = nodes.size(); - size_t numEdges = edges.size(); + int numNodes = int(poseGraph.nodes.size()); + int numEdges = int(poseGraph.edges.size()); + + std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges << std::endl; //! ApproxH = Approximate Hessian = J^T * information * J int iter = 0; //! converged is set to true when error/residual is within some limit - bool converged = false; + bool converged = false; float prevResidual = std::numeric_limits::max(); - - //! TODO: Try LM instead of GN - do { + do + { BlockSparseMat ApproxH(numNodes); Mat B(6 * numNodes, 1, float); float currentResidual = poseGraph.createLinearSystem(ApproxH, B); - Mat delta(6*numNodes, 1, float); + Mat delta(6 * numNodes, 1, float); bool success = sparseSolve(ApproxH, B, delta); - if(!success) + if (!success) { CV_Error(Error::StsNoConv, "Sparse solve failed"); return; } //! Check delta - poseGraph = updatePoseGraph(poseGraph, delta); - - std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; - //!TODO: Improve criterion and allow small increments in residual - if(currentResidual - prevResidual > 0f) + //! TODO: Improve criterion and allow small increments in residual + if((currentResidual - prevResidual) > params.maxAcceptableResIncre) break; - if((currentResidual - minResidual) < 0.00001f) + if ((currentResidual - params.minResidual) < 0.00001f) converged = true; - } while (iter < numIter && !converged) + poseGraph = poseGraph.update(delta); + std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; + + prevResidual = currentResidual; + + } while (iter < params.maxNumIters && !converged) } } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/pose_graph.h b/modules/rgbd/src/pose_graph.hpp similarity index 54% rename from modules/rgbd/src/pose_graph.h rename to modules/rgbd/src/pose_graph.hpp index 6310f4d7634..15343734492 100644 --- a/modules/rgbd/src/pose_graph.h +++ b/modules/rgbd/src/pose_graph.hpp @@ -1,53 +1,15 @@ #ifndef OPENCV_RGBD_GRAPH_NODE_H #define OPENCV_RGBD_GRAPH_NODE_H -#include "opencv2/core/affine.hpp" #include #include + +#include "opencv2/core/affine.hpp" +#include "sparse_block_matrix.h" namespace cv { namespace kinfu { -/*! - * \class BlockSparseMat - * Naive implementation of Sparse Block Matrix - */ -template -struct BlockSparseMat -{ - typedef std::unordered_map> IDtoBlockValueMap; - static constexpr int blockSize = blockM * blockN; - BlockSparseMat(int _nBlocks) : - nBlocks(_nBlocks), ijValue() - { } - - Matx66f& refBlock(int i, int j) - { - Point2i p(i, j); - auto it = ijValue.find(p); - if (it == ijValue.end()) - { - it = ijValue.insert({ p, Matx<_Tp, blockM, blockN>()}).first; - } - return it->second; - } - - float& refElem(int i, int j) - { - Point2i ib(i / blockSize, j / blockSize), iv(i % blockSize, j % blockSize); - return refBlock(ib.x, ib.y)(iv.x, iv.y); - } - - size_t nonZeroBlocks() const - { - return ijValue.size(); - } - - int nBlocks; - IDtoBlockValueMap ijValue; -}; - - /*! \class GraphNode * \brief Defines a node/variable that is optimizable in a posegraph * @@ -62,6 +24,7 @@ struct PoseGraphNode int getId() const { return nodeId; } Affine3f getPose() const { return pose; } void setPose(const Affine3f& _pose) { pose = _pose; } + private: int nodeId; Affine3f pose; @@ -77,8 +40,12 @@ struct PoseGraphEdge public: PoseGraphEdge(int _sourceNodeId, int _targetNodeId, const Affine3f& _transformation, const Matx66f& _information = Matx66f::eye()) - : sourceNodeId(_sourceNodeId), targetNodeId(_targetNodeId), transformation(_transformation), information(_information) - {} + : sourceNodeId(_sourceNodeId), + targetNodeId(_targetNodeId), + transformation(_transformation), + information(_information) + { + } virtual ~PoseGraphEdge() = default; int getSourceNodeId() const { return sourceNodeId; } @@ -94,64 +61,89 @@ struct PoseGraphEdge //! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold optimization //! Jose Luis Blanco //! Compactly represents the jacobian of the SE3 generator -static const std::array generatorJacobian = -{ //alpha +// clang-format off +static const std::array generatorJacobian = { // alpha Affine3f(Matx44f(0, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0)), - //beta + // beta Affine3f(Matx44f( 0, 0, 1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0)), - //gamma + // gamma Affine3f(Matx44f(0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)), - //x + // x Affine3f(Matx44f(0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)), - //y + // y Affine3f(Matx44f(0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0)), - //z + // z Affine3f(Matx44f(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0)) }; +// clang-format on class PoseGraph { - public: - typedef std::vector NodeVector; - typedef std::vector EdgeVector; + public: + typedef std::vector NodeVector; + typedef std::vector EdgeVector; - PoseGraph() {}; - virtual ~PoseGraph() = default; + explicit PoseGraph(){}; + virtual ~PoseGraph() = default; - void addNode(const PoseGraphNode& node); - void addEdge(const PoseGraphEdge& edge); + //! PoseGraph can be copied/cloned + PoseGraph(const PoseGraph& _poseGraph) = default; + PoseGraph& operator=(const PoseGraph& _poseGraph) = default; - bool nodeExists(int nodeId); + void addNode(const PoseGraphNode& node) { nodes.push_back(node); } + void addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } - bool isValid(); - static cv::Ptr updatePoseGraph(const PoseGraph& poseGraphPrevious, const Mat& delta) - static void optimize(PoseGraph& poseGraph, int numIters = 10, float min_residual = 1e-6); + bool nodeExists(int nodeId) const + { + return std::find_if(nodes.begin(), nodes.end(), + [nodeId](const PoseGraphNode& currNode) { return currNode.getId() == nodeId; }) != nodes.end(); + } - private: - //! @brief: Constructs a linear system and returns the residual of the current system - float createLinearSystem(BlockSparseMat& H, Mat& B); + bool isValid() const; + + PoseGraph update(const Mat& delta); + private: + //! @brief: Constructs a linear system and returns the residual of the current system + float createLinearSystem(BlockSparseMat& H, Mat& B); - NodeVector nodes; - EdgeVector edges; + NodeVector nodes; + EdgeVector edges; }; // namespace large_kinfu -} // namespace cv + +namespace Optimizer +{ + struct Params + { + int maxNumIters; + float minResidual; + float maxAcceptableResIncre; + + //TODO: Refine these constants + Params(): maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-3f) {}; + virtual ~Params() = default; + }; + + void optimizeGaussNewton(const Params& params, PoseGraph& poseGraph); + void optimizeLevenberg(const Params& params, PoseGraph& poseGraph); +} +} // namespace kinfu #endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */ diff --git a/modules/rgbd/src/sparse_block_matrix.h b/modules/rgbd/src/sparse_block_matrix.h new file mode 100644 index 00000000000..f180ec54971 --- /dev/null +++ b/modules/rgbd/src/sparse_block_matrix.h @@ -0,0 +1,137 @@ +// 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 + +#include "opencv2/core/types.hpp" + +#if defined(HAVE_EIGEN) +#include "opencv2/core/eigen.hpp" +#include +#include +#endif + +namespace cv +{ +namespace kinfu +{ +/*! + * \class BlockSparseMat + * Naive implementation of Sparse Block Matrix + */ +template +struct BlockSparseMat +{ + typedef std::unordered_map> IDtoBlockValueMap; + static constexpr int blockSize = blockM * blockN; + BlockSparseMat(int _nBlocks) : nBlocks(_nBlocks), ijValue() {} + + Matx66f& refBlock(int i, int j) + { + Point2i p(i, j); + auto it = ijValue.find(p); + if (it == ijValue.end()) + { + it = ijValue.insert({ p, Matx<_Tp, blockM, blockN>() }).first; + } + return it->second; + } + + float& refElem(int i, int j) + { + Point2i ib(i / blockSize, j / blockSize), iv(i % blockSize, j % blockSize); + return refBlock(ib.x, ib.y)(iv.x, iv.y); + } + + size_t nonZeroBlocks() const { return ijValue.size(); } + + int nBlocks; + IDtoBlockValueMap ijValue; +}; + +//! Function to solve a sparse linear system of equations HX = B +//! Requires Eigen +static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& X) +{ + const float matValThreshold = 0.001f; + bool result = false; + +#if defined(HAVE_EIGEN) + std::cout << "starting eigen-insertion..." << std::endl; + + std::vector> tripletList; + tripletList.reserve(H.ijValue.size() * H.blockSize * H.blockSize); + for (auto ijValue : H.ijValue) + { + int xb = ijValue.first.x, yb = ijValue.first.y; + Matx66f vblock = ijValue.second; + for (int i = 0; i < H.blockSize; i++) + { + for (int j = 0; j < H.blockSize; j++) + { + float val = vblock(i, j); + if (abs(val) >= matValThreshold) + { + tripletList.push_back(Eigen::Triplet(H.blockSize * xb + i, H.blockSize * yb + j, val)); + } + } + } + } + + Eigen::SparseMatrix bigA(H.blockSize * H.nBlocks, H.blockSize * H.nBlocks); + bigA.setFromTriplets(tripletList.begin(), tripletList.end()); + bigA.makeCompressed(); + + Eigen::VectorXf bigB; + cv2eigen(B, bigB); + + //!TODO: Check if this is required + if (!bigA.isApprox(bigA.transpose()) + { + CV_Error(Error::StsBadArg, "Sparse Matrix is not symmetric"); + return result; + } + + + // TODO: try this, LLT and Cholesky + // Eigen::SimplicialLDLT> solver; + Eigen::SimplicialCholesky, Eigen::NaturalOrdering> solver; + + std::cout << "starting eigen-compute..." << std::endl; + solver.compute(bigA); + + if (solver.info() != Eigen::Success) + { + std::cout << "failed to eigen-decompose" << std::endl; + result = false; + } + else + { + std::cout << "starting eigen-solve..." << std::endl; + + Eigen::VectorXf sx = solver.solve(bigB); + if (solver.info() != Eigen::Success) + { + std::cout << "failed to eigen-solve" << std::endl; + result = false; + } + else + { + x.resize(jtb.size); + eigen2cv(sx, x); + result = true; + } + } + +#else + std::cout << "no eigen library" << std::endl; + + CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); +#endif + + return result; +} + +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/submap.cpp b/modules/rgbd/src/submap.cpp index 51c787e72fd..72354a24de5 100644 --- a/modules/rgbd/src/submap.cpp +++ b/modules/rgbd/src/submap.cpp @@ -2,29 +2,21 @@ // 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 "submap.hpp" - #include "hash_tsdf.hpp" -#include "opencv2/core/cvstd_wrapper.hpp" -#include "pose_graph.h" -#include "precomp.hpp" + namespace cv { namespace kinfu { -Submap::Submap(SubmapId _submapId, const VolumeParams& volumeParams, const cv::Affine3f& _pose) - : submapId(_submapId), pose(_pose) -{ - volume = cv::makePtr(volumeParams); - std::cout << "Created volume\n"; -} SubmapManager::SubmapManager(const VolumeParams& _volumeParams) : volumeParams(_volumeParams) {} -SubmapId SubmapManager::createNewSubmap(bool isCurrentActiveMap, const Affine3f& pose) +int SubmapManager::createNewSubmap(bool isCurrentActiveMap, const Affine3f& pose) { - SubmapId newSubmapId = submaps.size(); - cv::Ptr prevSubmap = getCurrentSubmap(); + int newSubmapId = int(submaps.size()); + const Submap& prevSubmap = getCurrentSubmap(); cv::Ptr newSubmap = cv::makePtr(newSubmapId, volumeParams, pose); @@ -43,13 +35,13 @@ SubmapId SubmapManager::createNewSubmap(bool isCurrentActiveMap, const Affine3f& return idx; } -void SubmapManager::addPriorConstraint(SubmapId currId, const Affine3f& currPose) +void SubmapManager::addPriorConstraint(int currId, const Affine3f& currPose) { poseGraph.addNode(currId, cv::makePtr(currPose)); poseGraph.addEdge } -void SubmapManager::addCameraCameraConstraint(SubmapId prevId, SubmapId currId, const Affine3f& prevPose, +void SubmapManager::addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, const Affine3f& currPose) { //! 1. Add new posegraph node @@ -67,22 +59,12 @@ void SubmapManager::addCameraCameraConstraint(SubmapId prevId, SubmapId currId, poseGraph.addEdge(cv::makePtr(prevId, currId, Tprev2curr, information)); } -cv::Ptr SubmapManager::getCurrentSubmap(void) +Submap SubmapManager::getCurrentSubmap(void) { if (submaps.size() > 0) return submaps.at(submaps.size() - 1); else - return nullptr; -} -void SubmapManager::reset() -{ - //! Technically should delete all the submaps; - for (const auto& submap : submaps) - { - submap->volume->reset(); - } - submaps.clear(); } bool SubmapManager::shouldCreateSubmap(int currFrameId) diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 2ac9d8a290a..a665809bf9f 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -2,8 +2,8 @@ // 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_RGBD_SUBMAP_H__ -#define __OPENCV_RGBD_SUBMAP_H__ +#ifndef __OPENCV_RGBD_SUBMAP_HPP__ +#define __OPENCV_RGBD_SUBMAP_HPP__ #include @@ -11,41 +11,47 @@ #include #include "hash_tsdf.hpp" -#include "pose_graph.h" +#include "pose_graph.hpp" namespace cv { namespace kinfu { -typedef uint16_t SubmapId; -typedef uint16_t FrameId; - -//! T is either HashTSDFVolumeCPU or HashTSDFVolumeGPU class Submap { public: - Submap(SubmapId _submapId, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity()); + Submap(int _submapId, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity()) + : submapId(_submapId), pose(_pose), volume(volumeParams) + { + std::cout << "Created volume\n"; + } + virtual ~Submap() = default; - virtual size_t getTotalAllocatedBlocks() const { return volume->getTotalVolumeUnits(); }; - virtual size_t getVisibleBlocks(int currFrameId) const { return volume->getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD); }; + virtual size_t getTotalAllocatedBlocks() const { return volume.getTotalVolumeUnits(); }; + virtual size_t getVisibleBlocks(int currFrameId) const + { + return volume.getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD); + }; - virtual void setStartFrameId(FrameId _startFrameId) { startFrameId = _startFrameId; }; - virtual void setStopFrameId(FrameId _stopFrameId) { stopFrameId = _stopFrameId; }; + //! TODO: Possibly useless + virtual void setStartFrameId(int _startFrameId) { startFrameId = _startFrameId; }; + virtual void setStopFrameId(int _stopFrameId) { stopFrameId = _stopFrameId; }; - virtual SubmapId getId() const { return submapId; } + virtual int getId() const { return submapId; } virtual cv::Affine3f getPose() const { return pose; } public: //! TODO: Should we support submaps for regular volumes? static constexpr int FRAME_VISIBILITY_THRESHOLD = 5; - cv::Ptr> volume; + HashTSDFVolumeCPU volume; + private: - const SubmapId submapId; + const int submapId; cv::Affine3f pose; - FrameId startFrameId; - FrameId stopFrameId; + int startFrameId; + int stopFrameId; }; struct Constraint @@ -55,32 +61,38 @@ struct Constraint CURRENT_ACTIVE, ACTIVE, }; - SubmapId idx; + int idx; Type type; }; +/** + * @brief: Manages all the created submaps for a particular scene + */ class SubmapManager { public: SubmapManager(const VolumeParams& _volumeParams); virtual ~SubmapManager() = default; - void reset(); - SubmapId createNewSubmap(bool isCurrentActiveMap, const Affine3f& pose = cv::Affine3f::Identity()); - void removeSubmap(SubmapId _submapId); - size_t getTotalSubmaps(void) const { return submaps.size(); }; - Submap getSubmap(SubmapId _submapId) const; - cv::Ptr getCurrentSubmap(void); + void reset() { submapList.clear(); }; + + //! Adds a new submap/volume into the current list of managed/Active submaps + int createNewSubmap(bool isCurrentActiveMap, const Affine3f& pose = cv::Affine3f::Identity()); + void removeSubmap(int _submapId); + size_t getTotalSubmaps(void) const { return submapList.size(); }; + + Submap getSubmap(int _submapId) const; + Submap getCurrentSubmap(void) const; bool shouldCreateSubmap(int frameId); - void setPose(SubmapId _submapId); + void setPose(int _submapId); protected: + void addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, const Affine3f& currPose); - void addCameraCameraConstraint(SubmapId prevId, SubmapId currId, const Affine3f& prevPose, const Affine3f& currPose); VolumeParams volumeParams; - std::vector> submaps; + std::vector> submapList; std::vector constraints; PoseGraph poseGraph; @@ -88,4 +100,4 @@ class SubmapManager } // namespace kinfu } // namespace cv -#endif /* ifndef __OPENCV_RGBD_SUBMAP_H__ */ +#endif /* ifndef __OPENCV_RGBD_SUBMAP_HPP__ */ From a96cf66123449f1840b6f33249d0bc5103714006 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Sat, 18 Jul 2020 20:39:18 -0400 Subject: [PATCH 16/36] Track all submaps (no posegraph update yet) --- modules/rgbd/samples/large_kinfu_demo.cpp | 2 +- modules/rgbd/src/large_kinfu.cpp | 95 ++++++----- modules/rgbd/src/pose_graph.cpp | 55 +++---- modules/rgbd/src/pose_graph.hpp | 94 ++++++----- ...block_matrix.h => sparse_block_matrix.hpp} | 85 ++++++---- modules/rgbd/src/submap.cpp | 82 ++-------- modules/rgbd/src/submap.hpp | 151 +++++++++++++++--- 7 files changed, 322 insertions(+), 242 deletions(-) rename modules/rgbd/src/{sparse_block_matrix.h => sparse_block_matrix.hpp} (59%) diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp index 8378a120283..735c130b9f4 100644 --- a/modules/rgbd/samples/large_kinfu_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -502,7 +502,7 @@ int main(int argc, char **argv) (int)(getTickFrequency()/(newTime - prevTime))), Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); prevTime = newTime; - + std::cout << "Trying to render image: " << rendered.rows << " "<< rendered.cols << "\n"; imshow("render", rendered); int c = waitKey(1); diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index ddf17bfe81e..42204b486ec 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -9,8 +9,8 @@ #include "hash_tsdf.hpp" #include "kinfu_frame.hpp" #include "precomp.hpp" -#include "tsdf.hpp" #include "submap.hpp" +#include "tsdf.hpp" namespace cv { @@ -98,7 +98,7 @@ Ptr Params::hashTSDFParams(bool isCoarse) p->volumeParams.type = VolumeType::HASHTSDF; p->volumeParams.depthTruncThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); - p->volumeParams.unitResolution = 16; + p->volumeParams.unitResolution = 16; return p; } @@ -131,25 +131,20 @@ class LargeKinfuImpl : public LargeKinfu cv::Ptr icp; //! TODO: Submap manager and Pose graph optimizer - cv::Ptr submapMgr; - cv::Ptr currSubmap; + cv::Ptr> submapMgr; + cv::Ptr> currSubmap; int frameCounter; Affine3f pose; - std::vector pyrPoints; - std::vector pyrNormals; }; template LargeKinfuImpl::LargeKinfuImpl(const Params& _params) - : params(_params), - currSubmap(nullptr), - pyrPoints(), - pyrNormals() + : params(_params), currSubmap(nullptr) { - icp = makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh); + icp = makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh); - submapMgr = cv::makePtr(params.volumeParams); + submapMgr = cv::makePtr>(params.volumeParams); reset(); } @@ -232,39 +227,50 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) // use depth instead of distance submapMgr->createNewSubmap(true); currSubmap = submapMgr->getCurrentSubmap(); - currSubmap->volume->integrate(depth, params.depthFactor, pose, params.intr, frameCounter); - pyrPoints = newPoints; - pyrNormals = newNormals; - + currSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); + std::cout << "Integrated 1st frame" << std::endl; + currSubmap->pyrPoints = newPoints; + currSubmap->pyrNormals = newNormals; } else { - currSubmap = submapMgr->getCurrentSubmap(); - Affine3f affine; - bool success = icp->estimateTransform(affine, pyrPoints, pyrNormals, newPoints, newNormals); - if (!success) - return false; - - pose = pose * affine; - - float rnorm = (float)cv::norm(affine.rvec()); - float tnorm = (float)cv::norm(affine.translation()); - // We do not integrate volume if camera does not move - if ((rnorm + tnorm) / 2 >= params.tsdf_min_camera_movement) + bool trackingSuccess = false; + for (int i = 0; i < int(submapMgr->getTotalSubmaps()); i++) { - // use depth instead of distance - currSubmap->volume->integrate(depth, params.depthFactor, pose, params.intr, frameCounter); + cv::Ptr> currTrackingSubmap = submapMgr->getSubmap(i); + Affine3f affine; + + trackingSuccess = + icp->estimateTransform(affine, currTrackingSubmap->pyrPoints, currTrackingSubmap->pyrNormals, newPoints, newNormals); + if (trackingSuccess) + { + //! Compose current pose and add to camera trajectory + currTrackingSubmap->updateCameraPose(affine); + } + + if (currTrackingSubmap->getType() == Submap::Type::CURRENT_ACTIVE) + { + float rnorm = (float)cv::norm(affine.rvec()); + float tnorm = (float)cv::norm(affine.translation()); + // We do not integrate volume if camera does not move + if ((rnorm + tnorm) / 2 >= params.tsdf_min_camera_movement) + currTrackingSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); + } + currTrackingSubmap->raycast(currTrackingSubmap->getCurrentCameraPose(), params.intr, params.frameSize, currTrackingSubmap->pyrPoints[0], currTrackingSubmap->pyrNormals[0]); + + currTrackingSubmap->updatePyrPointsNormals(params.pyramidLevels); + + std::cout << "Submap: " << i << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks() << "\n"; + std::cout << "Submap: " << i << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter) << "\n"; } - MatType& points = pyrPoints[0]; - MatType& normals = pyrNormals[0]; - currSubmap->volume->raycast(pose, params.intr, params.frameSize, points, normals); - buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, params.pyramidLevels); - - std::cout << "Total allocated blocks: " << currSubmap->getTotalAllocatedBlocks() << "\n"; - std::cout << "Visible blocks: " << currSubmap->getVisibleBlocks(frameCounter) << "\n"; - if(submapMgr->shouldCreateSubmap(frameCounter)) + + if (submapMgr->shouldCreateSubmap(frameCounter)) { - int newIdx = submapMgr->createNewSubmap(true, pose, newPoints, newNormals); + submapMgr->createNewSubmap(true, frameCounter, pose); + currSubmap = submapMgr->getCurrentSubmap(); + currSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); + currSubmap->pyrPoints = newPoints; + currSubmap->pyrNormals = newNormals; } std::cout << "Number of submaps: " << submapMgr->getTotalSubmaps() << "\n"; } @@ -283,12 +289,13 @@ void LargeKinfuImpl::render(OutputArray image, const Matx44f& _cameraPo if ((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) || (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) { - renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose); + //! TODO: Can render be dependent on current submap + renderPointsNormals(currSubmap->pyrPoints[0], currSubmap->pyrNormals[0], image, params.lightPose); } else { MatType points, normals; - currSubmap->volume->raycast(cameraPose, params.intr, params.frameSize, points, normals); + currSubmap->raycast(cameraPose, params.intr, params.frameSize, points, normals); renderPointsNormals(points, normals, image, params.lightPose); } } @@ -296,19 +303,19 @@ void LargeKinfuImpl::render(OutputArray image, const Matx44f& _cameraPo template void LargeKinfuImpl::getCloud(OutputArray p, OutputArray n) const { - currSubmap->volume->fetchPointsNormals(p, n); + currSubmap->volume.fetchPointsNormals(p, n); } template void LargeKinfuImpl::getPoints(OutputArray points) const { - currSubmap->volume->fetchPointsNormals(points, noArray()); + currSubmap->volume.fetchPointsNormals(points, noArray()); } template void LargeKinfuImpl::getNormals(InputArray points, OutputArray normals) const { - currSubmap->volume->fetchNormals(points, normals); + currSubmap->volume.fetchNormals(points, normals); } // importing class diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index d0b51b533ca..c292f636c62 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -2,7 +2,7 @@ // 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 "pose_graph.h" +#include "pose_graph.hpp" #include #include @@ -15,10 +15,11 @@ namespace cv { namespace kinfu { -bool PoseGraph::isValid() + +bool PoseGraph::isValid() const { - size_t numNodes = nodes.size(); - size_t numEdges = edges.size(); + int numNodes = int(nodes.size()); + int numEdges = int(edges.size()); if (numNodes <= 0 || numEdges <= 0) return false; @@ -26,8 +27,8 @@ bool PoseGraph::isValid() std::unordered_set nodeSet; std::vector nodeList; - nodeList.push_back(nodes.at(0)->getId()); - nodeSet.insert(nodes.at(0)->getId()); + nodeList.push_back(nodes.at(0).getId()); + nodeSet.insert(nodes.at(0).getId()); bool isGraphConnected = false; if (!nodeList.empty()) @@ -35,9 +36,9 @@ bool PoseGraph::isValid() int currNodeId = nodeList.back(); nodeList.pop_back(); - for (size_t i = 0; i < numEdges; i++) + for (int i = 0; i < numEdges; i++) { - const PoseGraphEdge& potentialEdge = *edges.at(i); + const PoseGraphEdge& potentialEdge = edges.at(i); int nextNodeId = -1; if (potentialEdge.getSourceNodeId() == currNodeId) @@ -56,12 +57,12 @@ bool PoseGraph::isValid() } } - isGraphConnected = (nodeSet.size() == numNodes); + isGraphConnected = (int(nodeSet.size()) == numNodes); bool invalidEdgeNode = false; - for (size_t i = 0; i < numEdges; i++) + for (int i = 0; i < numEdges; i++) { - const PoseGraphEdge& edge = *edges.at(i); + const PoseGraphEdge& edge = edges.at(i); if ((nodeSet.count(edge.getSourceNodeId()) != 1) || (nodeSet.count(edge.getTargetNodeId()) != 1)) { invalidEdgeNode = true; @@ -70,14 +71,11 @@ bool PoseGraph::isValid() } return isGraphConnected && !invalidEdgeNode; } + return false; } -//! TODO: Reference: Open3D and linearizeOplus in g2o -void calcErrorJacobian() {} - float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) { - int numNodes = int(nodes.size()); int numEdges = int(edges.size()); float residual = 0; @@ -89,14 +87,14 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) const Affine3f targetPose_inv = targetPose.inv(); for (int i = 0; i < 6; i++) { - Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * generatorJacobian[i] * sourcePose; + Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(generatorJacobian[i]) * sourcePose; Vec3f rot = dError_dSource.rvec(); Vec3f trans = dError_dSource.translation(); jacSource.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); } for (int i = 0; i < 6; i++) { - Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * -generatorJacobian[i] * sourcePose; + Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(-generatorJacobian[i]) * sourcePose; Vec3f rot = dError_dSource.rvec(); Vec3f trans = dError_dSource.translation(); jacTarget.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); @@ -109,8 +107,8 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) const PoseGraphEdge& currEdge = edges.at(currEdgeNum); int sourceNodeId = currEdge.getSourceNodeId(); int targetNodeId = currEdge.getTargetNodeId(); - const Affine3f& sourcePose = nodes.at(sourceNodeId)->getPose(); - const Affine3f& targetPose = nodes.at(targetNodeId)->getPose(); + const Affine3f& sourcePose = nodes.at(sourceNodeId).getPose(); + const Affine3f& targetPose = nodes.at(targetNodeId).getPose(); Affine3f relativeSourceToTarget = targetPose.inv() * sourcePose; //! Edge transformation is source to target. (relativeConstraint should be identity if no error) @@ -128,7 +126,7 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) error[5] = trans[2]; Matx66f jacSource, jacTarget; - calcErrorJacobian(currEdge.transformation, sourcePose, targetPose, jacSource, jacTarget); + calcErrorJacobian(jacSource, jacTarget, currEdge.transformation, sourcePose, targetPose); Matx16f errorTransposeInfo = error.t() * currEdge.information; Matx66f jacSourceTransposeInfo = jacSource.t() * currEdge.information; @@ -151,7 +149,6 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) PoseGraph PoseGraph::update(const Mat& deltaVec) { int numNodes = int(nodes.size()); - int numEdges = int(edges.size()); //! Check: This should create a copy of the posegraph PoseGraph updatedPoseGraph(*this); @@ -159,11 +156,11 @@ PoseGraph PoseGraph::update(const Mat& deltaVec) for (int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) { Vec6f delta = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)); - Affine3f pose = nodes.at(currentNodeId)->getPose(); + Affine3f pose = nodes.at(currentNodeId).getPose(); Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3)); Affine3f updatedPose = currDelta * pose; - updatedPoseGraph.nodes.at(currentNodeId)->setPose(updatedPose); + updatedPoseGraph.nodes.at(currentNodeId).setPose(updatedPose); } return updatedPoseGraph; @@ -181,24 +178,24 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& return; } - int numNodes = int(poseGraph.nodes.size()); - int numEdges = int(poseGraph.edges.size()); + int numNodes = poseGraph.getNumNodes(); + int numEdges = poseGraph.getNumEdges(); std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges << std::endl; - //! ApproxH = Approximate Hessian = J^T * information * J int iter = 0; //! converged is set to true when error/residual is within some limit bool converged = false; float prevResidual = std::numeric_limits::max(); do { + //! ApproxH = Approximate Hessian = J^T * information * J BlockSparseMat ApproxH(numNodes); - Mat B(6 * numNodes, 1, float); + Mat B(6 * numNodes, 1, CV_32F); float currentResidual = poseGraph.createLinearSystem(ApproxH, B); - Mat delta(6 * numNodes, 1, float); + Mat delta(6 * numNodes, 1, CV_32F); bool success = sparseSolve(ApproxH, B, delta); if (!success) { @@ -218,7 +215,7 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& prevResidual = currentResidual; - } while (iter < params.maxNumIters && !converged) + } while (iter < params.maxNumIters && !converged); } } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 15343734492..57033741194 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -5,7 +5,7 @@ #include #include "opencv2/core/affine.hpp" -#include "sparse_block_matrix.h" +#include "sparse_block_matrix.hpp" namespace cv { namespace kinfu @@ -18,7 +18,7 @@ namespace kinfu struct PoseGraphNode { public: - explicit PoseGraphNode(int nodeId, const Affine3f& _pose) : pose(_pose) {} + explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) : nodeId(_nodeId), pose(_pose) {} virtual ~PoseGraphNode(); int getId() const { return nodeId; } @@ -62,36 +62,36 @@ struct PoseGraphEdge //! Jose Luis Blanco //! Compactly represents the jacobian of the SE3 generator // clang-format off -static const std::array generatorJacobian = { // alpha - Affine3f(Matx44f(0, 0, 0, 0, - 0, 0, -1, 0, - 0, 1, 0, 0, - 0, 0, 0, 0)), +static const std::array generatorJacobian = { // alpha + Matx44f(0, 0, 0, 0, + 0, 0, -1, 0, + 0, 1, 0, 0, + 0, 0, 0, 0), // beta - Affine3f(Matx44f( 0, 0, 1, 0, - 0, 0, 0, 0, - -1, 0, 0, 0, - 0, 0, 0, 0)), + Matx44f( 0, 0, 1, 0, + 0, 0, 0, 0, + -1, 0, 0, 0, + 0, 0, 0, 0), // gamma - Affine3f(Matx44f(0, -1, 0, 0, - 1, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0)), + Matx44f(0, -1, 0, 0, + 1, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0), // x - Affine3f(Matx44f(0, 0, 0, 1, - 0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0)), + Matx44f(0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0), // y - Affine3f(Matx44f(0, 0, 0, 0, - 0, 0, 0, 1, - 0, 0, 0, 0, - 0, 0, 0, 0)), + Matx44f(0, 0, 0, 0, + 0, 0, 0, 1, + 0, 0, 0, 0, + 0, 0, 0, 0), // z - Affine3f(Matx44f(0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 1, - 0, 0, 0, 0)) + Matx44f(0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 1, + 0, 0, 0, 0) }; // clang-format on @@ -120,30 +120,36 @@ class PoseGraph bool isValid() const; PoseGraph update(const Mat& delta); - private: + + int getNumNodes() const { return nodes.size(); } + int getNumEdges() const { return edges.size(); } + //! @brief: Constructs a linear system and returns the residual of the current system float createLinearSystem(BlockSparseMat& H, Mat& B); + private: + NodeVector nodes; EdgeVector edges; - -}; // namespace large_kinfu +}; namespace Optimizer { - struct Params - { - int maxNumIters; - float minResidual; - float maxAcceptableResIncre; - - //TODO: Refine these constants - Params(): maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-3f) {}; - virtual ~Params() = default; - }; - - void optimizeGaussNewton(const Params& params, PoseGraph& poseGraph); - void optimizeLevenberg(const Params& params, PoseGraph& poseGraph); -} +struct Params +{ + int maxNumIters; + float minResidual; + float maxAcceptableResIncre; + + // TODO: Refine these constants + Params() : maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-3f){}; + virtual ~Params() = default; +}; + +void optimizeGaussNewton(const Params& params, PoseGraph& poseGraph); +void optimizeLevenberg(const Params& params, PoseGraph& poseGraph); +} // namespace Optimizer + } // namespace kinfu +} // namespace cv #endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */ diff --git a/modules/rgbd/src/sparse_block_matrix.h b/modules/rgbd/src/sparse_block_matrix.hpp similarity index 59% rename from modules/rgbd/src/sparse_block_matrix.h rename to modules/rgbd/src/sparse_block_matrix.hpp index f180ec54971..dcc347dd67a 100644 --- a/modules/rgbd/src/sparse_block_matrix.h +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -2,11 +2,13 @@ // 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 #include #include "opencv2/core/types.hpp" #if defined(HAVE_EIGEN) +#include #include "opencv2/core/eigen.hpp" #include #include @@ -23,7 +25,19 @@ namespace kinfu template struct BlockSparseMat { - typedef std::unordered_map> IDtoBlockValueMap; + + struct Point2iHash + { + size_t operator()(const cv::Point2i& point) const noexcept + { + size_t seed = 0; + constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; + seed ^= std::hash()(point.x) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + seed ^= std::hash()(point.y) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); + return seed; + } + }; + typedef std::unordered_map, Point2iHash> IDtoBlockValueMap; static constexpr int blockSize = blockM * blockN; BlockSparseMat(int _nBlocks) : nBlocks(_nBlocks), ijValue() {} @@ -44,8 +58,38 @@ struct BlockSparseMat return refBlock(ib.x, ib.y)(iv.x, iv.y); } +#if defined (HAVE_EIGEN) + Eigen::SparseMatrix<_Tp> toEigen() const + { + std::vector> tripletList; + tripletList.reserve(ijValue.size() * blockSize * blockSize); + + for (auto ijv : ijValue) + { + int xb = ijv.first.x, yb = ijv.first.y; + Matx66f vblock = ijv.second; + for (int i = 0; i < blockSize; i++) + { + for (int j = 0; j < blockSize; j++) + { + float val = vblock(i, j); + if (abs(val) >= NON_ZERO_VAL_THRESHOLD) + { + tripletList.push_back(Eigen::Triplet(blockSize * xb + i, blockSize * yb + j, val)); + } + } + } + } + Eigen::SparseMatrix<_Tp> EigenMat(blockSize * nBlocks, blockSize * nBlocks); + EigenMat.setFromTriplets(tripletList.begin(), tripletList.end()); + EigenMat.makeCompressed(); + + return EigenMat; + } +#endif size_t nonZeroBlocks() const { return ijValue.size(); } + static constexpr float NON_ZERO_VAL_THRESHOLD = 0.0001f; int nBlocks; IDtoBlockValueMap ijValue; }; @@ -54,40 +98,18 @@ struct BlockSparseMat //! Requires Eigen static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& X) { - const float matValThreshold = 0.001f; bool result = false; #if defined(HAVE_EIGEN) std::cout << "starting eigen-insertion..." << std::endl; - std::vector> tripletList; - tripletList.reserve(H.ijValue.size() * H.blockSize * H.blockSize); - for (auto ijValue : H.ijValue) - { - int xb = ijValue.first.x, yb = ijValue.first.y; - Matx66f vblock = ijValue.second; - for (int i = 0; i < H.blockSize; i++) - { - for (int j = 0; j < H.blockSize; j++) - { - float val = vblock(i, j); - if (abs(val) >= matValThreshold) - { - tripletList.push_back(Eigen::Triplet(H.blockSize * xb + i, H.blockSize * yb + j, val)); - } - } - } - } - - Eigen::SparseMatrix bigA(H.blockSize * H.nBlocks, H.blockSize * H.nBlocks); - bigA.setFromTriplets(tripletList.begin(), tripletList.end()); - bigA.makeCompressed(); - + Eigen::SparseMatrix bigA = H.toEigen(); Eigen::VectorXf bigB; cv2eigen(B, bigB); //!TODO: Check if this is required - if (!bigA.isApprox(bigA.transpose()) + Eigen::SparseMatrix bigATranspose = bigA.transpose(); + if (!bigA.isApprox(bigATranspose)) { CV_Error(Error::StsBadArg, "Sparse Matrix is not symmetric"); return result; @@ -95,8 +117,7 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& // TODO: try this, LLT and Cholesky - // Eigen::SimplicialLDLT> solver; - Eigen::SimplicialCholesky, Eigen::NaturalOrdering> solver; + Eigen::SimplicialLDLT> solver; std::cout << "starting eigen-compute..." << std::endl; solver.compute(bigA); @@ -110,7 +131,7 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& { std::cout << "starting eigen-solve..." << std::endl; - Eigen::VectorXf sx = solver.solve(bigB); + Eigen::VectorXf solutionX = solver.solve(bigB); if (solver.info() != Eigen::Success) { std::cout << "failed to eigen-solve" << std::endl; @@ -118,18 +139,14 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& } else { - x.resize(jtb.size); - eigen2cv(sx, x); + eigen2cv(solutionX, X); result = true; } } - #else std::cout << "no eigen library" << std::endl; - CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented"); #endif - return result; } diff --git a/modules/rgbd/src/submap.cpp b/modules/rgbd/src/submap.cpp index 72354a24de5..83c2fd654d4 100644 --- a/modules/rgbd/src/submap.cpp +++ b/modules/rgbd/src/submap.cpp @@ -2,83 +2,35 @@ // 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 "submap.hpp" + #include "hash_tsdf.hpp" +#include "kinfu_frame.hpp" +#include "precomp.hpp" namespace cv { namespace kinfu { -SubmapManager::SubmapManager(const VolumeParams& _volumeParams) : volumeParams(_volumeParams) {} - -int SubmapManager::createNewSubmap(bool isCurrentActiveMap, const Affine3f& pose) -{ - int newSubmapId = int(submaps.size()); - const Submap& prevSubmap = getCurrentSubmap(); +/* void SubmapManager::addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, const Affine3f& currPose) + */ +/* { */ +/* //! 1. Add new posegraph node */ +/* //! 2. Add new posegraph constraint */ - cv::Ptr newSubmap = cv::makePtr(newSubmapId, volumeParams, pose); +/* //! TODO: Attempt registration between submaps */ +/* Affine3f Tprev2curr = currPose.inv() * currPose; */ +/* //! Constant information matrix for all odometry constraints */ +/* Matx66f information = Matx66f::eye() * 1000; */ - //! If first submap being added. Add prior constraint to PoseGraph - if (submaps.size() == 0 || !prevSubmap) - { - poseGraph.addNode(newSubmapId, cv::makePtr(pose)); - } - else - { - addCameraCameraConstraint(prevSubmap->getId(), newSubmap->getId(), prevSubmap->getPose(), newSubmap->getPose()); - } - submaps.push_back(newSubmap); - std::cout << "Created new submap\n"; +/* //! Inserts only if the element does not exist already */ +/* poseGraph.addNode(prevId, cv::makePtr(prevPose)); */ +/* poseGraph.addNode(currId, cv::makePtr(currPose)); */ - return idx; -} - -void SubmapManager::addPriorConstraint(int currId, const Affine3f& currPose) -{ - poseGraph.addNode(currId, cv::makePtr(currPose)); - poseGraph.addEdge -} - -void SubmapManager::addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, - const Affine3f& currPose) -{ - //! 1. Add new posegraph node - //! 2. Add new posegraph constraint - - //! TODO: Attempt registration between submaps - Affine3f Tprev2curr = currPose.inv() * currPose; - //! Constant information matrix for all odometry constraints - Matx66f information = Matx66f::eye() * 1000; - - //! Inserts only if the element does not exist already - poseGraph.addNode(prevId, cv::makePtr(prevPose)); - poseGraph.addNode(currId, cv::makePtr(currPose)); - - poseGraph.addEdge(cv::makePtr(prevId, currId, Tprev2curr, information)); -} - -Submap SubmapManager::getCurrentSubmap(void) -{ - if (submaps.size() > 0) - return submaps.at(submaps.size() - 1); - else - -} - -bool SubmapManager::shouldCreateSubmap(int currFrameId) -{ - cv::Ptr curr_submap = getCurrentSubmap(); - int allocate_blocks = curr_submap->getTotalAllocatedBlocks(); - int visible_blocks = curr_submap->getVisibleBlocks(currFrameId); - float ratio = float(visible_blocks) / float(allocate_blocks); - std::cout << "Ratio: " << ratio << "\n"; +/* poseGraph.addEdge(cv::makePtr(prevId, currId, Tprev2curr, information)); */ +/* } */ - if (ratio < 0.2f) - return true; - return false; -} } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index a665809bf9f..3ef22617bf2 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -8,7 +8,6 @@ #include #include -#include #include "hash_tsdf.hpp" #include "pose_graph.hpp" @@ -17,17 +16,43 @@ namespace cv { namespace kinfu { +template class Submap { public: - Submap(int _submapId, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity()) - : submapId(_submapId), pose(_pose), volume(volumeParams) + enum class Type { + ACTIVE = 0, + CURRENT_ACTIVE = 1, + LOOP_CLOSURE = 2, + LOST = 3 + }; + + Submap(int _id, Type _type, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(), int _startFrameId = 0) + : id(_id), type(_type), pose(_pose), volume(volumeParams), startFrameId(_startFrameId) + { + //! First camera pose is identity w.r.t submap pose + cameraTraj.emplace_back(Matx44f::eye()); std::cout << "Created volume\n"; } virtual ~Submap() = default; + virtual void updateCameraPose(const cv::Affine3f& cameraPose) + { + if (cameraTraj.size() == 0) + { + cameraTraj.push_back(cameraPose); + return; + } + cv::Affine3f currPose = cameraTraj.back() * cameraPose; + cameraTraj.push_back(currPose); + } + virtual void integrate(InputArray _depth, float depthFactor, const cv::kinfu::Intr& intrinsics, const int currframeId); + virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, + OutputArray points, OutputArray normals); + virtual void updatePyrPointsNormals(const int pyramidLevels); + virtual size_t getTotalAllocatedBlocks() const { return volume.getTotalVolumeUnits(); }; virtual size_t getVisibleBlocks(int currFrameId) const { @@ -38,66 +63,142 @@ class Submap virtual void setStartFrameId(int _startFrameId) { startFrameId = _startFrameId; }; virtual void setStopFrameId(int _stopFrameId) { stopFrameId = _stopFrameId; }; - virtual int getId() const { return submapId; } + virtual Type getType() const { return type; } + virtual int getId() const { return id; } virtual cv::Affine3f getPose() const { return pose; } + virtual cv::Affine3f getCurrentCameraPose() const + { + return cameraTraj.size() > 0 ? cameraTraj.back() : cv::Affine3f::Identity(); + } public: //! TODO: Should we support submaps for regular volumes? static constexpr int FRAME_VISIBILITY_THRESHOLD = 5; + //! TODO: Add support for GPU arrays (UMat) + std::vector pyrPoints; + std::vector pyrNormals; HashTSDFVolumeCPU volume; private: - const int submapId; + const int id; + Type type; cv::Affine3f pose; + //! Accumulates the camera to submap pose transformations + std::vector cameraTraj; int startFrameId; int stopFrameId; }; -struct Constraint +template +void Submap::integrate(InputArray _depth, float depthFactor, const cv::kinfu::Intr& intrinsics, + const int currFrameId) { - enum Type - { - CURRENT_ACTIVE, - ACTIVE, - }; - int idx; - Type type; -}; + int index = currFrameId - startFrameId; + std::cout << "Current frame ID: " << currFrameId << " startFrameId: " << startFrameId << "\n"; + std::cout << " Index: " << index << " Camera trajectory size: " << cameraTraj.size() << std::endl; + CV_Assert(currFrameId >= startFrameId); + CV_Assert(index >= 0 && index < int(cameraTraj.size())); + + const cv::Affine3f& currPose = cameraTraj.at(index); + volume.integrate(_depth, depthFactor, currPose, intrinsics, currFrameId); +} + +template +void Submap::raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, + OutputArray points, OutputArray normals) +{ + volume.raycast(cameraPose, intrinsics, frameSize, points, normals); +} + +template +void Submap::updatePyrPointsNormals(const int pyramidLevels) +{ + MatType& points = pyrPoints[0]; + MatType& normals = pyrNormals[0]; + + buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, pyramidLevels); +} /** * @brief: Manages all the created submaps for a particular scene */ +template class SubmapManager { public: - SubmapManager(const VolumeParams& _volumeParams); + SubmapManager(const VolumeParams& _volumeParams) + : volumeParams(_volumeParams) + {} virtual ~SubmapManager() = default; void reset() { submapList.clear(); }; + bool shouldCreateSubmap(int frameId); //! Adds a new submap/volume into the current list of managed/Active submaps - int createNewSubmap(bool isCurrentActiveMap, const Affine3f& pose = cv::Affine3f::Identity()); - void removeSubmap(int _submapId); - size_t getTotalSubmaps(void) const { return submapList.size(); }; + int createNewSubmap(bool isCurrentActiveMap, const int currFrameId = 0, const Affine3f& pose = cv::Affine3f::Identity()); - Submap getSubmap(int _submapId) const; - Submap getCurrentSubmap(void) const; + void removeSubmap(int _id); + size_t getTotalSubmaps(void) const { return submapList.size(); }; - bool shouldCreateSubmap(int frameId); + cv::Ptr> getSubmap(int _id) const; + cv::Ptr> getCurrentSubmap(void) const; - void setPose(int _submapId); + void setPose(int _id); protected: - void addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, const Affine3f& currPose); + /* void addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, const Affine3f& currPose); */ VolumeParams volumeParams; - std::vector> submapList; - std::vector constraints; + std::vector>> submapList; PoseGraph poseGraph; }; +template +int SubmapManager::createNewSubmap(bool isCurrentActiveMap, int currFrameId, const Affine3f& pose) +{ + int newId = int(submapList.size()); + typename Submap::Type type = + isCurrentActiveMap ? Submap::Type::CURRENT_ACTIVE : Submap::Type::ACTIVE; + cv::Ptr> newSubmap = cv::makePtr>(newId, type, volumeParams, pose, currFrameId); + + submapList.push_back(newSubmap); + std::cout << "Created new submap\n"; + return newId; +} + +template +cv::Ptr> SubmapManager::getSubmap(int _id) const +{ + CV_Assert(_id >= 0 && _id < int(submapList.size())); + if (submapList.size() > 0) + return submapList.at(_id); + return nullptr; +} + +template +cv::Ptr> SubmapManager::getCurrentSubmap(void) const +{ + if (submapList.size() > 0) + return submapList.back(); + return nullptr; +} + +template +bool SubmapManager::shouldCreateSubmap(int currFrameId) +{ + cv::Ptr> curr_submap = getCurrentSubmap(); + int allocate_blocks = curr_submap->getTotalAllocatedBlocks(); + int visible_blocks = curr_submap->getVisibleBlocks(currFrameId); + float ratio = float(visible_blocks) / float(allocate_blocks); + std::cout << "Ratio: " << ratio << "\n"; + + if (ratio < 0.2f) + return true; + return false; +} + } // namespace kinfu } // namespace cv #endif /* ifndef __OPENCV_RGBD_SUBMAP_HPP__ */ From 02fd31275ddef839ef469805eac5508ac315cf46 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Mon, 27 Jul 2020 23:20:50 -0400 Subject: [PATCH 17/36] Return inliers from ICP for weighting the constraints (Huber threshold based inliers pending) --- modules/rgbd/include/opencv2/rgbd/volume.hpp | 2 +- modules/rgbd/samples/large_kinfu_demo.cpp | 2 +- modules/rgbd/src/fast_icp.cpp | 65 ++++++++++++++------ modules/rgbd/src/fast_icp.hpp | 5 +- modules/rgbd/src/large_kinfu.cpp | 6 +- modules/rgbd/src/pose_graph.cpp | 45 +++++++------- modules/rgbd/src/pose_graph.hpp | 4 +- modules/rgbd/src/sparse_block_matrix.hpp | 9 ++- modules/rgbd/src/submap.hpp | 52 ++++++++++++---- 9 files changed, 131 insertions(+), 59 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 0680eee0af6..91cd2d61190 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -62,7 +62,7 @@ struct VolumeParams Number of voxels in each dimension for volumeUnit Applicable only for hashTSDF. */ - int unitResolution; + int unitResolution = 0; /** @brief Initial pose of the volume in meters */ cv::Affine3f pose; diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp index 735c130b9f4..01624d79d08 100644 --- a/modules/rgbd/samples/large_kinfu_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -396,7 +396,7 @@ int main(int argc, char **argv) ds->updateParams(*params); // Enables OpenCL explicitly (by default can be switched-off) - cv::setUseOptimized(true); + cv::setUseOptimized(false); if(!idle) largeKinfu = LargeKinfu::create(params); diff --git a/modules/rgbd/src/fast_icp.cpp b/modules/rgbd/src/fast_icp.cpp index 06d4a750647..3394035f223 100644 --- a/modules/rgbd/src/fast_icp.cpp +++ b/modules/rgbd/src/fast_icp.cpp @@ -4,6 +4,7 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +#include #include "precomp.hpp" #include "fast_icp.hpp" #include "opencl_kernels_rgbd.hpp" @@ -32,9 +33,12 @@ class ICPImpl : public ICP InputArray oldPoints, InputArray oldNormals, InputArray newPoints, InputArray newNormals ) const override; - + virtual std::tuple estimateTransformInliers(cv::Affine3f& transform, + InputArray oldPoints, InputArray oldNormals, + InputArray newPoints, InputArray newNormals + ) const override; template < typename T > - bool estimateTransformT(cv::Affine3f& transform, + bool estimateTransformT(cv::Affine3f& transform, int& numInliers, const vector& oldPoints, const vector& oldNormals, const vector& newPoints, const vector& newNormals ) const; @@ -42,7 +46,7 @@ class ICPImpl : public ICP virtual ~ICPImpl() { } template < typename T > - void getAb(const T& oldPts, const T& oldNrm, const T& newPts, const T& newNrm, + int getAb(const T& oldPts, const T& oldNrm, const T& newPts, const T& newNrm, cv::Affine3f pose, int level, cv::Matx66f& A, cv::Vec6f& b) const; private: @@ -57,10 +61,10 @@ ICPImpl::ICPImpl(const Intr _intrinsics, const std::vector &_iterations, fl { } -bool ICPImpl::estimateTransform(cv::Affine3f& transform, - InputArray _oldPoints, InputArray _oldNormals, - InputArray _newPoints, InputArray _newNormals - ) const +std::tuple ICPImpl::estimateTransformInliers(cv::Affine3f &transform, + InputArray _oldPoints, InputArray _oldNormals, + InputArray _newPoints, InputArray _newNormals + ) const { CV_TRACE_FUNCTION(); @@ -68,6 +72,8 @@ bool ICPImpl::estimateTransform(cv::Affine3f& transform, CV_Assert(_newPoints.size() == _newNormals.size()); CV_Assert(_oldPoints.size() == _newPoints.size()); + int numInliers; + bool success; #ifdef HAVE_OPENCL if(cv::ocl::isOpenCLActivated() && _oldPoints.isUMatVector() && _oldNormals.isUMatVector() && @@ -78,7 +84,8 @@ bool ICPImpl::estimateTransform(cv::Affine3f& transform, _newPoints.getUMatVector(np); _oldNormals.getUMatVector(on); _newNormals.getUMatVector(nn); - return estimateTransformT(transform, op, on, np, nn); + success = estimateTransformT(transform, numInliers, op, on, np, nn); + return std::make_tuple(success, numInliers); } #endif @@ -87,11 +94,21 @@ bool ICPImpl::estimateTransform(cv::Affine3f& transform, _newPoints.getMatVector(np); _oldNormals.getMatVector(on); _newNormals.getMatVector(nn); - return estimateTransformT(transform, op, on, np, nn); + success = estimateTransformT(transform, numInliers, op, on, np, nn); + return std::make_tuple(success, numInliers); +} + +bool ICPImpl::estimateTransform(cv::Affine3f& transform, + InputArray _oldPoints, InputArray _oldNormals, + InputArray _newPoints, InputArray _newNormals + ) const +{ + auto value = estimateTransformInliers(transform, _oldPoints, _oldNormals, _newPoints, _newNormals); + return std::get<0>(value); } template < typename T > -bool ICPImpl::estimateTransformT(cv::Affine3f& transform, +bool ICPImpl::estimateTransformT(cv::Affine3f& transform, int& numInliers, const vector& oldPoints, const vector& oldNormals, const vector& newPoints, const vector& newNormals ) const @@ -99,6 +116,7 @@ bool ICPImpl::estimateTransformT(cv::Affine3f& transform, CV_TRACE_FUNCTION(); transform = Affine3f::Identity(); + numInliers = 0; // Finally after ICP completion, if ICP succeeds numInliers will contain final iteration inliers for(size_t l = 0; l < iterations.size(); l++) { size_t level = iterations.size() - 1 - l; @@ -111,12 +129,15 @@ bool ICPImpl::estimateTransformT(cv::Affine3f& transform, Matx66f A; Vec6f b; - getAb(oldPts, oldNrm, newPts, newNrm, transform, (int)level, A, b); + numInliers = getAb(oldPts, oldNrm, newPts, newNrm, transform, (int)level, A, b); double det = cv::determinant(A); if (abs (det) < 1e-15 || cvIsNaN(det)) + { + numInliers = 0; return false; + } Vec6f x; // theoretically, any method of solving is applicable @@ -175,17 +196,18 @@ typedef Matx ABtype; struct GetAbInvoker : ParallelLoopBody { - GetAbInvoker(ABtype& _globalAb, Mutex& _mtx, + GetAbInvoker(ABtype& _globalAb, int& _numInliers, Mutex& _mtx, const Points& _oldPts, const Normals& _oldNrm, const Points& _newPts, const Normals& _newNrm, Affine3f _pose, Intr::Projector _proj, float _sqDistanceThresh, float _minCos) : ParallelLoopBody(), - globalSumAb(_globalAb), mtx(_mtx), + globalSumAb(_globalAb), numInliers(_numInliers), mtx(_mtx), oldPts(_oldPts), oldNrm(_oldNrm), newPts(_newPts), newNrm(_newNrm), pose(_pose), proj(_proj), sqDistanceThresh(_sqDistanceThresh), minCos(_minCos) { } virtual void operator ()(const Range& range) const override { + int localNumInliers = 0; #if USE_INTRINSICS CV_Assert(ptype::channels == 4); @@ -298,7 +320,7 @@ struct GetAbInvoker : ParallelLoopBody continue; // build point-wise vector ab = [ A | b ] - + localNumInliers = localNumInliers + 1; v_float32x4 VxNv = crossProduct(newP, oldN); Point3f VxN; VxN.x = VxNv.get0(); @@ -449,7 +471,7 @@ struct GetAbInvoker : ParallelLoopBody //try to optimize Point3f VxN = newP.cross(oldN); float ab[7] = {VxN.x, VxN.y, VxN.z, oldN.x, oldN.y, oldN.z, oldN.dot(-diff)}; - + localNumInliers++; // build point-wise upper-triangle matrix [ab^T * ab] w/o last row // which is [A^T*A | A^T*b] // and gather sum @@ -477,9 +499,11 @@ struct GetAbInvoker : ParallelLoopBody AutoLock al(mtx); globalSumAb += sumAB; + numInliers += localNumInliers; } ABtype& globalSumAb; + int& numInliers; Mutex& mtx; const Points& oldPts; const Normals& oldNrm; @@ -493,7 +517,7 @@ struct GetAbInvoker : ParallelLoopBody template <> -void ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm, +int ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm, cv::Affine3f pose, int level, cv::Matx66f& A, cv::Vec6f& b) const { CV_TRACE_FUNCTION(); @@ -502,10 +526,11 @@ void ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts CV_Assert(newPts.size() == newNrm.size()); ABtype sumAB = ABtype::zeros(); + int numInliers = 0; Mutex mutex; const Points op(oldPts), on(oldNrm); const Normals np(newPts), nn(newNrm); - GetAbInvoker invoker(sumAB, mutex, op, on, np, nn, pose, + GetAbInvoker invoker(sumAB, numInliers, mutex, op, on, np, nn, pose, intrinsics.scale(level).makeProjector(), distanceThreshold*distanceThreshold, cos(angleThreshold)); Range range(0, newPts.rows); @@ -523,6 +548,7 @@ void ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts b(i) = sumAB(i, 6); } + return numInliers; } ///////// GPU implementation ///////// @@ -530,7 +556,7 @@ void ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts #ifdef HAVE_OPENCL template <> -void ICPImpl::getAb(const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm, +int ICPImpl::getAb(const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm, Affine3f pose, int level, Matx66f &A, Vec6f &b) const { CV_TRACE_FUNCTION(); @@ -576,6 +602,7 @@ void ICPImpl::getAb(const UMat& oldPts, const UMat& oldNrm, const UMat& ne Intr::Projector proj = intrinsics.scale(level).makeProjector(); Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); + int numInliers = 0; UMat& groupedSumGpu = groupedSumBuffers[level]; groupedSumGpu.create(Size(ngroups.width*UTSIZE, ngroups.height), CV_32F); @@ -642,6 +669,8 @@ void ICPImpl::getAb(const UMat& oldPts, const UMat& oldNrm, const UMat& ne b(i) = sumAB(i, 6); } + // TODO: Figure out changes for GPU ICP + return numInliers; } #endif diff --git a/modules/rgbd/src/fast_icp.hpp b/modules/rgbd/src/fast_icp.hpp index 7a9f0694096..5f7ca60d976 100644 --- a/modules/rgbd/src/fast_icp.hpp +++ b/modules/rgbd/src/fast_icp.hpp @@ -22,7 +22,10 @@ class ICP InputArray oldPoints, InputArray oldNormals, InputArray newPoints, InputArray newNormals ) const = 0; - + virtual std::tuple estimateTransformInliers(cv::Affine3f& transform, + InputArray oldPoints, InputArray oldNormals, + InputArray newPoints, InputArray newNormals + ) const = 0; virtual ~ICP() { } protected: diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 42204b486ec..13cc91d7d80 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -240,12 +240,14 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) cv::Ptr> currTrackingSubmap = submapMgr->getSubmap(i); Affine3f affine; - trackingSuccess = - icp->estimateTransform(affine, currTrackingSubmap->pyrPoints, currTrackingSubmap->pyrNormals, newPoints, newNormals); + int numInliers = 0; + std::tie(trackingSuccess, numInliers) = + icp->estimateTransformInliers(affine, currTrackingSubmap->pyrPoints, currTrackingSubmap->pyrNormals, newPoints, newNormals); if (trackingSuccess) { //! Compose current pose and add to camera trajectory currTrackingSubmap->updateCameraPose(affine); + std::cout << "Number of inliers: " << numInliers << "\n"; } if (currTrackingSubmap->getType() == Submap::Type::CURRENT_ACTIVE) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index c292f636c62..bcb9a9c38ea 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -24,18 +24,18 @@ bool PoseGraph::isValid() const if (numNodes <= 0 || numEdges <= 0) return false; - std::unordered_set nodeSet; - std::vector nodeList; + std::unordered_set nodesVisited; + std::vector nodesToVisit; - nodeList.push_back(nodes.at(0).getId()); - nodeSet.insert(nodes.at(0).getId()); + nodesToVisit.push_back(nodes.at(0).getId()); bool isGraphConnected = false; - if (!nodeList.empty()) + while (!nodesToVisit.empty()) { - int currNodeId = nodeList.back(); - nodeList.pop_back(); + int currNodeId = nodesToVisit.back(); + nodesToVisit.pop_back(); + // Since each node does not maintain its neighbor list for (int i = 0; i < numEdges; i++) { const PoseGraphEdge& potentialEdge = edges.at(i); @@ -52,26 +52,29 @@ bool PoseGraph::isValid() const if (nextNodeId != -1) { - nodeList.push_back(nextNodeId); - nodeSet.insert(nextNodeId); + if(nodesVisited.count(currNodeId) == 0) + { + nodesVisited.insert(currNodeId); + } + nodesToVisit.push_back(nextNodeId); } } + } - isGraphConnected = (int(nodeSet.size()) == numNodes); + isGraphConnected = (int(nodesVisited.size()) == numNodes); - bool invalidEdgeNode = false; - for (int i = 0; i < numEdges; i++) + bool invalidEdgeNode = false; + for (int i = 0; i < numEdges; i++) + { + const PoseGraphEdge& edge = edges.at(i); + // edges have spurious source/target nodes + if ((nodesVisited.count(edge.getSourceNodeId()) != 1) || (nodesVisited.count(edge.getTargetNodeId()) != 1)) { - const PoseGraphEdge& edge = edges.at(i); - if ((nodeSet.count(edge.getSourceNodeId()) != 1) || (nodeSet.count(edge.getTargetNodeId()) != 1)) - { - invalidEdgeNode = true; - break; - } + invalidEdgeNode = true; + break; } - return isGraphConnected && !invalidEdgeNode; } - return false; + return isGraphConnected && !invalidEdgeNode; } float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) @@ -166,7 +169,7 @@ PoseGraph PoseGraph::update(const Mat& deltaVec) return updatedPoseGraph; } -//! NOTE: We follow the left-composition for the infinitesimal pose update +//! NOTE: We follow left-composition for the infinitesimal pose update void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& poseGraph) { PoseGraph poseGraphOriginal = poseGraph; diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 57033741194..f5aa176566d 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -18,15 +18,17 @@ namespace kinfu struct PoseGraphNode { public: - explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) : nodeId(_nodeId), pose(_pose) {} + explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) : nodeId(_nodeId), isFixed(false), pose(_pose) {} virtual ~PoseGraphNode(); int getId() const { return nodeId; } Affine3f getPose() const { return pose; } void setPose(const Affine3f& _pose) { pose = _pose; } + void setFixed(bool val = true) { isFixed = val; } private: int nodeId; + bool isFixed; Affine3f pose; }; diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index dcc347dd67a..5db7a48b207 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -37,11 +37,14 @@ struct BlockSparseMat return seed; } }; - typedef std::unordered_map, Point2iHash> IDtoBlockValueMap; + typedef Matx<_Tp, blockM, blockN> MatType; + typedef std::unordered_map IDtoBlockValueMap; static constexpr int blockSize = blockM * blockN; + + BlockSparseMat(int _nBlocks) : nBlocks(_nBlocks), ijValue() {} - Matx66f& refBlock(int i, int j) + MatType& refBlock(int i, int j) { Point2i p(i, j); auto it = ijValue.find(p); @@ -67,7 +70,7 @@ struct BlockSparseMat for (auto ijv : ijValue) { int xb = ijv.first.x, yb = ijv.first.y; - Matx66f vblock = ijv.second; + MatType vblock = ijv.second; for (int i = 0; i < blockSize; i++) { for (int j = 0; j < blockSize; j++) diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 3ef22617bf2..02ca652c920 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -29,7 +29,7 @@ class Submap }; Submap(int _id, Type _type, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(), int _startFrameId = 0) - : id(_id), type(_type), pose(_pose), volume(volumeParams), startFrameId(_startFrameId) + : id(_id), type(_type), pose(_pose), startFrameId(_startFrameId), volume(volumeParams) { //! First camera pose is identity w.r.t submap pose cameraTraj.emplace_back(Matx44f::eye()); @@ -71,13 +71,6 @@ class Submap return cameraTraj.size() > 0 ? cameraTraj.back() : cv::Affine3f::Identity(); } - public: - //! TODO: Should we support submaps for regular volumes? - static constexpr int FRAME_VISIBILITY_THRESHOLD = 5; - //! TODO: Add support for GPU arrays (UMat) - std::vector pyrPoints; - std::vector pyrNormals; - HashTSDFVolumeCPU volume; private: const int id; @@ -88,6 +81,14 @@ class Submap int startFrameId; int stopFrameId; + + public: + //! TODO: Should we support submaps for regular volumes? + static constexpr int FRAME_VISIBILITY_THRESHOLD = 5; + //! TODO: Add support for GPU arrays (UMat) + std::vector pyrPoints; + std::vector pyrNormals; + HashTSDFVolumeCPU volume; }; template @@ -146,6 +147,8 @@ class SubmapManager void setPose(int _id); + /* void updatePoseGraph(); */ + protected: /* void addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, const Affine3f& currPose); */ @@ -188,9 +191,9 @@ cv::Ptr> SubmapManager::getCurrentSubmap(void) const template bool SubmapManager::shouldCreateSubmap(int currFrameId) { - cv::Ptr> curr_submap = getCurrentSubmap(); - int allocate_blocks = curr_submap->getTotalAllocatedBlocks(); - int visible_blocks = curr_submap->getVisibleBlocks(currFrameId); + cv::Ptr> currSubmap = getCurrentSubmap(); + int allocate_blocks = currSubmap->getTotalAllocatedBlocks(); + int visible_blocks = currSubmap->getVisibleBlocks(currFrameId); float ratio = float(visible_blocks) / float(allocate_blocks); std::cout << "Ratio: " << ratio << "\n"; @@ -199,6 +202,33 @@ bool SubmapManager::shouldCreateSubmap(int currFrameId) return false; } +/* template */ +/* void SubmapManager::constructPoseGraph() */ +/* { */ +/* for(int i = 0; i < submapList.size(); i++) */ +/* { */ +/* cv::Ptr> currSubmap = submapList.at(i); */ +/* PoseGraphNode node(currSubmap->getId(), currSubmap->getPose()); */ + +/* if(currSubmap->getId() == 0) */ +/* node.setFixed(); */ + +/* poseGraph.addNode(node); */ +/* } */ + +/* //! TODO: How to add edges between pose graphs */ +/* for(int i = 0; i < submapList.size(); i++) */ +/* { */ + +/* } */ +/* } */ + +/* template */ +/* void SubmapManager::updatePoseGraph() */ +/* { */ +/* constructPoseGraph(); */ +/* } */ + } // namespace kinfu } // namespace cv #endif /* ifndef __OPENCV_RGBD_SUBMAP_HPP__ */ From 034970ae7f4da30b1aa03e88ad0a8e6dde248cb5 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Sat, 1 Aug 2020 12:06:32 -0400 Subject: [PATCH 18/36] Add updating constraints between submaps and retain same current map --- modules/rgbd/src/hash_tsdf.cpp | 1 + modules/rgbd/src/large_kinfu.cpp | 97 +++++----- modules/rgbd/src/submap.hpp | 315 ++++++++++++++++++++++--------- 3 files changed, 276 insertions(+), 137 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 4b768713902..fa91c76d317 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -176,6 +176,7 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, const c 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; } } diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 13cc91d7d80..9e296ca64b3 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -132,7 +132,6 @@ class LargeKinfuImpl : public LargeKinfu cv::Ptr icp; //! TODO: Submap manager and Pose graph optimizer cv::Ptr> submapMgr; - cv::Ptr> currSubmap; int frameCounter; Affine3f pose; @@ -140,12 +139,14 @@ class LargeKinfuImpl : public LargeKinfu template LargeKinfuImpl::LargeKinfuImpl(const Params& _params) - : params(_params), currSubmap(nullptr) + : params(_params) { icp = makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh); submapMgr = cv::makePtr>(params.volumeParams); reset(); + submapMgr->createNewSubmap(true); + } template @@ -222,59 +223,53 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) makeFrameFromDepth(depth, newPoints, newNormals, params.intr, params.pyramidLevels, params.depthFactor, params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size, params.truncateThreshold); - if (frameCounter == 0) - { - // use depth instead of distance - submapMgr->createNewSubmap(true); - currSubmap = submapMgr->getCurrentSubmap(); - currSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); - std::cout << "Integrated 1st frame" << std::endl; - currSubmap->pyrPoints = newPoints; - currSubmap->pyrNormals = newNormals; - } - else + + for (const auto& pActiveSubmapPair : submapMgr->activeSubmapList) { - bool trackingSuccess = false; - for (int i = 0; i < int(submapMgr->getTotalSubmaps()); i++) + //! Iterate over map? + int currTrackingId = pActiveSubmapPair.first; + Ptr> currTrackingSubmap = pActiveSubmapPair.second; + Affine3f affine; + std::cout << "Current tracking ID: " << currTrackingId << std::endl; + + if(frameCounter == 0) //! Only one current tracking map { - cv::Ptr> currTrackingSubmap = submapMgr->getSubmap(i); - Affine3f affine; - - int numInliers = 0; - std::tie(trackingSuccess, numInliers) = - icp->estimateTransformInliers(affine, currTrackingSubmap->pyrPoints, currTrackingSubmap->pyrNormals, newPoints, newNormals); - if (trackingSuccess) - { - //! Compose current pose and add to camera trajectory - currTrackingSubmap->updateCameraPose(affine); - std::cout << "Number of inliers: " << numInliers << "\n"; - } - - if (currTrackingSubmap->getType() == Submap::Type::CURRENT_ACTIVE) - { - float rnorm = (float)cv::norm(affine.rvec()); - float tnorm = (float)cv::norm(affine.translation()); - // We do not integrate volume if camera does not move - if ((rnorm + tnorm) / 2 >= params.tsdf_min_camera_movement) - currTrackingSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); - } - currTrackingSubmap->raycast(currTrackingSubmap->getCurrentCameraPose(), params.intr, params.frameSize, currTrackingSubmap->pyrPoints[0], currTrackingSubmap->pyrNormals[0]); - - currTrackingSubmap->updatePyrPointsNormals(params.pyramidLevels); - - std::cout << "Submap: " << i << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks() << "\n"; - std::cout << "Submap: " << i << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter) << "\n"; + currTrackingSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); + currTrackingSubmap->pyrPoints = newPoints; + currTrackingSubmap->pyrNormals = newNormals; + continue; } - if (submapMgr->shouldCreateSubmap(frameCounter)) + //1. Track + int numInliers = 0; + bool trackingSuccess = false; + std::tie(trackingSuccess, numInliers) = + icp->estimateTransformInliers(affine, currTrackingSubmap->pyrPoints, currTrackingSubmap->pyrNormals, newPoints, newNormals); + if (trackingSuccess) { - submapMgr->createNewSubmap(true, frameCounter, pose); - currSubmap = submapMgr->getCurrentSubmap(); - currSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); - currSubmap->pyrPoints = newPoints; - currSubmap->pyrNormals = newNormals; + //! Compose current pose and add to camera trajectory + currTrackingSubmap->composeCameraPose(affine); + std::cout << "Number of inliers: " << numInliers << "\n"; } - std::cout << "Number of submaps: " << submapMgr->getTotalSubmaps() << "\n"; + + //2. Integrate + float rnorm = (float)cv::norm(affine.rvec()); + float tnorm = (float)cv::norm(affine.translation()); + // We do not integrate volume if camera does not move + if ((rnorm + tnorm) / 2 >= params.tsdf_min_camera_movement) + currTrackingSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); + + //3. Raycast + currTrackingSubmap->raycast(currTrackingSubmap->cameraPose, params.intr, params.frameSize, currTrackingSubmap->pyrPoints[0], currTrackingSubmap->pyrNormals[0]); + + currTrackingSubmap->updatePyrPointsNormals(params.pyramidLevels); + + std::cout << "Submap: " << currTrackingId << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks() << "\n"; + std::cout << "Submap: " << currTrackingId << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter) << "\n"; + + //4. Update map + submapMgr->updateMap(frameCounter, newPoints, newNormals); + std::cout << "Number of submaps: " << submapMgr->submapList.size() << "\n"; } frameCounter++; return true; @@ -287,6 +282,7 @@ void LargeKinfuImpl::render(OutputArray image, const Matx44f& _cameraPo Affine3f cameraPose(_cameraPose); + auto currSubmap = submapMgr->getCurrentSubmap(); const Affine3f id = Affine3f::Identity(); if ((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) || (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) @@ -305,18 +301,21 @@ void LargeKinfuImpl::render(OutputArray image, const Matx44f& _cameraPo template void LargeKinfuImpl::getCloud(OutputArray p, OutputArray n) const { + auto currSubmap = submapMgr->getCurrentSubmap(); currSubmap->volume.fetchPointsNormals(p, n); } template void LargeKinfuImpl::getPoints(OutputArray points) const { + auto currSubmap = submapMgr->getCurrentSubmap(); currSubmap->volume.fetchPointsNormals(points, noArray()); } template void LargeKinfuImpl::getNormals(InputArray points, OutputArray normals) const { + auto currSubmap = submapMgr->getCurrentSubmap(); currSubmap->volume.fetchNormals(points, normals); } diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 02ca652c920..cfd75c6fddc 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -8,6 +8,7 @@ #include #include +#include #include "hash_tsdf.hpp" #include "pose_graph.hpp" @@ -22,32 +23,45 @@ class Submap public: enum class Type { - ACTIVE = 0, - CURRENT_ACTIVE = 1, - LOOP_CLOSURE = 2, - LOST = 3 + NEW = 0, + CURRENT = 1, + RELOCALISATION = 2, + LOOP_CLOSURE = 3, + LOST = 4 }; + struct PoseConstraint + { + Affine3f estimatedPose; + std::vector observations; + int weight; + + PoseConstraint() : weight(0){}; - Submap(int _id, Type _type, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(), int _startFrameId = 0) - : id(_id), type(_type), pose(_pose), startFrameId(_startFrameId), volume(volumeParams) + void accumulatePose(const Affine3f& _pose, int _weight = 1) + { + Matx44f accPose = estimatedPose.matrix * weight + _pose.matrix * _weight; + weight += _weight; + estimatedPose = Affine3f(accPose * 1 / float(weight)); + } + void addObservation(const Affine3f& _pose) { observations.push_back(_pose); } + }; + + typedef std::map Constraints; + + Submap(int _id, Type _type, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(), + int _startFrameId = 0) + : id(_id), + type(_type), + pose(_pose), + cameraPose(Affine3f::Identity()), + startFrameId(_startFrameId), + volume(volumeParams) { - //! First camera pose is identity w.r.t submap pose - cameraTraj.emplace_back(Matx44f::eye()); std::cout << "Created volume\n"; } virtual ~Submap() = default; - virtual void updateCameraPose(const cv::Affine3f& cameraPose) - { - if (cameraTraj.size() == 0) - { - cameraTraj.push_back(cameraPose); - return; - } - cv::Affine3f currPose = cameraTraj.back() * cameraPose; - cameraTraj.push_back(currPose); - } virtual void integrate(InputArray _depth, float depthFactor, const cv::kinfu::Intr& intrinsics, const int currframeId); virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, OutputArray points, OutputArray normals); @@ -57,34 +71,32 @@ class Submap virtual size_t getVisibleBlocks(int currFrameId) const { return volume.getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD); - }; + } //! TODO: Possibly useless virtual void setStartFrameId(int _startFrameId) { startFrameId = _startFrameId; }; virtual void setStopFrameId(int _stopFrameId) { stopFrameId = _stopFrameId; }; - virtual Type getType() const { return type; } - virtual int getId() const { return id; } - virtual cv::Affine3f getPose() const { return pose; } - virtual cv::Affine3f getCurrentCameraPose() const + void composeCameraPose(const cv::Affine3f& _relativePose) { cameraPose = cameraPose * _relativePose; } + PoseConstraint& getConstraint(const int _id) { - return cameraTraj.size() > 0 ? cameraTraj.back() : cv::Affine3f::Identity(); + //! Creates constraints if doesn't exist yet + return constraints[_id]; } - - private: + public: const int id; Type type; cv::Affine3f pose; - //! Accumulates the camera to submap pose transformations - std::vector cameraTraj; + cv::Affine3f cameraPose; + Constraints constraints; + int trackingAttempts = 0; int startFrameId; int stopFrameId; - - public: //! TODO: Should we support submaps for regular volumes? static constexpr int FRAME_VISIBILITY_THRESHOLD = 5; + //! TODO: Add support for GPU arrays (UMat) std::vector pyrPoints; std::vector pyrNormals; @@ -95,21 +107,15 @@ template void Submap::integrate(InputArray _depth, float depthFactor, const cv::kinfu::Intr& intrinsics, const int currFrameId) { - int index = currFrameId - startFrameId; - std::cout << "Current frame ID: " << currFrameId << " startFrameId: " << startFrameId << "\n"; - std::cout << " Index: " << index << " Camera trajectory size: " << cameraTraj.size() << std::endl; CV_Assert(currFrameId >= startFrameId); - CV_Assert(index >= 0 && index < int(cameraTraj.size())); - - const cv::Affine3f& currPose = cameraTraj.at(index); - volume.integrate(_depth, depthFactor, currPose, intrinsics, currFrameId); + volume.integrate(_depth, depthFactor, cameraPose, intrinsics, currFrameId); } template -void Submap::raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, +void Submap::raycast(const cv::Affine3f& _cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, OutputArray points, OutputArray normals) { - volume.raycast(cameraPose, intrinsics, frameSize, points, normals); + volume.raycast(_cameraPose, intrinsics, frameSize, points, normals); } template @@ -128,73 +134,107 @@ template class SubmapManager { public: - SubmapManager(const VolumeParams& _volumeParams) - : volumeParams(_volumeParams) - {} + typedef Submap SubmapT; + typedef std::map> IdToSubmapPtr; + + SubmapManager(const VolumeParams& _volumeParams) : volumeParams(_volumeParams) {} virtual ~SubmapManager() = default; void reset() { submapList.clear(); }; bool shouldCreateSubmap(int frameId); + //! Adds a new submap/volume into the current list of managed/Active submaps int createNewSubmap(bool isCurrentActiveMap, const int currFrameId = 0, const Affine3f& pose = cv::Affine3f::Identity()); void removeSubmap(int _id); - size_t getTotalSubmaps(void) const { return submapList.size(); }; - - cv::Ptr> getSubmap(int _id) const; - cv::Ptr> getCurrentSubmap(void) const; - - void setPose(int _id); + size_t numOfSubmaps(void) const { return submapList.size(); }; + size_t numOfActiveSubmaps(void) const { return activeSubmapList.size(); }; - /* void updatePoseGraph(); */ + Ptr getSubmap(int _id) const; + Ptr getActiveSubmap(int _id) const; + Ptr getCurrentSubmap(void) const; - protected: - /* void addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, const Affine3f& currPose); */ + bool updateConstraint(Ptr submap1, Ptr submap2); + void updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals); VolumeParams volumeParams; - std::vector>> submapList; + std::vector> submapList; + //! Maintains a pointer to active submaps in the entire submapList + IdToSubmapPtr activeSubmapList; PoseGraph poseGraph; }; template -int SubmapManager::createNewSubmap(bool isCurrentActiveMap, int currFrameId, const Affine3f& pose) +int SubmapManager::createNewSubmap(bool isCurrentMap, int currFrameId, const Affine3f& pose) { int newId = int(submapList.size()); - typename Submap::Type type = - isCurrentActiveMap ? Submap::Type::CURRENT_ACTIVE : Submap::Type::ACTIVE; - cv::Ptr> newSubmap = cv::makePtr>(newId, type, volumeParams, pose, currFrameId); + + typename SubmapT::Type type = isCurrentMap ? SubmapT::Type::CURRENT : SubmapT::Type::NEW; + Ptr newSubmap = cv::makePtr(newId, type, volumeParams, pose, currFrameId); submapList.push_back(newSubmap); + activeSubmapList[newId] = newSubmap; + std::cout << "Created new submap\n"; + return newId; } template -cv::Ptr> SubmapManager::getSubmap(int _id) const +Ptr> SubmapManager::getSubmap(int _id) const { + CV_Assert(submapList.size() > 0); CV_Assert(_id >= 0 && _id < int(submapList.size())); - if (submapList.size() > 0) - return submapList.at(_id); - return nullptr; + return submapList.at(_id); } template -cv::Ptr> SubmapManager::getCurrentSubmap(void) const +Ptr> SubmapManager::getActiveSubmap(int _id) const { - if (submapList.size() > 0) - return submapList.back(); + CV_Assert(submapList.size() > 0); + CV_Assert(_id >= 0); + return activeSubmapList.at(_id); +} + +template +Ptr> SubmapManager::getCurrentSubmap(void) const +{ + for (const auto& pSubmapPair : activeSubmapList) + { + if (pSubmapPair.second->type == SubmapT::Type::CURRENT) + return pSubmapPair.second; + } return nullptr; } template bool SubmapManager::shouldCreateSubmap(int currFrameId) { - cv::Ptr> currSubmap = getCurrentSubmap(); - int allocate_blocks = currSubmap->getTotalAllocatedBlocks(); - int visible_blocks = currSubmap->getVisibleBlocks(currFrameId); - float ratio = float(visible_blocks) / float(allocate_blocks); + Ptr currSubmap = nullptr; + for(const auto& pActiveSubmapPair : activeSubmapList) + { + auto submap = pActiveSubmapPair.second; + //! If there are already new submaps created, don't create more + if(submap->type == SubmapT::Type::NEW) + { + return false; + } + if(submap->type == SubmapT::Type::CURRENT) + { + currSubmap = submap; + } + } + //!TODO: This shouldn't be happening? since there should always be one active current submap + if(!currSubmap) + { + return false; + } + int allocate_blocks = currSubmap->getTotalAllocatedBlocks(); + int visible_blocks = currSubmap->getVisibleBlocks(currFrameId); + float ratio = float(visible_blocks) / float(allocate_blocks); + std::cout << "Ratio: " << ratio << "\n"; if (ratio < 0.2f) @@ -202,32 +242,131 @@ bool SubmapManager::shouldCreateSubmap(int currFrameId) return false; } -/* template */ -/* void SubmapManager::constructPoseGraph() */ -/* { */ -/* for(int i = 0; i < submapList.size(); i++) */ -/* { */ -/* cv::Ptr> currSubmap = submapList.at(i); */ -/* PoseGraphNode node(currSubmap->getId(), currSubmap->getPose()); */ +template +bool SubmapManager::updateConstraint(Ptr submap, Ptr currActiveSubmap) +{ + static constexpr int MAX_ITER = 10; + static constexpr float CONVERGE_WEIGHT_THRESHOLD = 0.01f; + static constexpr float INLIER_WEIGHT_THRESH = 0.75f; + static constexpr int MIN_INLIERS = 10; + + //! thresh = HUBER_THRESH + auto huberWeight = [](float residual, float thresh = 0.1f) -> float { + float rAbs = abs(residual); + if (rAbs < thresh) + return 1.0; + float numerator = sqrt(2 * thresh * rAbs - thresh * thresh); + return numerator / rAbs; + }; + + Affine3f TcameraToActiveSubmap = currActiveSubmap->cameraPose; + Affine3f TcameraToSubmap = submap->cameraPose; + + // ActiveSubmap -> submap transform + Affine3f candidateConstraint = TcameraToSubmap * TcameraToActiveSubmap.inv(); + std::cout << "Candidate constraint: " << candidateConstraint.matrix << "\n"; + typename SubmapT::PoseConstraint& currConstraint = currActiveSubmap->getConstraint(submap->id); + currConstraint.addObservation(candidateConstraint); + std::vector weights(currConstraint.observations.size() + 1, 1.0f); + + Affine3f prevConstraint = currActiveSubmap->getConstraint(submap->id).estimatedPose; + int prevWeight = currActiveSubmap->getConstraint(submap->id).weight; + + std::cout << "Previous constraint pose: " << prevConstraint.matrix << "\n previous Weight: " << prevWeight << "\n"; + + Vec6f meanConstraint; + float sumWeight = 0.0f; + for (int i = 0; i < MAX_ITER; i++) + { + Vec6f constraintVec; + for (int j = 0; j < int(weights.size() - 1); j++) + { + Affine3f currObservation = currConstraint.observations[j]; + cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec); + meanConstraint += weights[j] * constraintVec; + sumWeight += weights[j]; + } + // Heavier weight given to the estimatedPose + cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec); + meanConstraint += weights.back() * prevWeight * constraintVec; + sumWeight += prevWeight; + meanConstraint = meanConstraint * (1 / sumWeight); + + float residual = 0.0f; + float diff = 0; + for (int j = 0; j < int(weights.size()); j++) + { + float w; + if (j == int(weights.size() - 1)) + { + cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec); + w = prevWeight; + } + else + { + Affine3f currObservation = currConstraint.observations[j]; + cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec); + w = 1.0f; + } + + residual = norm(constraintVec - meanConstraint); + double newWeight = huberWeight(residual); + std::cout << "iteration: " << i << " residual: "<< residual << " " << j << "th weight before update: " << weights[j] << " after update: " << newWeight << "\n"; + diff += w * abs(newWeight - weights[j]); + weights[j] = newWeight; + } -/* if(currSubmap->getId() == 0) */ -/* node.setFixed(); */ + if (diff / (prevWeight + weights.size() - 1) < CONVERGE_WEIGHT_THRESHOLD) + break; + } -/* poseGraph.addNode(node); */ -/* } */ + int inliers = 0; + for (int i = 0; i < int(weights.size() - 1); i++) + { + if (weights[i] > INLIER_WEIGHT_THRESH) + inliers++; + } + std::cout << " inliers: " << inliers << "\n"; + if (inliers >= MIN_INLIERS) + { + currConstraint.accumulatePose(candidateConstraint); + Affine3f updatedPose = currConstraint.estimatedPose; + std::cout << "Current updated constraint pose : " << updatedPose.matrix << "\n"; + //! TODO: Should clear observations? not sure + currConstraint.observations.clear(); + return true; + } + return false; +} +template +void SubmapManager::updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals) +{ + Ptr currActiveSubmap = getCurrentSubmap(); + for (const auto& pSubmapPair : activeSubmapList) + { + auto submap = pSubmapPair.second; -/* //! TODO: How to add edges between pose graphs */ -/* for(int i = 0; i < submapList.size(); i++) */ -/* { */ + if (submap->type == SubmapT::Type::NEW) + { + // Check with previous estimate + bool success = updateConstraint(submap, currActiveSubmap); + if (success) + { + //! TODO: Check for visibility and change currentActiveMap + } + } + } -/* } */ -/* } */ + if (shouldCreateSubmap(_frameId)) + { + Affine3f newSubmapPose = currActiveSubmap->pose * currActiveSubmap->cameraPose; + int submapId = createNewSubmap(false, _frameId, newSubmapPose); + auto newSubmap = getSubmap(submapId); + newSubmap->pyrPoints = _framePoints; + newSubmap->pyrNormals = _frameNormals; -/* template */ -/* void SubmapManager::updatePoseGraph() */ -/* { */ -/* constructPoseGraph(); */ -/* } */ + } +} } // namespace kinfu } // namespace cv From 85532121e51c9a786f5e17e21de2b568189affde Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Mon, 3 Aug 2020 20:30:22 -0400 Subject: [PATCH 19/36] Fix constraints creation between submaps and allow for switching between submaps --- modules/rgbd/samples/large_kinfu_demo.cpp | 1 - modules/rgbd/src/large_kinfu.cpp | 7 +- modules/rgbd/src/submap.hpp | 282 +++++++++++++++------- 3 files changed, 204 insertions(+), 86 deletions(-) diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp index 01624d79d08..b2fdf57f5b0 100644 --- a/modules/rgbd/samples/large_kinfu_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -502,7 +502,6 @@ int main(int argc, char **argv) (int)(getTickFrequency()/(newTime - prevTime))), Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); prevTime = newTime; - std::cout << "Trying to render image: " << rendered.rows << " "<< rendered.cols << "\n"; imshow("render", rendered); int c = waitKey(1); diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 9e296ca64b3..dfa1c5e4ca7 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -224,11 +224,12 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size, params.truncateThreshold); - for (const auto& pActiveSubmapPair : submapMgr->activeSubmapList) + for (const auto& it : submapMgr->activeSubmaps) { //! Iterate over map? - int currTrackingId = pActiveSubmapPair.first; - Ptr> currTrackingSubmap = pActiveSubmapPair.second; + int currTrackingId = it.first; + auto submapData = it.second; + Ptr> currTrackingSubmap = submapMgr->getSubmap(currTrackingId); Affine3f affine; std::cout << "Current tracking ID: " << currTrackingId << std::endl; diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index cfd75c6fddc..84c430db210 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -8,6 +8,7 @@ #include #include +#include #include #include "hash_tsdf.hpp" @@ -21,18 +22,9 @@ template class Submap { public: - enum class Type - { - NEW = 0, - CURRENT = 1, - RELOCALISATION = 2, - LOOP_CLOSURE = 3, - LOST = 4 - }; struct PoseConstraint { Affine3f estimatedPose; - std::vector observations; int weight; PoseConstraint() : weight(0){}; @@ -41,17 +33,16 @@ class Submap { Matx44f accPose = estimatedPose.matrix * weight + _pose.matrix * _weight; weight += _weight; - estimatedPose = Affine3f(accPose * 1 / float(weight)); + accPose = accPose * (1/float(weight)); + estimatedPose = Affine3f(accPose); } - void addObservation(const Affine3f& _pose) { observations.push_back(_pose); } }; typedef std::map Constraints; - Submap(int _id, Type _type, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(), + Submap(int _id, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(), int _startFrameId = 0) : id(_id), - type(_type), pose(_pose), cameraPose(Affine3f::Identity()), startFrameId(_startFrameId), @@ -73,6 +64,13 @@ class Submap return volume.getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD); } + float calcVisibilityRatio(int currFrameId) const + { + int allocate_blocks = getTotalAllocatedBlocks(); + int visible_blocks = getVisibleBlocks(currFrameId); + return float(visible_blocks) / float(allocate_blocks); + } + //! TODO: Possibly useless virtual void setStartFrameId(int _startFrameId) { startFrameId = _startFrameId; }; virtual void setStopFrameId(int _stopFrameId) { stopFrameId = _stopFrameId; }; @@ -86,11 +84,9 @@ class Submap public: const int id; - Type type; cv::Affine3f pose; cv::Affine3f cameraPose; Constraints constraints; - int trackingAttempts = 0; int startFrameId; int stopFrameId; @@ -104,6 +100,7 @@ class Submap }; template + void Submap::integrate(InputArray _depth, float depthFactor, const cv::kinfu::Intr& intrinsics, const int currFrameId) { @@ -134,8 +131,24 @@ template class SubmapManager { public: + enum class Type + { + NEW = 0, + CURRENT = 1, + RELOCALISATION = 2, + LOOP_CLOSURE = 3, + LOST = 4 + }; + + struct ActiveSubmapData + { + Type type; + std::vector constraints; + int trackingAttempts; + }; typedef Submap SubmapT; typedef std::map> IdToSubmapPtr; + typedef std::unordered_map IdToActiveSubmaps; SubmapManager(const VolumeParams& _volumeParams) : volumeParams(_volumeParams) {} virtual ~SubmapManager() = default; @@ -143,25 +156,25 @@ class SubmapManager void reset() { submapList.clear(); }; bool shouldCreateSubmap(int frameId); + bool shouldChangeCurrSubmap(int _frameId, int toSubmapId); //! Adds a new submap/volume into the current list of managed/Active submaps int createNewSubmap(bool isCurrentActiveMap, const int currFrameId = 0, const Affine3f& pose = cv::Affine3f::Identity()); void removeSubmap(int _id); size_t numOfSubmaps(void) const { return submapList.size(); }; - size_t numOfActiveSubmaps(void) const { return activeSubmapList.size(); }; + size_t numOfActiveSubmaps(void) const { return activeSubmaps.size(); }; Ptr getSubmap(int _id) const; - Ptr getActiveSubmap(int _id) const; Ptr getCurrentSubmap(void) const; - bool updateConstraint(Ptr submap1, Ptr submap2); + bool estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose); void updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals); VolumeParams volumeParams; std::vector> submapList; //! Maintains a pointer to active submaps in the entire submapList - IdToSubmapPtr activeSubmapList; + IdToActiveSubmaps activeSubmaps; PoseGraph poseGraph; }; @@ -171,11 +184,13 @@ int SubmapManager::createNewSubmap(bool isCurrentMap, int currFrameId, { int newId = int(submapList.size()); - typename SubmapT::Type type = isCurrentMap ? SubmapT::Type::CURRENT : SubmapT::Type::NEW; - Ptr newSubmap = cv::makePtr(newId, type, volumeParams, pose, currFrameId); - + Ptr newSubmap = cv::makePtr(newId, volumeParams, pose, currFrameId); submapList.push_back(newSubmap); - activeSubmapList[newId] = newSubmap; + + ActiveSubmapData newSubmapData; + newSubmapData.trackingAttempts = 0; + newSubmapData.type = isCurrentMap ? Type::CURRENT : Type::NEW; + activeSubmaps[newId] = newSubmapData; std::cout << "Created new submap\n"; @@ -190,21 +205,13 @@ Ptr> SubmapManager::getSubmap(int _id) const return submapList.at(_id); } -template -Ptr> SubmapManager::getActiveSubmap(int _id) const -{ - CV_Assert(submapList.size() > 0); - CV_Assert(_id >= 0); - return activeSubmapList.at(_id); -} - template Ptr> SubmapManager::getCurrentSubmap(void) const { - for (const auto& pSubmapPair : activeSubmapList) + for (const auto& it : activeSubmaps) { - if (pSubmapPair.second->type == SubmapT::Type::CURRENT) - return pSubmapPair.second; + if (it.second.type == Type::CURRENT) + return getSubmap(it.first); } return nullptr; } @@ -212,28 +219,28 @@ Ptr> SubmapManager::getCurrentSubmap(void) const template bool SubmapManager::shouldCreateSubmap(int currFrameId) { - Ptr currSubmap = nullptr; - for(const auto& pActiveSubmapPair : activeSubmapList) + int currSubmapId = -1; + for (const auto& it : activeSubmaps) { - auto submap = pActiveSubmapPair.second; - //! If there are already new submaps created, don't create more - if(submap->type == SubmapT::Type::NEW) + auto submapData = it.second; + // No more than 1 new submap at a time! + if (submapData.type == Type::NEW) { return false; } - if(submap->type == SubmapT::Type::CURRENT) + if (submapData.type == Type::CURRENT) { - currSubmap = submap; + currSubmapId = it.first; } } - //!TODO: This shouldn't be happening? since there should always be one active current submap - if(!currSubmap) + //! TODO: This shouldn't be happening? since there should always be one active current submap + if (currSubmapId < 0) { return false; } - int allocate_blocks = currSubmap->getTotalAllocatedBlocks(); - int visible_blocks = currSubmap->getVisibleBlocks(currFrameId); - float ratio = float(visible_blocks) / float(allocate_blocks); + + Ptr currSubmap = getSubmap(currSubmapId); + float ratio = currSubmap->calcVisibilityRatio(currFrameId); std::cout << "Ratio: " << ratio << "\n"; @@ -243,15 +250,15 @@ bool SubmapManager::shouldCreateSubmap(int currFrameId) } template -bool SubmapManager::updateConstraint(Ptr submap, Ptr currActiveSubmap) +bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose) { static constexpr int MAX_ITER = 10; static constexpr float CONVERGE_WEIGHT_THRESHOLD = 0.01f; - static constexpr float INLIER_WEIGHT_THRESH = 0.75f; + static constexpr float INLIER_WEIGHT_THRESH = 0.8f; static constexpr int MIN_INLIERS = 10; //! thresh = HUBER_THRESH - auto huberWeight = [](float residual, float thresh = 0.1f) -> float { + auto huberWeight = [](float residual, float thresh = 0.05f) -> float { float rAbs = abs(residual); if (rAbs < thresh) return 1.0; @@ -259,21 +266,30 @@ bool SubmapManager::updateConstraint(Ptr submap, Ptr return numerator / rAbs; }; - Affine3f TcameraToActiveSubmap = currActiveSubmap->cameraPose; - Affine3f TcameraToSubmap = submap->cameraPose; + Ptr fromSubmap = getSubmap(fromSubmapId); + Ptr toSubmap = getSubmap(toSubmapId); + ActiveSubmapData& fromSubmapData = activeSubmaps.at(fromSubmapId); + + Affine3f TcameraToFromSubmap = fromSubmap->cameraPose; + Affine3f TcameraToToSubmap = toSubmap->cameraPose; + + // FromSubmap -> ToSubmap transform + Affine3f candidateConstraint = TcameraToToSubmap * TcameraToFromSubmap.inv(); + fromSubmapData.trackingAttempts++; + fromSubmapData.constraints.push_back(candidateConstraint); + + std::cout << "Candidate constraint from: " << fromSubmap->id << " to " << toSubmap->id << "\n" + << candidateConstraint.matrix << "\n"; + std::cout << "Constraint observations size: " << fromSubmapData.constraints.size() << "\n"; - // ActiveSubmap -> submap transform - Affine3f candidateConstraint = TcameraToSubmap * TcameraToActiveSubmap.inv(); - std::cout << "Candidate constraint: " << candidateConstraint.matrix << "\n"; - typename SubmapT::PoseConstraint& currConstraint = currActiveSubmap->getConstraint(submap->id); - currConstraint.addObservation(candidateConstraint); - std::vector weights(currConstraint.observations.size() + 1, 1.0f); + std::vector weights(fromSubmapData.constraints.size() + 1, 1.0f); - Affine3f prevConstraint = currActiveSubmap->getConstraint(submap->id).estimatedPose; - int prevWeight = currActiveSubmap->getConstraint(submap->id).weight; + Affine3f prevConstraint = fromSubmap->getConstraint(toSubmap->id).estimatedPose; + int prevWeight = fromSubmap->getConstraint(toSubmap->id).weight; - std::cout << "Previous constraint pose: " << prevConstraint.matrix << "\n previous Weight: " << prevWeight << "\n"; + std::cout << "Previous constraint pose: \n" << prevConstraint.matrix << "\n previous Weight: " << prevWeight << "\n"; + // Iterative reweighted least squares with huber threshold to find the inliers in the past observations Vec6f meanConstraint; float sumWeight = 0.0f; for (int i = 0; i < MAX_ITER; i++) @@ -281,7 +297,7 @@ bool SubmapManager::updateConstraint(Ptr submap, Ptr Vec6f constraintVec; for (int j = 0; j < int(weights.size() - 1); j++) { - Affine3f currObservation = currConstraint.observations[j]; + Affine3f currObservation = fromSubmapData.constraints[j]; cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec); meanConstraint += weights[j] * constraintVec; sumWeight += weights[j]; @@ -304,14 +320,15 @@ bool SubmapManager::updateConstraint(Ptr submap, Ptr } else { - Affine3f currObservation = currConstraint.observations[j]; + Affine3f currObservation = fromSubmapData.constraints[j]; cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec); w = 1.0f; } residual = norm(constraintVec - meanConstraint); double newWeight = huberWeight(residual); - std::cout << "iteration: " << i << " residual: "<< residual << " " << j << "th weight before update: " << weights[j] << " after update: " << newWeight << "\n"; + std::cout << "iteration: " << i << " residual: " << residual << " " << j + << "th weight before update: " << weights[j] << " after update: " << newWeight << "\n"; diff += w * abs(newWeight - weights[j]); weights[j] = newWeight; } @@ -320,51 +337,152 @@ bool SubmapManager::updateConstraint(Ptr submap, Ptr break; } - int inliers = 0; - for (int i = 0; i < int(weights.size() - 1); i++) + int localInliers = 0; + Matx44f inlierConstraint; + for (int i = 0; i < int(weights.size()); i++) { if (weights[i] > INLIER_WEIGHT_THRESH) - inliers++; + { + localInliers++; + inlierConstraint += fromSubmapData.constraints[i].matrix; + } } + inlierConstraint /= float(max(localInliers, 1)); + inlierPose = Affine3f(inlierConstraint); + inliers = localInliers; + + /* std::cout << "Rvec: " << rvec << " tvec: " << tvec << "\n"; */ + std::cout << inlierPose.matrix << "\n"; std::cout << " inliers: " << inliers << "\n"; + if (inliers >= MIN_INLIERS) { - currConstraint.accumulatePose(candidateConstraint); - Affine3f updatedPose = currConstraint.estimatedPose; - std::cout << "Current updated constraint pose : " << updatedPose.matrix << "\n"; - //! TODO: Should clear observations? not sure - currConstraint.observations.clear(); return true; } return false; } + +template +bool SubmapManager::shouldChangeCurrSubmap(int _frameId, int toSubmapId) +{ + auto toSubmap = getSubmap(toSubmapId); + auto toSubmapData = activeSubmaps.at(toSubmapId); + auto currActiveSubmap = getCurrentSubmap(); + + + int blocksInNewMap = toSubmap->getTotalAllocatedBlocks(); + float newRatio = toSubmap->calcVisibilityRatio(_frameId); + + float currRatio = currActiveSubmap->calcVisibilityRatio(_frameId); + + //! TODO: Check for a specific threshold? + if (blocksInNewMap <= 0) + return false; + if ((newRatio > currRatio) && (toSubmapData.type == Type::NEW)) + return true; + + return false; +} + template void SubmapManager::updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals) { - Ptr currActiveSubmap = getCurrentSubmap(); - for (const auto& pSubmapPair : activeSubmapList) + const int currSubmapId = getCurrentSubmap()->id; + int changedCurrentMapId = -1; + for (auto& it : activeSubmaps) { - auto submap = pSubmapPair.second; - - if (submap->type == SubmapT::Type::NEW) + int submapId = it.first; + auto& submapData = it.second; + if (submapData.type == Type::NEW || submapData.type == Type::LOOP_CLOSURE) { // Check with previous estimate - bool success = updateConstraint(submap, currActiveSubmap); + int inliers; + Affine3f inlierPose; + bool success = estimateConstraint(submapId, currSubmapId, inliers, inlierPose); + std::cout << "SubmapId: " << submapId << " Tracking attempts: " << submapData.trackingAttempts << "\n"; if (success) { + typename SubmapT::PoseConstraint& submapConstraint = getSubmap(submapId)->getConstraint(currSubmapId); + std::cout << "Updated fixed constraint with inlierPose: \n" << inlierPose.matrix << "\n"; + submapConstraint.accumulatePose(inlierPose, inliers); + std::cout << "Submap constraint estimated pose: \n" << submapConstraint.estimatedPose.matrix << "\n"; + submapData.constraints.clear(); + submapData.trackingAttempts = 0; + //! TODO: Check for visibility and change currentActiveMap + if (shouldChangeCurrSubmap(_frameId, submapId)) + { + //! TODO: Change submap and update constraints accordingly + std::cout << "Should change current map to the new map\n"; + changedCurrentMapId = submapId; + } } } } + std::vector createNewConstraintsList; + for (auto& it : activeSubmaps) + { + int submapId = it.first; + auto& submapData = it.second; + std::cout << "submapId: " << submapId << " SubmapType: " << static_cast(submapData.type) << "\n"; + std::cout << "changedCurrentMapId: " << changedCurrentMapId << "\n"; + + if (submapId == changedCurrentMapId) + { + submapData.type = Type::CURRENT; + std::cout << "Changed submapId: " << submapId << " to currentMap\n"; + } + if ((submapData.type == Type::CURRENT) && (changedCurrentMapId >= 0) && (submapId != changedCurrentMapId)) + { + submapData.type = Type::LOST; + createNewConstraintsList.push_back(submapId); + std::cout << "Marked submapId: " << submapId << " as lost\n"; + } + if ((submapData.type == Type::NEW || submapData.type == Type::LOOP_CLOSURE) && (changedCurrentMapId >= 0)) + { + //! TODO: Add a new type called NEW_LOST? + submapData.type = Type::LOST; + createNewConstraintsList.push_back(submapId); + std::cout << "Marked submapId: " << submapId << " as lost\n"; + } + } + + for (typename IdToActiveSubmaps::iterator it = activeSubmaps.begin(); it != activeSubmaps.end(); ) + { + auto& submapData = it->second; + std::cout << "submapId: " << it->first << " submapData type: " << static_cast(submapData.type) << "\n"; + if (submapData.type == Type::LOST) + { + std::cout << "Erased element: " << it->first; + it = activeSubmaps.erase(it); + } + else + { + it++; + } + } + + std::cout << "Active submap Data size after removing lost: " << activeSubmaps.size() << "\n"; + + for (std::vector::const_iterator it = createNewConstraintsList.begin(); it != createNewConstraintsList.end(); ++it) + { + int dataId = *it; + ActiveSubmapData newSubmapData; + newSubmapData.trackingAttempts = 0; + newSubmapData.type = Type::LOOP_CLOSURE; + activeSubmaps[dataId] = newSubmapData; + } + std::cout << "Active submap Data size after adding new data: " << activeSubmaps.size() << "\n"; + if (shouldCreateSubmap(_frameId)) { + Ptr currActiveSubmap = getCurrentSubmap(); Affine3f newSubmapPose = currActiveSubmap->pose * currActiveSubmap->cameraPose; - int submapId = createNewSubmap(false, _frameId, newSubmapPose); - auto newSubmap = getSubmap(submapId); - newSubmap->pyrPoints = _framePoints; - newSubmap->pyrNormals = _frameNormals; - + int submapId = createNewSubmap(false, _frameId, newSubmapPose); + auto newSubmap = getSubmap(submapId); + newSubmap->pyrPoints = _framePoints; + newSubmap->pyrNormals = _frameNormals; } } From 05db54bd2d20a6d160baad7fdbde000d32db6dc2 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 4 Aug 2020 13:17:46 -0400 Subject: [PATCH 20/36] - Fix bug in allocate volumeUnits - Simplify calculation of visibleBlocks --- modules/rgbd/src/hash_tsdf.cpp | 32 +++--------- modules/rgbd/src/hash_tsdf.hpp | 1 - modules/rgbd/src/submap.hpp | 95 +++++++++++++++------------------- 3 files changed, 49 insertions(+), 79 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index fa91c76d317..8fe26d76caf 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -75,7 +75,7 @@ struct AllocateVolumeUnitsInvoker : ParallelLoopBody for (int x = 0; x < depth.cols; x += depthStride) { depthType z = depthRow[x] * depthFactor; - if (z <= 0) + if (z <= 0 || z > volume.truncateThreshold) continue; Point3f camPoint = reproj(Point3f((float)x, (float)y, z)); @@ -112,7 +112,6 @@ struct AllocateVolumeUnitsInvoker : ParallelLoopBody cv::makePtr(volume.voxelSize, subvolumePose, volume.raycastStepFactor, volume.truncDist, volume.maxWeight, volumeDims); //! This volume unit will definitely be required for current integration - volumeUnit.index = tsdf_idx; volumeUnit.isActive = true; volumeUnit.lastVisibleIndex = frameId; volume.volumeUnits[tsdf_idx] = volumeUnit; @@ -145,7 +144,7 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, const c Range allocateRange(0, depth.rows); parallel_for_(allocateRange, allocate_i); - //! Get volumes that are in the current camera frame + //! Get keys for all the allocated volume Units std::vector totalVolUnits; for (const auto& keyvalue : volumeUnits) { @@ -558,33 +557,14 @@ void HashTSDFVolumeCPU::fetchNormals_(cv::InputArray _points, cv::OutputArray _n int HashTSDFVolumeCPU::getVisibleBlocks_(int currFrameId, int frameThreshold) const { int numVisibleBlocks = 0; - std::vector totalVolUnits; + //! TODO: Iterate over map parallely? for (const auto& keyvalue : volumeUnits) { - totalVolUnits.push_back(keyvalue.first); + const VolumeUnit& volumeUnit = keyvalue.second; + if(volumeUnit.lastVisibleIndex > (currFrameId - frameThreshold)) + numVisibleBlocks++; } - - Range checkVisibleRange(0, volumeUnits.size()); - //! TODO: Sum up via reduction - /* parallel_for_(checkVisibleRange, [&](const Range& range) { */ - - for (int i = checkVisibleRange.start; i < checkVisibleRange.end; ++i) - { - cv::Vec3i tsdf_idx = totalVolUnits[i]; - VolumeUnitMap::const_iterator it = volumeUnits.find(tsdf_idx); - if (it == volumeUnits.end()) - continue; - - if(it->second.lastVisibleIndex > (currFrameId - frameThreshold)) - { - //! Add count (parallel?) - numVisibleBlocks++; - } - } - return numVisibleBlocks; - /* }); */ - } } // namespace kinfu diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 013f1f45b1e..21931b36cfb 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -126,7 +126,6 @@ struct VolumeUnit ~VolumeUnit() = default; cv::Ptr pVolume; - cv::Vec3i index; int lastVisibleIndex = 0; bool isActive; }; diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 84c430db210..ac3291613f1 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -12,6 +12,7 @@ #include #include "hash_tsdf.hpp" +#include "opencv2/core/mat.inl.hpp" #include "pose_graph.hpp" namespace cv @@ -32,25 +33,19 @@ class Submap void accumulatePose(const Affine3f& _pose, int _weight = 1) { Matx44f accPose = estimatedPose.matrix * weight + _pose.matrix * _weight; - weight += _weight; - accPose = accPose * (1/float(weight)); - estimatedPose = Affine3f(accPose); + weight += _weight; + accPose /= float(weight); + estimatedPose = Affine3f(accPose); } }; - typedef std::map Constraints; Submap(int _id, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(), int _startFrameId = 0) - : id(_id), - pose(_pose), - cameraPose(Affine3f::Identity()), - startFrameId(_startFrameId), - volume(volumeParams) + : id(_id), pose(_pose), cameraPose(Affine3f::Identity()), startFrameId(_startFrameId), volume(volumeParams) { std::cout << "Created volume\n"; } - virtual ~Submap() = default; virtual void integrate(InputArray _depth, float depthFactor, const cv::kinfu::Intr& intrinsics, const int currframeId); @@ -172,8 +167,8 @@ class SubmapManager void updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals); VolumeParams volumeParams; + std::vector> submapList; - //! Maintains a pointer to active submaps in the entire submapList IdToActiveSubmaps activeSubmaps; PoseGraph poseGraph; @@ -184,13 +179,13 @@ int SubmapManager::createNewSubmap(bool isCurrentMap, int currFrameId, { int newId = int(submapList.size()); - Ptr newSubmap = cv::makePtr(newId, volumeParams, pose, currFrameId); + Ptr newSubmap = cv::makePtr(newId, volumeParams, pose, currFrameId); submapList.push_back(newSubmap); ActiveSubmapData newSubmapData; newSubmapData.trackingAttempts = 0; - newSubmapData.type = isCurrentMap ? Type::CURRENT : Type::NEW; - activeSubmaps[newId] = newSubmapData; + newSubmapData.type = isCurrentMap ? Type::CURRENT : Type::NEW; + activeSubmaps[newId] = newSubmapData; std::cout << "Created new submap\n"; @@ -240,7 +235,7 @@ bool SubmapManager::shouldCreateSubmap(int currFrameId) } Ptr currSubmap = getSubmap(currSubmapId); - float ratio = currSubmap->calcVisibilityRatio(currFrameId); + float ratio = currSubmap->calcVisibilityRatio(currFrameId); std::cout << "Ratio: " << ratio << "\n"; @@ -266,8 +261,8 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId return numerator / rAbs; }; - Ptr fromSubmap = getSubmap(fromSubmapId); - Ptr toSubmap = getSubmap(toSubmapId); + Ptr fromSubmap = getSubmap(fromSubmapId); + Ptr toSubmap = getSubmap(toSubmapId); ActiveSubmapData& fromSubmapData = activeSubmaps.at(fromSubmapId); Affine3f TcameraToFromSubmap = fromSubmap->cameraPose; @@ -306,7 +301,8 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec); meanConstraint += weights.back() * prevWeight * constraintVec; sumWeight += prevWeight; - meanConstraint = meanConstraint * (1 / sumWeight); + /* std::cout << "meanConstraint before average: " << meanConstraint << " sumWeight: " << sumWeight << "\n"; */ + meanConstraint /= float(sumWeight); float residual = 0.0f; float diff = 0; @@ -325,7 +321,10 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId w = 1.0f; } - residual = norm(constraintVec - meanConstraint); + std::cout << "meanConstraint: " << meanConstraint << " ConstraintVec: " << constraintVec << "\n"; + cv::Vec6f residualVec = (constraintVec - meanConstraint); + std::cout << "Residual Vec: " << residualVec << "\n"; + residual = norm(residualVec); double newWeight = huberWeight(residual); std::cout << "iteration: " << i << " residual: " << residual << " " << j << "th weight before update: " << weights[j] << " after update: " << newWeight << "\n"; @@ -344,12 +343,15 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId if (weights[i] > INLIER_WEIGHT_THRESH) { localInliers++; - inlierConstraint += fromSubmapData.constraints[i].matrix; + if (i == int(weights.size() - 1)) + inlierConstraint += prevConstraint.matrix; + else + inlierConstraint += fromSubmapData.constraints[i].matrix; } } inlierConstraint /= float(max(localInliers, 1)); inlierPose = Affine3f(inlierConstraint); - inliers = localInliers; + inliers = localInliers; /* std::cout << "Rvec: " << rvec << " tvec: " << tvec << "\n"; */ std::cout << inlierPose.matrix << "\n"; @@ -365,11 +367,10 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId template bool SubmapManager::shouldChangeCurrSubmap(int _frameId, int toSubmapId) { - auto toSubmap = getSubmap(toSubmapId); - auto toSubmapData = activeSubmaps.at(toSubmapId); + auto toSubmap = getSubmap(toSubmapId); + auto toSubmapData = activeSubmaps.at(toSubmapId); auto currActiveSubmap = getCurrentSubmap(); - int blocksInNewMap = toSubmap->getTotalAllocatedBlocks(); float newRatio = toSubmap->calcVisibilityRatio(_frameId); @@ -387,12 +388,12 @@ bool SubmapManager::shouldChangeCurrSubmap(int _frameId, int toSubmapId template void SubmapManager::updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals) { - const int currSubmapId = getCurrentSubmap()->id; - int changedCurrentMapId = -1; + const int currSubmapId = getCurrentSubmap()->id; + int changedCurrentMapId = -1; for (auto& it : activeSubmaps) { - int submapId = it.first; - auto& submapData = it.second; + int submapId = it.first; + auto& submapData = it.second; if (submapData.type == Type::NEW || submapData.type == Type::LOOP_CLOSURE) { // Check with previous estimate @@ -417,72 +418,62 @@ void SubmapManager::updateMap(int _frameId, std::vector _frame changedCurrentMapId = submapId; } } + else + { + //! If tried tracking for threshold number of times, mark the data as lost + } } } std::vector createNewConstraintsList; for (auto& it : activeSubmaps) { - int submapId = it.first; - auto& submapData = it.second; - std::cout << "submapId: " << submapId << " SubmapType: " << static_cast(submapData.type) << "\n"; - std::cout << "changedCurrentMapId: " << changedCurrentMapId << "\n"; + int submapId = it.first; + auto& submapData = it.second; if (submapId == changedCurrentMapId) { submapData.type = Type::CURRENT; - std::cout << "Changed submapId: " << submapId << " to currentMap\n"; } if ((submapData.type == Type::CURRENT) && (changedCurrentMapId >= 0) && (submapId != changedCurrentMapId)) { submapData.type = Type::LOST; createNewConstraintsList.push_back(submapId); - std::cout << "Marked submapId: " << submapId << " as lost\n"; } if ((submapData.type == Type::NEW || submapData.type == Type::LOOP_CLOSURE) && (changedCurrentMapId >= 0)) { //! TODO: Add a new type called NEW_LOST? submapData.type = Type::LOST; createNewConstraintsList.push_back(submapId); - std::cout << "Marked submapId: " << submapId << " as lost\n"; } } - for (typename IdToActiveSubmaps::iterator it = activeSubmaps.begin(); it != activeSubmaps.end(); ) + for (typename IdToActiveSubmaps::iterator it = activeSubmaps.begin(); it != activeSubmaps.end();) { auto& submapData = it->second; - std::cout << "submapId: " << it->first << " submapData type: " << static_cast(submapData.type) << "\n"; if (submapData.type == Type::LOST) - { - std::cout << "Erased element: " << it->first; it = activeSubmaps.erase(it); - } else - { it++; - } } - std::cout << "Active submap Data size after removing lost: " << activeSubmaps.size() << "\n"; - for (std::vector::const_iterator it = createNewConstraintsList.begin(); it != createNewConstraintsList.end(); ++it) { int dataId = *it; ActiveSubmapData newSubmapData; newSubmapData.trackingAttempts = 0; - newSubmapData.type = Type::LOOP_CLOSURE; - activeSubmaps[dataId] = newSubmapData; + newSubmapData.type = Type::LOOP_CLOSURE; + activeSubmaps[dataId] = newSubmapData; } - std::cout << "Active submap Data size after adding new data: " << activeSubmaps.size() << "\n"; if (shouldCreateSubmap(_frameId)) { Ptr currActiveSubmap = getCurrentSubmap(); - Affine3f newSubmapPose = currActiveSubmap->pose * currActiveSubmap->cameraPose; - int submapId = createNewSubmap(false, _frameId, newSubmapPose); - auto newSubmap = getSubmap(submapId); - newSubmap->pyrPoints = _framePoints; - newSubmap->pyrNormals = _frameNormals; + Affine3f newSubmapPose = currActiveSubmap->pose * currActiveSubmap->cameraPose; + int submapId = createNewSubmap(false, _frameId, newSubmapPose); + auto newSubmap = getSubmap(submapId); + newSubmap->pyrPoints = _framePoints; + newSubmap->pyrNormals = _frameNormals; } } From 508588451d39d791656d1e42b2279b7aaf26b621 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 4 Aug 2020 17:21:17 -0400 Subject: [PATCH 21/36] Remove inlier calculation in fast_icp (not required) --- modules/rgbd/src/fast_icp.cpp | 58 +++++++++----------------------- modules/rgbd/src/fast_icp.hpp | 4 --- modules/rgbd/src/large_kinfu.cpp | 10 ++---- 3 files changed, 17 insertions(+), 55 deletions(-) diff --git a/modules/rgbd/src/fast_icp.cpp b/modules/rgbd/src/fast_icp.cpp index 3394035f223..d410ad15199 100644 --- a/modules/rgbd/src/fast_icp.cpp +++ b/modules/rgbd/src/fast_icp.cpp @@ -33,12 +33,8 @@ class ICPImpl : public ICP InputArray oldPoints, InputArray oldNormals, InputArray newPoints, InputArray newNormals ) const override; - virtual std::tuple estimateTransformInliers(cv::Affine3f& transform, - InputArray oldPoints, InputArray oldNormals, - InputArray newPoints, InputArray newNormals - ) const override; template < typename T > - bool estimateTransformT(cv::Affine3f& transform, int& numInliers, + bool estimateTransformT(cv::Affine3f& transform, const vector& oldPoints, const vector& oldNormals, const vector& newPoints, const vector& newNormals ) const; @@ -46,7 +42,7 @@ class ICPImpl : public ICP virtual ~ICPImpl() { } template < typename T > - int getAb(const T& oldPts, const T& oldNrm, const T& newPts, const T& newNrm, + void getAb(const T& oldPts, const T& oldNrm, const T& newPts, const T& newNrm, cv::Affine3f pose, int level, cv::Matx66f& A, cv::Vec6f& b) const; private: @@ -61,10 +57,10 @@ ICPImpl::ICPImpl(const Intr _intrinsics, const std::vector &_iterations, fl { } -std::tuple ICPImpl::estimateTransformInliers(cv::Affine3f &transform, - InputArray _oldPoints, InputArray _oldNormals, - InputArray _newPoints, InputArray _newNormals - ) const +bool ICPImpl::estimateTransform(cv::Affine3f& transform, + InputArray _oldPoints, InputArray _oldNormals, + InputArray _newPoints, InputArray _newNormals + ) const { CV_TRACE_FUNCTION(); @@ -72,8 +68,6 @@ std::tuple ICPImpl::estimateTransformInliers(cv::Affine3f &transform, CV_Assert(_newPoints.size() == _newNormals.size()); CV_Assert(_oldPoints.size() == _newPoints.size()); - int numInliers; - bool success; #ifdef HAVE_OPENCL if(cv::ocl::isOpenCLActivated() && _oldPoints.isUMatVector() && _oldNormals.isUMatVector() && @@ -84,8 +78,7 @@ std::tuple ICPImpl::estimateTransformInliers(cv::Affine3f &transform, _newPoints.getUMatVector(np); _oldNormals.getUMatVector(on); _newNormals.getUMatVector(nn); - success = estimateTransformT(transform, numInliers, op, on, np, nn); - return std::make_tuple(success, numInliers); + return estimateTransformT(transform, op, on, np, nn); } #endif @@ -94,21 +87,11 @@ std::tuple ICPImpl::estimateTransformInliers(cv::Affine3f &transform, _newPoints.getMatVector(np); _oldNormals.getMatVector(on); _newNormals.getMatVector(nn); - success = estimateTransformT(transform, numInliers, op, on, np, nn); - return std::make_tuple(success, numInliers); -} - -bool ICPImpl::estimateTransform(cv::Affine3f& transform, - InputArray _oldPoints, InputArray _oldNormals, - InputArray _newPoints, InputArray _newNormals - ) const -{ - auto value = estimateTransformInliers(transform, _oldPoints, _oldNormals, _newPoints, _newNormals); - return std::get<0>(value); + return estimateTransformT(transform, op, on, np, nn); } template < typename T > -bool ICPImpl::estimateTransformT(cv::Affine3f& transform, int& numInliers, +bool ICPImpl::estimateTransformT(cv::Affine3f& transform, const vector& oldPoints, const vector& oldNormals, const vector& newPoints, const vector& newNormals ) const @@ -116,7 +99,6 @@ bool ICPImpl::estimateTransformT(cv::Affine3f& transform, int& numInliers, CV_TRACE_FUNCTION(); transform = Affine3f::Identity(); - numInliers = 0; // Finally after ICP completion, if ICP succeeds numInliers will contain final iteration inliers for(size_t l = 0; l < iterations.size(); l++) { size_t level = iterations.size() - 1 - l; @@ -129,15 +111,12 @@ bool ICPImpl::estimateTransformT(cv::Affine3f& transform, int& numInliers, Matx66f A; Vec6f b; - numInliers = getAb(oldPts, oldNrm, newPts, newNrm, transform, (int)level, A, b); + getAb(oldPts, oldNrm, newPts, newNrm, transform, (int)level, A, b); double det = cv::determinant(A); if (abs (det) < 1e-15 || cvIsNaN(det)) - { - numInliers = 0; return false; - } Vec6f x; // theoretically, any method of solving is applicable @@ -196,11 +175,11 @@ typedef Matx ABtype; struct GetAbInvoker : ParallelLoopBody { - GetAbInvoker(ABtype& _globalAb, int& _numInliers, Mutex& _mtx, + GetAbInvoker(ABtype& _globalAb, Mutex& _mtx, const Points& _oldPts, const Normals& _oldNrm, const Points& _newPts, const Normals& _newNrm, Affine3f _pose, Intr::Projector _proj, float _sqDistanceThresh, float _minCos) : ParallelLoopBody(), - globalSumAb(_globalAb), numInliers(_numInliers), mtx(_mtx), + globalSumAb(_globalAb), mtx(_mtx), oldPts(_oldPts), oldNrm(_oldNrm), newPts(_newPts), newNrm(_newNrm), pose(_pose), proj(_proj), sqDistanceThresh(_sqDistanceThresh), minCos(_minCos) { } @@ -499,11 +478,9 @@ struct GetAbInvoker : ParallelLoopBody AutoLock al(mtx); globalSumAb += sumAB; - numInliers += localNumInliers; } ABtype& globalSumAb; - int& numInliers; Mutex& mtx; const Points& oldPts; const Normals& oldNrm; @@ -517,7 +494,7 @@ struct GetAbInvoker : ParallelLoopBody template <> -int ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm, +void ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm, cv::Affine3f pose, int level, cv::Matx66f& A, cv::Vec6f& b) const { CV_TRACE_FUNCTION(); @@ -526,11 +503,10 @@ int ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, CV_Assert(newPts.size() == newNrm.size()); ABtype sumAB = ABtype::zeros(); - int numInliers = 0; Mutex mutex; const Points op(oldPts), on(oldNrm); const Normals np(newPts), nn(newNrm); - GetAbInvoker invoker(sumAB, numInliers, mutex, op, on, np, nn, pose, + GetAbInvoker invoker(sumAB, mutex, op, on, np, nn, pose, intrinsics.scale(level).makeProjector(), distanceThreshold*distanceThreshold, cos(angleThreshold)); Range range(0, newPts.rows); @@ -548,7 +524,6 @@ int ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, b(i) = sumAB(i, 6); } - return numInliers; } ///////// GPU implementation ///////// @@ -556,7 +531,7 @@ int ICPImpl::getAb(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, #ifdef HAVE_OPENCL template <> -int ICPImpl::getAb(const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm, +void ICPImpl::getAb(const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm, Affine3f pose, int level, Matx66f &A, Vec6f &b) const { CV_TRACE_FUNCTION(); @@ -602,7 +577,6 @@ int ICPImpl::getAb(const UMat& oldPts, const UMat& oldNrm, const UMat& new Intr::Projector proj = intrinsics.scale(level).makeProjector(); Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy); - int numInliers = 0; UMat& groupedSumGpu = groupedSumBuffers[level]; groupedSumGpu.create(Size(ngroups.width*UTSIZE, ngroups.height), CV_32F); @@ -669,8 +643,6 @@ int ICPImpl::getAb(const UMat& oldPts, const UMat& oldNrm, const UMat& new b(i) = sumAB(i, 6); } - // TODO: Figure out changes for GPU ICP - return numInliers; } #endif diff --git a/modules/rgbd/src/fast_icp.hpp b/modules/rgbd/src/fast_icp.hpp index 5f7ca60d976..a11a919da3e 100644 --- a/modules/rgbd/src/fast_icp.hpp +++ b/modules/rgbd/src/fast_icp.hpp @@ -22,10 +22,6 @@ class ICP InputArray oldPoints, InputArray oldNormals, InputArray newPoints, InputArray newNormals ) const = 0; - virtual std::tuple estimateTransformInliers(cv::Affine3f& transform, - InputArray oldPoints, InputArray oldNormals, - InputArray newPoints, InputArray newNormals - ) const = 0; virtual ~ICP() { } protected: diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index dfa1c5e4ca7..3e80609640b 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -242,16 +242,10 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) } //1. Track - int numInliers = 0; - bool trackingSuccess = false; - std::tie(trackingSuccess, numInliers) = - icp->estimateTransformInliers(affine, currTrackingSubmap->pyrPoints, currTrackingSubmap->pyrNormals, newPoints, newNormals); + bool trackingSuccess = + icp->estimateTransform(affine, currTrackingSubmap->pyrPoints, currTrackingSubmap->pyrNormals, newPoints, newNormals); if (trackingSuccess) - { - //! Compose current pose and add to camera trajectory currTrackingSubmap->composeCameraPose(affine); - std::cout << "Number of inliers: " << numInliers << "\n"; - } //2. Integrate float rnorm = (float)cv::norm(affine.rvec()); From de7ba9689698c314056eac4a5e229fad8eb809d3 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 4 Aug 2020 18:40:02 -0400 Subject: [PATCH 22/36] Modify readFile to allow reading other datasets easily --- modules/rgbd/samples/large_kinfu_demo.cpp | 8 ++--- modules/rgbd/src/large_kinfu.cpp | 25 ++++++++++----- modules/rgbd/src/submap.hpp | 39 +++++++++++++++-------- 3 files changed, 46 insertions(+), 26 deletions(-) diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp index b2fdf57f5b0..0284bbc351e 100644 --- a/modules/rgbd/samples/large_kinfu_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -34,16 +34,14 @@ static vector readDepth(std::string fileList) size_t slashIdx = fileList.rfind('/'); slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); dir = fileList.substr(0, slashIdx); - while(!file.eof()) { std::string s, imgPath; std::getline(file, s); if(s.empty() || s[0] == '#') continue; - std::stringstream ss; - ss << s; - double thumb; - ss >> thumb >> imgPath; + size_t spaceIdx = s.rfind(' '); + spaceIdx = spaceIdx != std::string::npos ? spaceIdx+1 : 0; + imgPath = s.substr(spaceIdx, s.size()); v.push_back(dir+'/'+imgPath); } diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 3e80609640b..8a324ea5113 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -224,6 +224,7 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size, params.truncateThreshold); + std::cout << "Current frameID: " << frameCounter << "\n"; for (const auto& it : submapMgr->activeSubmaps) { //! Iterate over map? @@ -246,13 +247,20 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) icp->estimateTransform(affine, currTrackingSubmap->pyrPoints, currTrackingSubmap->pyrNormals, newPoints, newNormals); if (trackingSuccess) currTrackingSubmap->composeCameraPose(affine); + else + { + std::cout << "Tracking failed" << std::endl; + } //2. Integrate - float rnorm = (float)cv::norm(affine.rvec()); - float tnorm = (float)cv::norm(affine.translation()); - // We do not integrate volume if camera does not move - if ((rnorm + tnorm) / 2 >= params.tsdf_min_camera_movement) - currTrackingSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); + if(submapData.type == SubmapManager::Type::NEW || submapData.type == SubmapManager::Type::CURRENT) + { + float rnorm = (float)cv::norm(affine.rvec()); + float tnorm = (float)cv::norm(affine.translation()); + // We do not integrate volume if camera does not move + if ((rnorm + tnorm) / 2 >= params.tsdf_min_camera_movement) + currTrackingSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); + } //3. Raycast currTrackingSubmap->raycast(currTrackingSubmap->cameraPose, params.intr, params.frameSize, currTrackingSubmap->pyrPoints[0], currTrackingSubmap->pyrNormals[0]); @@ -262,10 +270,11 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) std::cout << "Submap: " << currTrackingId << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks() << "\n"; std::cout << "Submap: " << currTrackingId << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter) << "\n"; - //4. Update map - submapMgr->updateMap(frameCounter, newPoints, newNormals); - std::cout << "Number of submaps: " << submapMgr->submapList.size() << "\n"; } + //4. Update map + submapMgr->updateMap(frameCounter, newPoints, newNormals); + std::cout << "Number of submaps: " << submapMgr->submapList.size() << "\n"; + frameCounter++; return true; } diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index ac3291613f1..705e2a88f2d 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -253,7 +253,7 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId static constexpr int MIN_INLIERS = 10; //! thresh = HUBER_THRESH - auto huberWeight = [](float residual, float thresh = 0.05f) -> float { + auto huberWeight = [](float residual, float thresh = 0.1f) -> float { float rAbs = abs(residual); if (rAbs < thresh) return 1.0; @@ -273,16 +273,16 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId fromSubmapData.trackingAttempts++; fromSubmapData.constraints.push_back(candidateConstraint); - std::cout << "Candidate constraint from: " << fromSubmap->id << " to " << toSubmap->id << "\n" - << candidateConstraint.matrix << "\n"; - std::cout << "Constraint observations size: " << fromSubmapData.constraints.size() << "\n"; + /* std::cout << "Candidate constraint from: " << fromSubmap->id << " to " << toSubmap->id << "\n" */ + /* << candidateConstraint.matrix << "\n"; */ + /* std::cout << "Constraint observations size: " << fromSubmapData.constraints.size() << "\n"; */ std::vector weights(fromSubmapData.constraints.size() + 1, 1.0f); Affine3f prevConstraint = fromSubmap->getConstraint(toSubmap->id).estimatedPose; int prevWeight = fromSubmap->getConstraint(toSubmap->id).weight; - std::cout << "Previous constraint pose: \n" << prevConstraint.matrix << "\n previous Weight: " << prevWeight << "\n"; + /* std::cout << "Previous constraint pose: \n" << prevConstraint.matrix << "\n previous Weight: " << prevWeight << "\n"; */ // Iterative reweighted least squares with huber threshold to find the inliers in the past observations Vec6f meanConstraint; @@ -321,13 +321,13 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId w = 1.0f; } - std::cout << "meanConstraint: " << meanConstraint << " ConstraintVec: " << constraintVec << "\n"; + /* std::cout << "meanConstraint: " << meanConstraint << " ConstraintVec: " << constraintVec << "\n"; */ cv::Vec6f residualVec = (constraintVec - meanConstraint); - std::cout << "Residual Vec: " << residualVec << "\n"; + /* std::cout << "Residual Vec: " << residualVec << "\n"; */ residual = norm(residualVec); double newWeight = huberWeight(residual); - std::cout << "iteration: " << i << " residual: " << residual << " " << j - << "th weight before update: " << weights[j] << " after update: " << newWeight << "\n"; + /* std::cout << "iteration: " << i << " residual: " << residual << " " << j */ + /* << "th weight before update: " << weights[j] << " after update: " << newWeight << "\n"; */ diff += w * abs(newWeight - weights[j]); weights[j] = newWeight; } @@ -353,9 +353,8 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId inlierPose = Affine3f(inlierConstraint); inliers = localInliers; - /* std::cout << "Rvec: " << rvec << " tvec: " << tvec << "\n"; */ - std::cout << inlierPose.matrix << "\n"; - std::cout << " inliers: " << inliers << "\n"; + /* std::cout << inlierPose.matrix << "\n"; */ + /* std::cout << " inliers: " << inliers << "\n"; */ if (inliers >= MIN_INLIERS) { @@ -404,7 +403,6 @@ void SubmapManager::updateMap(int _frameId, std::vector _frame if (success) { typename SubmapT::PoseConstraint& submapConstraint = getSubmap(submapId)->getConstraint(currSubmapId); - std::cout << "Updated fixed constraint with inlierPose: \n" << inlierPose.matrix << "\n"; submapConstraint.accumulatePose(inlierPose, inliers); std::cout << "Submap constraint estimated pose: \n" << submapConstraint.estimatedPose.matrix << "\n"; submapData.constraints.clear(); @@ -475,6 +473,21 @@ void SubmapManager::updateMap(int _frameId, std::vector _frame newSubmap->pyrPoints = _framePoints; newSubmap->pyrNormals = _frameNormals; } + + if(_frameId%100 == 0) + { + for(size_t i = 0; i < submapList.size(); i++) + { + Ptr currSubmap = submapList.at(i); + typename SubmapT::Constraints::const_iterator itBegin = currSubmap->constraints.begin(); + std::cout << "Constraint list for SubmapID: " << currSubmap->id << "\n"; + for(typename SubmapT::Constraints::const_iterator it = itBegin; it != currSubmap->constraints.end(); ++it) + { + const typename SubmapT::PoseConstraint& constraint = it->second; + std::cout << "[" << it->first << "] weight: " << constraint.weight << "\n " << constraint.estimatedPose.matrix << " \n"; + } + } + } } } // namespace kinfu From 963b086db16d195e0bb256288abcd1b7d789107b Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Fri, 7 Aug 2020 22:12:26 -0400 Subject: [PATCH 23/36] - Implement posegraph update, Gauss newton is unstable - Minor changes to Gauss newton and Sparse matrix. Residual still increases slightly over iterations --- modules/rgbd/src/large_kinfu.cpp | 14 ++- modules/rgbd/src/pose_graph.cpp | 126 ++++++++++++++++------- modules/rgbd/src/pose_graph.hpp | 12 ++- modules/rgbd/src/sparse_block_matrix.hpp | 26 ++--- modules/rgbd/src/submap.hpp | 68 +++++++++--- 5 files changed, 173 insertions(+), 73 deletions(-) diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 8a324ea5113..eff94026cce 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -227,7 +227,6 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) std::cout << "Current frameID: " << frameCounter << "\n"; for (const auto& it : submapMgr->activeSubmaps) { - //! Iterate over map? int currTrackingId = it.first; auto submapData = it.second; Ptr> currTrackingSubmap = submapMgr->getSubmap(currTrackingId); @@ -250,6 +249,7 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) else { std::cout << "Tracking failed" << std::endl; + continue; } //2. Integrate @@ -272,7 +272,17 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) } //4. Update map - submapMgr->updateMap(frameCounter, newPoints, newNormals); + bool isMapUpdated = submapMgr->updateMap(frameCounter, newPoints, newNormals); + + if(isMapUpdated) + { + // TODO: Convert constraints to posegraph + PoseGraph poseGraph = submapMgr->createPoseGraph(); + std::cout << "Created posegraph\n"; + Optimizer::optimizeGaussNewton(Optimizer::Params(), poseGraph); + /* submapMgr->adjustMap(poseGraph); */ + + } std::cout << "Number of submaps: " << submapMgr->submapList.size() << "\n"; frameCounter++; diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index bcb9a9c38ea..36735a21a12 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -15,7 +15,6 @@ namespace cv { namespace kinfu { - bool PoseGraph::isValid() const { int numNodes = int(nodes.size()); @@ -34,7 +33,7 @@ bool PoseGraph::isValid() const { int currNodeId = nodesToVisit.back(); nodesToVisit.pop_back(); - + std::cout << "Visiting node: " << currNodeId << "\n"; // Since each node does not maintain its neighbor list for (int i = 0; i < numEdges; i++) { @@ -49,20 +48,20 @@ bool PoseGraph::isValid() const { nextNodeId = potentialEdge.getSourceNodeId(); } - + std::cout << "Next node: " << nextNodeId << std::endl; if (nextNodeId != -1) { - if(nodesVisited.count(currNodeId) == 0) + if (nodesVisited.count(currNodeId) == 0) { nodesVisited.insert(currNodeId); + nodesToVisit.push_back(nextNodeId); } - nodesToVisit.push_back(nextNodeId); } } } isGraphConnected = (int(nodesVisited.size()) == numNodes); - + std::cout << "IsGraphConnected: " << isGraphConnected << std::endl; bool invalidEdgeNode = false; for (int i = 0; i < numEdges; i++) { @@ -81,26 +80,37 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) { int numEdges = int(edges.size()); - float residual = 0; + float residual = 0.0f; - //! Lmabda to calculate error jacobian for a particular constraint + // Lambda to calculate the error Jacobians w.r.t source and target poses auto calcErrorJacobian = [](Matx66f& jacSource, Matx66f& jacTarget, const Affine3f& relativePoseMeas, - const Affine3f& sourcePose, const Affine3f& targetPose) -> void { + const Affine3f& sourcePose, const Affine3f& targetPose) -> void + { const Affine3f relativePoseMeas_inv = relativePoseMeas.inv(); const Affine3f targetPose_inv = targetPose.inv(); for (int i = 0; i < 6; i++) { Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(generatorJacobian[i]) * sourcePose; - Vec3f rot = dError_dSource.rvec(); - Vec3f trans = dError_dSource.translation(); - jacSource.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); + Vec3f rot = dError_dSource.rvec(); + Vec3f trans = dError_dSource.translation(); + jacSource.val[i + 6*0] = rot(0); + jacSource.val[i + 6*1] = rot(1); + jacSource.val[i + 6*2] = rot(2); + jacSource.val[i + 6*3] = trans(0); + jacSource.val[i + 6*4] = trans(1); + jacSource.val[i + 6*5] = trans(2); } for (int i = 0; i < 6; i++) { Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(-generatorJacobian[i]) * sourcePose; - Vec3f rot = dError_dSource.rvec(); - Vec3f trans = dError_dSource.translation(); - jacTarget.col(i) = Vec6f(rot(0), rot(1), rot(2), trans(0), trans(1), trans(2)); + Vec3f rot = dError_dSource.rvec(); + Vec3f trans = dError_dSource.translation(); + jacTarget.val[i + 6*0] = rot(0); + jacTarget.val[i + 6*1] = rot(1); + jacTarget.val[i + 6*2] = rot(2); + jacTarget.val[i + 6*3] = trans(0); + jacTarget.val[i + 6*4] = trans(1); + jacTarget.val[i + 6*5] = trans(2); } }; @@ -117,35 +127,51 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) //! Edge transformation is source to target. (relativeConstraint should be identity if no error) Affine3f poseConstraint = currEdge.transformation.inv() * relativeSourceToTarget; - Vec6f error; + std::cout << "RelativeSourceToTarget: \n" << relativeSourceToTarget.matrix << "\n"; + std::cout << "Edge estimated transformation: \n" << currEdge.transformation.matrix << "\n"; + std::cout << "PoseConstraint: \n" << poseConstraint.matrix << "\n"; + + Matx61f error; Vec3f rot = poseConstraint.rvec(); Vec3f trans = poseConstraint.translation(); - error[0] = rot[0]; - error[1] = rot[1]; - error[2] = rot[2]; - error[3] = trans[0]; - error[4] = trans[1]; - error[5] = trans[2]; + error.val[0] = rot[0]; + error.val[1] = rot[1]; + error.val[2] = rot[2]; + error.val[3] = trans[0]; + error.val[4] = trans[1]; + error.val[5] = trans[2]; + + std::cout << "Error vector: \n" << error << std::endl; Matx66f jacSource, jacTarget; calcErrorJacobian(jacSource, jacTarget, currEdge.transformation, sourcePose, targetPose); + std::cout << "Source Jacobian: \n" << jacSource << std::endl; + std::cout << "Target Jacobian: \n" << jacTarget << std::endl; Matx16f errorTransposeInfo = error.t() * currEdge.information; Matx66f jacSourceTransposeInfo = jacSource.t() * currEdge.information; Matx66f jacTargetTransposeInfo = jacTarget.t() * currEdge.information; - residual += (errorTransposeInfo * error)[0]; + /* std::cout << "errorTransposeInfo: " << errorTransposeInfo << "\n"; */ + /* std::cout << "jacSourceTransposeInfo: " << jacSourceTransposeInfo << "\n"; */ + /* std::cout << "jacTargetTransposeInfo: " << jacTargetTransposeInfo << "\n"; */ + + float res = std::sqrt((errorTransposeInfo * error).val[0]); + residual += res; + std::cout << "sourceNodeId: " << sourceNodeId << " targetNodeId: " << targetNodeId << std::endl; H.refBlock(sourceNodeId, sourceNodeId) += jacSourceTransposeInfo * jacSource; H.refBlock(targetNodeId, targetNodeId) += jacTargetTransposeInfo * jacTarget; H.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget; H.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource; - B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += errorTransposeInfo * jacSource; - B.rowRange(6 * targetNodeId, 6 * (targetNodeId + 1)) += errorTransposeInfo * jacTarget; + + B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += (errorTransposeInfo * jacSource).reshape<6, 1>(); + B.rowRange(6 * targetNodeId, 6 * (targetNodeId + 1)) += (errorTransposeInfo * jacTarget).reshape<6, 1>(); } + std::cout << "Residual value: " << residual << std::endl; return residual; } @@ -161,6 +187,7 @@ PoseGraph PoseGraph::update(const Mat& deltaVec) Vec6f delta = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)); Affine3f pose = nodes.at(currentNodeId).getPose(); Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3)); + std::cout << "Current Delta for node ID: " << currentNodeId << " \n" << currDelta.matrix << std::endl; Affine3f updatedPose = currDelta * pose; updatedPoseGraph.nodes.at(currentNodeId).setPose(updatedPose); @@ -169,6 +196,20 @@ PoseGraph PoseGraph::update(const Mat& deltaVec) return updatedPoseGraph; } +Mat PoseGraph::getVector() +{ + int numNodes = int(nodes.size()); + Mat vector(6 * numNodes, 1, CV_32F, Scalar(0)); + for(int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) + { + Affine3f pose = nodes.at(currentNodeId).getPose(); + Vec3f rot = pose.rvec(); + Vec3f trans = pose.translation(); + vector.rowRange(6 * currentNodeId, 6 * (currentNodeId+1)) = Vec6f(rot.val[0], rot.val[1], rot.val[2], trans.val[0], trans.val[1], trans.val[2]); + } + return vector; +} + //! NOTE: We follow left-composition for the infinitesimal pose update void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& poseGraph) { @@ -187,38 +228,49 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges << std::endl; int iter = 0; - //! converged is set to true when error/residual is within some limit - bool converged = false; float prevResidual = std::numeric_limits::max(); do { + std::cout << "Current iteration: " << iter << std::endl; //! ApproxH = Approximate Hessian = J^T * information * J BlockSparseMat ApproxH(numNodes); - Mat B(6 * numNodes, 1, CV_32F); + Mat B = cv::Mat::zeros(6 * numNodes, 1, CV_32F); float currentResidual = poseGraph.createLinearSystem(ApproxH, B); - + std::cout << "Number of non-zero blocks in H: " << ApproxH.nonZeroBlocks() << "\n"; Mat delta(6 * numNodes, 1, CV_32F); + Mat X = poseGraph.getVector(); bool success = sparseSolve(ApproxH, B, delta); if (!success) { CV_Error(Error::StsNoConv, "Sparse solve failed"); return; } + std::cout << "delta update: " << delta << "\n delta norm: " << cv::norm(delta) << " X norm: " << cv::norm(X) << std::endl; + //! Check delta + if(cv::norm(delta) < 1e-6f * (cv::norm(X) + 1e-6f)) + { + std::cout << "Delta norm[" << cv::norm(delta) << "] < 1e-6f * X norm[" << cv::norm(X) <<"] + 1e-6f\n"; + break; + } + std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; //! TODO: Improve criterion and allow small increments in residual - if((currentResidual - prevResidual) > params.maxAcceptableResIncre) + if ((currentResidual - prevResidual) > params.maxAcceptableResIncre) + { + std::cout << "Current residual increased from prevResidual by at least " << params.maxAcceptableResIncre << std::endl; break; - if ((currentResidual - params.minResidual) < 0.00001f) - converged = true; - + } + //! If updates don't improve a lot means loss function basin has been reached + if ((currentResidual - params.minResidual) < 1e-5f) + { + std::cout << "Gauss newton converged \n"; + break; + } poseGraph = poseGraph.update(delta); - std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; - prevResidual = currentResidual; - - } while (iter < params.maxNumIters && !converged); + } while (iter < params.maxNumIters); } } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index f5aa176566d..0a314185d24 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -18,8 +18,8 @@ namespace kinfu struct PoseGraphNode { public: - explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) : nodeId(_nodeId), isFixed(false), pose(_pose) {} - virtual ~PoseGraphNode(); + explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) : nodeId(_nodeId), isFixed(false), pose(_pose) {} + virtual ~PoseGraphNode() = default; int getId() const { return nodeId; } Affine3f getPose() const { return pose; } @@ -64,7 +64,8 @@ struct PoseGraphEdge //! Jose Luis Blanco //! Compactly represents the jacobian of the SE3 generator // clang-format off -static const std::array generatorJacobian = { // alpha +static const std::array generatorJacobian = { + // alpha Matx44f(0, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, 0, @@ -126,11 +127,12 @@ class PoseGraph int getNumNodes() const { return nodes.size(); } int getNumEdges() const { return edges.size(); } + Mat getVector(); + //! @brief: Constructs a linear system and returns the residual of the current system float createLinearSystem(BlockSparseMat& H, Mat& B); private: - NodeVector nodes; EdgeVector edges; }; @@ -144,7 +146,7 @@ struct Params float maxAcceptableResIncre; // TODO: Refine these constants - Params() : maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-3f){}; + Params() : maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-2f){}; virtual ~Params() = default; }; diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 5db7a48b207..d4a56c4b979 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -9,9 +9,10 @@ #if defined(HAVE_EIGEN) #include -#include "opencv2/core/eigen.hpp" #include #include + +#include "opencv2/core/eigen.hpp" #endif namespace cv @@ -25,7 +26,6 @@ namespace kinfu template struct BlockSparseMat { - struct Point2iHash { size_t operator()(const cv::Point2i& point) const noexcept @@ -41,7 +41,6 @@ struct BlockSparseMat typedef std::unordered_map IDtoBlockValueMap; static constexpr int blockSize = blockM * blockN; - BlockSparseMat(int _nBlocks) : nBlocks(_nBlocks), ijValue() {} MatType& refBlock(int i, int j) @@ -50,7 +49,7 @@ struct BlockSparseMat auto it = ijValue.find(p); if (it == ijValue.end()) { - it = ijValue.insert({ p, Matx<_Tp, blockM, blockN>() }).first; + it = ijValue.insert({ p, Matx<_Tp, blockM, blockN>::zeros() }).first; } return it->second; } @@ -61,29 +60,28 @@ struct BlockSparseMat return refBlock(ib.x, ib.y)(iv.x, iv.y); } -#if defined (HAVE_EIGEN) +#if defined(HAVE_EIGEN) Eigen::SparseMatrix<_Tp> toEigen() const { std::vector> tripletList; - tripletList.reserve(ijValue.size() * blockSize * blockSize); - + tripletList.reserve(ijValue.size() * blockSize); for (auto ijv : ijValue) { int xb = ijv.first.x, yb = ijv.first.y; MatType vblock = ijv.second; - for (int i = 0; i < blockSize; i++) + for (int i = 0; i < blockM; i++) { - for (int j = 0; j < blockSize; j++) + for (int j = 0; j < blockN; j++) { float val = vblock(i, j); if (abs(val) >= NON_ZERO_VAL_THRESHOLD) { - tripletList.push_back(Eigen::Triplet(blockSize * xb + i, blockSize * yb + j, val)); + tripletList.push_back(Eigen::Triplet(blockM * xb + i, blockN * yb + j, val)); } } } } - Eigen::SparseMatrix<_Tp> EigenMat(blockSize * nBlocks, blockSize * nBlocks); + Eigen::SparseMatrix<_Tp> EigenMat(blockM * nBlocks, blockN * nBlocks); EigenMat.setFromTriplets(tripletList.begin(), tripletList.end()); EigenMat.makeCompressed(); @@ -104,13 +102,11 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& bool result = false; #if defined(HAVE_EIGEN) - std::cout << "starting eigen-insertion..." << std::endl; - Eigen::SparseMatrix bigA = H.toEigen(); Eigen::VectorXf bigB; cv2eigen(B, bigB); - //!TODO: Check if this is required + //! TODO: Check if this is required Eigen::SparseMatrix bigATranspose = bigA.transpose(); if (!bigA.isApprox(bigATranspose)) { @@ -118,13 +114,11 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& return result; } - // TODO: try this, LLT and Cholesky Eigen::SimplicialLDLT> solver; std::cout << "starting eigen-compute..." << std::endl; solver.compute(bigA); - if (solver.info() != Eigen::Success) { std::cout << "failed to eigen-decompose" << std::endl; diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 705e2a88f2d..09057744a00 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -163,8 +163,10 @@ class SubmapManager Ptr getSubmap(int _id) const; Ptr getCurrentSubmap(void) const; - bool estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose); - void updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals); + int estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose); + bool updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals); + + PoseGraph createPoseGraph(); VolumeParams volumeParams; @@ -245,12 +247,13 @@ bool SubmapManager::shouldCreateSubmap(int currFrameId) } template -bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose) +int SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose) { static constexpr int MAX_ITER = 10; static constexpr float CONVERGE_WEIGHT_THRESHOLD = 0.01f; static constexpr float INLIER_WEIGHT_THRESH = 0.8f; static constexpr int MIN_INLIERS = 10; + static constexpr int MAX_TRACKING_ATTEMPTS = 25; //! thresh = HUBER_THRESH auto huberWeight = [](float residual, float thresh = 0.1f) -> float { @@ -358,9 +361,14 @@ bool SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId if (inliers >= MIN_INLIERS) { - return true; + return 1; } - return false; + if(fromSubmapData.trackingAttempts - inliers > (MAX_TRACKING_ATTEMPTS - MIN_INLIERS)) + { + return -1; + } + + return 0; } template @@ -385,10 +393,13 @@ bool SubmapManager::shouldChangeCurrSubmap(int _frameId, int toSubmapId } template -void SubmapManager::updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals) +bool SubmapManager::updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals) { - const int currSubmapId = getCurrentSubmap()->id; + bool mapUpdated = false; int changedCurrentMapId = -1; + + const int currSubmapId = getCurrentSubmap()->id; + for (auto& it : activeSubmaps) { int submapId = it.first; @@ -398,9 +409,9 @@ void SubmapManager::updateMap(int _frameId, std::vector _frame // Check with previous estimate int inliers; Affine3f inlierPose; - bool success = estimateConstraint(submapId, currSubmapId, inliers, inlierPose); + int constraintUpdate = estimateConstraint(submapId, currSubmapId, inliers, inlierPose); std::cout << "SubmapId: " << submapId << " Tracking attempts: " << submapData.trackingAttempts << "\n"; - if (success) + if (constraintUpdate == 1) { typename SubmapT::PoseConstraint& submapConstraint = getSubmap(submapId)->getConstraint(currSubmapId); submapConstraint.accumulatePose(inlierPose, inliers); @@ -408,17 +419,16 @@ void SubmapManager::updateMap(int _frameId, std::vector _frame submapData.constraints.clear(); submapData.trackingAttempts = 0; - //! TODO: Check for visibility and change currentActiveMap if (shouldChangeCurrSubmap(_frameId, submapId)) { - //! TODO: Change submap and update constraints accordingly std::cout << "Should change current map to the new map\n"; changedCurrentMapId = submapId; } + mapUpdated = true; } - else + else if(constraintUpdate == -1) { - //! If tried tracking for threshold number of times, mark the data as lost + submapData.type = Type::LOST; } } } @@ -474,6 +484,7 @@ void SubmapManager::updateMap(int _frameId, std::vector _frame newSubmap->pyrNormals = _frameNormals; } + // Debugging only if(_frameId%100 == 0) { for(size_t i = 0; i < submapList.size(); i++) @@ -488,6 +499,37 @@ void SubmapManager::updateMap(int _frameId, std::vector _frame } } } + + return mapUpdated; +} + +template +PoseGraph SubmapManager::createPoseGraph() +{ + PoseGraph localPoseGraph; + + for(const Ptr currSubmap : submapList) + { + PoseGraphNode currNode(currSubmap->id, currSubmap->pose); + if(currSubmap->id == 0) + { + currNode.setFixed(); + } + localPoseGraph.addNode(currNode); + } + + for(const Ptr currSubmap : submapList) + { + const typename SubmapT::Constraints& constraintList = currSubmap->constraints; + for(const auto& currConstraintPair : constraintList) + { + // TODO: Handle case with duplicate constraints A -> B and B -> A + Matx66f informationMatrix = Matx66f::eye() * (currConstraintPair.second.weight/10); + PoseGraphEdge currEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix); + localPoseGraph.addEdge(currEdge); + } + } + return localPoseGraph; } } // namespace kinfu From 713cdef9333f0e6d898a2378207d32d1136c6a71 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Mon, 17 Aug 2020 03:30:31 -0400 Subject: [PATCH 24/36] Implement simplified levenberg marquardt --- modules/rgbd/src/hash_tsdf.cpp | 2 +- modules/rgbd/src/large_kinfu.cpp | 6 +- modules/rgbd/src/pose_graph.cpp | 197 ++++++++++++++++++----- modules/rgbd/src/pose_graph.hpp | 19 ++- modules/rgbd/src/sparse_block_matrix.hpp | 28 ++-- modules/rgbd/src/submap.hpp | 38 +++-- 6 files changed, 221 insertions(+), 69 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 8fe26d76caf..a9614cd51ed 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -347,7 +347,7 @@ struct HashRaycastInvoker : ParallelLoopBody float stepSize = 0.5f * blockSize; cv::Vec3i volUnitLocalIdx; - //! Does the subvolume exist in hashtable + //! The subvolume exists in hashtable if (it != volume.volumeUnits.end()) { currVolumeUnit = std::dynamic_pointer_cast(it->second.pVolume); diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index eff94026cce..f337b1ad609 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -277,10 +277,10 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) if(isMapUpdated) { // TODO: Convert constraints to posegraph - PoseGraph poseGraph = submapMgr->createPoseGraph(); + PoseGraph poseGraph = submapMgr->MapToPoseGraph(); std::cout << "Created posegraph\n"; - Optimizer::optimizeGaussNewton(Optimizer::Params(), poseGraph); - /* submapMgr->adjustMap(poseGraph); */ + Optimizer::optimizeLevenberg(Optimizer::Params(), poseGraph); + submapMgr->PoseGraphToMap(poseGraph); } std::cout << "Number of submaps: " << submapMgr->submapList.size() << "\n"; diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 36735a21a12..207026191a5 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -4,9 +4,11 @@ #include "pose_graph.hpp" +#include #include #include +#include "opencv2/core/base.hpp" #include "opencv2/core/eigen.hpp" #include "opencv2/core/mat.hpp" #include "opencv2/core/matx.hpp" @@ -15,6 +17,23 @@ namespace cv { namespace kinfu { +void PoseGraph::addEdge(const PoseGraphEdge& edge) +{ + /* std::vector::iterator it = std::find(edges.begin(), edges.end(), edge); */ + /* if(it == edges.end()) */ + /* { */ + edges.push_back(edge); + /* } */ + /* else */ + /* { */ + /* PoseGraphEdge& existingEdge = *it; */ + /* if(existingEdge.getSourceNodeId() == edge.getTargetNodeId()) */ + /* { */ + /* Affine3f transform = edge.transformation.inv(); */ + + /* } */ +} + bool PoseGraph::isValid() const { int numNodes = int(nodes.size()); @@ -84,33 +103,34 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) // Lambda to calculate the error Jacobians w.r.t source and target poses auto calcErrorJacobian = [](Matx66f& jacSource, Matx66f& jacTarget, const Affine3f& relativePoseMeas, - const Affine3f& sourcePose, const Affine3f& targetPose) -> void - { + const Affine3f& sourcePose, const Affine3f& targetPose) -> void { const Affine3f relativePoseMeas_inv = relativePoseMeas.inv(); const Affine3f targetPose_inv = targetPose.inv(); for (int i = 0; i < 6; i++) { - Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(generatorJacobian[i]) * sourcePose; - Vec3f rot = dError_dSource.rvec(); - Vec3f trans = dError_dSource.translation(); - jacSource.val[i + 6*0] = rot(0); - jacSource.val[i + 6*1] = rot(1); - jacSource.val[i + 6*2] = rot(2); - jacSource.val[i + 6*3] = trans(0); - jacSource.val[i + 6*4] = trans(1); - jacSource.val[i + 6*5] = trans(2); + Affine3f dError_dSource = + relativePoseMeas_inv * targetPose_inv * cv::Affine3f(generatorJacobian[i]) * sourcePose; + Vec3f rot = dError_dSource.rvec(); + Vec3f trans = dError_dSource.translation(); + jacSource.val[i + 6 * 0] = rot(0); + jacSource.val[i + 6 * 1] = rot(1); + jacSource.val[i + 6 * 2] = rot(2); + jacSource.val[i + 6 * 3] = trans(0); + jacSource.val[i + 6 * 4] = trans(1); + jacSource.val[i + 6 * 5] = trans(2); } for (int i = 0; i < 6; i++) { - Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(-generatorJacobian[i]) * sourcePose; - Vec3f rot = dError_dSource.rvec(); - Vec3f trans = dError_dSource.translation(); - jacTarget.val[i + 6*0] = rot(0); - jacTarget.val[i + 6*1] = rot(1); - jacTarget.val[i + 6*2] = rot(2); - jacTarget.val[i + 6*3] = trans(0); - jacTarget.val[i + 6*4] = trans(1); - jacTarget.val[i + 6*5] = trans(2); + Affine3f dError_dSource = + relativePoseMeas_inv * targetPose_inv * cv::Affine3f(-generatorJacobian[i]) * sourcePose; + Vec3f rot = dError_dSource.rvec(); + Vec3f trans = dError_dSource.translation(); + jacTarget.val[i + 6 * 0] = rot(0); + jacTarget.val[i + 6 * 1] = rot(1); + jacTarget.val[i + 6 * 2] = rot(2); + jacTarget.val[i + 6 * 3] = trans(0); + jacTarget.val[i + 6 * 4] = trans(1); + jacTarget.val[i + 6 * 5] = trans(2); } }; @@ -127,8 +147,8 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) //! Edge transformation is source to target. (relativeConstraint should be identity if no error) Affine3f poseConstraint = currEdge.transformation.inv() * relativeSourceToTarget; - std::cout << "RelativeSourceToTarget: \n" << relativeSourceToTarget.matrix << "\n"; - std::cout << "Edge estimated transformation: \n" << currEdge.transformation.matrix << "\n"; + /* std::cout << "RelativeSourceToTarget: \n" << relativeSourceToTarget.matrix << "\n"; */ + /* std::cout << "Edge estimated transformation: \n" << currEdge.transformation.matrix << "\n"; */ std::cout << "PoseConstraint: \n" << poseConstraint.matrix << "\n"; Matx61f error; @@ -142,12 +162,12 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) error.val[4] = trans[1]; error.val[5] = trans[2]; - std::cout << "Error vector: \n" << error << std::endl; + /* std::cout << "Error vector: \n" << error << std::endl; */ Matx66f jacSource, jacTarget; calcErrorJacobian(jacSource, jacTarget, currEdge.transformation, sourcePose, targetPose); - std::cout << "Source Jacobian: \n" << jacSource << std::endl; - std::cout << "Target Jacobian: \n" << jacTarget << std::endl; + /* std::cout << "Source Jacobian: \n" << jacSource << std::endl; */ + /* std::cout << "Target Jacobian: \n" << jacTarget << std::endl; */ Matx16f errorTransposeInfo = error.t() * currEdge.information; Matx66f jacSourceTransposeInfo = jacSource.t() * currEdge.information; @@ -166,7 +186,6 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) H.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget; H.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource; - B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += (errorTransposeInfo * jacSource).reshape<6, 1>(); B.rowRange(6 * targetNodeId, 6 * (targetNodeId + 1)) += (errorTransposeInfo * jacTarget).reshape<6, 1>(); } @@ -184,10 +203,10 @@ PoseGraph PoseGraph::update(const Mat& deltaVec) for (int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) { - Vec6f delta = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)); - Affine3f pose = nodes.at(currentNodeId).getPose(); - Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3)); - std::cout << "Current Delta for node ID: " << currentNodeId << " \n" << currDelta.matrix << std::endl; + Vec6f delta = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)); + Affine3f pose = nodes.at(currentNodeId).getPose(); + delta = -delta; + Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3)); Affine3f updatedPose = currDelta * pose; updatedPoseGraph.nodes.at(currentNodeId).setPose(updatedPose); @@ -200,16 +219,29 @@ Mat PoseGraph::getVector() { int numNodes = int(nodes.size()); Mat vector(6 * numNodes, 1, CV_32F, Scalar(0)); - for(int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) + for (int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) { Affine3f pose = nodes.at(currentNodeId).getPose(); - Vec3f rot = pose.rvec(); - Vec3f trans = pose.translation(); - vector.rowRange(6 * currentNodeId, 6 * (currentNodeId+1)) = Vec6f(rot.val[0], rot.val[1], rot.val[2], trans.val[0], trans.val[1], trans.val[2]); + Vec3f rot = pose.rvec(); + Vec3f trans = pose.translation(); + vector.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)) = + Vec6f(rot.val[0], rot.val[1], rot.val[2], trans.val[0], trans.val[1], trans.val[2]); } return vector; } +bool Optimizer::isStepSizeSmall(const Mat& delta, float minStepSize) +{ + float maxDeltaNorm = 0.0f; + for (int i = 0; i < delta.rows; i++) + { + float val = abs(delta.at(i, 0)); + if (val > maxDeltaNorm) + maxDeltaNorm = val; + } + return maxDeltaNorm < minStepSize; +} + //! NOTE: We follow left-composition for the infinitesimal pose update void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& poseGraph) { @@ -227,7 +259,7 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges << std::endl; - int iter = 0; + int iter = 0; float prevResidual = std::numeric_limits::max(); do { @@ -239,27 +271,30 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& float currentResidual = poseGraph.createLinearSystem(ApproxH, B); std::cout << "Number of non-zero blocks in H: " << ApproxH.nonZeroBlocks() << "\n"; Mat delta(6 * numNodes, 1, CV_32F); - Mat X = poseGraph.getVector(); + Mat X = poseGraph.getVector(); bool success = sparseSolve(ApproxH, B, delta); if (!success) { CV_Error(Error::StsNoConv, "Sparse solve failed"); return; } - std::cout << "delta update: " << delta << "\n delta norm: " << cv::norm(delta) << " X norm: " << cv::norm(X) << std::endl; + std::cout << "delta update: " << delta << "\n delta norm: " << cv::norm(delta) << " X norm: " << cv::norm(X) + << std::endl; //! Check delta - if(cv::norm(delta) < 1e-6f * (cv::norm(X) + 1e-6f)) + if (isStepSizeSmall(delta, params.minStepSize)) { - std::cout << "Delta norm[" << cv::norm(delta) << "] < 1e-6f * X norm[" << cv::norm(X) <<"] + 1e-6f\n"; + std::cout << "Delta norm[" << cv::norm(delta) << "] < " << params.minStepSize << std::endl; break; } + std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; //! TODO: Improve criterion and allow small increments in residual if ((currentResidual - prevResidual) > params.maxAcceptableResIncre) { - std::cout << "Current residual increased from prevResidual by at least " << params.maxAcceptableResIncre << std::endl; + std::cout << "Current residual increased from prevResidual by at least " << params.maxAcceptableResIncre + << std::endl; break; } //! If updates don't improve a lot means loss function basin has been reached @@ -268,9 +303,89 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& std::cout << "Gauss newton converged \n"; break; } - poseGraph = poseGraph.update(delta); + poseGraph = poseGraph.update(delta); prevResidual = currentResidual; + iter++; } while (iter < params.maxNumIters); } + +void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& poseGraph) +{ + PoseGraph poseGraphOriginal = poseGraph; + + if (!poseGraphOriginal.isValid()) + { + CV_Error(Error::StsBadArg, "Invalid PoseGraph that is either not connected or has invalid nodes"); + return; + } + + int numNodes = poseGraph.getNumNodes(); + int numEdges = poseGraph.getNumEdges(); + + std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges << std::endl; + int iter = 0; + float lambda = 1e-3f; + const float factor = 2; + float prevResidual = std::numeric_limits::max(); + do + { + std::cout << "Current iteration: " << iter << std::endl; + + BlockSparseMat ApproxH(numNodes); + Mat B = cv::Mat::zeros(6 * numNodes, 1, CV_32F); + + float currentResidual = poseGraph.createLinearSystem(ApproxH, B); + Mat delta(6 * numNodes, 1, CV_32F); + Mat X = poseGraph.getVector(); + + //! H_LM = H + lambda * diag(H); + Mat Hdiag = ApproxH.diagonal(); + for (int i = 0; i < Hdiag.rows; i++) + { + ApproxH.refElem(i, i) += lambda * Hdiag.at(i, 0); + } + bool success = sparseSolve(ApproxH, B, delta); + if (!success) + { + CV_Error(Error::StsNoConv, "Sparse solve failed"); + return; + } + std::cout << "\n delta norm: " << cv::norm(delta) << " X norm: " << cv::norm(X) + << std::endl; + + //! If step size is too small break + if (isStepSizeSmall(delta, params.minStepSize)) + { + std::cout << "Step size is too small. stopping\n"; + break; + } + std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; + + float reduction = currentResidual - prevResidual; + if (reduction < 0) + { + //! Accept update + lambda = lambda / factor; + poseGraph = poseGraph.update(delta); + prevResidual = currentResidual; + std::cout << "Accepting update: " << reduction << " new lambda: " << lambda << std::endl; + } + else if (reduction > 0) + { + //! Reject update + lambda = lambda * factor; + std::cout << "Rejecting update: " << reduction << " new lambda: " << lambda << std::endl; + } + + //! TODO: Change to another paramter, this is dummy + if (abs(reduction) < 1e-4f) + { + std::cout << "Reduction in residual too small, converged " << std::endl; + break; + } + iter++; + } while (iter < params.maxNumIters); +} + } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 0a314185d24..ce53f9188c2 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -25,6 +25,7 @@ struct PoseGraphNode Affine3f getPose() const { return pose; } void setPose(const Affine3f& _pose) { pose = _pose; } void setFixed(bool val = true) { isFixed = val; } + bool isPoseFixed() const { return isFixed; } private: int nodeId; @@ -53,6 +54,14 @@ struct PoseGraphEdge int getSourceNodeId() const { return sourceNodeId; } int getTargetNodeId() const { return targetNodeId; } + bool operator==(const PoseGraphEdge& edge) + { + if((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) || + (edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId)) + return true; + return false; + } + public: int sourceNodeId; int targetNodeId; @@ -98,6 +107,7 @@ static const std::array generatorJacobian = { }; // clang-format on + class PoseGraph { public: @@ -112,7 +122,7 @@ class PoseGraph PoseGraph& operator=(const PoseGraph& _poseGraph) = default; void addNode(const PoseGraphNode& node) { nodes.push_back(node); } - void addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } + void addEdge(const PoseGraphEdge& edge); bool nodeExists(int nodeId) const { @@ -132,7 +142,7 @@ class PoseGraph //! @brief: Constructs a linear system and returns the residual of the current system float createLinearSystem(BlockSparseMat& H, Mat& B); - private: + public: NodeVector nodes; EdgeVector edges; }; @@ -144,14 +154,17 @@ struct Params int maxNumIters; float minResidual; float maxAcceptableResIncre; + float minStepSize; // TODO: Refine these constants - Params() : maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-2f){}; + Params() : maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-3f), minStepSize(1e-4f){}; virtual ~Params() = default; }; void optimizeGaussNewton(const Params& params, PoseGraph& poseGraph); void optimizeLevenberg(const Params& params, PoseGraph& poseGraph); + +bool isStepSizeSmall(const Mat& delta, float minStepSize); } // namespace Optimizer } // namespace kinfu diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index d4a56c4b979..2bd0f10b533 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -5,6 +5,7 @@ #include #include +#include "opencv2/core/base.hpp" #include "opencv2/core/types.hpp" #if defined(HAVE_EIGEN) @@ -54,6 +55,19 @@ struct BlockSparseMat return it->second; } + Mat diagonal() + { + // Diagonal max length is the number of columns in the sparse matrix + int diagLength = blockN * nBlocks; + cv::Mat diag = cv::Mat::zeros(diagLength, 1, CV_32F); + + for (int i = 0; i < diagLength; i++) + { + diag.at(i, 0) = refElem(i, i); + } + return diag; + } + float& refElem(int i, int j) { Point2i ib(i / blockSize, j / blockSize), iv(i % blockSize, j % blockSize); @@ -100,34 +114,29 @@ struct BlockSparseMat static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& X) { bool result = false; - #if defined(HAVE_EIGEN) Eigen::SparseMatrix bigA = H.toEigen(); Eigen::VectorXf bigB; cv2eigen(B, bigB); - //! TODO: Check if this is required - Eigen::SparseMatrix bigATranspose = bigA.transpose(); - if (!bigA.isApprox(bigATranspose)) + Eigen::SparseMatrix bigAtranspose = bigA.transpose(); + if(!bigA.isApprox(bigAtranspose)) { - CV_Error(Error::StsBadArg, "Sparse Matrix is not symmetric"); + CV_Error(Error::StsBadArg, "H matrix is not symmetrical"); return result; } - // TODO: try this, LLT and Cholesky Eigen::SimplicialLDLT> solver; - std::cout << "starting eigen-compute..." << std::endl; solver.compute(bigA); if (solver.info() != Eigen::Success) { + std::cout << "solver.info(): " << solver.info() << std::endl; std::cout << "failed to eigen-decompose" << std::endl; result = false; } else { - std::cout << "starting eigen-solve..." << std::endl; - Eigen::VectorXf solutionX = solver.solve(bigB); if (solver.info() != Eigen::Success) { @@ -146,6 +155,5 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& #endif return result; } - } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 09057744a00..4f07ea7a9c4 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -166,7 +166,8 @@ class SubmapManager int estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose); bool updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals); - PoseGraph createPoseGraph(); + PoseGraph MapToPoseGraph(); + void PoseGraphToMap(const PoseGraph& updatedPoseGraph); VolumeParams volumeParams; @@ -504,19 +505,10 @@ bool SubmapManager::updateMap(int _frameId, std::vector _frame } template -PoseGraph SubmapManager::createPoseGraph() +PoseGraph SubmapManager::MapToPoseGraph() { PoseGraph localPoseGraph; - for(const Ptr currSubmap : submapList) - { - PoseGraphNode currNode(currSubmap->id, currSubmap->pose); - if(currSubmap->id == 0) - { - currNode.setFixed(); - } - localPoseGraph.addNode(currNode); - } for(const Ptr currSubmap : submapList) { @@ -529,9 +521,33 @@ PoseGraph SubmapManager::createPoseGraph() localPoseGraph.addEdge(currEdge); } } + + for(const Ptr currSubmap : submapList) + { + PoseGraphNode currNode(currSubmap->id, currSubmap->pose); + if(currSubmap->id == 0) + { + currNode.setFixed(); + } + localPoseGraph.addNode(currNode); + } + + + return localPoseGraph; } +template +void SubmapManager::PoseGraphToMap(const PoseGraph &updatedPoseGraph) +{ + for(const Ptr currSubmap : submapList) + { + const PoseGraphNode& currNode = updatedPoseGraph.nodes.at(currSubmap->id); + if(!currNode.isPoseFixed()) + currSubmap->pose = currNode.getPose(); + } +} + } // namespace kinfu } // namespace cv #endif /* ifndef __OPENCV_RGBD_SUBMAP_HPP__ */ From bac316b27ee7fe099615550aa44dc0eb0d02d237 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 18 Aug 2020 22:03:31 -0400 Subject: [PATCH 25/36] Bug fixes for Levenberg Marquardt and minor changes --- modules/rgbd/include/opencv2/rgbd/volume.hpp | 20 ++- modules/rgbd/src/hash_tsdf.cpp | 8 +- modules/rgbd/src/pose_graph.cpp | 160 +++++++++++-------- modules/rgbd/src/pose_graph.hpp | 4 +- modules/rgbd/src/sparse_block_matrix.hpp | 6 +- modules/rgbd/src/submap.hpp | 6 +- modules/rgbd/src/tsdf.cpp | 15 +- modules/rgbd/src/volume.cpp | 4 +- 8 files changed, 133 insertions(+), 90 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 1dbf3ada146..5d99ab16293 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -19,16 +19,19 @@ class CV_EXPORTS_W Volume { public: Volume(float _voxelSize, Matx44f _pose, float _raycastStepFactor) - : voxelSize(_voxelSize), voxelSizeInv(1.0f / voxelSize), pose(_pose), raycastStepFactor(_raycastStepFactor) + : voxelSize(_voxelSize), + voxelSizeInv(1.0f / voxelSize), + pose(_pose), + raycastStepFactor(_raycastStepFactor) { } virtual ~Volume(){}; - virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, - const int frameId = 0) = 0; - virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, Size frameSize, OutputArray points, - OutputArray normals) const = 0; + virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, + const kinfu::Intr& intrinsics, const int frameId = 0) = 0; + virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, Size frameSize, + OutputArray points, OutputArray normals) const = 0; virtual void fetchNormals(InputArray points, OutputArray _normals) const = 0; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0; virtual void reset() = 0; @@ -101,10 +104,11 @@ struct VolumeParams static Ptr coarseParams(VolumeType _volumeType); }; -Ptr makeVolume(const VolumeParams& _volumeParams); +CV_EXPORTS_W Ptr makeVolume(const VolumeParams& _volumeParams); +CV_EXPORTS_W Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, + float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, Vec3i _resolution); -Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, - int _maxWeight, float _truncateThreshold, Vec3i _resolution); } // namespace kinfu } // namespace cv #endif diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 552387e75d9..ca8678f3c4d 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -41,7 +41,7 @@ HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, Matx44f _pose, float _ray } HashTSDFVolumeCPU::HashTSDFVolumeCPU(const VolumeParams& _params, bool _zFirstMemOrder) - : Base(_params.voxelSize, _params.pose, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + : Base(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) { } @@ -264,7 +264,7 @@ inline TsdfVoxel HashTSDFVolumeCPU::at_(const Point3f& point) const return volumeUnit->at(volUnitLocalIdx); } -inline TsdfType HashTSDFVolumeCPU::interpolateVoxel(const Point3f& point) const +inline TsdfType HashTSDFVolumeCPU::interpolateVoxel_(const Point3f& point) const { Point3f neighbourCoords[] = { Point3f(0, 0, 0), Point3f(0, 0, 1), Point3f(0, 1, 0), Point3f(0, 1, 1), Point3f(1, 0, 0), Point3f(1, 0, 1), Point3f(1, 1, 0), Point3f(1, 1, 1) }; @@ -305,8 +305,8 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel_(Point3f point) const pointPrev[c] -= voxelSize; pointNext[c] += voxelSize; - // normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; - normal[c] = interpolateVoxel(Point3f(pointNext)) - interpolateVoxel(Point3f(pointPrev)); + normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; + /* normal[c] = interpolateVoxel(Point3f(pointNext)) - interpolateVoxel(Point3f(pointPrev)); */ pointPrev[c] = pointVec[c]; pointNext[c] = pointVec[c]; diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 207026191a5..a5217f51c5f 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -70,9 +70,9 @@ bool PoseGraph::isValid() const std::cout << "Next node: " << nextNodeId << std::endl; if (nextNodeId != -1) { - if (nodesVisited.count(currNodeId) == 0) + nodesVisited.insert(currNodeId); + if (nodesVisited.count(nextNodeId) == 0) { - nodesVisited.insert(currNodeId); nodesToVisit.push_back(nextNodeId); } } @@ -86,7 +86,8 @@ bool PoseGraph::isValid() const { const PoseGraphEdge& edge = edges.at(i); // edges have spurious source/target nodes - if ((nodesVisited.count(edge.getSourceNodeId()) != 1) || (nodesVisited.count(edge.getTargetNodeId()) != 1)) + if ((nodesVisited.count(edge.getSourceNodeId()) != 1) || + (nodesVisited.count(edge.getTargetNodeId()) != 1)) { invalidEdgeNode = true; break; @@ -102,14 +103,15 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) float residual = 0.0f; // Lambda to calculate the error Jacobians w.r.t source and target poses - auto calcErrorJacobian = [](Matx66f& jacSource, Matx66f& jacTarget, const Affine3f& relativePoseMeas, - const Affine3f& sourcePose, const Affine3f& targetPose) -> void { + auto calcErrorJacobian = [](Matx66f& jacSource, Matx66f& jacTarget, + const Affine3f& relativePoseMeas, const Affine3f& sourcePose, + const Affine3f& targetPose) -> void { const Affine3f relativePoseMeas_inv = relativePoseMeas.inv(); const Affine3f targetPose_inv = targetPose.inv(); for (int i = 0; i < 6; i++) { - Affine3f dError_dSource = - relativePoseMeas_inv * targetPose_inv * cv::Affine3f(generatorJacobian[i]) * sourcePose; + Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * + cv::Affine3f(generatorJacobian[i]) * sourcePose; Vec3f rot = dError_dSource.rvec(); Vec3f trans = dError_dSource.translation(); jacSource.val[i + 6 * 0] = rot(0); @@ -121,8 +123,8 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) } for (int i = 0; i < 6; i++) { - Affine3f dError_dSource = - relativePoseMeas_inv * targetPose_inv * cv::Affine3f(-generatorJacobian[i]) * sourcePose; + Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * + cv::Affine3f(-generatorJacobian[i]) * sourcePose; Vec3f rot = dError_dSource.rvec(); Vec3f trans = dError_dSource.translation(); jacTarget.val[i + 6 * 0] = rot(0); @@ -144,13 +146,10 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) const Affine3f& targetPose = nodes.at(targetNodeId).getPose(); Affine3f relativeSourceToTarget = targetPose.inv() * sourcePose; - //! Edge transformation is source to target. (relativeConstraint should be identity if no error) + //! Edge transformation is source to target. (relativeConstraint should be identity if no + //! error) Affine3f poseConstraint = currEdge.transformation.inv() * relativeSourceToTarget; - /* std::cout << "RelativeSourceToTarget: \n" << relativeSourceToTarget.matrix << "\n"; */ - /* std::cout << "Edge estimated transformation: \n" << currEdge.transformation.matrix << "\n"; */ - std::cout << "PoseConstraint: \n" << poseConstraint.matrix << "\n"; - Matx61f error; Vec3f rot = poseConstraint.rvec(); Vec3f trans = poseConstraint.translation(); @@ -162,35 +161,25 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) error.val[4] = trans[1]; error.val[5] = trans[2]; - /* std::cout << "Error vector: \n" << error << std::endl; */ - Matx66f jacSource, jacTarget; calcErrorJacobian(jacSource, jacTarget, currEdge.transformation, sourcePose, targetPose); - /* std::cout << "Source Jacobian: \n" << jacSource << std::endl; */ - /* std::cout << "Target Jacobian: \n" << jacTarget << std::endl; */ Matx16f errorTransposeInfo = error.t() * currEdge.information; Matx66f jacSourceTransposeInfo = jacSource.t() * currEdge.information; Matx66f jacTargetTransposeInfo = jacTarget.t() * currEdge.information; - /* std::cout << "errorTransposeInfo: " << errorTransposeInfo << "\n"; */ - /* std::cout << "jacSourceTransposeInfo: " << jacSourceTransposeInfo << "\n"; */ - /* std::cout << "jacTargetTransposeInfo: " << jacTargetTransposeInfo << "\n"; */ + residual += std::sqrt((errorTransposeInfo * error).val[0]); - float res = std::sqrt((errorTransposeInfo * error).val[0]); - residual += res; - - std::cout << "sourceNodeId: " << sourceNodeId << " targetNodeId: " << targetNodeId << std::endl; H.refBlock(sourceNodeId, sourceNodeId) += jacSourceTransposeInfo * jacSource; H.refBlock(targetNodeId, targetNodeId) += jacTargetTransposeInfo * jacTarget; H.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget; H.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource; - B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += (errorTransposeInfo * jacSource).reshape<6, 1>(); - B.rowRange(6 * targetNodeId, 6 * (targetNodeId + 1)) += (errorTransposeInfo * jacTarget).reshape<6, 1>(); + B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += + (errorTransposeInfo * jacSource).reshape<6, 1>(); + B.rowRange(6 * targetNodeId, 6 * (targetNodeId + 1)) += + (errorTransposeInfo * jacTarget).reshape<6, 1>(); } - - std::cout << "Residual value: " << residual << std::endl; return residual; } @@ -203,10 +192,12 @@ PoseGraph PoseGraph::update(const Mat& deltaVec) for (int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) { - Vec6f delta = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)); - Affine3f pose = nodes.at(currentNodeId).getPose(); - delta = -delta; - Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3)); + if (nodes.at(currentNodeId).isPoseFixed()) + continue; + Vec6f delta = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)); + Affine3f pose = nodes.at(currentNodeId).getPose(); + delta = -delta; + Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3)); Affine3f updatedPose = currDelta * pose; updatedPoseGraph.nodes.at(currentNodeId).setPose(updatedPose); @@ -230,6 +221,39 @@ Mat PoseGraph::getVector() return vector; } +float PoseGraph::computeResidual() +{ + int numEdges = int(edges.size()); + + float residual = 0.0f; + for (int currEdgeNum = 0; currEdgeNum < numEdges; currEdgeNum++) + { + const PoseGraphEdge& currEdge = edges.at(currEdgeNum); + int sourceNodeId = currEdge.getSourceNodeId(); + int targetNodeId = currEdge.getTargetNodeId(); + const Affine3f& sourcePose = nodes.at(sourceNodeId).getPose(); + const Affine3f& targetPose = nodes.at(targetNodeId).getPose(); + + Affine3f relativeSourceToTarget = targetPose.inv() * sourcePose; + Affine3f poseConstraint = currEdge.transformation.inv() * relativeSourceToTarget; + + Matx61f error; + Vec3f rot = poseConstraint.rvec(); + Vec3f trans = poseConstraint.translation(); + + error.val[0] = rot[0]; + error.val[1] = rot[1]; + error.val[2] = rot[2]; + error.val[3] = trans[0]; + error.val[4] = trans[1]; + error.val[5] = trans[2]; + + Matx16f errorTransposeInfo = error.t() * currEdge.information; + residual += std::sqrt((errorTransposeInfo * error).val[0]); + } + return residual; +} + bool Optimizer::isStepSizeSmall(const Mat& delta, float minStepSize) { float maxDeltaNorm = 0.0f; @@ -250,14 +274,16 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& if (!poseGraphOriginal.isValid()) //! Should print some error { - CV_Error(Error::StsBadArg, "Invalid PoseGraph that is either not connected or has invalid nodes"); + CV_Error(Error::StsBadArg, + "Invalid PoseGraph that is either not connected or has invalid nodes"); return; } int numNodes = poseGraph.getNumNodes(); int numEdges = poseGraph.getNumEdges(); - std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges << std::endl; + std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges + << std::endl; int iter = 0; float prevResidual = std::numeric_limits::max(); @@ -278,23 +304,25 @@ void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& CV_Error(Error::StsNoConv, "Sparse solve failed"); return; } - std::cout << "delta update: " << delta << "\n delta norm: " << cv::norm(delta) << " X norm: " << cv::norm(X) - << std::endl; + std::cout << "delta update: " << delta << "\n delta norm: " << cv::norm(delta) + << " X norm: " << cv::norm(X) << std::endl; //! Check delta if (isStepSizeSmall(delta, params.minStepSize)) { - std::cout << "Delta norm[" << cv::norm(delta) << "] < " << params.minStepSize << std::endl; + std::cout << "Delta norm[" << cv::norm(delta) << "] < " << params.minStepSize + << std::endl; break; } - std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; + std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual + << "\n"; //! TODO: Improve criterion and allow small increments in residual if ((currentResidual - prevResidual) > params.maxAcceptableResIncre) { - std::cout << "Current residual increased from prevResidual by at least " << params.maxAcceptableResIncre - << std::endl; + std::cout << "Current residual increased from prevResidual by at least " + << params.maxAcceptableResIncre << std::endl; break; } //! If updates don't improve a lot means loss function basin has been reached @@ -315,76 +343,82 @@ void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& po if (!poseGraphOriginal.isValid()) { - CV_Error(Error::StsBadArg, "Invalid PoseGraph that is either not connected or has invalid nodes"); + CV_Error(Error::StsBadArg, + "Invalid PoseGraph that is either not connected or has invalid nodes"); return; } int numNodes = poseGraph.getNumNodes(); int numEdges = poseGraph.getNumEdges(); - std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges << std::endl; + std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" << std::endl; int iter = 0; - float lambda = 1e-3f; + float lambda = 0.01; const float factor = 2; float prevResidual = std::numeric_limits::max(); - do + + while (iter < params.maxNumIters) { std::cout << "Current iteration: " << iter << std::endl; BlockSparseMat ApproxH(numNodes); Mat B = cv::Mat::zeros(6 * numNodes, 1, CV_32F); - float currentResidual = poseGraph.createLinearSystem(ApproxH, B); - Mat delta(6 * numNodes, 1, CV_32F); - Mat X = poseGraph.getVector(); - + prevResidual = poseGraph.createLinearSystem(ApproxH, B); //! H_LM = H + lambda * diag(H); Mat Hdiag = ApproxH.diagonal(); for (int i = 0; i < Hdiag.rows; i++) { ApproxH.refElem(i, i) += lambda * Hdiag.at(i, 0); } + + Mat delta(6 * numNodes, 1, CV_32F); bool success = sparseSolve(ApproxH, B, delta); if (!success) { CV_Error(Error::StsNoConv, "Sparse solve failed"); return; } - std::cout << "\n delta norm: " << cv::norm(delta) << " X norm: " << cv::norm(X) - << std::endl; //! If step size is too small break if (isStepSizeSmall(delta, params.minStepSize)) { - std::cout << "Step size is too small. stopping\n"; + std::cout << "Step size is too small.\n"; break; } - std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; + + PoseGraph poseGraphNew = poseGraph.update(delta); + float currentResidual = poseGraphNew.computeResidual(); + std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual + << "\n"; float reduction = currentResidual - prevResidual; if (reduction < 0) { //! Accept update lambda = lambda / factor; - poseGraph = poseGraph.update(delta); - prevResidual = currentResidual; - std::cout << "Accepting update: " << reduction << " new lambda: " << lambda << std::endl; + poseGraph = poseGraphNew; + std::cout << "Accepting update: " << reduction << " new lambda: " << lambda + << std::endl; + + if (abs(reduction) < (prevResidual * params.minResidualDecrease)) + { + std::cout << "Reduction in residual too small, converged " << (prevResidual * params.minResidualDecrease) << std::endl; + break; + } } else if (reduction > 0) { //! Reject update lambda = lambda * factor; - std::cout << "Rejecting update: " << reduction << " new lambda: " << lambda << std::endl; + std::cout << "Rejecting update: " << reduction << " new lambda: " << lambda + << std::endl; } //! TODO: Change to another paramter, this is dummy - if (abs(reduction) < 1e-4f) - { - std::cout << "Reduction in residual too small, converged " << std::endl; - break; - } + prevResidual = currentResidual; iter++; - } while (iter < params.maxNumIters); + } } } // namespace kinfu diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index ce53f9188c2..e525c3c1b08 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -138,6 +138,7 @@ class PoseGraph int getNumEdges() const { return edges.size(); } Mat getVector(); + float computeResidual(); //! @brief: Constructs a linear system and returns the residual of the current system float createLinearSystem(BlockSparseMat& H, Mat& B); @@ -155,9 +156,10 @@ struct Params float minResidual; float maxAcceptableResIncre; float minStepSize; + float minResidualDecrease; // TODO: Refine these constants - Params() : maxNumIters(40), minResidual(1e-3f), maxAcceptableResIncre(1e-3f), minStepSize(1e-4f){}; + Params() : maxNumIters(50), minResidual(1e-3f), maxAcceptableResIncre(1e-3f), minStepSize(1e-6f), minResidualDecrease(1e-6f){}; virtual ~Params() = default; }; diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 2bd0f10b533..7594b11b379 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -40,7 +40,6 @@ struct BlockSparseMat }; typedef Matx<_Tp, blockM, blockN> MatType; typedef std::unordered_map IDtoBlockValueMap; - static constexpr int blockSize = blockM * blockN; BlockSparseMat(int _nBlocks) : nBlocks(_nBlocks), ijValue() {} @@ -70,7 +69,7 @@ struct BlockSparseMat float& refElem(int i, int j) { - Point2i ib(i / blockSize, j / blockSize), iv(i % blockSize, j % blockSize); + Point2i ib(i / blockM, j / blockN), iv(i % blockM, j % blockN); return refBlock(ib.x, ib.y)(iv.x, iv.y); } @@ -78,7 +77,7 @@ struct BlockSparseMat Eigen::SparseMatrix<_Tp> toEigen() const { std::vector> tripletList; - tripletList.reserve(ijValue.size() * blockSize); + tripletList.reserve(ijValue.size() * blockM * blockN); for (auto ijv : ijValue) { int xb = ijv.first.x, yb = ijv.first.y; @@ -131,7 +130,6 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& solver.compute(bigA); if (solver.info() != Eigen::Success) { - std::cout << "solver.info(): " << solver.info() << std::endl; std::cout << "failed to eigen-decompose" << std::endl; result = false; } diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 4f07ea7a9c4..3cb29604c98 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -100,14 +100,14 @@ void Submap::integrate(InputArray _depth, float depthFactor, const cv:: const int currFrameId) { CV_Assert(currFrameId >= startFrameId); - volume.integrate(_depth, depthFactor, cameraPose, intrinsics, currFrameId); + volume.integrate(_depth, depthFactor, cameraPose.matrix, intrinsics, currFrameId); } template void Submap::raycast(const cv::Affine3f& _cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, OutputArray points, OutputArray normals) { - volume.raycast(_cameraPose, intrinsics, frameSize, points, normals); + volume.raycast(_cameraPose.matrix, intrinsics, frameSize, points, normals); } template @@ -517,6 +517,7 @@ PoseGraph SubmapManager::MapToPoseGraph() { // TODO: Handle case with duplicate constraints A -> B and B -> A Matx66f informationMatrix = Matx66f::eye() * (currConstraintPair.second.weight/10); + /* Matx66f informationMatrix = Matx66f::eye(); */ PoseGraphEdge currEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix); localPoseGraph.addEdge(currEdge); } @@ -545,6 +546,7 @@ void SubmapManager::PoseGraphToMap(const PoseGraph &updatedPoseGraph) const PoseGraphNode& currNode = updatedPoseGraph.nodes.at(currSubmap->id); if(!currNode.isPoseFixed()) currSubmap->pose = currNode.getPose(); + std::cout << "Current node: " << currSubmap->id << " Updated Pose: \n" << currSubmap->pose.matrix << std::endl; } } diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index eb17dd71600..1efa4b070c5 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -430,7 +430,7 @@ void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44 const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); - + CV_UNUSED(frameId); CV_Assert(_depth.type() == DEPTH_TYPE); CV_Assert(!_depth.empty()); Depth depth = _depth.getMat(); @@ -1141,6 +1141,7 @@ void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); + CV_UNUSED(frameId); CV_Assert(!_depth.empty()); UMat depth = _depth.getUMat(); @@ -1423,21 +1424,21 @@ Ptr makeTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastSt #ifdef HAVE_OPENCL if (ocl::useOpenCL()) return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, - _resolution); + _resolution); #endif return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, - _resolution); + _resolution); } Ptr makeTSDFVolume(const VolumeParams& _params) { #ifdef HAVE_OPENCL if (ocl::useOpenCL()) - return makePtr(_params.voxelSize, _params.pose, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, - _params.resolution); + return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, + _params.tsdfTruncDist, _params.maxWeight, _params.resolution); #endif - return makePtr(_params.voxelSize, _params.pose, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, - _params.resolution); + return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, + _params.tsdfTruncDist, _params.maxWeight, _params.resolution); } diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index c8bca09180a..8f0cc5257b5 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -2,16 +2,17 @@ // 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 #include "hash_tsdf.hpp" -#include "precomp.hpp" #include "tsdf.hpp" namespace cv { namespace kinfu { + Ptr VolumeParams::defaultParams(VolumeType _volumeType) { VolumeParams params; @@ -68,6 +69,7 @@ Ptr makeVolume(const VolumeParams& _volumeParams) default: return nullptr; } } + Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, Vec3i _resolution) From bbe56b7e19f84a087337f346b9eb7aae8181cc2f Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 18 Aug 2020 22:33:39 -0400 Subject: [PATCH 26/36] minor changes --- modules/rgbd/src/pose_graph.cpp | 94 +++------------------------------ modules/rgbd/src/pose_graph.hpp | 2 - 2 files changed, 8 insertions(+), 88 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index a5217f51c5f..3c23cb194e5 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -19,19 +19,7 @@ namespace kinfu { void PoseGraph::addEdge(const PoseGraphEdge& edge) { - /* std::vector::iterator it = std::find(edges.begin(), edges.end(), edge); */ - /* if(it == edges.end()) */ - /* { */ edges.push_back(edge); - /* } */ - /* else */ - /* { */ - /* PoseGraphEdge& existingEdge = *it; */ - /* if(existingEdge.getSourceNodeId() == edge.getTargetNodeId()) */ - /* { */ - /* Affine3f transform = edge.transformation.inv(); */ - - /* } */ } bool PoseGraph::isValid() const @@ -53,6 +41,7 @@ bool PoseGraph::isValid() const int currNodeId = nodesToVisit.back(); nodesToVisit.pop_back(); std::cout << "Visiting node: " << currNodeId << "\n"; + nodesVisited.insert(currNodeId); // Since each node does not maintain its neighbor list for (int i = 0; i < numEdges; i++) { @@ -67,10 +56,9 @@ bool PoseGraph::isValid() const { nextNodeId = potentialEdge.getSourceNodeId(); } - std::cout << "Next node: " << nextNodeId << std::endl; if (nextNodeId != -1) { - nodesVisited.insert(currNodeId); + std::cout << "Next node: " << nextNodeId << std::endl; if (nodesVisited.count(nextNodeId) == 0) { nodesToVisit.push_back(nextNodeId); @@ -80,7 +68,7 @@ bool PoseGraph::isValid() const } isGraphConnected = (int(nodesVisited.size()) == numNodes); - std::cout << "IsGraphConnected: " << isGraphConnected << std::endl; + std::cout << "nodesVisited: " << nodesVisited.size() << " IsGraphConnected: " << isGraphConnected << std::endl; bool invalidEdgeNode = false; for (int i = 0; i < numEdges; i++) { @@ -266,77 +254,11 @@ bool Optimizer::isStepSizeSmall(const Mat& delta, float minStepSize) return maxDeltaNorm < minStepSize; } -//! NOTE: We follow left-composition for the infinitesimal pose update -void Optimizer::optimizeGaussNewton(const Optimizer::Params& params, PoseGraph& poseGraph) -{ - PoseGraph poseGraphOriginal = poseGraph; - //! Check if posegraph is valid - if (!poseGraphOriginal.isValid()) - //! Should print some error - { - CV_Error(Error::StsBadArg, - "Invalid PoseGraph that is either not connected or has invalid nodes"); - return; - } - - int numNodes = poseGraph.getNumNodes(); - int numEdges = poseGraph.getNumEdges(); - - std::cout << "Optimizing posegraph with nodes: " << numNodes << " edges: " << numEdges - << std::endl; - - int iter = 0; - float prevResidual = std::numeric_limits::max(); - do - { - std::cout << "Current iteration: " << iter << std::endl; - //! ApproxH = Approximate Hessian = J^T * information * J - BlockSparseMat ApproxH(numNodes); - Mat B = cv::Mat::zeros(6 * numNodes, 1, CV_32F); - - float currentResidual = poseGraph.createLinearSystem(ApproxH, B); - std::cout << "Number of non-zero blocks in H: " << ApproxH.nonZeroBlocks() << "\n"; - Mat delta(6 * numNodes, 1, CV_32F); - Mat X = poseGraph.getVector(); - bool success = sparseSolve(ApproxH, B, delta); - if (!success) - { - CV_Error(Error::StsNoConv, "Sparse solve failed"); - return; - } - std::cout << "delta update: " << delta << "\n delta norm: " << cv::norm(delta) - << " X norm: " << cv::norm(X) << std::endl; - - //! Check delta - if (isStepSizeSmall(delta, params.minStepSize)) - { - std::cout << "Delta norm[" << cv::norm(delta) << "] < " << params.minStepSize - << std::endl; - break; - } - - std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual - << "\n"; - - //! TODO: Improve criterion and allow small increments in residual - if ((currentResidual - prevResidual) > params.maxAcceptableResIncre) - { - std::cout << "Current residual increased from prevResidual by at least " - << params.maxAcceptableResIncre << std::endl; - break; - } - //! If updates don't improve a lot means loss function basin has been reached - if ((currentResidual - params.minResidual) < 1e-5f) - { - std::cout << "Gauss newton converged \n"; - break; - } - poseGraph = poseGraph.update(delta); - prevResidual = currentResidual; - iter++; - } while (iter < params.maxNumIters); -} +/* float Optimizer::stepQuality(float currentResidual, float prevResidual, Mat delta, Mat B) */ +/* { */ +/* } */ +//! NOTE: We follow left-composition for the infinitesimal pose update void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& poseGraph) { PoseGraph poseGraphOriginal = poseGraph; @@ -410,7 +332,7 @@ void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& po else if (reduction > 0) { //! Reject update - lambda = lambda * factor; + lambda = lambda * 2 * factor; std::cout << "Rejecting update: " << reduction << " new lambda: " << lambda << std::endl; } diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index e525c3c1b08..c264d2e3879 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -163,9 +163,7 @@ struct Params virtual ~Params() = default; }; -void optimizeGaussNewton(const Params& params, PoseGraph& poseGraph); void optimizeLevenberg(const Params& params, PoseGraph& poseGraph); - bool isStepSizeSmall(const Mat& delta, float minStepSize); } // namespace Optimizer From 6c0cd5cd0ff79be47c451cae3cf17e1814ecfcc0 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Fri, 21 Aug 2020 20:33:33 -0400 Subject: [PATCH 27/36] Fixes, but Optimizer is still not well behaved --- modules/rgbd/src/pose_graph.cpp | 132 ++++++++++++++--------- modules/rgbd/src/pose_graph.hpp | 31 ++++-- modules/rgbd/src/sparse_block_matrix.hpp | 4 +- modules/rgbd/src/submap.hpp | 4 +- 4 files changed, 104 insertions(+), 67 deletions(-) diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 3c23cb194e5..f5e7eef7021 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -17,10 +17,7 @@ namespace cv { namespace kinfu { -void PoseGraph::addEdge(const PoseGraphEdge& edge) -{ - edges.push_back(edge); -} +void PoseGraph::addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } bool PoseGraph::isValid() const { @@ -58,7 +55,8 @@ bool PoseGraph::isValid() const } if (nextNodeId != -1) { - std::cout << "Next node: " << nextNodeId << std::endl; + std::cout << "Next node: " << nextNodeId << " " << nodesVisited.count(nextNodeId) + << std::endl; if (nodesVisited.count(nextNodeId) == 0) { nodesToVisit.push_back(nextNodeId); @@ -68,7 +66,8 @@ bool PoseGraph::isValid() const } isGraphConnected = (int(nodesVisited.size()) == numNodes); - std::cout << "nodesVisited: " << nodesVisited.size() << " IsGraphConnected: " << isGraphConnected << std::endl; + std::cout << "nodesVisited: " << nodesVisited.size() + << " IsGraphConnected: " << isGraphConnected << std::endl; bool invalidEdgeNode = false; for (int i = 0; i < numEdges; i++) { @@ -84,7 +83,7 @@ bool PoseGraph::isValid() const return isGraphConnected && !invalidEdgeNode; } -float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) +float PoseGraph::createLinearSystem(BlockSparseMat& hessian, Mat& B) { int numEdges = int(edges.size()); @@ -100,27 +99,27 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) { Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(generatorJacobian[i]) * sourcePose; - Vec3f rot = dError_dSource.rvec(); - Vec3f trans = dError_dSource.translation(); - jacSource.val[i + 6 * 0] = rot(0); - jacSource.val[i + 6 * 1] = rot(1); - jacSource.val[i + 6 * 2] = rot(2); - jacSource.val[i + 6 * 3] = trans(0); - jacSource.val[i + 6 * 4] = trans(1); - jacSource.val[i + 6 * 5] = trans(2); + Vec3f rot = dError_dSource.rvec(); + Vec3f trans = dError_dSource.translation(); + jacSource(i, 0) = rot(0); + jacSource(i, 1) = rot(1); + jacSource(i, 2) = rot(2); + jacSource(i, 3) = trans(0); + jacSource(i, 4) = trans(1); + jacSource(i, 5) = trans(2); } for (int i = 0; i < 6; i++) { - Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * + Affine3f dError_dTarget = relativePoseMeas_inv * targetPose_inv * cv::Affine3f(-generatorJacobian[i]) * sourcePose; - Vec3f rot = dError_dSource.rvec(); - Vec3f trans = dError_dSource.translation(); - jacTarget.val[i + 6 * 0] = rot(0); - jacTarget.val[i + 6 * 1] = rot(1); - jacTarget.val[i + 6 * 2] = rot(2); - jacTarget.val[i + 6 * 3] = trans(0); - jacTarget.val[i + 6 * 4] = trans(1); - jacTarget.val[i + 6 * 5] = trans(2); + Vec3f rot = dError_dTarget.rvec(); + Vec3f trans = dError_dTarget.translation(); + jacTarget(i, 0) = rot(0); + jacTarget(i, 1) = rot(1); + jacTarget(i, 2) = rot(2); + jacTarget(i, 3) = trans(0); + jacTarget(i, 4) = trans(1); + jacTarget(i, 5) = trans(2); } }; @@ -156,12 +155,12 @@ float PoseGraph::createLinearSystem(BlockSparseMat& H, Mat& B) Matx66f jacSourceTransposeInfo = jacSource.t() * currEdge.information; Matx66f jacTargetTransposeInfo = jacTarget.t() * currEdge.information; - residual += std::sqrt((errorTransposeInfo * error).val[0]); + residual += 0.5*(errorTransposeInfo * error).val[0]; - H.refBlock(sourceNodeId, sourceNodeId) += jacSourceTransposeInfo * jacSource; - H.refBlock(targetNodeId, targetNodeId) += jacTargetTransposeInfo * jacTarget; - H.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget; - H.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource; + hessian.refBlock(sourceNodeId, sourceNodeId) += jacSourceTransposeInfo * jacSource; + hessian.refBlock(targetNodeId, targetNodeId) += jacTargetTransposeInfo * jacTarget; + hessian.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget; + hessian.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource; B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += (errorTransposeInfo * jacSource).reshape<6, 1>(); @@ -184,7 +183,6 @@ PoseGraph PoseGraph::update(const Mat& deltaVec) continue; Vec6f delta = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)); Affine3f pose = nodes.at(currentNodeId).getPose(); - delta = -delta; Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3)); Affine3f updatedPose = currDelta * pose; @@ -237,7 +235,7 @@ float PoseGraph::computeResidual() error.val[5] = trans[2]; Matx16f errorTransposeInfo = error.t() * currEdge.information; - residual += std::sqrt((errorTransposeInfo * error).val[0]); + residual += 0.5*(errorTransposeInfo * error).val[0]; } return residual; } @@ -254,9 +252,22 @@ bool Optimizer::isStepSizeSmall(const Mat& delta, float minStepSize) return maxDeltaNorm < minStepSize; } -/* float Optimizer::stepQuality(float currentResidual, float prevResidual, Mat delta, Mat B) */ -/* { */ -/* } */ +float Optimizer::stepQuality(float currentResidual, float prevResidual, const Mat& delta, + const Mat& B, const Mat& predB) +{ + float actualReduction = prevResidual - currentResidual; + float predictedReduction = 0.0f; + for (int i = 0; i < predB.cols; i++) + { + predictedReduction -= (0.5 * predB.at(i, 0) * delta.at(i, 0) + + B.at(i, 0) * delta.at(i, 0)); + } + std::cout << " Actual reduction: " << actualReduction + << " Prediction reduction: " << predictedReduction << std::endl; + if (predictedReduction < 0) + return actualReduction / abs(predictedReduction); + return actualReduction / predictedReduction; +} //! NOTE: We follow left-composition for the infinitesimal pose update void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& poseGraph) @@ -273,7 +284,8 @@ void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& po int numNodes = poseGraph.getNumNodes(); int numEdges = poseGraph.getNumEdges(); - std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" << std::endl; + std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" + << std::endl; int iter = 0; float lambda = 0.01; const float factor = 2; @@ -283,24 +295,26 @@ void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& po { std::cout << "Current iteration: " << iter << std::endl; - BlockSparseMat ApproxH(numNodes); + BlockSparseMat hessian(numNodes); Mat B = cv::Mat::zeros(6 * numNodes, 1, CV_32F); - prevResidual = poseGraph.createLinearSystem(ApproxH, B); + prevResidual = poseGraph.createLinearSystem(hessian, B); //! H_LM = H + lambda * diag(H); - Mat Hdiag = ApproxH.diagonal(); + Mat Hdiag = hessian.diagonal(); for (int i = 0; i < Hdiag.rows; i++) { - ApproxH.refElem(i, i) += lambda * Hdiag.at(i, 0); + hessian.refElem(i, i) += lambda * Hdiag.at(i, 0); } Mat delta(6 * numNodes, 1, CV_32F); - bool success = sparseSolve(ApproxH, B, delta); + Mat predB(6 * numNodes, 1, CV_32F); + bool success = sparseSolve(hessian, B, delta, predB); if (!success) { CV_Error(Error::StsNoConv, "Sparse solve failed"); return; } + delta = -delta; //! If step size is too small break if (isStepSizeSmall(delta, params.minStepSize)) @@ -314,29 +328,41 @@ void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& po std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual << "\n"; - float reduction = currentResidual - prevResidual; - if (reduction < 0) + float quality = stepQuality(currentResidual, prevResidual, delta, B, predB); + std::cout << " Step Quality: " << quality << std::endl; + /* float reduction = currentResidual - prevResidual; */ + bool stepSuccess = false; + if (quality > 0.75) { //! Accept update lambda = lambda / factor; poseGraph = poseGraphNew; - std::cout << "Accepting update: " << reduction << " new lambda: " << lambda - << std::endl; + std::cout << "Accepting update new lambda: " << lambda << std::endl; - if (abs(reduction) < (prevResidual * params.minResidualDecrease)) - { - std::cout << "Reduction in residual too small, converged " << (prevResidual * params.minResidualDecrease) << std::endl; - break; - } + stepSuccess = true; } - else if (reduction > 0) + else if (quality > 0.25) + { + poseGraph = poseGraphNew; + std::cout << "Accepting update no update to lambda: " << lambda << std::endl; + stepSuccess = true; + } + else { //! Reject update lambda = lambda * 2 * factor; - std::cout << "Rejecting update: " << reduction << " new lambda: " << lambda - << std::endl; + std::cout << "Rejecting update new lambda: " << lambda << std::endl; + stepSuccess = false; + } + if(stepSuccess) + { + if (abs(currentResidual - prevResidual) < (prevResidual * params.minResidualDecrease)) + { + std::cout << "Reduction in residual too small, converged " + << (prevResidual * params.minResidualDecrease) << std::endl; + break; + } } - //! TODO: Change to another paramter, this is dummy prevResidual = currentResidual; iter++; diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index c264d2e3879..3614894093a 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -18,7 +18,10 @@ namespace kinfu struct PoseGraphNode { public: - explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) : nodeId(_nodeId), isFixed(false), pose(_pose) {} + explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) + : nodeId(_nodeId), isFixed(false), pose(_pose) + { + } virtual ~PoseGraphNode() = default; int getId() const { return nodeId; } @@ -56,8 +59,8 @@ struct PoseGraphEdge bool operator==(const PoseGraphEdge& edge) { - if((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) || - (edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId)) + if ((edge.getSourceNodeId() == sourceNodeId && edge.getTargetNodeId() == targetNodeId) || + (edge.getSourceNodeId() == targetNodeId && edge.getTargetNodeId() == sourceNodeId)) return true; return false; } @@ -69,9 +72,8 @@ struct PoseGraphEdge Matx66f information; }; -//! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold optimization -//! Jose Luis Blanco -//! Compactly represents the jacobian of the SE3 generator +//! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold +//! optimization Jose Luis Blanco Compactly represents the jacobian of the SE3 generator // clang-format off static const std::array generatorJacobian = { // alpha @@ -107,7 +109,6 @@ static const std::array generatorJacobian = { }; // clang-format on - class PoseGraph { public: @@ -126,8 +127,9 @@ class PoseGraph bool nodeExists(int nodeId) const { - return std::find_if(nodes.begin(), nodes.end(), - [nodeId](const PoseGraphNode& currNode) { return currNode.getId() == nodeId; }) != nodes.end(); + return std::find_if(nodes.begin(), nodes.end(), [nodeId](const PoseGraphNode& currNode) { + return currNode.getId() == nodeId; + }) != nodes.end(); } bool isValid() const; @@ -141,7 +143,7 @@ class PoseGraph float computeResidual(); //! @brief: Constructs a linear system and returns the residual of the current system - float createLinearSystem(BlockSparseMat& H, Mat& B); + float createLinearSystem(BlockSparseMat& hessian, Mat& B); public: NodeVector nodes; @@ -159,12 +161,19 @@ struct Params float minResidualDecrease; // TODO: Refine these constants - Params() : maxNumIters(50), minResidual(1e-3f), maxAcceptableResIncre(1e-3f), minStepSize(1e-6f), minResidualDecrease(1e-6f){}; + Params() + : maxNumIters(50), + minResidual(1e-3f), + maxAcceptableResIncre(1e-3f), + minStepSize(1e-6f), + minResidualDecrease(1e-5f){}; virtual ~Params() = default; }; void optimizeLevenberg(const Params& params, PoseGraph& poseGraph); bool isStepSizeSmall(const Mat& delta, float minStepSize); +float stepQuality(float currentResidual, float prevResidual, const Mat& delta, const Mat& B, + const Mat& predB); } // namespace Optimizer } // namespace kinfu diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp index 7594b11b379..0e607af639d 100644 --- a/modules/rgbd/src/sparse_block_matrix.hpp +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -110,7 +110,7 @@ struct BlockSparseMat //! Function to solve a sparse linear system of equations HX = B //! Requires Eigen -static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& X) +static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& X, Mat& predB) { bool result = false; #if defined(HAVE_EIGEN) @@ -136,6 +136,7 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& else { Eigen::VectorXf solutionX = solver.solve(bigB); + Eigen::VectorXf predBEigen = bigA * solutionX; if (solver.info() != Eigen::Success) { std::cout << "failed to eigen-solve" << std::endl; @@ -144,6 +145,7 @@ static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& else { eigen2cv(solutionX, X); + eigen2cv(predBEigen, predB); result = true; } } diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 3cb29604c98..284ec7db4a3 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -516,8 +516,8 @@ PoseGraph SubmapManager::MapToPoseGraph() for(const auto& currConstraintPair : constraintList) { // TODO: Handle case with duplicate constraints A -> B and B -> A - Matx66f informationMatrix = Matx66f::eye() * (currConstraintPair.second.weight/10); - /* Matx66f informationMatrix = Matx66f::eye(); */ + /* Matx66f informationMatrix = Matx66f::eye() * (currConstraintPair.second.weight/10); */ + Matx66f informationMatrix = Matx66f::eye(); PoseGraphEdge currEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix); localPoseGraph.addEdge(currEdge); } From 53b149d8165de6a02c6be3497776b34f4dfbc017 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Mon, 24 Aug 2020 18:03:08 -0400 Subject: [PATCH 28/36] Working Ceres optimizer --- modules/rgbd/CMakeLists.txt | 11 ++ modules/rgbd/src/large_kinfu.cpp | 2 +- modules/rgbd/src/pose_graph.cpp | 300 +++++-------------------------- modules/rgbd/src/pose_graph.hpp | 191 ++++++++++++++++---- 4 files changed, 216 insertions(+), 288 deletions(-) diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt index 7f2f6a67257..133879bc07a 100644 --- a/modules/rgbd/CMakeLists.txt +++ b/modules/rgbd/CMakeLists.txt @@ -1,2 +1,13 @@ set(the_description "RGBD algorithms") + +find_package(Ceres QUIET) + +if(Ceres_FOUND) + add_definitions("-DCERES_FOUND=1") +else() + add_definitions("-DCERES_FOUND=0") + message(STATUS "CERES support is disabled. Ceres Solver is Required for Posegraph optimization") +endif() + ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python) +ocv_target_link_libraries(${the_module} ${CERES_LIBRARIES}) diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index f337b1ad609..f3cd2634302 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -279,7 +279,7 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) // TODO: Convert constraints to posegraph PoseGraph poseGraph = submapMgr->MapToPoseGraph(); std::cout << "Created posegraph\n"; - Optimizer::optimizeLevenberg(Optimizer::Params(), poseGraph); + Optimizer::optimizeCeres(poseGraph); submapMgr->PoseGraphToMap(poseGraph); } diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index f5e7eef7021..a895ea7a48e 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -8,10 +8,9 @@ #include #include -#include "opencv2/core/base.hpp" -#include "opencv2/core/eigen.hpp" -#include "opencv2/core/mat.hpp" -#include "opencv2/core/matx.hpp" +#if defined(CERES_FOUND) +#include "ceres/ceres.h" +#endif namespace cv { @@ -83,194 +82,55 @@ bool PoseGraph::isValid() const return isGraphConnected && !invalidEdgeNode; } -float PoseGraph::createLinearSystem(BlockSparseMat& hessian, Mat& B) +#if defined(CERES_FOUND) +void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem) { - int numEdges = int(edges.size()); - - float residual = 0.0f; - - // Lambda to calculate the error Jacobians w.r.t source and target poses - auto calcErrorJacobian = [](Matx66f& jacSource, Matx66f& jacTarget, - const Affine3f& relativePoseMeas, const Affine3f& sourcePose, - const Affine3f& targetPose) -> void { - const Affine3f relativePoseMeas_inv = relativePoseMeas.inv(); - const Affine3f targetPose_inv = targetPose.inv(); - for (int i = 0; i < 6; i++) - { - Affine3f dError_dSource = relativePoseMeas_inv * targetPose_inv * - cv::Affine3f(generatorJacobian[i]) * sourcePose; - Vec3f rot = dError_dSource.rvec(); - Vec3f trans = dError_dSource.translation(); - jacSource(i, 0) = rot(0); - jacSource(i, 1) = rot(1); - jacSource(i, 2) = rot(2); - jacSource(i, 3) = trans(0); - jacSource(i, 4) = trans(1); - jacSource(i, 5) = trans(2); - } - for (int i = 0; i < 6; i++) - { - Affine3f dError_dTarget = relativePoseMeas_inv * targetPose_inv * - cv::Affine3f(-generatorJacobian[i]) * sourcePose; - Vec3f rot = dError_dTarget.rvec(); - Vec3f trans = dError_dTarget.translation(); - jacTarget(i, 0) = rot(0); - jacTarget(i, 1) = rot(1); - jacTarget(i, 2) = rot(2); - jacTarget(i, 3) = trans(0); - jacTarget(i, 4) = trans(1); - jacTarget(i, 5) = trans(2); - } - }; - - //! Compute linear system of equations - for (int currEdgeNum = 0; currEdgeNum < numEdges; currEdgeNum++) - { - const PoseGraphEdge& currEdge = edges.at(currEdgeNum); - int sourceNodeId = currEdge.getSourceNodeId(); - int targetNodeId = currEdge.getTargetNodeId(); - const Affine3f& sourcePose = nodes.at(sourceNodeId).getPose(); - const Affine3f& targetPose = nodes.at(targetNodeId).getPose(); - - Affine3f relativeSourceToTarget = targetPose.inv() * sourcePose; - //! Edge transformation is source to target. (relativeConstraint should be identity if no - //! error) - Affine3f poseConstraint = currEdge.transformation.inv() * relativeSourceToTarget; - - Matx61f error; - Vec3f rot = poseConstraint.rvec(); - Vec3f trans = poseConstraint.translation(); - - error.val[0] = rot[0]; - error.val[1] = rot[1]; - error.val[2] = rot[2]; - error.val[3] = trans[0]; - error.val[4] = trans[1]; - error.val[5] = trans[2]; - - Matx66f jacSource, jacTarget; - calcErrorJacobian(jacSource, jacTarget, currEdge.transformation, sourcePose, targetPose); - - Matx16f errorTransposeInfo = error.t() * currEdge.information; - Matx66f jacSourceTransposeInfo = jacSource.t() * currEdge.information; - Matx66f jacTargetTransposeInfo = jacTarget.t() * currEdge.information; - - residual += 0.5*(errorTransposeInfo * error).val[0]; - - hessian.refBlock(sourceNodeId, sourceNodeId) += jacSourceTransposeInfo * jacSource; - hessian.refBlock(targetNodeId, targetNodeId) += jacTargetTransposeInfo * jacTarget; - hessian.refBlock(sourceNodeId, targetNodeId) += jacSourceTransposeInfo * jacTarget; - hessian.refBlock(targetNodeId, sourceNodeId) += jacTargetTransposeInfo * jacSource; - - B.rowRange(6 * sourceNodeId, 6 * (sourceNodeId + 1)) += - (errorTransposeInfo * jacSource).reshape<6, 1>(); - B.rowRange(6 * targetNodeId, 6 * (targetNodeId + 1)) += - (errorTransposeInfo * jacTarget).reshape<6, 1>(); - } - return residual; -} - -PoseGraph PoseGraph::update(const Mat& deltaVec) -{ - int numNodes = int(nodes.size()); - - //! Check: This should create a copy of the posegraph - PoseGraph updatedPoseGraph(*this); - - for (int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) + int numEdges = poseGraph.getNumEdges(); + int numNodes = poseGraph.getNumNodes(); + if (numEdges == 0) { - if (nodes.at(currentNodeId).isPoseFixed()) - continue; - Vec6f delta = deltaVec.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)); - Affine3f pose = nodes.at(currentNodeId).getPose(); - Affine3f currDelta = Affine3f(Vec3f(delta.val), Vec3f(delta.val + 3)); - Affine3f updatedPose = currDelta * pose; - - updatedPoseGraph.nodes.at(currentNodeId).setPose(updatedPose); + CV_Error(Error::StsBadArg, "PoseGraph has no edges, no optimization to be done"); + return; } - return updatedPoseGraph; -} + ceres::LossFunction* lossFunction = nullptr; + // TODO: Experiment with SE3 parameterization + ceres::LocalParameterization* quatLocalParameterization = + new ceres::EigenQuaternionParameterization; -Mat PoseGraph::getVector() -{ - int numNodes = int(nodes.size()); - Mat vector(6 * numNodes, 1, CV_32F, Scalar(0)); - for (int currentNodeId = 0; currentNodeId < numNodes; currentNodeId++) + for (int currEdgeNum = 0; currEdgeNum < numEdges; ++currEdgeNum) { - Affine3f pose = nodes.at(currentNodeId).getPose(); - Vec3f rot = pose.rvec(); - Vec3f trans = pose.translation(); - vector.rowRange(6 * currentNodeId, 6 * (currentNodeId + 1)) = - Vec6f(rot.val[0], rot.val[1], rot.val[2], trans.val[0], trans.val[1], trans.val[2]); - } - return vector; -} - -float PoseGraph::computeResidual() -{ - int numEdges = int(edges.size()); - - float residual = 0.0f; - for (int currEdgeNum = 0; currEdgeNum < numEdges; currEdgeNum++) - { - const PoseGraphEdge& currEdge = edges.at(currEdgeNum); + const PoseGraphEdge& currEdge = poseGraph.edges.at(currEdgeNum); int sourceNodeId = currEdge.getSourceNodeId(); int targetNodeId = currEdge.getTargetNodeId(); - const Affine3f& sourcePose = nodes.at(sourceNodeId).getPose(); - const Affine3f& targetPose = nodes.at(targetNodeId).getPose(); + Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).pose; + Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).pose; - Affine3f relativeSourceToTarget = targetPose.inv() * sourcePose; - Affine3f poseConstraint = currEdge.transformation.inv() * relativeSourceToTarget; + const Matx66f& informationMatrix = currEdge.information; - Matx61f error; - Vec3f rot = poseConstraint.rvec(); - Vec3f trans = poseConstraint.translation(); + ceres::CostFunction* costFunction = + Pose3dErrorFunctor::create(currEdge.transformation, informationMatrix); - error.val[0] = rot[0]; - error.val[1] = rot[1]; - error.val[2] = rot[2]; - error.val[3] = trans[0]; - error.val[4] = trans[1]; - error.val[5] = trans[2]; - - Matx16f errorTransposeInfo = error.t() * currEdge.information; - residual += 0.5*(errorTransposeInfo * error).val[0]; - } - return residual; -} - -bool Optimizer::isStepSizeSmall(const Mat& delta, float minStepSize) -{ - float maxDeltaNorm = 0.0f; - for (int i = 0; i < delta.rows; i++) - { - float val = abs(delta.at(i, 0)); - if (val > maxDeltaNorm) - maxDeltaNorm = val; + problem.AddResidualBlock(costFunction, lossFunction, sourcePose.t.data(), + sourcePose.r.coeffs().data(), targetPose.t.data(), + targetPose.r.coeffs().data()); + problem.SetParameterization(sourcePose.r.coeffs().data(), quatLocalParameterization); + problem.SetParameterization(targetPose.r.coeffs().data(), quatLocalParameterization); } - return maxDeltaNorm < minStepSize; -} -float Optimizer::stepQuality(float currentResidual, float prevResidual, const Mat& delta, - const Mat& B, const Mat& predB) -{ - float actualReduction = prevResidual - currentResidual; - float predictedReduction = 0.0f; - for (int i = 0; i < predB.cols; i++) + for (int currNodeId = 0; currNodeId < numNodes; ++currNodeId) { - predictedReduction -= (0.5 * predB.at(i, 0) * delta.at(i, 0) + - B.at(i, 0) * delta.at(i, 0)); + PoseGraphNode& currNode = poseGraph.nodes.at(currNodeId); + if (currNode.isPoseFixed()) + { + problem.SetParameterBlockConstant(currNode.pose.t.data()); + problem.SetParameterBlockConstant(currNode.pose.r.coeffs().data()); + } } - std::cout << " Actual reduction: " << actualReduction - << " Prediction reduction: " << predictedReduction << std::endl; - if (predictedReduction < 0) - return actualReduction / abs(predictedReduction); - return actualReduction / predictedReduction; } +#endif -//! NOTE: We follow left-composition for the infinitesimal pose update -void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& poseGraph) +void Optimizer::optimizeCeres(PoseGraph& poseGraph) { PoseGraph poseGraphOriginal = poseGraph; @@ -283,90 +143,26 @@ void Optimizer::optimizeLevenberg(const Optimizer::Params& params, PoseGraph& po int numNodes = poseGraph.getNumNodes(); int numEdges = poseGraph.getNumEdges(); - std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" << std::endl; - int iter = 0; - float lambda = 0.01; - const float factor = 2; - float prevResidual = std::numeric_limits::max(); - - while (iter < params.maxNumIters) - { - std::cout << "Current iteration: " << iter << std::endl; - - BlockSparseMat hessian(numNodes); - Mat B = cv::Mat::zeros(6 * numNodes, 1, CV_32F); - - prevResidual = poseGraph.createLinearSystem(hessian, B); - //! H_LM = H + lambda * diag(H); - Mat Hdiag = hessian.diagonal(); - for (int i = 0; i < Hdiag.rows; i++) - { - hessian.refElem(i, i) += lambda * Hdiag.at(i, 0); - } - Mat delta(6 * numNodes, 1, CV_32F); - Mat predB(6 * numNodes, 1, CV_32F); - bool success = sparseSolve(hessian, B, delta, predB); - if (!success) - { - CV_Error(Error::StsNoConv, "Sparse solve failed"); - return; - } - delta = -delta; +#if defined(CERES_FOUND) + ceres::Problem problem; + createOptimizationProblem(poseGraph, problem); - //! If step size is too small break - if (isStepSizeSmall(delta, params.minStepSize)) - { - std::cout << "Step size is too small.\n"; - break; - } + ceres::Solver::Options options; + options.max_num_iterations = 100; + options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; - PoseGraph poseGraphNew = poseGraph.update(delta); - float currentResidual = poseGraphNew.computeResidual(); - std::cout << " Current residual: " << currentResidual << " Prev residual: " << prevResidual - << "\n"; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); - float quality = stepQuality(currentResidual, prevResidual, delta, B, predB); - std::cout << " Step Quality: " << quality << std::endl; - /* float reduction = currentResidual - prevResidual; */ - bool stepSuccess = false; - if (quality > 0.75) - { - //! Accept update - lambda = lambda / factor; - poseGraph = poseGraphNew; - std::cout << "Accepting update new lambda: " << lambda << std::endl; + std::cout << summary.FullReport() << '\n'; - stepSuccess = true; - } - else if (quality > 0.25) - { - poseGraph = poseGraphNew; - std::cout << "Accepting update no update to lambda: " << lambda << std::endl; - stepSuccess = true; - } - else - { - //! Reject update - lambda = lambda * 2 * factor; - std::cout << "Rejecting update new lambda: " << lambda << std::endl; - stepSuccess = false; - } - if(stepSuccess) - { - if (abs(currentResidual - prevResidual) < (prevResidual * params.minResidualDecrease)) - { - std::cout << "Reduction in residual too small, converged " - << (prevResidual * params.minResidualDecrease) << std::endl; - break; - } - } - //! TODO: Change to another paramter, this is dummy - prevResidual = currentResidual; - iter++; - } + std::cout << "Is solution usable: " << summary.IsSolutionUsable() << std::endl; +#else + CV_Error(Error::StsNotImplemented, "Ceres required for Pose Graph optimization"); +#endif } } // namespace kinfu diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 3614894093a..91c06f1519e 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -5,7 +5,16 @@ #include #include "opencv2/core/affine.hpp" -#include "sparse_block_matrix.hpp" +#if defined(HAVE_EIGEN) +#include +#include +#include "opencv2/core/eigen.hpp" +#endif + +#if defined(CERES_FOUND) +#include +#endif + namespace cv { namespace kinfu @@ -15,25 +24,106 @@ namespace kinfu * * Detailed description */ +#if defined(HAVE_EIGEN) +struct Pose3d +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Eigen::Vector3d t; + Eigen::Quaterniond r; + + Pose3d() + { + t.setZero(); + r.setIdentity(); + }; + Pose3d(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation) + : t(translation), r(Eigen::Quaterniond(rotation)) + { + normalizeRotation(); + } + + Pose3d(const Matx33d& rotation, const Vec3d& translation) + { + Eigen::Matrix3d R; + cv2eigen(rotation, R); + cv2eigen(translation, t); + r = Eigen::Quaterniond(R); + normalizeRotation(); + } + + explicit Pose3d(const Matx44f& pose) + { + Matx33d rotation(pose.val[0], pose.val[1], pose.val[2], pose.val[4], pose.val[5], + pose.val[6], pose.val[8], pose.val[9], pose.val[10]); + Vec3d translation(pose.val[3], pose.val[7], pose.val[11]); + Pose3d(rotation, translation); + } + + // NOTE: Eigen overloads quaternion multiplication appropriately + inline Pose3d operator*(const Pose3d& otherPose) const + { + Pose3d out(*this); + out.t += r * otherPose.t; + out.r *= otherPose.r; + out.normalizeRotation(); + return out; + } + + inline Pose3d& operator*=(const Pose3d& otherPose) + { + t += otherPose.t; + r *= otherPose.r; + normalizeRotation(); + return *this; + } + + inline Pose3d inverse() const + { + Pose3d out; + out.r = r.conjugate(); + out.t = out.r * (t * -1.0); + return out; + } + + inline void normalizeRotation() + { + if (r.w() < 0) + r.coeffs() *= -1.0; + r.normalize(); + } +}; +#endif + struct PoseGraphNode { public: explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) - : nodeId(_nodeId), isFixed(false), pose(_pose) + : nodeId(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) { } virtual ~PoseGraphNode() = default; int getId() const { return nodeId; } - Affine3f getPose() const { return pose; } - void setPose(const Affine3f& _pose) { pose = _pose; } + inline Affine3d getPose() const + { + const Eigen::Matrix3d& rotation = pose.r.toRotationMatrix(); + const Eigen::Vector3d& translation = pose.t; + Matx33d rot; + Vec3d trans; + eigen2cv(rotation, rot); + eigen2cv(translation, trans); + Affine3d poseMatrix(rot, trans); + return poseMatrix; + } + void setPose(const Pose3d& _pose) { pose = _pose; } void setFixed(bool val = true) { isFixed = val; } bool isPoseFixed() const { return isFixed; } - private: + public: int nodeId; bool isFixed; - Affine3f pose; + Pose3d pose; }; /*! \class PoseGraphEdge @@ -48,7 +138,7 @@ struct PoseGraphEdge const Matx66f& _information = Matx66f::eye()) : sourceNodeId(_sourceNodeId), targetNodeId(_targetNodeId), - transformation(_transformation), + transformation(_transformation.rotation(), _transformation.translation()), information(_information) { } @@ -68,7 +158,7 @@ struct PoseGraphEdge public: int sourceNodeId; int targetNodeId; - Affine3f transformation; + Pose3d transformation; Matx66f information; }; @@ -134,17 +224,9 @@ class PoseGraph bool isValid() const; - PoseGraph update(const Mat& delta); - int getNumNodes() const { return nodes.size(); } int getNumEdges() const { return edges.size(); } - Mat getVector(); - float computeResidual(); - - //! @brief: Constructs a linear system and returns the residual of the current system - float createLinearSystem(BlockSparseMat& hessian, Mat& B); - public: NodeVector nodes; EdgeVector edges; @@ -152,28 +234,67 @@ class PoseGraph namespace Optimizer { -struct Params +void optimizeCeres(PoseGraph& poseGraph); + +#if defined(CERES_FOUND) +void createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem); + +//! Error Functor required for Ceres to obtain an auto differentiable cost function +class Pose3dErrorFunctor { - int maxNumIters; - float minResidual; - float maxAcceptableResIncre; - float minStepSize; - float minResidualDecrease; - - // TODO: Refine these constants - Params() - : maxNumIters(50), - minResidual(1e-3f), - maxAcceptableResIncre(1e-3f), - minStepSize(1e-6f), - minResidualDecrease(1e-5f){}; - virtual ~Params() = default; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Pose3dErrorFunctor(const Pose3d& _poseMeasurement, const Matx66d& _sqrtInformation) + : poseMeasurement(_poseMeasurement) + { + cv2eigen(_sqrtInformation, sqrtInfo); + } + Pose3dErrorFunctor(const Pose3d& _poseMeasurement, + const Eigen::Matrix& _sqrtInformation) + : poseMeasurement(_poseMeasurement), sqrtInfo(_sqrtInformation) + { + } + + template + bool operator()(const T* const _pSourceTrans, const T* const _pSourceQuat, + const T* const _pTargetTrans, const T* const _pTargetQuat, T* _pResidual) const + { + Eigen::Map> sourceTrans(_pSourceTrans); + Eigen::Map> targetTrans(_pTargetTrans); + Eigen::Map> sourceQuat(_pSourceQuat); + Eigen::Map> targetQuat(_pTargetQuat); + Eigen::Map> residual(_pResidual); + + Eigen::Quaternion targetQuatInv = targetQuat.conjugate(); + + Eigen::Quaternion relativeQuat = targetQuatInv * sourceQuat; + Eigen::Matrix relativeTrans = targetQuatInv * (targetTrans - sourceTrans); + + //! Definition should actually be relativeQuat * poseMeasurement.r.conjugate() + Eigen::Quaternion deltaRot = + poseMeasurement.r.template cast() * relativeQuat.conjugate(); + + residual.template block<3, 1>(0, 0) = relativeTrans - poseMeasurement.t.template cast(); + residual.template block<3, 1>(3, 0) = T(2.0) * deltaRot.vec(); + + residual.applyOnTheLeft(sqrtInfo.template cast()); + + return true; + } + + static ceres::CostFunction* create(const Pose3d& _poseMeasurement, + const Matx66f& _sqrtInformation) + { + return new ceres::AutoDiffCostFunction( + new Pose3dErrorFunctor(_poseMeasurement, _sqrtInformation)); + } + + private: + const Pose3d poseMeasurement; + Eigen::Matrix sqrtInfo; }; +#endif -void optimizeLevenberg(const Params& params, PoseGraph& poseGraph); -bool isStepSizeSmall(const Mat& delta, float minStepSize); -float stepQuality(float currentResidual, float prevResidual, const Mat& delta, const Mat& B, - const Mat& predB); } // namespace Optimizer } // namespace kinfu From 5ede5b1258617695a4b2abb43d73d2a53785d17e Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Thu, 27 Aug 2020 12:14:28 -0400 Subject: [PATCH 29/36] - Reorganize IO code for samples in a separate file - Minor fix for Ceres preprocessor definition - Remove unused generatorJacobian, will be used for opencv implementation of levenberg marquardt - Doxygen docs fix - Minor preprocessor fixes --- modules/rgbd/CMakeLists.txt | 8 +- .../rgbd/include/opencv2/rgbd/io_utils.hpp | 313 ++++++++++++++ modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 14 +- .../rgbd/include/opencv2/rgbd/large_kinfu.hpp | 28 +- modules/rgbd/samples/kinfu_demo.cpp | 263 +----------- modules/rgbd/samples/large_kinfu_demo.cpp | 393 +++--------------- modules/rgbd/src/hash_tsdf.hpp | 2 +- modules/rgbd/src/pose_graph.cpp | 23 +- modules/rgbd/src/pose_graph.hpp | 106 +++-- modules/rgbd/src/submap.cpp | 36 -- 10 files changed, 480 insertions(+), 706 deletions(-) create mode 100644 modules/rgbd/include/opencv2/rgbd/io_utils.hpp delete mode 100644 modules/rgbd/src/submap.cpp diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt index 133879bc07a..247c788a9c1 100644 --- a/modules/rgbd/CMakeLists.txt +++ b/modules/rgbd/CMakeLists.txt @@ -1,13 +1,11 @@ set(the_description "RGBD algorithms") find_package(Ceres QUIET) +ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python) +ocv_target_link_libraries(${the_module} ${CERES_LIBRARIES}) if(Ceres_FOUND) - add_definitions("-DCERES_FOUND=1") + ocv_target_compile_definitions(${the_module} PUBLIC CERES_FOUND) else() - add_definitions("-DCERES_FOUND=0") message(STATUS "CERES support is disabled. Ceres Solver is Required for Posegraph optimization") endif() - -ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python) -ocv_target_link_libraries(${the_module} ${CERES_LIBRARIES}) diff --git a/modules/rgbd/include/opencv2/rgbd/io_utils.hpp b/modules/rgbd/include/opencv2/rgbd/io_utils.hpp new file mode 100644 index 00000000000..c96d6c5345d --- /dev/null +++ b/modules/rgbd/include/opencv2/rgbd/io_utils.hpp @@ -0,0 +1,313 @@ +// 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_RGBS_IO_UTILS_HPP +#define OPENCV_RGBS_IO_UTILS_HPP + +#include +#include +#include +#include +#include +#include +#include + +namespace cv +{ +namespace io_utils +{ + +static std::vector readDepth(const std::string& fileList) +{ + std::vector v; + + std::fstream file(fileList); + if (!file.is_open()) + throw std::runtime_error("Failed to read depth list"); + + std::string dir; + size_t slashIdx = fileList.rfind('/'); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); + + while (!file.eof()) + { + std::string s, imgPath; + std::getline(file, s); + if (s.empty() || s[0] == '#') + continue; + std::stringstream ss; + ss << s; + double thumb; + ss >> thumb >> imgPath; + v.push_back(dir + '/' + imgPath); + } + + return v; +} + +struct DepthWriter +{ + DepthWriter(std::string fileList) : file(fileList, std::ios::out), count(0), dir() + { + size_t slashIdx = fileList.rfind('/'); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); + + if (!file.is_open()) + throw std::runtime_error("Failed to write depth list"); + + file << "# depth maps saved from device" << std::endl; + file << "# useless_number filename" << std::endl; + } + + void append(InputArray _depth) + { + Mat depth = _depth.getMat(); + std::string depthFname = cv::format("%04d.png", count); + std::string fullDepthFname = dir + '/' + depthFname; + if (!imwrite(fullDepthFname, depth)) + throw std::runtime_error("Failed to write depth to file " + fullDepthFname); + file << count++ << " " << depthFname << std::endl; + } + + std::fstream file; + int count; + std::string dir; +}; + +namespace Kinect2Params +{ +static const Size frameSize = Size(512, 424); +// approximate values, no guarantee to be correct +static const float focal = 366.1f; +static const float cx = 258.2f; +static const float cy = 204.f; +static const float k1 = 0.12f; +static const float k2 = -0.34f; +static const float k3 = 0.12f; +}; // namespace Kinect2Params + +struct DepthSource +{ + public: + enum Type + { + DEPTH_LIST, + DEPTH_KINECT2_LIST, + DEPTH_KINECT2, + DEPTH_REALSENSE + }; + + DepthSource(int cam) : DepthSource("", cam) {} + + DepthSource(String fileListName) : DepthSource(fileListName, -1) {} + + DepthSource(String fileListName, int cam) + : depthFileList(fileListName.empty() ? std::vector() + : readDepth(fileListName)), + frameIdx(0), + undistortMap1(), + undistortMap2() + { + if (cam >= 0) + { + vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); + if (vc.isOpened()) + { + sourceType = Type::DEPTH_KINECT2; + } + else + { + vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); + if (vc.isOpened()) + { + sourceType = Type::DEPTH_REALSENSE; + } + } + } + else + { + vc = VideoCapture(); + sourceType = Type::DEPTH_KINECT2_LIST; + } + } + + UMat getDepth() + { + UMat out; + if (!vc.isOpened()) + { + if (frameIdx < depthFileList.size()) + { + Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH); + f.copyTo(out); + } + else + { + return UMat(); + } + } + else + { + vc.grab(); + switch (sourceType) + { + case Type::DEPTH_KINECT2: vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); break; + case Type::DEPTH_REALSENSE: vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); break; + default: + // unknown depth source + vc.retrieve(out); + } + + // workaround for Kinect 2 + if (sourceType == Type::DEPTH_KINECT2) + { + out = out(Rect(Point(), Kinect2Params::frameSize)); + + UMat outCopy; + // linear remap adds gradient between valid and invalid pixels + // which causes garbage, use nearest instead + remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST); + + cv::flip(outCopy, out, 1); + } + } + if (out.empty()) + throw std::runtime_error("Matrix is empty"); + return out; + } + + bool empty() { return depthFileList.empty() && !(vc.isOpened()); } + + void updateIntrinsics(Matx33f& _intrinsics, Size& _frameSize, float& _depthFactor) + { + if (vc.isOpened()) + { + // this should be set in according to user's depth sensor + int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH); + int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT); + + // it's recommended to calibrate sensor to obtain its intrinsics + float fx, fy, cx, cy; + float depthFactor = 1000.f; + Size frameSize; + if (sourceType == Type::DEPTH_KINECT2) + { + fx = fy = Kinect2Params::focal; + cx = Kinect2Params::cx; + cy = Kinect2Params::cy; + + frameSize = Kinect2Params::frameSize; + } + else + { + if (sourceType == Type::DEPTH_REALSENSE) + { + fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); + fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); + depthFactor = 1.f / (float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); + } + else + { + fx = fy = + (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); + } + + cx = w / 2 - 0.5f; + cy = h / 2 - 0.5f; + + frameSize = Size(w, h); + } + + Matx33f camMatrix = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); + _intrinsics = camMatrix; + _frameSize = frameSize; + _depthFactor = depthFactor; + } + } + + void updateVolumeParams(const Vec3i& _resolution, float& _voxelSize, float& _tsdfTruncDist, + Affine3f& _volumePose, float& _depthTruncateThreshold) + { + float volumeSize = 3.0f; + _depthTruncateThreshold = 0.0f; + // RealSense has shorter depth range, some params should be tuned + if (sourceType == Type::DEPTH_REALSENSE) + { + volumeSize = 1.f; + _voxelSize = volumeSize / _resolution[0]; + _tsdfTruncDist = 0.01f; + _depthTruncateThreshold = 2.5f; + } + _volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.05f)); + } + + void updateICPParams(float& _icpDistThresh, float& _bilateralSigmaDepth) + { + _icpDistThresh = 0.1f; + _bilateralSigmaDepth = 0.04f; + // RealSense has shorter depth range, some params should be tuned + if (sourceType == Type::DEPTH_REALSENSE) + { + _icpDistThresh = 0.01f; + _bilateralSigmaDepth = 0.01f; + } + } + + void updateParams(large_kinfu::Params& params) + { + if (vc.isOpened()) + { + updateIntrinsics(params.intr, params.frameSize, params.depthFactor); + updateVolumeParams(params.volumeParams.resolution, params.volumeParams.voxelSize, + params.volumeParams.tsdfTruncDist, params.volumeParams.pose, + params.truncateThreshold); + updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth); + + if (sourceType == Type::DEPTH_KINECT2) + { + Matx distCoeffs; + distCoeffs(0) = Kinect2Params::k1; + distCoeffs(1) = Kinect2Params::k2; + distCoeffs(4) = Kinect2Params::k3; + + initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, + params.frameSize, CV_16SC2, undistortMap1, undistortMap2); + } + } + } + + void updateParams(kinfu::Params& params) + { + if (vc.isOpened()) + { + updateIntrinsics(params.intr, params.frameSize, params.depthFactor); + updateVolumeParams(params.volumeDims, params.voxelSize, + params.tsdf_trunc_dist, params.volumePose, params.truncateThreshold); + updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth); + + if (sourceType == Type::DEPTH_KINECT2) + { + Matx distCoeffs; + distCoeffs(0) = Kinect2Params::k1; + distCoeffs(1) = Kinect2Params::k2; + distCoeffs(4) = Kinect2Params::k3; + + initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, + params.frameSize, CV_16SC2, undistortMap1, undistortMap2); + } + } + } + + std::vector depthFileList; + size_t frameIdx; + VideoCapture vc; + UMat undistortMap1, undistortMap2; + Type sourceType; +}; +} // namespace io_utils + +} // namespace cv +#endif /* ifndef OPENCV_RGBS_IO_UTILS_HPP */ diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 49f7fc6a885..95f732b6769 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -24,22 +24,22 @@ struct CV_EXPORTS_W Params /** * @brief Constructor for Params * Sets the initial pose of the TSDF volume. - * @param volumeIntialPoseRot rotation matrix - * @param volumeIntialPoseTransl translation vector + * @param volumeInitialPoseRot rotation matrix + * @param volumeInitialPoseTransl translation vector */ - CV_WRAP Params(Matx33f volumeIntialPoseRot, Vec3f volumeIntialPoseTransl) + CV_WRAP Params(Matx33f volumeInitialPoseRot, Vec3f volumeInitialPoseTransl) { - setInitialVolumePose(volumeIntialPoseRot,volumeIntialPoseTransl); + setInitialVolumePose(volumeInitialPoseRot,volumeInitialPoseTransl); } /** * @brief Constructor for Params * Sets the initial pose of the TSDF volume. - * @param volumeIntialPose 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume + * @param volumeInitialPose 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume */ - CV_WRAP Params(Matx44f volumeIntialPose) + CV_WRAP Params(Matx44f volumeInitialPose) { - setInitialVolumePose(volumeIntialPose); + setInitialVolumePose(volumeInitialPose); } /** diff --git a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp index c9842e1b82e..7f1428c6d51 100644 --- a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp @@ -84,29 +84,35 @@ struct CV_EXPORTS_W Params kinfu::VolumeParams volumeParams; }; -/** @brief KinectFusion implementation +/** @brief Large Scale Dense Depth Fusion implementation - This class implements a 3d reconstruction algorithm described in - @cite kinectfusion paper. + This class implements a 3d reconstruction algorithm for larger environments using + Spatially hashed TSDF volume "Submaps". + It also runs a periodic posegraph optimization to minimize drift in tracking over long sequences. + Currently the algorithm does not implement a relocalization or loop closure module. + Potentially a Bag of words implementation or RGBD relocalization as described in + Glocker et al. ISMAR 2013 will be implemented It takes a sequence of depth images taken from depth sensor (or any depth images source such as stereo camera matching algorithm or even raymarching renderer). The output can be obtained as a vector of points and their normals or can be Phong-rendered from given camera pose. - An internal representation of a model is a voxel cuboid that keeps TSDF values - which are a sort of distances to the surface (for details read the @cite kinectfusion article + An internal representation of a model is a spatially hashed voxel cube that stores TSDF values + which represent the distance to the closest surface (for details read the @cite kinectfusion article about TSDF). There is no interface to that representation yet. - LargeKinfu uses OpenCL acceleration automatically if available. - To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL(). + For posegraph optimization, a Submap abstraction over the Volume class is created. + New submaps are added to the model when there is low visibility overlap between current viewing frustrum + and the existing volume/model. Multiple submaps are simultaneously tracked and a posegraph is created and + optimized periodically. - This implementation is based on [kinfu-remake](https://github.com/Nerei/kinfu_remake). + LargeKinfu does not use any OpenCL acceleration yet. + To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL(). - Note that the KinectFusion algorithm was patented and its use may be restricted by - the list of patents mentioned in README.md file in this module directory. + This implementation is inspired from Kintinuous, InfiniTAM and other SOTA algorithms - That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion. + You need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion. */ class CV_EXPORTS_W LargeKinfu { diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index 147ffaa2d8e..93eb264d2fd 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -10,273 +10,16 @@ #include #include #include +#include using namespace cv; using namespace cv::kinfu; -using namespace std; +using namespace cv::io_utils; #ifdef HAVE_OPENCV_VIZ #include #endif -static vector readDepth(std::string fileList); - -static vector readDepth(std::string fileList) -{ - vector v; - - fstream file(fileList); - if(!file.is_open()) - throw std::runtime_error("Failed to read depth list"); - - std::string dir; - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - - while(!file.eof()) - { - std::string s, imgPath; - std::getline(file, s); - if(s.empty() || s[0] == '#') continue; - std::stringstream ss; - ss << s; - double thumb; - ss >> thumb >> imgPath; - v.push_back(dir+'/'+imgPath); - } - - return v; -} - -struct DepthWriter -{ - DepthWriter(string fileList) : - file(fileList, ios::out), count(0), dir() - { - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - - if(!file.is_open()) - throw std::runtime_error("Failed to write depth list"); - - file << "# depth maps saved from device" << endl; - file << "# useless_number filename" << endl; - } - - void append(InputArray _depth) - { - Mat depth = _depth.getMat(); - string depthFname = cv::format("%04d.png", count); - string fullDepthFname = dir + '/' + depthFname; - if(!imwrite(fullDepthFname, depth)) - throw std::runtime_error("Failed to write depth to file " + fullDepthFname); - file << count++ << " " << depthFname << endl; - } - - fstream file; - int count; - string dir; -}; - -namespace Kinect2Params -{ - static const Size frameSize = Size(512, 424); - // approximate values, no guarantee to be correct - static const float focal = 366.1f; - static const float cx = 258.2f; - static const float cy = 204.f; - static const float k1 = 0.12f; - static const float k2 = -0.34f; - static const float k3 = 0.12f; -}; - -struct DepthSource -{ -public: - enum Type - { - DEPTH_LIST, - DEPTH_KINECT2_LIST, - DEPTH_KINECT2, - DEPTH_REALSENSE - }; - - DepthSource(int cam) : - DepthSource("", cam) - { } - - DepthSource(String fileListName) : - DepthSource(fileListName, -1) - { } - - DepthSource(String fileListName, int cam) : - depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), - frameIdx(0), - undistortMap1(), - undistortMap2() - { - if(cam >= 0) - { - vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); - if(vc.isOpened()) - { - sourceType = Type::DEPTH_KINECT2; - } - else - { - vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); - if(vc.isOpened()) - { - sourceType = Type::DEPTH_REALSENSE; - } - } - } - else - { - vc = VideoCapture(); - sourceType = Type::DEPTH_KINECT2_LIST; - } - } - - UMat getDepth() - { - UMat out; - if (!vc.isOpened()) - { - if (frameIdx < depthFileList.size()) - { - Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH); - f.copyTo(out); - } - else - { - return UMat(); - } - } - else - { - vc.grab(); - switch (sourceType) - { - case Type::DEPTH_KINECT2: - vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); - break; - case Type::DEPTH_REALSENSE: - vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); - break; - default: - // unknown depth source - vc.retrieve(out); - } - - // workaround for Kinect 2 - if(sourceType == Type::DEPTH_KINECT2) - { - out = out(Rect(Point(), Kinect2Params::frameSize)); - - UMat outCopy; - // linear remap adds gradient between valid and invalid pixels - // which causes garbage, use nearest instead - remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST); - - cv::flip(outCopy, out, 1); - } - } - if (out.empty()) - throw std::runtime_error("Matrix is empty"); - return out; - } - - bool empty() - { - return depthFileList.empty() && !(vc.isOpened()); - } - - void updateParams(Params& params) - { - if (vc.isOpened()) - { - // this should be set in according to user's depth sensor - int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH); - int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT); - - // it's recommended to calibrate sensor to obtain its intrinsics - float fx, fy, cx, cy; - float depthFactor = 1000.f; - Size frameSize; - if(sourceType == Type::DEPTH_KINECT2) - { - fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; - - frameSize = Kinect2Params::frameSize; - } - else - { - if(sourceType == Type::DEPTH_REALSENSE) - { - fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); - fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); - depthFactor = 1.f/(float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); - } - else - { - fx = fy = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); - } - - cx = w/2 - 0.5f; - cy = h/2 - 0.5f; - - frameSize = Size(w, h); - } - - Matx33f camMatrix = Matx33f(fx, 0, cx, - 0, fy, cy, - 0, 0, 1); - - params.frameSize = frameSize; - params.intr = camMatrix; - params.depthFactor = depthFactor; - - // RealSense has shorter depth range, some params should be tuned - if(sourceType == Type::DEPTH_REALSENSE) - { - // all sizes in meters - float cubeSize = 1.f; - params.voxelSize = cubeSize/params.volumeDims[0]; - params.tsdf_trunc_dist = 0.01f; - params.icpDistThresh = 0.01f; - params.volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, - -cubeSize/2.f, - 0.05f)); - params.truncateThreshold = 2.5f; - params.bilateral_sigma_depth = 0.01f; - } - - if(sourceType == Type::DEPTH_KINECT2) - { - Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; - - initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), - camMatrix, frameSize, CV_16SC2, - undistortMap1, undistortMap2); - } - } - } - - vector depthFileList; - size_t frameIdx; - VideoCapture vc; - UMat undistortMap1, undistortMap2; - Type sourceType; -}; - #ifdef HAVE_OPENCV_VIZ const std::string vizWindowName = "cloud"; @@ -329,7 +72,7 @@ int main(int argc, char **argv) bool coarse = false; bool idle = false; bool useHashTSDF = false; - string recordPath; + std::string recordPath; CommandLineParser parser(argc, argv, keys); parser.about(message); diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp index 0284bbc351e..793def2f58a 100644 --- a/modules/rgbd/samples/large_kinfu_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -2,287 +2,32 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory -#include #include -#include +#include #include #include +#include +#include #include using namespace cv; using namespace cv::kinfu; using namespace cv::large_kinfu; -using namespace std; +using namespace cv::io_utils; #ifdef HAVE_OPENCV_VIZ #include #endif -static vector readDepth(std::string fileList); - -static vector readDepth(std::string fileList) -{ - vector v; - - fstream file(fileList); - if(!file.is_open()) - throw std::runtime_error("Failed to read depth list"); - - std::string dir; - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - while(!file.eof()) - { - std::string s, imgPath; - std::getline(file, s); - if(s.empty() || s[0] == '#') continue; - size_t spaceIdx = s.rfind(' '); - spaceIdx = spaceIdx != std::string::npos ? spaceIdx+1 : 0; - imgPath = s.substr(spaceIdx, s.size()); - v.push_back(dir+'/'+imgPath); - } - - return v; -} - -struct DepthWriter -{ - DepthWriter(string fileList) : - file(fileList, ios::out), count(0), dir() - { - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - - if(!file.is_open()) - throw std::runtime_error("Failed to write depth list"); - - file << "# depth maps saved from device" << endl; - file << "# useless_number filename" << endl; - } - - void append(InputArray _depth) - { - Mat depth = _depth.getMat(); - string depthFname = cv::format("%04d.png", count); - string fullDepthFname = dir + '/' + depthFname; - if(!imwrite(fullDepthFname, depth)) - throw std::runtime_error("Failed to write depth to file " + fullDepthFname); - file << count++ << " " << depthFname << endl; - } - - fstream file; - int count; - string dir; -}; - -namespace Kinect2Params -{ - static const Size frameSize = Size(512, 424); - // approximate values, no guarantee to be correct - static const float focal = 366.1f; - static const float cx = 258.2f; - static const float cy = 204.f; - static const float k1 = 0.12f; - static const float k2 = -0.34f; - static const float k3 = 0.12f; -}; - -struct DepthSource -{ -public: - enum Type - { - DEPTH_LIST, - DEPTH_KINECT2_LIST, - DEPTH_KINECT2, - DEPTH_REALSENSE - }; - - DepthSource(int cam) : - DepthSource("", cam) - { } - - DepthSource(String fileListName) : - DepthSource(fileListName, -1) - { } - - DepthSource(String fileListName, int cam) : - depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), - frameIdx(0), - undistortMap1(), - undistortMap2() - { - if(cam >= 0) - { - vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); - if(vc.isOpened()) - { - sourceType = Type::DEPTH_KINECT2; - } - else - { - vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); - if(vc.isOpened()) - { - sourceType = Type::DEPTH_REALSENSE; - } - } - } - else - { - vc = VideoCapture(); - sourceType = Type::DEPTH_KINECT2_LIST; - } - } - - UMat getDepth() - { - UMat out; - if (!vc.isOpened()) - { - if (frameIdx < depthFileList.size()) - { - Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH); - f.copyTo(out); - } - else - { - return UMat(); - } - } - else - { - vc.grab(); - switch (sourceType) - { - case Type::DEPTH_KINECT2: - vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); - break; - case Type::DEPTH_REALSENSE: - vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); - break; - default: - // unknown depth source - vc.retrieve(out); - } - - // workaround for Kinect 2 - if(sourceType == Type::DEPTH_KINECT2) - { - out = out(Rect(Point(), Kinect2Params::frameSize)); - - UMat outCopy; - // linear remap adds gradient between valid and invalid pixels - // which causes garbage, use nearest instead - remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST); - - cv::flip(outCopy, out, 1); - } - } - if (out.empty()) - throw std::runtime_error("Matrix is empty"); - return out; - } - - bool empty() - { - return depthFileList.empty() && !(vc.isOpened()); - } - - void updateParams(large_kinfu::Params& params) - { - if (vc.isOpened()) - { - // this should be set in according to user's depth sensor - int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH); - int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT); - - // it's recommended to calibrate sensor to obtain its intrinsics - float fx, fy, cx, cy; - float depthFactor = 1000.f; - Size frameSize; - if(sourceType == Type::DEPTH_KINECT2) - { - fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; - - frameSize = Kinect2Params::frameSize; - } - else - { - if(sourceType == Type::DEPTH_REALSENSE) - { - fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); - fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); - depthFactor = 1.f/(float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); - } - else - { - fx = fy = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); - } - - cx = w/2 - 0.5f; - cy = h/2 - 0.5f; - - frameSize = Size(w, h); - } - - Matx33f camMatrix = Matx33f(fx, 0, cx, - 0, fy, cy, - 0, 0, 1); - - params.frameSize = frameSize; - params.intr = camMatrix; - params.depthFactor = depthFactor; - - // RealSense has shorter depth range, some params should be tuned - if(sourceType == Type::DEPTH_REALSENSE) - { - // all sizes in meters - float cubeSize = 1.f; - params.volumeParams.voxelSize = cubeSize/params.volumeParams.resolution[0]; - params.volumeParams.tsdfTruncDist = 0.01f; - params.icpDistThresh = 0.01f; - params.volumeParams.pose = Affine3f().translate(Vec3f(-cubeSize/2.f, - -cubeSize/2.f, - 0.05f)); - params.truncateThreshold = 2.5f; - params.bilateral_sigma_depth = 0.01f; - } - - if(sourceType == Type::DEPTH_KINECT2) - { - Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; - - initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), - camMatrix, frameSize, CV_16SC2, - undistortMap1, undistortMap2); - } - } - } - - vector depthFileList; - size_t frameIdx; - VideoCapture vc; - UMat undistortMap1, undistortMap2; - Type sourceType; -}; - #ifdef HAVE_OPENCV_VIZ const std::string vizWindowName = "cloud"; struct PauseCallbackArgs { - PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) - { } + PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) {} LargeKinfu& largeKinfu; }; @@ -290,9 +35,9 @@ struct PauseCallbackArgs void pauseCallback(const viz::MouseEvent& me, void* args); void pauseCallback(const viz::MouseEvent& me, void* args) { - if(me.type == viz::MouseEvent::Type::MouseMove || - me.type == viz::MouseEvent::Type::MouseScrollDown || - me.type == viz::MouseEvent::Type::MouseScrollUp) + if (me.type == viz::MouseEvent::Type::MouseMove || + me.type == viz::MouseEvent::Type::MouseScrollDown || + me.type == viz::MouseEvent::Type::MouseScrollUp) { PauseCallbackArgs pca = *((PauseCallbackArgs*)(args)); viz::Viz3d window(vizWindowName); @@ -304,60 +49,54 @@ void pauseCallback(const viz::MouseEvent& me, void* args) } #endif -static const char* keys = -{ +static const char* keys = { "{help h usage ? | | print this message }" "{depth | | Path to depth.txt file listing a set of depth images }" "{camera |0| Index of depth camera to be used as a depth source }" "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," - " in coarse mode points and normals are displayed }" - "{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}" + " in coarse mode points and normals are displayed }" "{idle | | Do not run KinFu, just display depth frames }" "{record | | Write depth frames to specified file list" - " (the same format as for the 'depth' key) }" + " (the same format as for the 'depth' key) }" }; static const std::string message = - "\nThis demo uses live depth input or RGB-D dataset taken from" - "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" - "\nto demonstrate KinectFusion implementation \n"; - + "\nThis demo uses live depth input or RGB-D dataset taken from" + "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" + "\nto demonstrate KinectFusion implementation" + "\nThis module uses the newer hashtable based TSDFVolume (relatively fast) for larger " + "reconstructions by default\n"; -int main(int argc, char **argv) +int main(int argc, char** argv) { bool coarse = false; - bool idle = false; - bool useHashTSDF = false; - string recordPath; + bool idle = false; + std::string recordPath; CommandLineParser parser(argc, argv, keys); parser.about(message); - if(!parser.check()) + if (!parser.check()) { parser.printMessage(); parser.printErrors(); return -1; } - if(parser.has("help")) + if (parser.has("help")) { parser.printMessage(); return 0; } - if(parser.has("coarse")) + if (parser.has("coarse")) { coarse = true; } - if(parser.has("record")) + if (parser.has("record")) { recordPath = parser.get("record"); } - if(parser.has("useHashTSDF")) - { - useHashTSDF = true; - } - if(parser.has("idle")) + if (parser.has("idle")) { idle = true; } @@ -376,19 +115,13 @@ int main(int argc, char **argv) } Ptr depthWriter; - if(!recordPath.empty()) + if (!recordPath.empty()) depthWriter = makePtr(recordPath); - Ptr params; + Ptr params; Ptr largeKinfu; - if(coarse) - params = Params::coarseParams(); - else - params = Params::defaultParams(); - - if(useHashTSDF) - params = Params::hashTSDFParams(coarse); + params = large_kinfu::Params::hashTSDFParams(coarse); // These params can be different for each depth sensor ds->updateParams(*params); @@ -396,7 +129,7 @@ int main(int argc, char **argv) // Enables OpenCL explicitly (by default can be switched-off) cv::setUseOptimized(false); - if(!idle) + if (!idle) largeKinfu = LargeKinfu::create(params); #ifdef HAVE_OPENCV_VIZ @@ -411,32 +144,34 @@ int main(int argc, char **argv) int64 prevTime = getTickCount(); - for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) + for (UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) { - if(depthWriter) + if (depthWriter) depthWriter->append(frame); #ifdef HAVE_OPENCV_VIZ - if(pause) + if (pause) { // doesn't happen in idle mode largeKinfu->getCloud(points, normals); - if(!points.empty() && !normals.empty()) + if (!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); - viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05, + viz::Color::gray()); window.showWidget("cloud", cloudWidget); window.showWidget("normals", cloudNormals); Vec3d volSize = largeKinfu->getParams().volumeParams.voxelSize * Vec3d(largeKinfu->getParams().volumeParams.resolution); - window.showWidget("cube", viz::WCube(Vec3d::all(0), - volSize), + window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), largeKinfu->getParams().volumeParams.pose); PauseCallbackArgs pca(*largeKinfu); window.registerMouseCallback(pauseCallback, (void*)&pca); - window.showWidget("text", viz::WText(cv::String("Move camera in this window. " - "Close the window or press Q to resume"), Point())); + window.showWidget("text", + viz::WText(cv::String("Move camera in this window. " + "Close the window or press Q to resume"), + Point())); window.spin(); window.removeWidget("text"); window.removeWidget("cloud"); @@ -451,12 +186,12 @@ int main(int argc, char **argv) { UMat cvt8; float depthFactor = params->depthFactor; - convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor); - if(!idle) + convertScaleAbs(frame, cvt8, 0.25 * 256. / depthFactor); + if (!idle) { imshow("depth", cvt8); - if(!largeKinfu->update(frame)) + if (!largeKinfu->update(frame)) { largeKinfu->reset(); std::cout << "reset" << std::endl; @@ -464,23 +199,23 @@ int main(int argc, char **argv) #ifdef HAVE_OPENCV_VIZ else { - if(coarse) + if (coarse) { largeKinfu->getCloud(points, normals); - if(!points.empty() && !normals.empty()) + if (!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); - viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, + /*scale*/ 0.05, viz::Color::gray()); window.showWidget("cloud", cloudWidget); window.showWidget("normals", cloudNormals); } } - //window.showWidget("worldAxes", viz::WCoordinateSystem()); + // window.showWidget("worldAxes", viz::WCoordinateSystem()); Vec3d volSize = largeKinfu->getParams().volumeParams.voxelSize * largeKinfu->getParams().volumeParams.resolution; - window.showWidget("cube", viz::WCube(Vec3d::all(0), - volSize), + window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), largeKinfu->getParams().volumeParams.pose); window.setViewerPose(largeKinfu->getPose()); window.spinOnce(1, true); @@ -496,31 +231,29 @@ int main(int argc, char **argv) } int64 newTime = getTickCount(); - putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit", - (int)(getTickFrequency()/(newTime - prevTime))), - Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); + putText(rendered, + cv::format("FPS: %2d press R to reset, P to pause, Q to quit", + (int)(getTickFrequency() / (newTime - prevTime))), + Point(0, rendered.rows - 1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); prevTime = newTime; imshow("render", rendered); int c = waitKey(1); switch (c) { - case 'r': - if(!idle) - largeKinfu->reset(); - break; - case 'q': - return 0; + case 'r': + if (!idle) + largeKinfu->reset(); + break; + case 'q': return 0; #ifdef HAVE_OPENCV_VIZ - case 'p': - if(!idle) - pause = true; + case 'p': + if (!idle) + pause = true; #endif - default: - break; + default: break; } } return 0; } - diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 61ec7a90376..22bfb888996 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -55,7 +55,7 @@ class HashTSDFVolume : public Volume return static_cast(this)->getVisibleBlocks_(currFrameId, frameThreshold); } - TsdfVoxel at(const Vec3i& volumeIdx) const + virtual TsdfVoxel at(const Vec3i& volumeIdx) const { const Derived* derived = static_cast(this); return derived->at_(volumeIdx); diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index a895ea7a48e..629d6260a7f 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -4,24 +4,23 @@ #include "pose_graph.hpp" +#include #include #include #include #if defined(CERES_FOUND) -#include "ceres/ceres.h" +#include #endif namespace cv { namespace kinfu { -void PoseGraph::addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } - bool PoseGraph::isValid() const { - int numNodes = int(nodes.size()); - int numEdges = int(edges.size()); + int numNodes = getNumNodes(); + int numEdges = getNumEdges(); if (numNodes <= 0 || numEdges <= 0) return false; @@ -82,7 +81,7 @@ bool PoseGraph::isValid() const return isGraphConnected && !invalidEdgeNode; } -#if defined(CERES_FOUND) +#if defined(CERES_FOUND) && defined(HAVE_EIGEN) void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem) { int numEdges = poseGraph.getNumEdges(); @@ -103,8 +102,8 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& const PoseGraphEdge& currEdge = poseGraph.edges.at(currEdgeNum); int sourceNodeId = currEdge.getSourceNodeId(); int targetNodeId = currEdge.getTargetNodeId(); - Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).pose; - Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).pose; + Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).se3Pose; + Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).se3Pose; const Matx66f& informationMatrix = currEdge.information; @@ -123,8 +122,8 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& PoseGraphNode& currNode = poseGraph.nodes.at(currNodeId); if (currNode.isPoseFixed()) { - problem.SetParameterBlockConstant(currNode.pose.t.data()); - problem.SetParameterBlockConstant(currNode.pose.r.coeffs().data()); + problem.SetParameterBlockConstant(currNode.se3Pose.t.data()); + problem.SetParameterBlockConstant(currNode.se3Pose.r.coeffs().data()); } } } @@ -146,7 +145,7 @@ void Optimizer::optimizeCeres(PoseGraph& poseGraph) std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" << std::endl; -#if defined(CERES_FOUND) +#if defined(CERES_FOUND) && defined(HAVE_EIGEN) ceres::Problem problem; createOptimizationProblem(poseGraph, problem); @@ -161,7 +160,7 @@ void Optimizer::optimizeCeres(PoseGraph& poseGraph) std::cout << "Is solution usable: " << summary.IsSolutionUsable() << std::endl; #else - CV_Error(Error::StsNotImplemented, "Ceres required for Pose Graph optimization"); + CV_Error(Error::StsNotImplemented, "Ceres and Eigen required for Pose Graph optimization"); #endif } diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 91c06f1519e..d92731fc903 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -93,37 +93,55 @@ struct Pose3d r.normalize(); } }; -#endif struct PoseGraphNode { public: explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) - : nodeId(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) + : nodeId(_nodeId), isFixed(false), pose(_pose) { +#if defined(HAVE_EIGEN) + se3Pose = Pose3d(_pose.rotation(), _pose.translation()); +#endif } virtual ~PoseGraphNode() = default; int getId() const { return nodeId; } - inline Affine3d getPose() const + inline Affine3f getPose() const + { + return pose; + } + void setPose(const Affine3f& _pose) + { + pose = _pose; +#if defined(HAVE_EIGEN) + se3Pose = Pose3d(pose.rotation(), pose.translation()); +#endif + } +#if defined(HAVE_EIGEN) + void setPose(const Pose3d& _pose) { - const Eigen::Matrix3d& rotation = pose.r.toRotationMatrix(); - const Eigen::Vector3d& translation = pose.t; + se3Pose = _pose; + const Eigen::Matrix3d& rotation = se3Pose.r.toRotationMatrix(); + const Eigen::Vector3d& translation = se3Pose.t; Matx33d rot; Vec3d trans; eigen2cv(rotation, rot); eigen2cv(translation, trans); Affine3d poseMatrix(rot, trans); - return poseMatrix; + pose = poseMatrix; } - void setPose(const Pose3d& _pose) { pose = _pose; } +#endif void setFixed(bool val = true) { isFixed = val; } bool isPoseFixed() const { return isFixed; } public: int nodeId; bool isFixed; - Pose3d pose; + Affine3f pose; +#if defined(HAVE_EIGEN) + Pose3d se3Pose; +#endif }; /*! \class PoseGraphEdge @@ -165,38 +183,38 @@ struct PoseGraphEdge //! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold //! optimization Jose Luis Blanco Compactly represents the jacobian of the SE3 generator // clang-format off -static const std::array generatorJacobian = { - // alpha - Matx44f(0, 0, 0, 0, - 0, 0, -1, 0, - 0, 1, 0, 0, - 0, 0, 0, 0), - // beta - Matx44f( 0, 0, 1, 0, - 0, 0, 0, 0, - -1, 0, 0, 0, - 0, 0, 0, 0), - // gamma - Matx44f(0, -1, 0, 0, - 1, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0), - // x - Matx44f(0, 0, 0, 1, - 0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0), - // y - Matx44f(0, 0, 0, 0, - 0, 0, 0, 1, - 0, 0, 0, 0, - 0, 0, 0, 0), - // z - Matx44f(0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 1, - 0, 0, 0, 0) -}; +/* static const std::array generatorJacobian = { */ +/* // alpha */ +/* Matx44f(0, 0, 0, 0, */ +/* 0, 0, -1, 0, */ +/* 0, 1, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // beta */ +/* Matx44f( 0, 0, 1, 0, */ +/* 0, 0, 0, 0, */ +/* -1, 0, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // gamma */ +/* Matx44f(0, -1, 0, 0, */ +/* 1, 0, 0, 0, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // x */ +/* Matx44f(0, 0, 0, 1, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // y */ +/* Matx44f(0, 0, 0, 0, */ +/* 0, 0, 0, 1, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // z */ +/* Matx44f(0, 0, 0, 0, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 1, */ +/* 0, 0, 0, 0) */ +/* }; */ // clang-format on class PoseGraph @@ -213,7 +231,7 @@ class PoseGraph PoseGraph& operator=(const PoseGraph& _poseGraph) = default; void addNode(const PoseGraphNode& node) { nodes.push_back(node); } - void addEdge(const PoseGraphEdge& edge); + void addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } bool nodeExists(int nodeId) const { @@ -224,14 +242,14 @@ class PoseGraph bool isValid() const; - int getNumNodes() const { return nodes.size(); } - int getNumEdges() const { return edges.size(); } + int getNumNodes() const { return int(nodes.size()); } + int getNumEdges() const { return int(edges.size()); } public: NodeVector nodes; EdgeVector edges; }; - +#endif namespace Optimizer { void optimizeCeres(PoseGraph& poseGraph); diff --git a/modules/rgbd/src/submap.cpp b/modules/rgbd/src/submap.cpp deleted file mode 100644 index 83c2fd654d4..00000000000 --- a/modules/rgbd/src/submap.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// 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 "submap.hpp" - -#include "hash_tsdf.hpp" -#include "kinfu_frame.hpp" -#include "precomp.hpp" - -namespace cv -{ -namespace kinfu -{ - -/* void SubmapManager::addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, const Affine3f& currPose) - */ -/* { */ -/* //! 1. Add new posegraph node */ -/* //! 2. Add new posegraph constraint */ - -/* //! TODO: Attempt registration between submaps */ -/* Affine3f Tprev2curr = currPose.inv() * currPose; */ -/* //! Constant information matrix for all odometry constraints */ -/* Matx66f information = Matx66f::eye() * 1000; */ - -/* //! Inserts only if the element does not exist already */ -/* poseGraph.addNode(prevId, cv::makePtr(prevPose)); */ -/* poseGraph.addNode(currId, cv::makePtr(currPose)); */ - -/* poseGraph.addEdge(cv::makePtr(prevId, currId, Tprev2curr, information)); */ -/* } */ - - -} // namespace kinfu -} // namespace cv From 386b2acaf34e0e11efe7d06a77776fc16f31951c Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Thu, 27 Aug 2020 12:14:28 -0400 Subject: [PATCH 30/36] - Reorganize IO code for samples in a separate file - Minor fix for Ceres preprocessor definition - Remove unused generatorJacobian, will be used for opencv implementation of levenberg marquardt - Doxygen docs fix - Minor preprocessor fixes - Move inline functions to header, and make function params const references --- modules/rgbd/CMakeLists.txt | 8 +- .../rgbd/include/opencv2/rgbd/io_utils.hpp | 313 ++++++++++++++ modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 14 +- .../rgbd/include/opencv2/rgbd/large_kinfu.hpp | 28 +- modules/rgbd/include/opencv2/rgbd/volume.hpp | 18 +- modules/rgbd/samples/kinfu_demo.cpp | 263 +----------- modules/rgbd/samples/large_kinfu_demo.cpp | 393 +++--------------- modules/rgbd/src/hash_tsdf.cpp | 104 +---- modules/rgbd/src/hash_tsdf.hpp | 121 +++++- modules/rgbd/src/pose_graph.cpp | 28 +- modules/rgbd/src/pose_graph.hpp | 107 +++-- modules/rgbd/src/submap.cpp | 36 -- modules/rgbd/src/submap.hpp | 29 +- modules/rgbd/src/tsdf.cpp | 4 +- modules/rgbd/src/tsdf.hpp | 4 +- 15 files changed, 614 insertions(+), 856 deletions(-) create mode 100644 modules/rgbd/include/opencv2/rgbd/io_utils.hpp delete mode 100644 modules/rgbd/src/submap.cpp diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt index 133879bc07a..247c788a9c1 100644 --- a/modules/rgbd/CMakeLists.txt +++ b/modules/rgbd/CMakeLists.txt @@ -1,13 +1,11 @@ set(the_description "RGBD algorithms") find_package(Ceres QUIET) +ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python) +ocv_target_link_libraries(${the_module} ${CERES_LIBRARIES}) if(Ceres_FOUND) - add_definitions("-DCERES_FOUND=1") + ocv_target_compile_definitions(${the_module} PUBLIC CERES_FOUND) else() - add_definitions("-DCERES_FOUND=0") message(STATUS "CERES support is disabled. Ceres Solver is Required for Posegraph optimization") endif() - -ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python) -ocv_target_link_libraries(${the_module} ${CERES_LIBRARIES}) diff --git a/modules/rgbd/include/opencv2/rgbd/io_utils.hpp b/modules/rgbd/include/opencv2/rgbd/io_utils.hpp new file mode 100644 index 00000000000..c96d6c5345d --- /dev/null +++ b/modules/rgbd/include/opencv2/rgbd/io_utils.hpp @@ -0,0 +1,313 @@ +// 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_RGBS_IO_UTILS_HPP +#define OPENCV_RGBS_IO_UTILS_HPP + +#include +#include +#include +#include +#include +#include +#include + +namespace cv +{ +namespace io_utils +{ + +static std::vector readDepth(const std::string& fileList) +{ + std::vector v; + + std::fstream file(fileList); + if (!file.is_open()) + throw std::runtime_error("Failed to read depth list"); + + std::string dir; + size_t slashIdx = fileList.rfind('/'); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); + + while (!file.eof()) + { + std::string s, imgPath; + std::getline(file, s); + if (s.empty() || s[0] == '#') + continue; + std::stringstream ss; + ss << s; + double thumb; + ss >> thumb >> imgPath; + v.push_back(dir + '/' + imgPath); + } + + return v; +} + +struct DepthWriter +{ + DepthWriter(std::string fileList) : file(fileList, std::ios::out), count(0), dir() + { + size_t slashIdx = fileList.rfind('/'); + slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); + dir = fileList.substr(0, slashIdx); + + if (!file.is_open()) + throw std::runtime_error("Failed to write depth list"); + + file << "# depth maps saved from device" << std::endl; + file << "# useless_number filename" << std::endl; + } + + void append(InputArray _depth) + { + Mat depth = _depth.getMat(); + std::string depthFname = cv::format("%04d.png", count); + std::string fullDepthFname = dir + '/' + depthFname; + if (!imwrite(fullDepthFname, depth)) + throw std::runtime_error("Failed to write depth to file " + fullDepthFname); + file << count++ << " " << depthFname << std::endl; + } + + std::fstream file; + int count; + std::string dir; +}; + +namespace Kinect2Params +{ +static const Size frameSize = Size(512, 424); +// approximate values, no guarantee to be correct +static const float focal = 366.1f; +static const float cx = 258.2f; +static const float cy = 204.f; +static const float k1 = 0.12f; +static const float k2 = -0.34f; +static const float k3 = 0.12f; +}; // namespace Kinect2Params + +struct DepthSource +{ + public: + enum Type + { + DEPTH_LIST, + DEPTH_KINECT2_LIST, + DEPTH_KINECT2, + DEPTH_REALSENSE + }; + + DepthSource(int cam) : DepthSource("", cam) {} + + DepthSource(String fileListName) : DepthSource(fileListName, -1) {} + + DepthSource(String fileListName, int cam) + : depthFileList(fileListName.empty() ? std::vector() + : readDepth(fileListName)), + frameIdx(0), + undistortMap1(), + undistortMap2() + { + if (cam >= 0) + { + vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); + if (vc.isOpened()) + { + sourceType = Type::DEPTH_KINECT2; + } + else + { + vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); + if (vc.isOpened()) + { + sourceType = Type::DEPTH_REALSENSE; + } + } + } + else + { + vc = VideoCapture(); + sourceType = Type::DEPTH_KINECT2_LIST; + } + } + + UMat getDepth() + { + UMat out; + if (!vc.isOpened()) + { + if (frameIdx < depthFileList.size()) + { + Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH); + f.copyTo(out); + } + else + { + return UMat(); + } + } + else + { + vc.grab(); + switch (sourceType) + { + case Type::DEPTH_KINECT2: vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); break; + case Type::DEPTH_REALSENSE: vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); break; + default: + // unknown depth source + vc.retrieve(out); + } + + // workaround for Kinect 2 + if (sourceType == Type::DEPTH_KINECT2) + { + out = out(Rect(Point(), Kinect2Params::frameSize)); + + UMat outCopy; + // linear remap adds gradient between valid and invalid pixels + // which causes garbage, use nearest instead + remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST); + + cv::flip(outCopy, out, 1); + } + } + if (out.empty()) + throw std::runtime_error("Matrix is empty"); + return out; + } + + bool empty() { return depthFileList.empty() && !(vc.isOpened()); } + + void updateIntrinsics(Matx33f& _intrinsics, Size& _frameSize, float& _depthFactor) + { + if (vc.isOpened()) + { + // this should be set in according to user's depth sensor + int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH); + int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT); + + // it's recommended to calibrate sensor to obtain its intrinsics + float fx, fy, cx, cy; + float depthFactor = 1000.f; + Size frameSize; + if (sourceType == Type::DEPTH_KINECT2) + { + fx = fy = Kinect2Params::focal; + cx = Kinect2Params::cx; + cy = Kinect2Params::cy; + + frameSize = Kinect2Params::frameSize; + } + else + { + if (sourceType == Type::DEPTH_REALSENSE) + { + fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); + fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); + depthFactor = 1.f / (float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); + } + else + { + fx = fy = + (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); + } + + cx = w / 2 - 0.5f; + cy = h / 2 - 0.5f; + + frameSize = Size(w, h); + } + + Matx33f camMatrix = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); + _intrinsics = camMatrix; + _frameSize = frameSize; + _depthFactor = depthFactor; + } + } + + void updateVolumeParams(const Vec3i& _resolution, float& _voxelSize, float& _tsdfTruncDist, + Affine3f& _volumePose, float& _depthTruncateThreshold) + { + float volumeSize = 3.0f; + _depthTruncateThreshold = 0.0f; + // RealSense has shorter depth range, some params should be tuned + if (sourceType == Type::DEPTH_REALSENSE) + { + volumeSize = 1.f; + _voxelSize = volumeSize / _resolution[0]; + _tsdfTruncDist = 0.01f; + _depthTruncateThreshold = 2.5f; + } + _volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.05f)); + } + + void updateICPParams(float& _icpDistThresh, float& _bilateralSigmaDepth) + { + _icpDistThresh = 0.1f; + _bilateralSigmaDepth = 0.04f; + // RealSense has shorter depth range, some params should be tuned + if (sourceType == Type::DEPTH_REALSENSE) + { + _icpDistThresh = 0.01f; + _bilateralSigmaDepth = 0.01f; + } + } + + void updateParams(large_kinfu::Params& params) + { + if (vc.isOpened()) + { + updateIntrinsics(params.intr, params.frameSize, params.depthFactor); + updateVolumeParams(params.volumeParams.resolution, params.volumeParams.voxelSize, + params.volumeParams.tsdfTruncDist, params.volumeParams.pose, + params.truncateThreshold); + updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth); + + if (sourceType == Type::DEPTH_KINECT2) + { + Matx distCoeffs; + distCoeffs(0) = Kinect2Params::k1; + distCoeffs(1) = Kinect2Params::k2; + distCoeffs(4) = Kinect2Params::k3; + + initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, + params.frameSize, CV_16SC2, undistortMap1, undistortMap2); + } + } + } + + void updateParams(kinfu::Params& params) + { + if (vc.isOpened()) + { + updateIntrinsics(params.intr, params.frameSize, params.depthFactor); + updateVolumeParams(params.volumeDims, params.voxelSize, + params.tsdf_trunc_dist, params.volumePose, params.truncateThreshold); + updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth); + + if (sourceType == Type::DEPTH_KINECT2) + { + Matx distCoeffs; + distCoeffs(0) = Kinect2Params::k1; + distCoeffs(1) = Kinect2Params::k2; + distCoeffs(4) = Kinect2Params::k3; + + initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, + params.frameSize, CV_16SC2, undistortMap1, undistortMap2); + } + } + } + + std::vector depthFileList; + size_t frameIdx; + VideoCapture vc; + UMat undistortMap1, undistortMap2; + Type sourceType; +}; +} // namespace io_utils + +} // namespace cv +#endif /* ifndef OPENCV_RGBS_IO_UTILS_HPP */ diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 49f7fc6a885..95f732b6769 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -24,22 +24,22 @@ struct CV_EXPORTS_W Params /** * @brief Constructor for Params * Sets the initial pose of the TSDF volume. - * @param volumeIntialPoseRot rotation matrix - * @param volumeIntialPoseTransl translation vector + * @param volumeInitialPoseRot rotation matrix + * @param volumeInitialPoseTransl translation vector */ - CV_WRAP Params(Matx33f volumeIntialPoseRot, Vec3f volumeIntialPoseTransl) + CV_WRAP Params(Matx33f volumeInitialPoseRot, Vec3f volumeInitialPoseTransl) { - setInitialVolumePose(volumeIntialPoseRot,volumeIntialPoseTransl); + setInitialVolumePose(volumeInitialPoseRot,volumeInitialPoseTransl); } /** * @brief Constructor for Params * Sets the initial pose of the TSDF volume. - * @param volumeIntialPose 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume + * @param volumeInitialPose 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume */ - CV_WRAP Params(Matx44f volumeIntialPose) + CV_WRAP Params(Matx44f volumeInitialPose) { - setInitialVolumePose(volumeIntialPose); + setInitialVolumePose(volumeInitialPose); } /** diff --git a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp index c9842e1b82e..7f1428c6d51 100644 --- a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp @@ -84,29 +84,35 @@ struct CV_EXPORTS_W Params kinfu::VolumeParams volumeParams; }; -/** @brief KinectFusion implementation +/** @brief Large Scale Dense Depth Fusion implementation - This class implements a 3d reconstruction algorithm described in - @cite kinectfusion paper. + This class implements a 3d reconstruction algorithm for larger environments using + Spatially hashed TSDF volume "Submaps". + It also runs a periodic posegraph optimization to minimize drift in tracking over long sequences. + Currently the algorithm does not implement a relocalization or loop closure module. + Potentially a Bag of words implementation or RGBD relocalization as described in + Glocker et al. ISMAR 2013 will be implemented It takes a sequence of depth images taken from depth sensor (or any depth images source such as stereo camera matching algorithm or even raymarching renderer). The output can be obtained as a vector of points and their normals or can be Phong-rendered from given camera pose. - An internal representation of a model is a voxel cuboid that keeps TSDF values - which are a sort of distances to the surface (for details read the @cite kinectfusion article + An internal representation of a model is a spatially hashed voxel cube that stores TSDF values + which represent the distance to the closest surface (for details read the @cite kinectfusion article about TSDF). There is no interface to that representation yet. - LargeKinfu uses OpenCL acceleration automatically if available. - To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL(). + For posegraph optimization, a Submap abstraction over the Volume class is created. + New submaps are added to the model when there is low visibility overlap between current viewing frustrum + and the existing volume/model. Multiple submaps are simultaneously tracked and a posegraph is created and + optimized periodically. - This implementation is based on [kinfu-remake](https://github.com/Nerei/kinfu_remake). + LargeKinfu does not use any OpenCL acceleration yet. + To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL(). - Note that the KinectFusion algorithm was patented and its use may be restricted by - the list of patents mentioned in README.md file in this module directory. + This implementation is inspired from Kintinuous, InfiniTAM and other SOTA algorithms - That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion. + You need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion. */ class CV_EXPORTS_W LargeKinfu { diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 5d99ab16293..ef3dcde5a15 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -29,12 +29,12 @@ class CV_EXPORTS_W Volume virtual ~Volume(){}; virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) = 0; - virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, Size frameSize, - OutputArray points, OutputArray normals) const = 0; - virtual void fetchNormals(InputArray points, OutputArray _normals) const = 0; - virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0; - virtual void reset() = 0; + const kinfu::Intr& intrinsics, const int frameId = 0) = 0; + virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, + const Size& frameSize, OutputArray points, OutputArray normals) const = 0; + virtual void fetchNormals(InputArray points, OutputArray _normals) const = 0; + virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0; + virtual void reset() = 0; public: const float voxelSize; @@ -49,7 +49,7 @@ enum class VolumeType HASHTSDF = 1 }; -struct VolumeParams +struct CV_EXPORTS_W VolumeParams { /** @brief Type of Volume Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units) @@ -106,8 +106,8 @@ struct VolumeParams CV_EXPORTS_W Ptr makeVolume(const VolumeParams& _volumeParams); CV_EXPORTS_W Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, - float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, Vec3i _resolution); + float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, Vec3i _resolution); } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index 147ffaa2d8e..93eb264d2fd 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -10,273 +10,16 @@ #include #include #include +#include using namespace cv; using namespace cv::kinfu; -using namespace std; +using namespace cv::io_utils; #ifdef HAVE_OPENCV_VIZ #include #endif -static vector readDepth(std::string fileList); - -static vector readDepth(std::string fileList) -{ - vector v; - - fstream file(fileList); - if(!file.is_open()) - throw std::runtime_error("Failed to read depth list"); - - std::string dir; - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - - while(!file.eof()) - { - std::string s, imgPath; - std::getline(file, s); - if(s.empty() || s[0] == '#') continue; - std::stringstream ss; - ss << s; - double thumb; - ss >> thumb >> imgPath; - v.push_back(dir+'/'+imgPath); - } - - return v; -} - -struct DepthWriter -{ - DepthWriter(string fileList) : - file(fileList, ios::out), count(0), dir() - { - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - - if(!file.is_open()) - throw std::runtime_error("Failed to write depth list"); - - file << "# depth maps saved from device" << endl; - file << "# useless_number filename" << endl; - } - - void append(InputArray _depth) - { - Mat depth = _depth.getMat(); - string depthFname = cv::format("%04d.png", count); - string fullDepthFname = dir + '/' + depthFname; - if(!imwrite(fullDepthFname, depth)) - throw std::runtime_error("Failed to write depth to file " + fullDepthFname); - file << count++ << " " << depthFname << endl; - } - - fstream file; - int count; - string dir; -}; - -namespace Kinect2Params -{ - static const Size frameSize = Size(512, 424); - // approximate values, no guarantee to be correct - static const float focal = 366.1f; - static const float cx = 258.2f; - static const float cy = 204.f; - static const float k1 = 0.12f; - static const float k2 = -0.34f; - static const float k3 = 0.12f; -}; - -struct DepthSource -{ -public: - enum Type - { - DEPTH_LIST, - DEPTH_KINECT2_LIST, - DEPTH_KINECT2, - DEPTH_REALSENSE - }; - - DepthSource(int cam) : - DepthSource("", cam) - { } - - DepthSource(String fileListName) : - DepthSource(fileListName, -1) - { } - - DepthSource(String fileListName, int cam) : - depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), - frameIdx(0), - undistortMap1(), - undistortMap2() - { - if(cam >= 0) - { - vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); - if(vc.isOpened()) - { - sourceType = Type::DEPTH_KINECT2; - } - else - { - vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); - if(vc.isOpened()) - { - sourceType = Type::DEPTH_REALSENSE; - } - } - } - else - { - vc = VideoCapture(); - sourceType = Type::DEPTH_KINECT2_LIST; - } - } - - UMat getDepth() - { - UMat out; - if (!vc.isOpened()) - { - if (frameIdx < depthFileList.size()) - { - Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH); - f.copyTo(out); - } - else - { - return UMat(); - } - } - else - { - vc.grab(); - switch (sourceType) - { - case Type::DEPTH_KINECT2: - vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); - break; - case Type::DEPTH_REALSENSE: - vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); - break; - default: - // unknown depth source - vc.retrieve(out); - } - - // workaround for Kinect 2 - if(sourceType == Type::DEPTH_KINECT2) - { - out = out(Rect(Point(), Kinect2Params::frameSize)); - - UMat outCopy; - // linear remap adds gradient between valid and invalid pixels - // which causes garbage, use nearest instead - remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST); - - cv::flip(outCopy, out, 1); - } - } - if (out.empty()) - throw std::runtime_error("Matrix is empty"); - return out; - } - - bool empty() - { - return depthFileList.empty() && !(vc.isOpened()); - } - - void updateParams(Params& params) - { - if (vc.isOpened()) - { - // this should be set in according to user's depth sensor - int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH); - int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT); - - // it's recommended to calibrate sensor to obtain its intrinsics - float fx, fy, cx, cy; - float depthFactor = 1000.f; - Size frameSize; - if(sourceType == Type::DEPTH_KINECT2) - { - fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; - - frameSize = Kinect2Params::frameSize; - } - else - { - if(sourceType == Type::DEPTH_REALSENSE) - { - fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); - fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); - depthFactor = 1.f/(float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); - } - else - { - fx = fy = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); - } - - cx = w/2 - 0.5f; - cy = h/2 - 0.5f; - - frameSize = Size(w, h); - } - - Matx33f camMatrix = Matx33f(fx, 0, cx, - 0, fy, cy, - 0, 0, 1); - - params.frameSize = frameSize; - params.intr = camMatrix; - params.depthFactor = depthFactor; - - // RealSense has shorter depth range, some params should be tuned - if(sourceType == Type::DEPTH_REALSENSE) - { - // all sizes in meters - float cubeSize = 1.f; - params.voxelSize = cubeSize/params.volumeDims[0]; - params.tsdf_trunc_dist = 0.01f; - params.icpDistThresh = 0.01f; - params.volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, - -cubeSize/2.f, - 0.05f)); - params.truncateThreshold = 2.5f; - params.bilateral_sigma_depth = 0.01f; - } - - if(sourceType == Type::DEPTH_KINECT2) - { - Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; - - initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), - camMatrix, frameSize, CV_16SC2, - undistortMap1, undistortMap2); - } - } - } - - vector depthFileList; - size_t frameIdx; - VideoCapture vc; - UMat undistortMap1, undistortMap2; - Type sourceType; -}; - #ifdef HAVE_OPENCV_VIZ const std::string vizWindowName = "cloud"; @@ -329,7 +72,7 @@ int main(int argc, char **argv) bool coarse = false; bool idle = false; bool useHashTSDF = false; - string recordPath; + std::string recordPath; CommandLineParser parser(argc, argv, keys); parser.about(message); diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp index 0284bbc351e..793def2f58a 100644 --- a/modules/rgbd/samples/large_kinfu_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -2,287 +2,32 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html -// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory -#include #include -#include +#include #include #include +#include +#include #include using namespace cv; using namespace cv::kinfu; using namespace cv::large_kinfu; -using namespace std; +using namespace cv::io_utils; #ifdef HAVE_OPENCV_VIZ #include #endif -static vector readDepth(std::string fileList); - -static vector readDepth(std::string fileList) -{ - vector v; - - fstream file(fileList); - if(!file.is_open()) - throw std::runtime_error("Failed to read depth list"); - - std::string dir; - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - while(!file.eof()) - { - std::string s, imgPath; - std::getline(file, s); - if(s.empty() || s[0] == '#') continue; - size_t spaceIdx = s.rfind(' '); - spaceIdx = spaceIdx != std::string::npos ? spaceIdx+1 : 0; - imgPath = s.substr(spaceIdx, s.size()); - v.push_back(dir+'/'+imgPath); - } - - return v; -} - -struct DepthWriter -{ - DepthWriter(string fileList) : - file(fileList, ios::out), count(0), dir() - { - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - - if(!file.is_open()) - throw std::runtime_error("Failed to write depth list"); - - file << "# depth maps saved from device" << endl; - file << "# useless_number filename" << endl; - } - - void append(InputArray _depth) - { - Mat depth = _depth.getMat(); - string depthFname = cv::format("%04d.png", count); - string fullDepthFname = dir + '/' + depthFname; - if(!imwrite(fullDepthFname, depth)) - throw std::runtime_error("Failed to write depth to file " + fullDepthFname); - file << count++ << " " << depthFname << endl; - } - - fstream file; - int count; - string dir; -}; - -namespace Kinect2Params -{ - static const Size frameSize = Size(512, 424); - // approximate values, no guarantee to be correct - static const float focal = 366.1f; - static const float cx = 258.2f; - static const float cy = 204.f; - static const float k1 = 0.12f; - static const float k2 = -0.34f; - static const float k3 = 0.12f; -}; - -struct DepthSource -{ -public: - enum Type - { - DEPTH_LIST, - DEPTH_KINECT2_LIST, - DEPTH_KINECT2, - DEPTH_REALSENSE - }; - - DepthSource(int cam) : - DepthSource("", cam) - { } - - DepthSource(String fileListName) : - DepthSource(fileListName, -1) - { } - - DepthSource(String fileListName, int cam) : - depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), - frameIdx(0), - undistortMap1(), - undistortMap2() - { - if(cam >= 0) - { - vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); - if(vc.isOpened()) - { - sourceType = Type::DEPTH_KINECT2; - } - else - { - vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); - if(vc.isOpened()) - { - sourceType = Type::DEPTH_REALSENSE; - } - } - } - else - { - vc = VideoCapture(); - sourceType = Type::DEPTH_KINECT2_LIST; - } - } - - UMat getDepth() - { - UMat out; - if (!vc.isOpened()) - { - if (frameIdx < depthFileList.size()) - { - Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH); - f.copyTo(out); - } - else - { - return UMat(); - } - } - else - { - vc.grab(); - switch (sourceType) - { - case Type::DEPTH_KINECT2: - vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); - break; - case Type::DEPTH_REALSENSE: - vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); - break; - default: - // unknown depth source - vc.retrieve(out); - } - - // workaround for Kinect 2 - if(sourceType == Type::DEPTH_KINECT2) - { - out = out(Rect(Point(), Kinect2Params::frameSize)); - - UMat outCopy; - // linear remap adds gradient between valid and invalid pixels - // which causes garbage, use nearest instead - remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST); - - cv::flip(outCopy, out, 1); - } - } - if (out.empty()) - throw std::runtime_error("Matrix is empty"); - return out; - } - - bool empty() - { - return depthFileList.empty() && !(vc.isOpened()); - } - - void updateParams(large_kinfu::Params& params) - { - if (vc.isOpened()) - { - // this should be set in according to user's depth sensor - int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH); - int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT); - - // it's recommended to calibrate sensor to obtain its intrinsics - float fx, fy, cx, cy; - float depthFactor = 1000.f; - Size frameSize; - if(sourceType == Type::DEPTH_KINECT2) - { - fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; - - frameSize = Kinect2Params::frameSize; - } - else - { - if(sourceType == Type::DEPTH_REALSENSE) - { - fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); - fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); - depthFactor = 1.f/(float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE); - } - else - { - fx = fy = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); - } - - cx = w/2 - 0.5f; - cy = h/2 - 0.5f; - - frameSize = Size(w, h); - } - - Matx33f camMatrix = Matx33f(fx, 0, cx, - 0, fy, cy, - 0, 0, 1); - - params.frameSize = frameSize; - params.intr = camMatrix; - params.depthFactor = depthFactor; - - // RealSense has shorter depth range, some params should be tuned - if(sourceType == Type::DEPTH_REALSENSE) - { - // all sizes in meters - float cubeSize = 1.f; - params.volumeParams.voxelSize = cubeSize/params.volumeParams.resolution[0]; - params.volumeParams.tsdfTruncDist = 0.01f; - params.icpDistThresh = 0.01f; - params.volumeParams.pose = Affine3f().translate(Vec3f(-cubeSize/2.f, - -cubeSize/2.f, - 0.05f)); - params.truncateThreshold = 2.5f; - params.bilateral_sigma_depth = 0.01f; - } - - if(sourceType == Type::DEPTH_KINECT2) - { - Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; - - initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), - camMatrix, frameSize, CV_16SC2, - undistortMap1, undistortMap2); - } - } - } - - vector depthFileList; - size_t frameIdx; - VideoCapture vc; - UMat undistortMap1, undistortMap2; - Type sourceType; -}; - #ifdef HAVE_OPENCV_VIZ const std::string vizWindowName = "cloud"; struct PauseCallbackArgs { - PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) - { } + PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) {} LargeKinfu& largeKinfu; }; @@ -290,9 +35,9 @@ struct PauseCallbackArgs void pauseCallback(const viz::MouseEvent& me, void* args); void pauseCallback(const viz::MouseEvent& me, void* args) { - if(me.type == viz::MouseEvent::Type::MouseMove || - me.type == viz::MouseEvent::Type::MouseScrollDown || - me.type == viz::MouseEvent::Type::MouseScrollUp) + if (me.type == viz::MouseEvent::Type::MouseMove || + me.type == viz::MouseEvent::Type::MouseScrollDown || + me.type == viz::MouseEvent::Type::MouseScrollUp) { PauseCallbackArgs pca = *((PauseCallbackArgs*)(args)); viz::Viz3d window(vizWindowName); @@ -304,60 +49,54 @@ void pauseCallback(const viz::MouseEvent& me, void* args) } #endif -static const char* keys = -{ +static const char* keys = { "{help h usage ? | | print this message }" "{depth | | Path to depth.txt file listing a set of depth images }" "{camera |0| Index of depth camera to be used as a depth source }" "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," - " in coarse mode points and normals are displayed }" - "{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}" + " in coarse mode points and normals are displayed }" "{idle | | Do not run KinFu, just display depth frames }" "{record | | Write depth frames to specified file list" - " (the same format as for the 'depth' key) }" + " (the same format as for the 'depth' key) }" }; static const std::string message = - "\nThis demo uses live depth input or RGB-D dataset taken from" - "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" - "\nto demonstrate KinectFusion implementation \n"; - + "\nThis demo uses live depth input or RGB-D dataset taken from" + "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" + "\nto demonstrate KinectFusion implementation" + "\nThis module uses the newer hashtable based TSDFVolume (relatively fast) for larger " + "reconstructions by default\n"; -int main(int argc, char **argv) +int main(int argc, char** argv) { bool coarse = false; - bool idle = false; - bool useHashTSDF = false; - string recordPath; + bool idle = false; + std::string recordPath; CommandLineParser parser(argc, argv, keys); parser.about(message); - if(!parser.check()) + if (!parser.check()) { parser.printMessage(); parser.printErrors(); return -1; } - if(parser.has("help")) + if (parser.has("help")) { parser.printMessage(); return 0; } - if(parser.has("coarse")) + if (parser.has("coarse")) { coarse = true; } - if(parser.has("record")) + if (parser.has("record")) { recordPath = parser.get("record"); } - if(parser.has("useHashTSDF")) - { - useHashTSDF = true; - } - if(parser.has("idle")) + if (parser.has("idle")) { idle = true; } @@ -376,19 +115,13 @@ int main(int argc, char **argv) } Ptr depthWriter; - if(!recordPath.empty()) + if (!recordPath.empty()) depthWriter = makePtr(recordPath); - Ptr params; + Ptr params; Ptr largeKinfu; - if(coarse) - params = Params::coarseParams(); - else - params = Params::defaultParams(); - - if(useHashTSDF) - params = Params::hashTSDFParams(coarse); + params = large_kinfu::Params::hashTSDFParams(coarse); // These params can be different for each depth sensor ds->updateParams(*params); @@ -396,7 +129,7 @@ int main(int argc, char **argv) // Enables OpenCL explicitly (by default can be switched-off) cv::setUseOptimized(false); - if(!idle) + if (!idle) largeKinfu = LargeKinfu::create(params); #ifdef HAVE_OPENCV_VIZ @@ -411,32 +144,34 @@ int main(int argc, char **argv) int64 prevTime = getTickCount(); - for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) + for (UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth()) { - if(depthWriter) + if (depthWriter) depthWriter->append(frame); #ifdef HAVE_OPENCV_VIZ - if(pause) + if (pause) { // doesn't happen in idle mode largeKinfu->getCloud(points, normals); - if(!points.empty() && !normals.empty()) + if (!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); - viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05, + viz::Color::gray()); window.showWidget("cloud", cloudWidget); window.showWidget("normals", cloudNormals); Vec3d volSize = largeKinfu->getParams().volumeParams.voxelSize * Vec3d(largeKinfu->getParams().volumeParams.resolution); - window.showWidget("cube", viz::WCube(Vec3d::all(0), - volSize), + window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), largeKinfu->getParams().volumeParams.pose); PauseCallbackArgs pca(*largeKinfu); window.registerMouseCallback(pauseCallback, (void*)&pca); - window.showWidget("text", viz::WText(cv::String("Move camera in this window. " - "Close the window or press Q to resume"), Point())); + window.showWidget("text", + viz::WText(cv::String("Move camera in this window. " + "Close the window or press Q to resume"), + Point())); window.spin(); window.removeWidget("text"); window.removeWidget("cloud"); @@ -451,12 +186,12 @@ int main(int argc, char **argv) { UMat cvt8; float depthFactor = params->depthFactor; - convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor); - if(!idle) + convertScaleAbs(frame, cvt8, 0.25 * 256. / depthFactor); + if (!idle) { imshow("depth", cvt8); - if(!largeKinfu->update(frame)) + if (!largeKinfu->update(frame)) { largeKinfu->reset(); std::cout << "reset" << std::endl; @@ -464,23 +199,23 @@ int main(int argc, char **argv) #ifdef HAVE_OPENCV_VIZ else { - if(coarse) + if (coarse) { largeKinfu->getCloud(points, normals); - if(!points.empty() && !normals.empty()) + if (!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); - viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray()); + viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, + /*scale*/ 0.05, viz::Color::gray()); window.showWidget("cloud", cloudWidget); window.showWidget("normals", cloudNormals); } } - //window.showWidget("worldAxes", viz::WCoordinateSystem()); + // window.showWidget("worldAxes", viz::WCoordinateSystem()); Vec3d volSize = largeKinfu->getParams().volumeParams.voxelSize * largeKinfu->getParams().volumeParams.resolution; - window.showWidget("cube", viz::WCube(Vec3d::all(0), - volSize), + window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), largeKinfu->getParams().volumeParams.pose); window.setViewerPose(largeKinfu->getPose()); window.spinOnce(1, true); @@ -496,31 +231,29 @@ int main(int argc, char **argv) } int64 newTime = getTickCount(); - putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit", - (int)(getTickFrequency()/(newTime - prevTime))), - Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); + putText(rendered, + cv::format("FPS: %2d press R to reset, P to pause, Q to quit", + (int)(getTickFrequency() / (newTime - prevTime))), + Point(0, rendered.rows - 1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255)); prevTime = newTime; imshow("render", rendered); int c = waitKey(1); switch (c) { - case 'r': - if(!idle) - largeKinfu->reset(); - break; - case 'q': - return 0; + case 'r': + if (!idle) + largeKinfu->reset(); + break; + case 'q': return 0; #ifdef HAVE_OPENCV_VIZ - case 'p': - if(!idle) - pause = true; + case 'p': + if (!idle) + pause = true; #endif - default: - break; + default: break; } } return 0; } - diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index ca8678f3c4d..c62bf332ff7 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -202,120 +202,26 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, const M }); } -Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx_(Point3f p) const +Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx_(const Point3f& p) const { return Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), cvFloor(p.z / volumeUnitSize)); } -Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume_(Vec3i volumeUnitIdx) const +Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume_(const Vec3i& volumeUnitIdx) const { return Point3f(volumeUnitIdx[0] * volumeUnitSize, volumeUnitIdx[1] * volumeUnitSize, volumeUnitIdx[2] * volumeUnitSize); } -Point3f HashTSDFVolumeCPU::voxelCoordToVolume_(Vec3i voxelIdx) const +Point3f HashTSDFVolumeCPU::voxelCoordToVolume_(const Vec3i& voxelIdx) const { return Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); } -Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord_(Point3f point) const +Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord_(const Point3f& point) const { return Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } -inline TsdfVoxel HashTSDFVolumeCPU::at_(const Vec3i& volumeIdx) const -{ - Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), - cvFloor(volumeIdx[2] / volumeUnitResolution)); - - VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); - if (it == volumeUnits.end()) - { - TsdfVoxel dummy; - dummy.tsdf = 1.f; - dummy.weight = 0; - return dummy; - } - Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - - Vec3i volUnitLocalIdx = - volumeIdx - Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, - volumeUnitIdx[2] * volumeUnitResolution); - - volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return volumeUnit->at(volUnitLocalIdx); -} - -inline TsdfVoxel HashTSDFVolumeCPU::at_(const Point3f& point) const -{ - Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); - VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); - if (it == volumeUnits.end()) - { - TsdfVoxel dummy; - dummy.tsdf = 1.f; - dummy.weight = 0; - return dummy; - } - Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - - Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); - Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); - volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return volumeUnit->at(volUnitLocalIdx); -} - -inline TsdfType HashTSDFVolumeCPU::interpolateVoxel_(const Point3f& point) const -{ - Point3f neighbourCoords[] = { Point3f(0, 0, 0), Point3f(0, 0, 1), Point3f(0, 1, 0), Point3f(0, 1, 1), - Point3f(1, 0, 0), Point3f(1, 0, 1), Point3f(1, 1, 0), Point3f(1, 1, 1) }; - - 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; - - TsdfType vx[8]; - for (int i = 0; i < 8; i++) - vx[i] = at(neighbourCoords[i] * voxelSize + point).tsdf; - - TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); - TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); - TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); - TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); - - TsdfType v0 = v00 + ty * (v01 - v00); - TsdfType v1 = v10 + ty * (v11 - v10); - - return v0 + tx * (v1 - v0); -} - -inline Point3f HashTSDFVolumeCPU::getNormalVoxel_(Point3f point) const -{ - Vec3f pointVec(point); - Vec3f normal = Vec3f(0, 0, 0); - - Vec3f pointPrev = point; - Vec3f pointNext = point; - - for (int c = 0; c < 3; c++) - { - pointPrev[c] -= voxelSize; - pointNext[c] += voxelSize; - - normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; - /* normal[c] = interpolateVoxel(Point3f(pointNext)) - interpolateVoxel(Point3f(pointPrev)); */ - - pointPrev[c] = pointVec[c]; - pointNext[c] = pointVec[c]; - } - // std::cout << normal << std::endl; - float nv = sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]); - return nv < 0.0001f ? nan3 : normal / nv; -} - struct HashRaycastInvoker : ParallelLoopBody { HashRaycastInvoker(Points& _points, Normals& _normals, const Matx44f& cameraPose, const Intr& intrinsics, @@ -425,7 +331,7 @@ struct HashRaycastInvoker : ParallelLoopBody const Intr::Reprojector reproj; }; -void HashTSDFVolumeCPU::raycast_(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, Size frameSize, +void HashTSDFVolumeCPU::raycast_(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 61ec7a90376..996c0a57f1c 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -33,7 +33,7 @@ class HashTSDFVolume : public Volume Derived* derived = static_cast(this); derived->integrate_(_depth, depthFactor, cameraPose, intrinsics, frameId); } - virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, Size frameSize, OutputArray points, + virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override { const Derived* derived = static_cast(this); @@ -55,7 +55,7 @@ class HashTSDFVolume : public Volume return static_cast(this)->getVisibleBlocks_(currFrameId, frameThreshold); } - TsdfVoxel at(const Vec3i& volumeIdx) const + virtual TsdfVoxel at(const Vec3i& volumeIdx) const { const Derived* derived = static_cast(this); return derived->at_(volumeIdx); @@ -73,27 +73,27 @@ class HashTSDFVolume : public Volume const Derived* derived = static_cast(this); return derived->interpolateVoxel_(point); } - Point3f getNormalVoxel(Point3f p) const + Point3f getNormalVoxel(const Point3f& point) const { const Derived* derived = static_cast(this); - return derived->getNormalVoxel_(p); + return derived->getNormalVoxel_(point); } //! Utility functions for coordinate transformations - Vec3i volumeToVolumeUnitIdx(Point3f point) const + Vec3i volumeToVolumeUnitIdx(const Point3f& point) const { return static_cast(this)->volumeToVolumeUnitIdx_(point); } - Point3f volumeUnitIdxToVolume(Vec3i volumeUnitIdx) const + Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const { return static_cast(this)->volumeUnitIdxToVolume_(volumeUnitIdx); } - Point3f voxelCoordToVolume(Vec3i voxelIdx) const + Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const { return static_cast(this)->voxelCoordToVolume_(voxelIdx); } - Vec3i volumeToVoxelCoord(Point3f point) const { return static_cast(this)->volumeToVoxelCoord_(point); } + Vec3i volumeToVoxelCoord(const Point3f& point) const { return static_cast(this)->volumeToVoxelCoord_(point); } public: int maxWeight; @@ -153,7 +153,7 @@ class HashTSDFVolumeCPU : public HashTSDFVolume void integrate_(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0); - void raycast_(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, Size frameSize, OutputArray points, + void raycast_(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const; void fetchNormals_(InputArray points, OutputArray _normals) const; @@ -164,21 +164,108 @@ class HashTSDFVolumeCPU : public HashTSDFVolume int getVisibleBlocks_(int currFrameId, int frameThreshold) const; //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) - TsdfVoxel at_(const Vec3i& volumeIdx) const; + TsdfVoxel at_(const Vec3i& volumeIdx) const + { + Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), + cvFloor(volumeIdx[2] / volumeUnitResolution)); + + VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); + if (it == volumeUnits.end()) + { + TsdfVoxel dummy; + dummy.tsdf = 1.f; + dummy.weight = 0; + return dummy; + } + Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + + Vec3i volUnitLocalIdx = + volumeIdx - Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, + volumeUnitIdx[2] * volumeUnitResolution); + + volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + return volumeUnit->at(volUnitLocalIdx); + } //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) - TsdfVoxel at_(const Point3f& point) const; + TsdfVoxel at_(const Point3f& point) const + { + Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); + VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); + if (it == volumeUnits.end()) + { + TsdfVoxel dummy; + dummy.tsdf = 1.f; + dummy.weight = 0; + return dummy; + } + Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + + Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); + Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); + volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + return volumeUnit->at(volUnitLocalIdx); + } + + TsdfType interpolateVoxel_(const Point3f& point) const + { + Point3f neighbourCoords[] = { Point3f(0, 0, 0), Point3f(0, 0, 1), Point3f(0, 1, 0), Point3f(0, 1, 1), + Point3f(1, 0, 0), Point3f(1, 0, 1), Point3f(1, 1, 0), Point3f(1, 1, 1) }; + + int ix = cvFloor(point.x); + int iy = cvFloor(point.y); + int iz = cvFloor(point.z); - inline TsdfType interpolateVoxel_(const Point3f& point) const; - Point3f getNormalVoxel_(Point3f p) const; + float tx = point.x - ix; + float ty = point.y - iy; + float tz = point.z - iz; + + TsdfType vx[8]; + for (int i = 0; i < 8; i++) + vx[i] = at(neighbourCoords[i] * voxelSize + point).tsdf; + + TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); + + TsdfType v0 = v00 + ty * (v01 - v00); + TsdfType v1 = v10 + ty * (v11 - v10); + + return v0 + tx * (v1 - v0); + } + + Point3f getNormalVoxel_(const Point3f& point) const + { + Vec3f pointVec(point); + Vec3f normal = Vec3f(0, 0, 0); + + Vec3f pointPrev = point; + Vec3f pointNext = point; + + for (int c = 0; c < 3; c++) + { + pointPrev[c] -= voxelSize; + pointNext[c] += voxelSize; + + normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; + /* normal[c] = interpolateVoxel(Point3f(pointNext)) - interpolateVoxel(Point3f(pointPrev)); */ + + pointPrev[c] = pointVec[c]; + pointNext[c] = pointVec[c]; + } + // std::cout << normal << std::endl; + float nv = sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]); + return nv < 0.0001f ? nan3 : normal / nv; + } //! Utility functions for coordinate transformations - Vec3i volumeToVolumeUnitIdx_(Point3f point) const; - Point3f volumeUnitIdxToVolume_(Vec3i volumeUnitIdx) const; + Vec3i volumeToVolumeUnitIdx_(const Point3f& point) const; + Point3f volumeUnitIdxToVolume_(const Vec3i& volumeUnitIdx) const; - Point3f voxelCoordToVolume_(Vec3i voxelIdx) const; - Vec3i volumeToVoxelCoord_(Point3f point) const; + Point3f voxelCoordToVolume_(const Vec3i& voxelIdx) const; + Vec3i volumeToVoxelCoord_(const Point3f& point) const; public: //! Hashtable of individual smaller volume units diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index a895ea7a48e..9b09db1b1f3 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -4,24 +4,23 @@ #include "pose_graph.hpp" +#include #include #include #include #if defined(CERES_FOUND) -#include "ceres/ceres.h" +#include #endif namespace cv { namespace kinfu { -void PoseGraph::addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } - bool PoseGraph::isValid() const { - int numNodes = int(nodes.size()); - int numEdges = int(edges.size()); + int numNodes = getNumNodes(); + int numEdges = getNumEdges(); if (numNodes <= 0 || numEdges <= 0) return false; @@ -82,7 +81,7 @@ bool PoseGraph::isValid() const return isGraphConnected && !invalidEdgeNode; } -#if defined(CERES_FOUND) +#if defined(CERES_FOUND) && defined(HAVE_EIGEN) void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem) { int numEdges = poseGraph.getNumEdges(); @@ -103,13 +102,14 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& const PoseGraphEdge& currEdge = poseGraph.edges.at(currEdgeNum); int sourceNodeId = currEdge.getSourceNodeId(); int targetNodeId = currEdge.getTargetNodeId(); - Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).pose; - Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).pose; + Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).se3Pose; + Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).se3Pose; const Matx66f& informationMatrix = currEdge.information; - ceres::CostFunction* costFunction = - Pose3dErrorFunctor::create(currEdge.transformation, informationMatrix); + ceres::CostFunction* costFunction = Pose3dErrorFunctor::create( + Pose3d(currEdge.transformation.rotation(), currEdge.transformation.translation()), + informationMatrix); problem.AddResidualBlock(costFunction, lossFunction, sourcePose.t.data(), sourcePose.r.coeffs().data(), targetPose.t.data(), @@ -123,8 +123,8 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& PoseGraphNode& currNode = poseGraph.nodes.at(currNodeId); if (currNode.isPoseFixed()) { - problem.SetParameterBlockConstant(currNode.pose.t.data()); - problem.SetParameterBlockConstant(currNode.pose.r.coeffs().data()); + problem.SetParameterBlockConstant(currNode.se3Pose.t.data()); + problem.SetParameterBlockConstant(currNode.se3Pose.r.coeffs().data()); } } } @@ -146,7 +146,7 @@ void Optimizer::optimizeCeres(PoseGraph& poseGraph) std::cout << "Optimizing PoseGraph with " << numNodes << " nodes and " << numEdges << " edges" << std::endl; -#if defined(CERES_FOUND) +#if defined(CERES_FOUND) && defined(HAVE_EIGEN) ceres::Problem problem; createOptimizationProblem(poseGraph, problem); @@ -161,7 +161,7 @@ void Optimizer::optimizeCeres(PoseGraph& poseGraph) std::cout << "Is solution usable: " << summary.IsSolutionUsable() << std::endl; #else - CV_Error(Error::StsNotImplemented, "Ceres required for Pose Graph optimization"); + CV_Error(Error::StsNotImplemented, "Ceres and Eigen required for Pose Graph optimization"); #endif } diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 91c06f1519e..9b8ad8dd20c 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -99,31 +99,50 @@ struct PoseGraphNode { public: explicit PoseGraphNode(int _nodeId, const Affine3f& _pose) - : nodeId(_nodeId), isFixed(false), pose(_pose.rotation(), _pose.translation()) + : nodeId(_nodeId), isFixed(false), pose(_pose) { +#if defined(HAVE_EIGEN) + se3Pose = Pose3d(_pose.rotation(), _pose.translation()); +#endif } virtual ~PoseGraphNode() = default; int getId() const { return nodeId; } - inline Affine3d getPose() const + inline Affine3f getPose() const + { + return pose; + } + void setPose(const Affine3f& _pose) { - const Eigen::Matrix3d& rotation = pose.r.toRotationMatrix(); - const Eigen::Vector3d& translation = pose.t; + pose = _pose; +#if defined(HAVE_EIGEN) + se3Pose = Pose3d(pose.rotation(), pose.translation()); +#endif + } +#if defined(HAVE_EIGEN) + void setPose(const Pose3d& _pose) + { + se3Pose = _pose; + const Eigen::Matrix3d& rotation = se3Pose.r.toRotationMatrix(); + const Eigen::Vector3d& translation = se3Pose.t; Matx33d rot; Vec3d trans; eigen2cv(rotation, rot); eigen2cv(translation, trans); Affine3d poseMatrix(rot, trans); - return poseMatrix; + pose = poseMatrix; } - void setPose(const Pose3d& _pose) { pose = _pose; } +#endif void setFixed(bool val = true) { isFixed = val; } bool isPoseFixed() const { return isFixed; } public: int nodeId; bool isFixed; - Pose3d pose; + Affine3f pose; +#if defined(HAVE_EIGEN) + Pose3d se3Pose; +#endif }; /*! \class PoseGraphEdge @@ -138,7 +157,7 @@ struct PoseGraphEdge const Matx66f& _information = Matx66f::eye()) : sourceNodeId(_sourceNodeId), targetNodeId(_targetNodeId), - transformation(_transformation.rotation(), _transformation.translation()), + transformation(_transformation), information(_information) { } @@ -158,45 +177,45 @@ struct PoseGraphEdge public: int sourceNodeId; int targetNodeId; - Pose3d transformation; + Affine3f transformation; Matx66f information; }; //! @brief Reference: A tutorial on SE(3) transformation parameterizations and on-manifold //! optimization Jose Luis Blanco Compactly represents the jacobian of the SE3 generator // clang-format off -static const std::array generatorJacobian = { - // alpha - Matx44f(0, 0, 0, 0, - 0, 0, -1, 0, - 0, 1, 0, 0, - 0, 0, 0, 0), - // beta - Matx44f( 0, 0, 1, 0, - 0, 0, 0, 0, - -1, 0, 0, 0, - 0, 0, 0, 0), - // gamma - Matx44f(0, -1, 0, 0, - 1, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0), - // x - Matx44f(0, 0, 0, 1, - 0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0), - // y - Matx44f(0, 0, 0, 0, - 0, 0, 0, 1, - 0, 0, 0, 0, - 0, 0, 0, 0), - // z - Matx44f(0, 0, 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 1, - 0, 0, 0, 0) -}; +/* static const std::array generatorJacobian = { */ +/* // alpha */ +/* Matx44f(0, 0, 0, 0, */ +/* 0, 0, -1, 0, */ +/* 0, 1, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // beta */ +/* Matx44f( 0, 0, 1, 0, */ +/* 0, 0, 0, 0, */ +/* -1, 0, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // gamma */ +/* Matx44f(0, -1, 0, 0, */ +/* 1, 0, 0, 0, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // x */ +/* Matx44f(0, 0, 0, 1, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // y */ +/* Matx44f(0, 0, 0, 0, */ +/* 0, 0, 0, 1, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 0), */ +/* // z */ +/* Matx44f(0, 0, 0, 0, */ +/* 0, 0, 0, 0, */ +/* 0, 0, 0, 1, */ +/* 0, 0, 0, 0) */ +/* }; */ // clang-format on class PoseGraph @@ -213,7 +232,7 @@ class PoseGraph PoseGraph& operator=(const PoseGraph& _poseGraph) = default; void addNode(const PoseGraphNode& node) { nodes.push_back(node); } - void addEdge(const PoseGraphEdge& edge); + void addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } bool nodeExists(int nodeId) const { @@ -224,8 +243,8 @@ class PoseGraph bool isValid() const; - int getNumNodes() const { return nodes.size(); } - int getNumEdges() const { return edges.size(); } + int getNumNodes() const { return int(nodes.size()); } + int getNumEdges() const { return int(edges.size()); } public: NodeVector nodes; diff --git a/modules/rgbd/src/submap.cpp b/modules/rgbd/src/submap.cpp deleted file mode 100644 index 83c2fd654d4..00000000000 --- a/modules/rgbd/src/submap.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// 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 "submap.hpp" - -#include "hash_tsdf.hpp" -#include "kinfu_frame.hpp" -#include "precomp.hpp" - -namespace cv -{ -namespace kinfu -{ - -/* void SubmapManager::addCameraCameraConstraint(int prevId, int currId, const Affine3f& prevPose, const Affine3f& currPose) - */ -/* { */ -/* //! 1. Add new posegraph node */ -/* //! 2. Add new posegraph constraint */ - -/* //! TODO: Attempt registration between submaps */ -/* Affine3f Tprev2curr = currPose.inv() * currPose; */ -/* //! Constant information matrix for all odometry constraints */ -/* Matx66f information = Matx66f::eye() * 1000; */ - -/* //! Inserts only if the element does not exist already */ -/* poseGraph.addNode(prevId, cv::makePtr(prevPose)); */ -/* poseGraph.addNode(currId, cv::makePtr(currPose)); */ - -/* poseGraph.addEdge(cv::makePtr(prevId, currId, Tprev2curr, information)); */ -/* } */ - - -} // namespace kinfu -} // namespace cv diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index 284ec7db4a3..fffb891f4d9 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -53,8 +53,8 @@ class Submap OutputArray points, OutputArray normals); virtual void updatePyrPointsNormals(const int pyramidLevels); - virtual size_t getTotalAllocatedBlocks() const { return volume.getTotalVolumeUnits(); }; - virtual size_t getVisibleBlocks(int currFrameId) const + virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); }; + virtual int getVisibleBlocks(int currFrameId) const { return volume.getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD); } @@ -257,11 +257,11 @@ int SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, static constexpr int MAX_TRACKING_ATTEMPTS = 25; //! thresh = HUBER_THRESH - auto huberWeight = [](float residual, float thresh = 0.1f) -> float { + auto huberWeight = [](double residual, double thresh = 0.1) -> double { float rAbs = abs(residual); if (rAbs < thresh) return 1.0; - float numerator = sqrt(2 * thresh * rAbs - thresh * thresh); + double numerator = sqrt(2 * thresh * rAbs - thresh * thresh); return numerator / rAbs; }; @@ -277,17 +277,11 @@ int SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, fromSubmapData.trackingAttempts++; fromSubmapData.constraints.push_back(candidateConstraint); - /* std::cout << "Candidate constraint from: " << fromSubmap->id << " to " << toSubmap->id << "\n" */ - /* << candidateConstraint.matrix << "\n"; */ - /* std::cout << "Constraint observations size: " << fromSubmapData.constraints.size() << "\n"; */ - - std::vector weights(fromSubmapData.constraints.size() + 1, 1.0f); + std::vector weights(fromSubmapData.constraints.size() + 1, 1.0f); Affine3f prevConstraint = fromSubmap->getConstraint(toSubmap->id).estimatedPose; int prevWeight = fromSubmap->getConstraint(toSubmap->id).weight; - /* std::cout << "Previous constraint pose: \n" << prevConstraint.matrix << "\n previous Weight: " << prevWeight << "\n"; */ - // Iterative reweighted least squares with huber threshold to find the inliers in the past observations Vec6f meanConstraint; float sumWeight = 0.0f; @@ -305,14 +299,13 @@ int SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec); meanConstraint += weights.back() * prevWeight * constraintVec; sumWeight += prevWeight; - /* std::cout << "meanConstraint before average: " << meanConstraint << " sumWeight: " << sumWeight << "\n"; */ meanConstraint /= float(sumWeight); - float residual = 0.0f; - float diff = 0; + double residual = 0.0; + double diff = 0.0; for (int j = 0; j < int(weights.size()); j++) { - float w; + int w; if (j == int(weights.size() - 1)) { cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec); @@ -322,16 +315,12 @@ int SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, { Affine3f currObservation = fromSubmapData.constraints[j]; cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec); - w = 1.0f; + w = 1; } - /* std::cout << "meanConstraint: " << meanConstraint << " ConstraintVec: " << constraintVec << "\n"; */ cv::Vec6f residualVec = (constraintVec - meanConstraint); - /* std::cout << "Residual Vec: " << residualVec << "\n"; */ residual = norm(residualVec); double newWeight = huberWeight(residual); - /* std::cout << "iteration: " << i << " residual: " << residual << " " << j */ - /* << "th weight before update: " << weights[j] << " after update: " << newWeight << "\n"; */ diff += w * abs(newWeight - weights[j]); weights[j] = newWeight; } diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 1efa4b070c5..49ee6c74c3d 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -922,7 +922,7 @@ struct RaycastInvoker : ParallelLoopBody }; -void TSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrinsics, Size frameSize, +void TSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); @@ -1185,7 +1185,7 @@ void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, } -void TSDFVolumeGPU::raycast(const Matx44f& cameraPose, const Intr& intrinsics, Size frameSize, +void TSDFVolumeGPU::raycast(const Matx44f& cameraPose, const Intr& intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index ffafc432230..2c015ed4ca6 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -57,7 +57,7 @@ class TSDFVolumeCPU : public TSDFVolume virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; - virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, Size frameSize, + virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; virtual void fetchNormals(InputArray points, OutputArray _normals) const override; @@ -90,7 +90,7 @@ class TSDFVolumeGPU : public TSDFVolume virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; - virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, Size frameSize, + virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; From 60523b61f6aa4132d9bf845a0b3c0219fdf9c4f3 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Fri, 28 Aug 2020 10:33:06 -0400 Subject: [PATCH 31/36] - Add Python bindings for volume struct - Remove makeVolume(const VolumeParams&) Python binding due to compilation issues - Minor changes according to comments --- modules/rgbd/include/opencv2/rgbd/volume.hpp | 28 +++-- .../opencv2/rgbd => samples}/io_utils.hpp | 0 modules/rgbd/samples/kinfu_demo.cpp | 3 +- modules/rgbd/samples/large_kinfu_demo.cpp | 9 +- modules/rgbd/src/fast_icp.cpp | 3 - modules/rgbd/src/hash_tsdf.cpp | 94 ++++++++++++++ modules/rgbd/src/hash_tsdf.hpp | 119 +++--------------- modules/rgbd/src/large_kinfu.cpp | 4 +- modules/rgbd/src/pose_graph.hpp | 3 +- modules/rgbd/src/submap.hpp | 14 +-- modules/rgbd/src/volume.cpp | 25 ++-- 11 files changed, 166 insertions(+), 136 deletions(-) rename modules/rgbd/{include/opencv2/rgbd => samples}/io_utils.hpp (100%) diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index ef3dcde5a15..33f63b1fbfb 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -54,57 +54,61 @@ struct CV_EXPORTS_W VolumeParams /** @brief Type of Volume Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units) */ - VolumeType type; + CV_PROP_RW VolumeType type; /** @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 */ - Vec3i resolution; + CV_PROP_RW Vec3i resolution; /** @brief Resolution of volumeUnit in voxel space Number of voxels in each dimension for volumeUnit Applicable only for hashTSDF. */ - int unitResolution = 0; + CV_PROP_RW int unitResolution = {0}; /** @brief Initial pose of the volume in meters */ Affine3f pose; /** @brief Length of voxels in meters */ - float voxelSize; + CV_PROP_RW float voxelSize; /** @brief TSDF truncation distance Distances greater than value from surface will be truncated to 1.0 */ - float tsdfTruncDist; + CV_PROP_RW float tsdfTruncDist; /** @brief Max number of frames to integrate per voxel - Each voxel stops integration after the maxWeight is crossed + Represents the max number of frames over which a running average + of the TSDF is calculated for a voxel */ - int maxWeight; + CV_PROP_RW int maxWeight; /** @brief Threshold for depth truncation in meters Truncates the depth greater than threshold to 0 */ - float depthTruncThreshold; + CV_PROP_RW float depthTruncThreshold; /** @brief Length of single raycast step Describes the percentage of voxel length that is skipped per march */ - float raycastStepFactor; + CV_PROP_RW float raycastStepFactor; /** @brief Default set of parameters that provide higher quality reconstruction at the cost of slow performance. */ - static Ptr defaultParams(VolumeType _volumeType); + CV_WRAP static Ptr defaultParams(VolumeType _volumeType); /** @brief Coarse set of parameters that provides relatively higher performance at the cost of reconstrution quality. */ - static Ptr coarseParams(VolumeType _volumeType); + CV_WRAP static Ptr coarseParams(VolumeType _volumeType); }; -CV_EXPORTS_W Ptr makeVolume(const VolumeParams& _volumeParams); + +Ptr makeVolume(const VolumeParams& _volumeParams); CV_EXPORTS_W Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, Vec3i _resolution); diff --git a/modules/rgbd/include/opencv2/rgbd/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp similarity index 100% rename from modules/rgbd/include/opencv2/rgbd/io_utils.hpp rename to modules/rgbd/samples/io_utils.hpp diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index 93eb264d2fd..e264ba38bbe 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -10,7 +10,8 @@ #include #include #include -#include + +#include "io_utils.hpp" using namespace cv; using namespace cv::kinfu; diff --git a/modules/rgbd/samples/large_kinfu_demo.cpp b/modules/rgbd/samples/large_kinfu_demo.cpp index 793def2f58a..40d14ca8cae 100644 --- a/modules/rgbd/samples/large_kinfu_demo.cpp +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -10,9 +10,10 @@ #include #include #include -#include #include +#include "io_utils.hpp" + using namespace cv; using namespace cv::kinfu; using namespace cv::large_kinfu; @@ -55,7 +56,7 @@ static const char* keys = { "{camera |0| Index of depth camera to be used as a depth source }" "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," " in coarse mode points and normals are displayed }" - "{idle | | Do not run KinFu, just display depth frames }" + "{idle | | Do not run LargeKinfu, just display depth frames }" "{record | | Write depth frames to specified file list" " (the same format as for the 'depth' key) }" }; @@ -63,7 +64,7 @@ static const char* keys = { static const std::string message = "\nThis demo uses live depth input or RGB-D dataset taken from" "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" - "\nto demonstrate KinectFusion implementation" + "\nto demonstrate Submap based large environment reconstruction" "\nThis module uses the newer hashtable based TSDFVolume (relatively fast) for larger " "reconstructions by default\n"; @@ -126,7 +127,7 @@ int main(int argc, char** argv) // These params can be different for each depth sensor ds->updateParams(*params); - // Enables OpenCL explicitly (by default can be switched-off) + // Disabled until there is no OpenCL accelerated HashTSDF is available cv::setUseOptimized(false); if (!idle) diff --git a/modules/rgbd/src/fast_icp.cpp b/modules/rgbd/src/fast_icp.cpp index d410ad15199..2ead1a8f527 100644 --- a/modules/rgbd/src/fast_icp.cpp +++ b/modules/rgbd/src/fast_icp.cpp @@ -186,7 +186,6 @@ struct GetAbInvoker : ParallelLoopBody virtual void operator ()(const Range& range) const override { - int localNumInliers = 0; #if USE_INTRINSICS CV_Assert(ptype::channels == 4); @@ -299,7 +298,6 @@ struct GetAbInvoker : ParallelLoopBody continue; // build point-wise vector ab = [ A | b ] - localNumInliers = localNumInliers + 1; v_float32x4 VxNv = crossProduct(newP, oldN); Point3f VxN; VxN.x = VxNv.get0(); @@ -450,7 +448,6 @@ struct GetAbInvoker : ParallelLoopBody //try to optimize Point3f VxN = newP.cross(oldN); float ab[7] = {VxN.x, VxN.y, VxN.z, oldN.x, oldN.y, oldN.z, oldN.dot(-diff)}; - localNumInliers++; // build point-wise upper-triangle matrix [ab^T * ab] w/o last row // which is [A^T*A | A^T*b] // and gather sum diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index c62bf332ff7..9af7ddb05bd 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -202,6 +202,100 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, const M }); } +TsdfVoxel HashTSDFVolumeCPU::at_(const Vec3i& volumeIdx) const +{ + Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), + cvFloor(volumeIdx[2] / volumeUnitResolution)); + + VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); + if (it == volumeUnits.end()) + { + TsdfVoxel dummy; + dummy.tsdf = 1.f; + dummy.weight = 0; + return dummy; + } + Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + + Vec3i volUnitLocalIdx = + volumeIdx - Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, + volumeUnitIdx[2] * volumeUnitResolution); + + volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + return volumeUnit->at(volUnitLocalIdx); +} + +Point3f HashTSDFVolumeCPU::getNormalVoxel_(const Point3f &point) const +{ + Vec3f pointVec(point); + Vec3f normal = Vec3f(0, 0, 0); + + Vec3f pointPrev = point; + Vec3f pointNext = point; + + for (int c = 0; c < 3; c++) + { + pointPrev[c] -= voxelSize; + pointNext[c] += voxelSize; + + normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; + /* normal[c] = interpolateVoxel(Point3f(pointNext)) - interpolateVoxel(Point3f(pointPrev)); */ + + pointPrev[c] = pointVec[c]; + pointNext[c] = pointVec[c]; + } + // std::cout << normal << std::endl; + float nv = sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]); + return nv < 0.0001f ? nan3 : normal / nv; +} + +TsdfVoxel HashTSDFVolumeCPU::at_(const Point3f& point) const +{ + Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); + VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); + if (it == volumeUnits.end()) + { + TsdfVoxel dummy; + dummy.tsdf = 1.f; + dummy.weight = 0; + return dummy; + } + Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + + Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); + Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); + volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + return volumeUnit->at(volUnitLocalIdx); +} + +TsdfType HashTSDFVolumeCPU::interpolateVoxel_(const Point3f& point) const +{ + Point3f neighbourCoords[] = { Point3f(0, 0, 0), Point3f(0, 0, 1), Point3f(0, 1, 0), Point3f(0, 1, 1), + Point3f(1, 0, 0), Point3f(1, 0, 1), Point3f(1, 1, 0), Point3f(1, 1, 1) }; + + 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; + + TsdfType vx[8]; + for (int i = 0; i < 8; i++) + vx[i] = at(neighbourCoords[i] * voxelSize + point).tsdf; + + TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); + TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); + TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); + TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); + + TsdfType v0 = v00 + ty * (v01 - v00); + TsdfType v1 = v10 + ty * (v11 - v10); + + return v0 + tx * (v1 - v0); +} + Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx_(const Point3f& p) const { return Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), cvFloor(p.z / volumeUnitSize)); diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index 996c0a57f1c..01130d2a33f 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -49,20 +49,20 @@ class HashTSDFVolume : public Volume const Derived* derived = static_cast(this); derived->fetchPointsNormals_(points, normals); } - size_t getTotalVolumeUnits() const { return static_cast(this)->getTotalVolumeUnits_(); } - int getVisibleBlocks(int currFrameId, int frameThreshold) const + inline size_t getTotalVolumeUnits() const { return static_cast(this)->getTotalVolumeUnits_(); } + inline int getVisibleBlocks(int currFrameId, int frameThreshold) const { return static_cast(this)->getVisibleBlocks_(currFrameId, frameThreshold); } - virtual TsdfVoxel at(const Vec3i& volumeIdx) const + inline TsdfVoxel at(const Vec3i& volumeIdx) const { const Derived* derived = static_cast(this); return derived->at_(volumeIdx); } //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) - virtual TsdfVoxel at(const Point3f& point) const + inline TsdfVoxel at(const Point3f& point) const { const Derived* derived = static_cast(this); return derived->at_(point); @@ -73,27 +73,27 @@ class HashTSDFVolume : public Volume const Derived* derived = static_cast(this); return derived->interpolateVoxel_(point); } - Point3f getNormalVoxel(const Point3f& point) const + inline Point3f getNormalVoxel(const Point3f& point) const { const Derived* derived = static_cast(this); return derived->getNormalVoxel_(point); } //! Utility functions for coordinate transformations - Vec3i volumeToVolumeUnitIdx(const Point3f& point) const + inline Vec3i volumeToVolumeUnitIdx(const Point3f& point) const { return static_cast(this)->volumeToVolumeUnitIdx_(point); } - Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const + inline Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const { return static_cast(this)->volumeUnitIdxToVolume_(volumeUnitIdx); } - Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const + inline Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const { return static_cast(this)->voxelCoordToVolume_(voxelIdx); } - Vec3i volumeToVoxelCoord(const Point3f& point) const { return static_cast(this)->volumeToVoxelCoord_(point); } + inline Vec3i volumeToVoxelCoord(const Point3f& point) const { return static_cast(this)->volumeToVoxelCoord_(point); } public: int maxWeight; @@ -164,101 +164,15 @@ class HashTSDFVolumeCPU : public HashTSDFVolume int getVisibleBlocks_(int currFrameId, int frameThreshold) const; //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) - TsdfVoxel at_(const Vec3i& volumeIdx) const - { - Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), - cvFloor(volumeIdx[2] / volumeUnitResolution)); - - VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); - if (it == volumeUnits.end()) - { - TsdfVoxel dummy; - dummy.tsdf = 1.f; - dummy.weight = 0; - return dummy; - } - Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - - Vec3i volUnitLocalIdx = - volumeIdx - Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, - volumeUnitIdx[2] * volumeUnitResolution); - - volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return volumeUnit->at(volUnitLocalIdx); - } + TsdfVoxel at_(const Vec3i& volumeIdx) const; //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) - TsdfVoxel at_(const Point3f& point) const - { - Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); - VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); - if (it == volumeUnits.end()) - { - TsdfVoxel dummy; - dummy.tsdf = 1.f; - dummy.weight = 0; - return dummy; - } - Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - - Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); - Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); - volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); - return volumeUnit->at(volUnitLocalIdx); - } - - TsdfType interpolateVoxel_(const Point3f& point) const - { - Point3f neighbourCoords[] = { Point3f(0, 0, 0), Point3f(0, 0, 1), Point3f(0, 1, 0), Point3f(0, 1, 1), - Point3f(1, 0, 0), Point3f(1, 0, 1), Point3f(1, 1, 0), Point3f(1, 1, 1) }; - - 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; - - TsdfType vx[8]; - for (int i = 0; i < 8; i++) - vx[i] = at(neighbourCoords[i] * voxelSize + point).tsdf; + TsdfVoxel at_(const Point3f& point) const; - TsdfType v00 = vx[0] + tz * (vx[1] - vx[0]); - TsdfType v01 = vx[2] + tz * (vx[3] - vx[2]); - TsdfType v10 = vx[4] + tz * (vx[5] - vx[4]); - TsdfType v11 = vx[6] + tz * (vx[7] - vx[6]); + TsdfType interpolateVoxel_(const Point3f& point) const; - TsdfType v0 = v00 + ty * (v01 - v00); - TsdfType v1 = v10 + ty * (v11 - v10); - - return v0 + tx * (v1 - v0); - } - - Point3f getNormalVoxel_(const Point3f& point) const - { - Vec3f pointVec(point); - Vec3f normal = Vec3f(0, 0, 0); - - Vec3f pointPrev = point; - Vec3f pointNext = point; - - for (int c = 0; c < 3; c++) - { - pointPrev[c] -= voxelSize; - pointNext[c] += voxelSize; - - normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; - /* normal[c] = interpolateVoxel(Point3f(pointNext)) - interpolateVoxel(Point3f(pointPrev)); */ - - pointPrev[c] = pointVec[c]; - pointNext[c] = pointVec[c]; - } - // std::cout << normal << std::endl; - float nv = sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]); - return nv < 0.0001f ? nan3 : normal / nv; - } + Point3f getNormalVoxel_(const Point3f& point) const; //! Utility functions for coordinate transformations Vec3i volumeToVolumeUnitIdx_(const Point3f& point) const; @@ -271,6 +185,13 @@ class HashTSDFVolumeCPU : public HashTSDFVolume //! Hashtable of individual smaller volume units VolumeUnitMap volumeUnits; }; + +template +Ptr> makeHashTSDFVolume(const VolumeParams& _volumeParams) +{ + return makePtr(_volumeParams); +} + template Ptr> makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16) diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index f3cd2634302..671408477f6 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -28,8 +28,8 @@ Ptr Params::defaultParams() float fx, fy, cx, cy; fx = fy = 525.f; - cx = p.frameSize.width / 2 - 0.5f; - cy = p.frameSize.height / 2 - 0.5f; + cx = p.frameSize.width / 2.0f - 0.5f; + cy = p.frameSize.height / 2.0f - 0.5f; p.intr = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); // 5000 for the 16-bit PNG files diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 48f1048f8b1..9b8ad8dd20c 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -93,6 +93,7 @@ struct Pose3d r.normalize(); } }; +#endif struct PoseGraphNode { @@ -249,7 +250,7 @@ class PoseGraph NodeVector nodes; EdgeVector edges; }; -#endif + namespace Optimizer { void optimizeCeres(PoseGraph& poseGraph); diff --git a/modules/rgbd/src/submap.hpp b/modules/rgbd/src/submap.hpp index fffb891f4d9..983ee610c75 100644 --- a/modules/rgbd/src/submap.hpp +++ b/modules/rgbd/src/submap.hpp @@ -257,11 +257,11 @@ int SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, static constexpr int MAX_TRACKING_ATTEMPTS = 25; //! thresh = HUBER_THRESH - auto huberWeight = [](double residual, double thresh = 0.1) -> double { + auto huberWeight = [](float residual, float thresh = 0.1f) -> float { float rAbs = abs(residual); if (rAbs < thresh) return 1.0; - double numerator = sqrt(2 * thresh * rAbs - thresh * thresh); + float numerator = sqrt(2 * thresh * rAbs - thresh * thresh); return numerator / rAbs; }; @@ -277,7 +277,7 @@ int SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, fromSubmapData.trackingAttempts++; fromSubmapData.constraints.push_back(candidateConstraint); - std::vector weights(fromSubmapData.constraints.size() + 1, 1.0f); + std::vector weights(fromSubmapData.constraints.size() + 1, 1.0f); Affine3f prevConstraint = fromSubmap->getConstraint(toSubmap->id).estimatedPose; int prevWeight = fromSubmap->getConstraint(toSubmap->id).weight; @@ -301,8 +301,8 @@ int SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, sumWeight += prevWeight; meanConstraint /= float(sumWeight); - double residual = 0.0; - double diff = 0.0; + float residual = 0.0f; + float diff = 0.0f; for (int j = 0; j < int(weights.size()); j++) { int w; @@ -319,8 +319,8 @@ int SubmapManager::estimateConstraint(int fromSubmapId, int toSubmapId, } cv::Vec6f residualVec = (constraintVec - meanConstraint); - residual = norm(residualVec); - double newWeight = huberWeight(residual); + residual = float(norm(residualVec)); + float newWeight = huberWeight(residual); diff += w * abs(newWeight - weights[j]); weights[j] = newWeight; } diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index 8f0cc5257b5..a961e760848 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -2,23 +2,24 @@ // 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 #include "hash_tsdf.hpp" +#include "opencv2/core/base.hpp" +#include "precomp.hpp" #include "tsdf.hpp" namespace cv { namespace kinfu { - Ptr VolumeParams::defaultParams(VolumeType _volumeType) { VolumeParams params; params.type = _volumeType; params.maxWeight = 64; params.raycastStepFactor = 0.25f; + params.unitResolution = 0; // unitResolution not used for TSDF float volumeSize = 3.0f; params.pose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)); switch (params.type) @@ -32,9 +33,10 @@ Ptr VolumeParams::defaultParams(VolumeType _volumeType) params.unitResolution = 16; params.voxelSize = volumeSize / 512.f; params.depthTruncThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); - default: - //! TODO: Should throw some exception or error break; + default: + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); + return nullptr; } params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters @@ -53,8 +55,12 @@ Ptr VolumeParams::coarseParams(VolumeType _volumeType) params->resolution = Vec3i::all(128); params->voxelSize = volumeSize / 128.f; break; - case VolumeType::HASHTSDF: params->voxelSize = volumeSize / 128.f; break; - default: break; + case VolumeType::HASHTSDF: + params->voxelSize = volumeSize / 128.f; + break; + default: + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); + return nullptr; } params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters return params; @@ -66,7 +72,9 @@ Ptr makeVolume(const VolumeParams& _volumeParams) { case VolumeType::TSDF: return kinfu::makeTSDFVolume(_volumeParams); break; case VolumeType::HASHTSDF: return makePtr(_volumeParams); break; - default: return nullptr; + default: + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); + return nullptr; } } @@ -86,7 +94,10 @@ Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, _voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); } else + { + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); return nullptr; + } } } // namespace kinfu From aea037ad2d808a33487c4ca6a095896feac3d003 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Mon, 31 Aug 2020 19:54:36 -0400 Subject: [PATCH 32/36] - Remove dynafu::Params() since it is identical to kinfu::Params() - Use common functions for dynafu_demo - Suppress "unreachable code" in volume.cpp --- modules/rgbd/include/opencv2/rgbd/dynafu.hpp | 100 +-------- modules/rgbd/samples/dynafu_demo.cpp | 204 +------------------ modules/rgbd/src/dynafu.cpp | 70 ------- modules/rgbd/src/fast_icp.cpp | 1 - modules/rgbd/src/volume.cpp | 76 +++---- 5 files changed, 43 insertions(+), 408 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/dynafu.hpp b/modules/rgbd/include/opencv2/rgbd/dynafu.hpp index d057ebe76c6..fae69c48eef 100644 --- a/modules/rgbd/include/opencv2/rgbd/dynafu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/dynafu.hpp @@ -10,103 +10,11 @@ #include "opencv2/core.hpp" #include "opencv2/core/affine.hpp" +#include "kinfu.hpp" + namespace cv { namespace dynafu { -struct CV_EXPORTS_W Params -{ - /** @brief Default parameters - A set of parameters which provides better model quality, can be very slow. - */ - CV_WRAP static Ptr defaultParams(); - - /** @brief Coarse parameters - A set of parameters which provides better speed, can fail to match frames - in case of rapid sensor motion. - */ - CV_WRAP static Ptr coarseParams(); - - /** @brief frame size in pixels */ - CV_PROP_RW Size frameSize; - - /** @brief camera intrinsics */ - CV_PROP Matx33f intr; - - /** @brief pre-scale per 1 meter for input values - - Typical values are: - * 5000 per 1 meter for the 16-bit PNG files of TUM database - * 1000 per 1 meter for Kinect 2 device - * 1 per 1 meter for the 32-bit float images in the ROS bag files - */ - CV_PROP_RW float depthFactor; - - /** @brief Depth sigma in meters for bilateral smooth */ - CV_PROP_RW float bilateral_sigma_depth; - /** @brief Spatial sigma in pixels for bilateral smooth */ - CV_PROP_RW float bilateral_sigma_spatial; - /** @brief Kernel size in pixels for bilateral smooth */ - CV_PROP_RW int bilateral_kernel_size; - - /** @brief Number of pyramid levels for ICP */ - CV_PROP_RW int pyramidLevels; - - /** @brief Resolution of voxel space - - Number of voxels in each dimension. - */ - CV_PROP_RW Vec3i volumeDims; - /** @brief Size of voxel in meters */ - CV_PROP_RW float voxelSize; - - /** @brief Minimal camera movement in meters - - Integrate new depth frame only if camera movement exceeds this value. - */ - CV_PROP_RW float tsdf_min_camera_movement; - - /** @brief initial volume pose in meters */ - Affine3f volumePose; - - /** @brief distance to truncate in meters - - Distances to surface that exceed this value will be truncated to 1.0. - */ - CV_PROP_RW float tsdf_trunc_dist; - - /** @brief max number of frames per voxel - - Each voxel keeps running average of distances no longer than this value. - */ - CV_PROP_RW int tsdf_max_weight; - - /** @brief A length of one raycast step - - How much voxel sizes we skip each raycast step - */ - CV_PROP_RW float raycast_step_factor; - - // gradient delta in voxel sizes - // fixed at 1.0f - // float gradient_delta_factor; - - /** @brief light pose for rendering in meters */ - CV_PROP Vec3f lightPose; - - /** @brief distance theshold for ICP in meters */ - CV_PROP_RW float icpDistThresh; - /** angle threshold for ICP in radians */ - CV_PROP_RW float icpAngleThresh; - /** number of ICP iterations for each pyramid level */ - CV_PROP std::vector icpIterations; - - /** @brief Threshold for depth truncation in meters - - All depth values beyond this threshold will be set to zero - */ - CV_PROP_RW float truncateThreshold; -}; - /** @brief DynamicFusion implementation This class implements a 3d reconstruction algorithm as described in @cite dynamicfusion. @@ -132,11 +40,11 @@ struct CV_EXPORTS_W Params class CV_EXPORTS_W DynaFu { public: - CV_WRAP static Ptr create(const Ptr& _params); + CV_WRAP static Ptr create(const Ptr& _params); virtual ~DynaFu(); /** @brief Get current parameters */ - virtual const Params& getParams() const = 0; + virtual const kinfu::Params& getParams() const = 0; /** @brief Renders a volume into an image diff --git a/modules/rgbd/samples/dynafu_demo.cpp b/modules/rgbd/samples/dynafu_demo.cpp index b69d8725f9c..8b27021ea5a 100644 --- a/modules/rgbd/samples/dynafu_demo.cpp +++ b/modules/rgbd/samples/dynafu_demo.cpp @@ -13,208 +13,16 @@ #include #include #include +#include "io_utils.hpp" using namespace cv; using namespace cv::dynafu; -using namespace std; +using namespace cv::io_utils; #ifdef HAVE_OPENCV_VIZ #include #endif -static vector readDepth(std::string fileList); - -static vector readDepth(std::string fileList) -{ - vector v; - - fstream file(fileList); - if(!file.is_open()) - throw std::runtime_error("Failed to read depth list"); - - std::string dir; - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - - while(!file.eof()) - { - std::string s, imgPath; - std::getline(file, s); - if(s.empty() || s[0] == '#') continue; - std::stringstream ss; - ss << s; - double thumb; - ss >> thumb >> imgPath; - v.push_back(dir+'/'+imgPath); - } - - return v; -} - -struct DepthWriter -{ - DepthWriter(string fileList) : - file(fileList, ios::out), count(0), dir() - { - size_t slashIdx = fileList.rfind('/'); - slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\'); - dir = fileList.substr(0, slashIdx); - - if(!file.is_open()) - throw std::runtime_error("Failed to write depth list"); - - file << "# depth maps saved from device" << endl; - file << "# useless_number filename" << endl; - } - - void append(InputArray _depth) - { - Mat depth = _depth.getMat(); - string depthFname = cv::format("%04d.png", count); - string fullDepthFname = dir + '/' + depthFname; - if(!imwrite(fullDepthFname, depth)) - throw std::runtime_error("Failed to write depth to file " + fullDepthFname); - file << count++ << " " << depthFname << endl; - } - - fstream file; - int count; - string dir; -}; - -namespace Kinect2Params -{ - static const Size frameSize = Size(512, 424); - // approximate values, no guarantee to be correct - static const float focal = 366.1f; - static const float cx = 258.2f; - static const float cy = 204.f; - static const float k1 = 0.12f; - static const float k2 = -0.34f; - static const float k3 = 0.12f; -}; - -struct DepthSource -{ -public: - DepthSource(int cam) : - DepthSource("", cam) - { } - - DepthSource(String fileListName) : - DepthSource(fileListName, -1) - { } - - DepthSource(String fileListName, int cam) : - depthFileList(fileListName.empty() ? vector() : readDepth(fileListName)), - frameIdx(0), - vc( cam >= 0 ? VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam) : VideoCapture()), - undistortMap1(), - undistortMap2(), - useKinect2Workarounds(true) - { - } - - UMat getDepth() - { - UMat out; - if (!vc.isOpened()) - { - if (frameIdx < depthFileList.size()) - { - Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH); - f.copyTo(out); - } - else - { - return UMat(); - } - } - else - { - vc.grab(); - vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); - - // workaround for Kinect 2 - if(useKinect2Workarounds) - { - out = out(Rect(Point(), Kinect2Params::frameSize)); - - UMat outCopy; - // linear remap adds gradient between valid and invalid pixels - // which causes garbage, use nearest instead - remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST); - - cv::flip(outCopy, out, 1); - } - } - if (out.empty()) - throw std::runtime_error("Matrix is empty"); - return out; - } - - bool empty() - { - return depthFileList.empty() && !(vc.isOpened()); - } - - void updateParams(Params& params) - { - if (vc.isOpened()) - { - // this should be set in according to user's depth sensor - int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH); - int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT); - - float focal = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH); - - // it's recommended to calibrate sensor to obtain its intrinsics - float fx, fy, cx, cy; - Size frameSize; - if(useKinect2Workarounds) - { - fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; - - frameSize = Kinect2Params::frameSize; - } - else - { - fx = fy = focal; - cx = w/2 - 0.5f; - cy = h/2 - 0.5f; - - frameSize = Size(w, h); - } - - Matx33f camMatrix = Matx33f(fx, 0, cx, - 0, fy, cy, - 0, 0, 1); - - params.frameSize = frameSize; - params.intr = camMatrix; - params.depthFactor = 1000.f; - - Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; - if(useKinect2Workarounds) - initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(), - camMatrix, frameSize, CV_16SC2, - undistortMap1, undistortMap2); - } - } - - vector depthFileList; - size_t frameIdx; - VideoCapture vc; - UMat undistortMap1, undistortMap2; - bool useKinect2Workarounds; -}; - #ifdef HAVE_OPENCV_VIZ const std::string vizWindowName = "cloud"; @@ -265,7 +73,7 @@ int main(int argc, char **argv) { bool coarse = false; bool idle = false; - string recordPath; + std::string recordPath; CommandLineParser parser(argc, argv, keys); parser.about(message); @@ -312,13 +120,13 @@ int main(int argc, char **argv) if(!recordPath.empty()) depthWriter = makePtr(recordPath); - Ptr params; + Ptr params; Ptr df; if(coarse) - params = Params::coarseParams(); + params = kinfu::Params::coarseParams(); else - params = Params::defaultParams(); + params = kinfu::Params::defaultParams(); // These params can be different for each depth sensor ds->updateParams(*params); diff --git a/modules/rgbd/src/dynafu.cpp b/modules/rgbd/src/dynafu.cpp index 0677f4242c1..57a8bd85f34 100644 --- a/modules/rgbd/src/dynafu.cpp +++ b/modules/rgbd/src/dynafu.cpp @@ -34,76 +34,6 @@ namespace cv { namespace dynafu { using namespace kinfu; -Ptr Params::defaultParams() -{ - Params p; - - p.frameSize = Size(640, 480); - - float fx, fy, cx, cy; - fx = fy = 525.f; - cx = p.frameSize.width/2 - 0.5f; - cy = p.frameSize.height/2 - 0.5f; - p.intr = Matx33f(fx, 0, cx, - 0, fy, cy, - 0, 0, 1); - - // 5000 for the 16-bit PNG files - // 1 for the 32-bit float images in the ROS bag files - p.depthFactor = 5000; - - // sigma_depth is scaled by depthFactor when calling bilateral filter - p.bilateral_sigma_depth = 0.04f; //meter - p.bilateral_sigma_spatial = 4.5; //pixels - p.bilateral_kernel_size = 7; //pixels - - p.icpAngleThresh = (float)(30. * CV_PI / 180.); // radians - p.icpDistThresh = 0.1f; // meters - - p.icpIterations = {10, 5, 4}; - p.pyramidLevels = (int)p.icpIterations.size(); - - p.tsdf_min_camera_movement = 0.f; //meters, disabled - - p.volumeDims = Vec3i::all(512); //number of voxels - - float volSize = 3.f; - p.voxelSize = volSize/512.f; //meters - - // default pose of volume cube - p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f)); - p.tsdf_trunc_dist = 0.04f; //meters; - p.tsdf_max_weight = 64; //frames - - p.raycast_step_factor = 0.25f; //in voxel sizes - // gradient delta factor is fixed at 1.0f and is not used - //p.gradient_delta_factor = 0.5f; //in voxel sizes - - //p.lightPose = p.volume_pose.translation()/4; //meters - p.lightPose = Vec3f::all(0.f); //meters - - // depth truncation is not used by default but can be useful in some scenes - p.truncateThreshold = 0.f; //meters - - return makePtr(p); -} - -Ptr Params::coarseParams() -{ - Ptr p = defaultParams(); - - p->icpIterations = {5, 3, 2}; - p->pyramidLevels = (int)p->icpIterations.size(); - - float volSize = 3.f; - p->volumeDims = Vec3i::all(128); //number of voxels - p->voxelSize = volSize/128.f; - - p->raycast_step_factor = 0.75f; //in voxel sizes - - return p; -} - // T should be Mat or UMat template< typename T > class DynaFuImpl : public DynaFu diff --git a/modules/rgbd/src/fast_icp.cpp b/modules/rgbd/src/fast_icp.cpp index 2ead1a8f527..ab25eb94f0d 100644 --- a/modules/rgbd/src/fast_icp.cpp +++ b/modules/rgbd/src/fast_icp.cpp @@ -4,7 +4,6 @@ // This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory -#include #include "precomp.hpp" #include "fast_icp.hpp" #include "opencl_kernels_rgbd.hpp" diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index a961e760848..8177213e8ab 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -22,25 +22,23 @@ Ptr VolumeParams::defaultParams(VolumeType _volumeType) params.unitResolution = 0; // unitResolution not used for TSDF float volumeSize = 3.0f; params.pose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)); - switch (params.type) + if(params.type == VolumeType::TSDF) { - case VolumeType::TSDF: - params.resolution = Vec3i::all(512); - params.voxelSize = volumeSize / 512.f; - params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF - break; - case VolumeType::HASHTSDF: - params.unitResolution = 16; - params.voxelSize = volumeSize / 512.f; - params.depthTruncThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); - break; - default: - CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); - return nullptr; + params.resolution = Vec3i::all(512); + params.voxelSize = volumeSize / 512.f; + params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF + params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters + return makePtr(params); } - params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters - - return makePtr(params); + else if(params.type == VolumeType::HASHTSDF) + { + 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"); } Ptr VolumeParams::coarseParams(VolumeType _volumeType) @@ -49,33 +47,29 @@ Ptr VolumeParams::coarseParams(VolumeType _volumeType) params->raycastStepFactor = 0.75f; float volumeSize = 3.0f; - switch (params->type) + if(params->type == VolumeType::TSDF) + { + params->resolution = Vec3i::all(128); + params->voxelSize = volumeSize / 128.f; + params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters + return params; + } + else if(params->type == VolumeType::HASHTSDF) { - case VolumeType::TSDF: - params->resolution = Vec3i::all(128); - params->voxelSize = volumeSize / 128.f; - break; - case VolumeType::HASHTSDF: - params->voxelSize = volumeSize / 128.f; - break; - default: - CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); - return nullptr; + params->voxelSize = volumeSize / 128.f; + params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters + return params; } - params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters - return params; + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } Ptr makeVolume(const VolumeParams& _volumeParams) { - switch (_volumeParams.type) - { - case VolumeType::TSDF: return kinfu::makeTSDFVolume(_volumeParams); break; - case VolumeType::HASHTSDF: return makePtr(_volumeParams); break; - default: - CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); - return nullptr; - } + if(_volumeParams.type == VolumeType::TSDF) + return kinfu::makeTSDFVolume(_volumeParams); + else if(_volumeParams.type == VolumeType::HASHTSDF) + return kinfu::makeHashTSDFVolume(_volumeParams); + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, @@ -93,11 +87,7 @@ Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, return makeHashTSDFVolume( _voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); } - else - { - CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); - return nullptr; - } + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } } // namespace kinfu From 9599c3a61958b32668de4bc08030cb3410b75ad7 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 22 Sep 2020 15:32:17 -0400 Subject: [PATCH 33/36] Minor API changes --- modules/rgbd/src/large_kinfu.cpp | 2 +- modules/rgbd/src/pose_graph.cpp | 2 +- modules/rgbd/src/pose_graph.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/rgbd/src/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp index 671408477f6..dedeabb82db 100644 --- a/modules/rgbd/src/large_kinfu.cpp +++ b/modules/rgbd/src/large_kinfu.cpp @@ -279,7 +279,7 @@ bool LargeKinfuImpl::updateT(const MatType& _depth) // TODO: Convert constraints to posegraph PoseGraph poseGraph = submapMgr->MapToPoseGraph(); std::cout << "Created posegraph\n"; - Optimizer::optimizeCeres(poseGraph); + Optimizer::optimize(poseGraph); submapMgr->PoseGraphToMap(poseGraph); } diff --git a/modules/rgbd/src/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp index 9b09db1b1f3..b525e455129 100644 --- a/modules/rgbd/src/pose_graph.cpp +++ b/modules/rgbd/src/pose_graph.cpp @@ -130,7 +130,7 @@ void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& } #endif -void Optimizer::optimizeCeres(PoseGraph& poseGraph) +void Optimizer::optimize(PoseGraph& poseGraph) { PoseGraph poseGraphOriginal = poseGraph; diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp index 9b8ad8dd20c..bb0164723be 100644 --- a/modules/rgbd/src/pose_graph.hpp +++ b/modules/rgbd/src/pose_graph.hpp @@ -253,7 +253,7 @@ class PoseGraph namespace Optimizer { -void optimizeCeres(PoseGraph& poseGraph); +void optimize(PoseGraph& poseGraph); #if defined(CERES_FOUND) void createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem); From dfce89969405b777a2739548200893260a276375 Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Tue, 22 Sep 2020 16:31:50 -0400 Subject: [PATCH 34/36] Minor --- modules/rgbd/src/hash_tsdf.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 8fc2bebfcda..c4205e4a83b 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -1,6 +1,7 @@ // 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 "hash_tsdf.hpp" #include @@ -13,7 +14,6 @@ #include "opencv2/core/cvstd.hpp" #include "opencv2/core/utility.hpp" #include "opencv2/core/utils/trace.hpp" -#include "precomp.hpp" #include "utils.hpp" #define USE_INTERPOLATION_IN_GETNORMAL 1 @@ -23,6 +23,7 @@ namespace cv { namespace kinfu { + static inline TsdfType floatToTsdf(float num) { //CV_Assert(-1 < num <= 1); @@ -113,6 +114,7 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, const M } } } + mutex.lock(); for (const auto& tsdf_idx : localAccessVolUnits) { @@ -198,6 +200,7 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, const M } }); } + cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx_(const cv::Point3f& p) const { return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), @@ -223,7 +226,8 @@ cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord_(const cv::Point3f& point) const TsdfVoxel HashTSDFVolumeCPU::at_(const Vec3i& volumeIdx) const { - Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), + Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), + cvFloor(volumeIdx[1] / volumeUnitResolution), cvFloor(volumeIdx[2] / volumeUnitResolution)); VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); @@ -236,9 +240,9 @@ TsdfVoxel HashTSDFVolumeCPU::at_(const Vec3i& volumeIdx) const } Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - Vec3i volUnitLocalIdx = - volumeIdx - Vec3i(volumeUnitIdx[0] * volumeUnitResolution, volumeUnitIdx[1] * volumeUnitResolution, - volumeUnitIdx[2] * volumeUnitResolution); + Vec3i volUnitLocalIdx = volumeIdx - Vec3i(volumeUnitIdx[0] * volumeUnitResolution, + volumeUnitIdx[1] * volumeUnitResolution, + volumeUnitIdx[2] * volumeUnitResolution); volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); return volumeUnit->at(volUnitLocalIdx); @@ -566,7 +570,8 @@ struct HashRaycastInvoker : ParallelLoopBody float tcurr = tmin; Vec3i prevVolumeUnitIdx = - Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), std::numeric_limits::min()); + Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), + std::numeric_limits::min()); float tprev = tcurr; TsdfType prevTsdf = volume.truncDist; From 48ee51e9d2fe62e1d9edd4a611bc2f7de64032ec Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Sat, 10 Oct 2020 19:29:44 -0400 Subject: [PATCH 35/36] Remove CRTP for HashTSDF class --- modules/rgbd/src/hash_tsdf.cpp | 39 ++++---- modules/rgbd/src/hash_tsdf.hpp | 168 +++++++++------------------------ 2 files changed, 61 insertions(+), 146 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index c4205e4a83b..e8bad129dfc 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -37,8 +37,7 @@ static inline float tsdfToFloat(TsdfType num) return float(num) * (-1.f / 128.f); } -template -HashTSDFVolume::HashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, +HashTSDFVolume::HashTSDFVolume(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) : Volume(_voxelSize, _pose, _raycastStepFactor), maxWeight(_maxWeight), @@ -50,26 +49,26 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, Matx44f _pose, float _ truncDist = std::max(_truncDist, 4.0f * voxelSize); } -HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, +HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) - : Base(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, + :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) { } HashTSDFVolumeCPU::HashTSDFVolumeCPU(const VolumeParams& _params, bool _zFirstMemOrder) - : Base(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + : HashTSDFVolume(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) { } // zero volume, leave rest params the same -void HashTSDFVolumeCPU::reset_() +void HashTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); volumeUnits.clear(); } -void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) +void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); @@ -201,30 +200,30 @@ void HashTSDFVolumeCPU::integrate_(InputArray _depth, float depthFactor, const M }); } -cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx_(const cv::Point3f& p) const +cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx(const cv::Point3f& p) const { return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), cvFloor(p.z / volumeUnitSize)); } -cv::Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume_(const cv::Vec3i& volumeUnitIdx) const +cv::Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume(const cv::Vec3i& volumeUnitIdx) const { return cv::Point3f(volumeUnitIdx[0] * volumeUnitSize, volumeUnitIdx[1] * volumeUnitSize, volumeUnitIdx[2] * volumeUnitSize); } -cv::Point3f HashTSDFVolumeCPU::voxelCoordToVolume_(const cv::Vec3i& voxelIdx) const +cv::Point3f HashTSDFVolumeCPU::voxelCoordToVolume(const cv::Vec3i& voxelIdx) const { return cv::Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); } -cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord_(const cv::Point3f& point) const +cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(const cv::Point3f& point) const { return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), cvFloor(point.z * voxelSizeInv)); } -TsdfVoxel HashTSDFVolumeCPU::at_(const Vec3i& volumeIdx) const +TsdfVoxel HashTSDFVolumeCPU::at(const Vec3i& volumeIdx) const { Vec3i volumeUnitIdx = Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), cvFloor(volumeIdx[1] / volumeUnitResolution), @@ -248,7 +247,7 @@ TsdfVoxel HashTSDFVolumeCPU::at_(const Vec3i& volumeIdx) const return volumeUnit->at(volUnitLocalIdx); } -TsdfVoxel HashTSDFVolumeCPU::at_(const Point3f& point) const +TsdfVoxel HashTSDFVolumeCPU::at(const Point3f& point) const { Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); @@ -338,7 +337,7 @@ inline float interpolate(float tx, float ty, float tz, float vx[8]) } #endif -float HashTSDFVolumeCPU::interpolateVoxelPoint_(const Point3f& point) const +float HashTSDFVolumeCPU::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} }; @@ -383,13 +382,13 @@ float HashTSDFVolumeCPU::interpolateVoxelPoint_(const Point3f& point) const return interpolate(tx, ty, tz, vx); } -inline float HashTSDFVolumeCPU::interpolateVoxel_(const cv::Point3f& point) const +inline float HashTSDFVolumeCPU::interpolateVoxel(const cv::Point3f& point) const { return interpolateVoxelPoint(point * voxelSizeInv); } -Point3f HashTSDFVolumeCPU::getNormalVoxel_(const Point3f &point) const +Point3f HashTSDFVolumeCPU::getNormalVoxel(const Point3f &point) const { Vec3f normal = Vec3f(0, 0, 0); @@ -638,7 +637,7 @@ struct HashRaycastInvoker : ParallelLoopBody const Intr::Reprojector reproj; }; -void HashTSDFVolumeCPU::raycast_(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, +void HashTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); @@ -719,7 +718,7 @@ struct HashFetchPointsNormalsInvoker : ParallelLoopBody mutable Mutex mutex; }; -void HashTSDFVolumeCPU::fetchPointsNormals_(OutputArray _points, OutputArray _normals) const +void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); @@ -756,7 +755,7 @@ void HashTSDFVolumeCPU::fetchPointsNormals_(OutputArray _points, OutputArray _no } } -void HashTSDFVolumeCPU::fetchNormals_(InputArray _points, OutputArray _normals) const +void HashTSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); @@ -785,7 +784,7 @@ void HashTSDFVolumeCPU::fetchNormals_(InputArray _points, OutputArray _normals) } } -int HashTSDFVolumeCPU::getVisibleBlocks_(int currFrameId, int frameThreshold) const +int HashTSDFVolumeCPU::getVisibleBlocks(int currFrameId, int frameThreshold) const { int numVisibleBlocks = 0; //! TODO: Iterate over map parallely? diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index fcb8247b1a9..cbe4cffea03 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -15,109 +15,6 @@ namespace cv { namespace kinfu { -template -class HashTSDFVolume : public Volume -{ - public: - virtual ~HashTSDFVolume() = default; - - virtual void reset() override - { - Derived* derived = static_cast(this); - derived->reset_(); - } - - virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, - const int frameId = 0) override - { - Derived* derived = static_cast(this); - derived->integrate_(_depth, depthFactor, cameraPose, intrinsics, frameId); - } - virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, - OutputArray normals) const override - { - const Derived* derived = static_cast(this); - derived->raycast_(cameraPose, intrinsics, frameSize, points, normals); - } - virtual void fetchNormals(InputArray points, OutputArray _normals) const override - { - const Derived* derived = static_cast(this); - derived->fetchNormals_(points, _normals); - } - virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override - { - const Derived* derived = static_cast(this); - derived->fetchPointsNormals_(points, normals); - } - inline size_t getTotalVolumeUnits() const { return static_cast(this)->getTotalVolumeUnits_(); } - inline int getVisibleBlocks(int currFrameId, int frameThreshold) const - { - return static_cast(this)->getVisibleBlocks_(currFrameId, frameThreshold); - } - - inline TsdfVoxel at(const Vec3i& volumeIdx) const - { - const Derived* derived = static_cast(this); - return derived->at_(volumeIdx); - } - //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = - //! 1m) - inline TsdfVoxel at(const Point3f& point) const - { - const Derived* derived = static_cast(this); - return derived->at_(point); - } - - inline float interpolateVoxelPoint(const Point3f& point) const - { - const Derived* derived = static_cast(this); - return derived->interpolateVoxelPoint_(point); - } - - inline float interpolateVoxel_(const cv::Point3f& point) const - { - const Derived* derived = static_cast(this); - return derived->interpolateVoxel_(point); - } - inline Point3f getNormalVoxel(const Point3f& point) const - { - const Derived* derived = static_cast(this); - return derived->getNormalVoxel_(point); - } - - //! Utility functions for coordinate transformations - inline Vec3i volumeToVolumeUnitIdx(const Point3f& point) const - { - return static_cast(this)->volumeToVolumeUnitIdx_(point); - } - inline Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const - { - return static_cast(this)->volumeUnitIdxToVolume_(volumeUnitIdx); - } - - inline Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const - { - return static_cast(this)->voxelCoordToVolume_(voxelIdx); - } - inline Vec3i volumeToVoxelCoord(const Point3f& point) const { return static_cast(this)->volumeToVoxelCoord_(point); } - - public: - int maxWeight; - float truncDist; - float truncateThreshold; - int volumeUnitResolution; - float volumeUnitSize; - bool zFirstMemOrder; - - protected: - //! dimension in voxels, size in meters - //! Use fixed volume cuboid - //! Can be only called by derived class - HashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); - friend Derived; -}; - struct VolumeUnit { VolumeUnit() : pVolume(nullptr){}; @@ -146,46 +43,65 @@ struct tsdf_hash typedef std::unordered_set VolumeUnitIndexSet; typedef std::unordered_map VolumeUnitMap; -class HashTSDFVolumeCPU : public HashTSDFVolume +class HashTSDFVolume : public Volume +{ + public: + // dimension in voxels, size in meters + //! Use fixed volume cuboid + HashTSDFVolume(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float _truncateThreshold, int _volumeUnitRes, + bool zFirstMemOrder = true); + + virtual ~HashTSDFVolume() = default; + + public: + int maxWeight; + float truncDist; + float truncateThreshold; + int volumeUnitResolution; + float volumeUnitSize; + bool zFirstMemOrder; +}; + +class HashTSDFVolumeCPU : public HashTSDFVolume { - typedef HashTSDFVolume Base; public: // dimension in voxels, size in meters - HashTSDFVolumeCPU(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, + HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); - void integrate_(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, - const int frameId = 0); - void raycast_(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, - OutputArray normals) const; + void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, + const int frameId = 0) override; + void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, + OutputArray normals) const override; - void fetchNormals_(InputArray points, OutputArray _normals) const; - void fetchPointsNormals_(OutputArray points, OutputArray normals) const; + void fetchNormals(InputArray points, OutputArray _normals) const override; + void fetchPointsNormals(OutputArray points, OutputArray normals) const override; - void reset_(); - size_t getTotalVolumeUnits_() const { return volumeUnits.size(); } - int getVisibleBlocks_(int currFrameId, int frameThreshold) const; + void reset() override; + size_t getTotalVolumeUnits() const { return volumeUnits.size(); } + int getVisibleBlocks(int currFrameId, int frameThreshold) const; //! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length) - TsdfVoxel at_(const Vec3i& volumeIdx) const; + TsdfVoxel at(const Vec3i& volumeIdx) const; //! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit = //! 1m) - TsdfVoxel at_(const Point3f& point) const; + TsdfVoxel at(const Point3f& point) const; - float interpolateVoxelPoint_(const Point3f& point) const; - float interpolateVoxel_(const cv::Point3f& point) const; - Point3f getNormalVoxel_(const cv::Point3f& p) const; + float interpolateVoxelPoint(const Point3f& point) const; + float interpolateVoxel(const cv::Point3f& point) const; + Point3f getNormalVoxel(const cv::Point3f& p) const; //! Utility functions for coordinate transformations - Vec3i volumeToVolumeUnitIdx_(const Point3f& point) const; - Point3f volumeUnitIdxToVolume_(const Vec3i& volumeUnitIdx) const; + Vec3i volumeToVolumeUnitIdx(const Point3f& point) const; + Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const; - Point3f voxelCoordToVolume_(const Vec3i& voxelIdx) const; - Vec3i volumeToVoxelCoord_(const Point3f& point) const; + Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; + Vec3i volumeToVoxelCoord(const Point3f& point) const; public: //! Hashtable of individual smaller volume units @@ -193,13 +109,13 @@ class HashTSDFVolumeCPU : public HashTSDFVolume }; template -Ptr> makeHashTSDFVolume(const VolumeParams& _volumeParams) +Ptr makeHashTSDFVolume(const VolumeParams& _volumeParams) { return makePtr(_volumeParams); } template -Ptr> makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, +Ptr makeHashTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, float truncateThreshold, int volumeUnitResolution = 16) { return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, truncateThreshold, From daa800f4e5b273b9e281125b7735541e511ab4ee Mon Sep 17 00:00:00 2001 From: Akash Sharma Date: Sat, 10 Oct 2020 20:10:58 -0400 Subject: [PATCH 36/36] Bug fixes for HashTSDF integration --- modules/rgbd/src/hash_tsdf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index e8bad129dfc..69baad8e360 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -76,7 +76,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma Depth depth = _depth.getMat(); //! Compute volumes to be allocated - const int depthStride = 1; + const int depthStride = int(log2(volumeUnitResolution)); const float invDepthFactor = 1.f / depthFactor; const Intr::Reprojector reproj(intrinsics.makeReprojector()); const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose)); @@ -573,7 +573,7 @@ struct HashRaycastInvoker : ParallelLoopBody std::numeric_limits::min()); float tprev = tcurr; - TsdfType prevTsdf = volume.truncDist; + float prevTsdf = volume.truncDist; Ptr currVolumeUnit; while (tcurr < tmax) {