diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt index 7f2f6a672..247c788a9 100644 --- a/modules/rgbd/CMakeLists.txt +++ b/modules/rgbd/CMakeLists.txt @@ -1,2 +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) + ocv_target_compile_definitions(${the_module} PUBLIC CERES_FOUND) +else() + message(STATUS "CERES support is disabled. Ceres Solver is Required for Posegraph optimization") +endif() diff --git a/modules/rgbd/include/opencv2/rgbd.hpp b/modules/rgbd/include/opencv2/rgbd.hpp index 37b2927cd..d4ac749c2 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/dynafu.hpp b/modules/rgbd/include/opencv2/rgbd/dynafu.hpp index d057ebe76..fae69c48e 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/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 8fcc3189a..95f732b67 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); } /** @@ -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 000000000..7f1428c6d --- /dev/null +++ b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp @@ -0,0 +1,143 @@ +// 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 Large Scale Dense Depth Fusion implementation + + 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 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. + + 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. + + LargeKinfu does not use any OpenCL acceleration yet. + To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL(). + + This implementation is inspired from Kintinuous, InfiniTAM and other SOTA algorithms + + 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 3d10e2dd9..33f63b1fb 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -18,7 +18,7 @@ namespace kinfu class CV_EXPORTS_W Volume { public: - Volume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor) + Volume(float _voxelSize, Matx44f _pose, float _raycastStepFactor) : voxelSize(_voxelSize), voxelSizeInv(1.0f / voxelSize), pose(_pose), @@ -28,19 +28,18 @@ class CV_EXPORTS_W Volume virtual ~Volume(){}; - virtual void integrate(InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics) = 0; - virtual void raycast(const cv::Matx44f& 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; + 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, + 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; const float voxelSizeInv; - const cv::Affine3f pose; + const Affine3f pose; const float raycastStepFactor; }; @@ -50,9 +49,70 @@ enum class VolumeType HASHTSDF = 1 }; -CV_EXPORTS_W cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Matx44f _pose, - float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, Vec3i _resolution); +struct CV_EXPORTS_W VolumeParams +{ + /** @brief Type of Volume + Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units) + */ + 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 + */ + CV_PROP_RW Vec3i resolution; + + /** @brief Resolution of volumeUnit in voxel space + Number of voxels in each dimension for volumeUnit + Applicable only for hashTSDF. + */ + CV_PROP_RW int unitResolution = {0}; + + /** @brief Initial pose of the volume in meters */ + Affine3f pose; + + /** @brief Length of voxels in meters */ + CV_PROP_RW float voxelSize; + + /** @brief TSDF truncation distance + Distances greater than value from surface will be truncated to 1.0 + */ + CV_PROP_RW float tsdfTruncDist; + + /** @brief Max number of frames to integrate per voxel + Represents the max number of frames over which a running average + of the TSDF is calculated for a voxel + */ + CV_PROP_RW int maxWeight; + + /** @brief Threshold for depth truncation in meters + Truncates the depth greater than threshold to 0 + */ + CV_PROP_RW float depthTruncThreshold; + + /** @brief Length of single raycast step + Describes the percentage of voxel length that is skipped per march + */ + CV_PROP_RW float raycastStepFactor; + + /** @brief Default set of parameters that provide higher quality reconstruction + at the cost of slow performance. + */ + CV_WRAP static Ptr defaultParams(VolumeType _volumeType); + + /** @brief Coarse set of parameters that provides relatively higher performance + at the cost of reconstrution quality. + */ + CV_WRAP static Ptr coarseParams(VolumeType _volumeType); +}; + + +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); + } // namespace kinfu } // namespace cv #endif diff --git a/modules/rgbd/samples/dynafu_demo.cpp b/modules/rgbd/samples/dynafu_demo.cpp index b69d8725f..8b27021ea 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/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp new file mode 100644 index 000000000..c96d6c534 --- /dev/null +++ b/modules/rgbd/samples/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/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index 147ffaa2d..e264ba38b 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -11,272 +11,16 @@ #include #include +#include "io_utils.hpp" + 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 +73,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 new file mode 100644 index 000000000..40d14ca8c --- /dev/null +++ b/modules/rgbd/samples/large_kinfu_demo.cpp @@ -0,0 +1,260 @@ +// 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 + +#include "io_utils.hpp" + +using namespace cv; +using namespace cv::kinfu; +using namespace cv::large_kinfu; +using namespace cv::io_utils; + +#ifdef HAVE_OPENCV_VIZ +#include +#endif + +#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 }" + "{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) }" +}; + +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 Submap based large environment reconstruction" + "\nThis module uses the newer hashtable based TSDFVolume (relatively fast) for larger " + "reconstructions by default\n"; + +int main(int argc, char** argv) +{ + bool coarse = false; + bool idle = false; + std::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("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; + + params = large_kinfu::Params::hashTSDFParams(coarse); + + // These params can be different for each depth sensor + ds->updateParams(*params); + + // Disabled until there is no OpenCL accelerated HashTSDF is available + cv::setUseOptimized(false); + + 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/dynafu.cpp b/modules/rgbd/src/dynafu.cpp index d663275d3..002be7bd6 100644 --- a/modules/rgbd/src/dynafu.cpp +++ b/modules/rgbd/src/dynafu.cpp @@ -82,76 +82,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 06d4a7506..ab25eb94f 100644 --- a/modules/rgbd/src/fast_icp.cpp +++ b/modules/rgbd/src/fast_icp.cpp @@ -32,7 +32,6 @@ public: InputArray oldPoints, InputArray oldNormals, InputArray newPoints, InputArray newNormals ) const override; - template < typename T > bool estimateTransformT(cv::Affine3f& transform, const vector& oldPoints, const vector& oldNormals, @@ -298,7 +297,6 @@ struct GetAbInvoker : ParallelLoopBody continue; // build point-wise vector ab = [ A | b ] - v_float32x4 VxNv = crossProduct(newP, oldN); Point3f VxN; VxN.x = VxNv.get0(); @@ -449,7 +447,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)}; - // 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/fast_icp.hpp b/modules/rgbd/src/fast_icp.hpp index 7a9f06940..a11a919da 100644 --- a/modules/rgbd/src/fast_icp.hpp +++ b/modules/rgbd/src/fast_icp.hpp @@ -22,7 +22,6 @@ public: InputArray oldPoints, InputArray oldNormals, InputArray newPoints, InputArray newNormals ) const = 0; - virtual ~ICP() { } protected: diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b41450021..69baad8e3 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -37,9 +37,8 @@ static inline float tsdfToFloat(TsdfType num) return float(num) * (-1.f / 128.f); } -HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, float _truncateThreshold, - int _volumeUnitRes, bool _zFirstMemOrder) +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), truncateThreshold(_truncateThreshold), @@ -50,14 +49,18 @@ HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _rayca truncDist = std::max(_truncDist, 4.0f * voxelSize); } -HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, float _truncateThreshold, - int _volumeUnitRes, bool _zFirstMemOrder) - : HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, - _truncateThreshold, _volumeUnitRes, _zFirstMemOrder) +HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, float _truncateThreshold, int _volumeUnitRes, bool _zFirstMemOrder) + :HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold, _volumeUnitRes, + _zFirstMemOrder) { } +HashTSDFVolumeCPU::HashTSDFVolumeCPU(const VolumeParams& _params, bool _zFirstMemOrder) + : HashTSDFVolume(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, + _params.depthTruncThreshold, _params.unitResolution, _zFirstMemOrder) +{ +} // zero volume, leave rest params the same void HashTSDFVolumeCPU::reset() { @@ -65,7 +68,7 @@ void HashTSDFVolumeCPU::reset() volumeUnits.clear(); } -void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics) +void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); @@ -73,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)); @@ -135,6 +138,7 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx)).matrix; vu.pVolume = makePtr(voxelSize, subvolumePose, raycastStepFactor, truncDist, maxWeight, volumeDims); //! This volume unit will definitely be required for current integration + vu.lastVisibleIndex = frameId; vu.isActive = true; } @@ -169,7 +173,8 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows) { assert(it != volumeUnits.end()); - it->second.isActive = true; + it->second.lastVisibleIndex = frameId; + it->second.isActive = true; } } }); @@ -195,34 +200,34 @@ void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Ma }); } -cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx(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(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(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(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)); } -inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const +TsdfVoxel HashTSDFVolumeCPU::at(const Vec3i& volumeIdx) const { - cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), - cvFloor(volumeIdx[1] / volumeUnitResolution), - cvFloor(volumeIdx[2] / volumeUnitResolution)); + 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()) @@ -232,21 +237,19 @@ inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const dummy.weight = 0; return dummy; } - cv::Ptr volumeUnit = - std::dynamic_pointer_cast(it->second.pVolume); + Ptr volumeUnit = std::dynamic_pointer_cast(it->second.pVolume); - cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, - volumeUnitIdx[1] * volumeUnitResolution, - volumeUnitIdx[2] * volumeUnitResolution); + Vec3i volUnitLocalIdx = volumeIdx - Vec3i(volumeUnitIdx[0] * volumeUnitResolution, + volumeUnitIdx[1] * volumeUnitResolution, + volumeUnitIdx[2] * volumeUnitResolution); - volUnitLocalIdx = - cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); + volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); return volumeUnit->at(volUnitLocalIdx); } -inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const +TsdfVoxel HashTSDFVolumeCPU::at(const Point3f& point) const { - cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); + Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); if (it == volumeUnits.end()) { @@ -255,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); + 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])); + Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); + Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); + volUnitLocalIdx = Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); return volumeUnit->at(volUnitLocalIdx); } @@ -386,7 +387,8 @@ inline float HashTSDFVolumeCPU::interpolateVoxel(const cv::Point3f& point) const return interpolateVoxelPoint(point * voxelSizeInv); } -inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const + +Point3f HashTSDFVolumeCPU::getNormalVoxel(const Point3f &point) const { Vec3f normal = Vec3f(0, 0, 0); @@ -527,8 +529,8 @@ inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const struct HashRaycastInvoker : ParallelLoopBody { - HashRaycastInvoker(Points& _points, Normals& _normals, const Matx44f& cameraPose, - const Intr& intrinsics, const HashTSDFVolumeCPU& _volume) + HashRaycastInvoker(Points& _points, Normals& _normals, const Matx44f& cameraPose, const Intr& intrinsics, + const HashTSDFVolumeCPU& _volume) : ParallelLoopBody(), points(_points), normals(_normals), @@ -559,41 +561,38 @@ 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()); + Vec3i prevVolumeUnitIdx = + Vec3i(std::numeric_limits::min(), std::numeric_limits::min(), + std::numeric_limits::min()); float tprev = tcurr; float prevTsdf = volume.truncDist; - cv::Ptr currVolumeUnit; + Ptr currVolumeUnit; while (tcurr < tmax) { - Point3f currRayPos = orig + tcurr * rayDirV; - cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); + Point3f currRayPos = orig + tcurr * rayDirV; + Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); VolumeUnitMap::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); float currTsdf = prevTsdf; int currWeight = 0; float stepSize = 0.5f * blockSize; - cv::Vec3i volUnitLocalIdx; + 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); - cv::Point3f currVolUnitPos = - volume.volumeUnitIdxToVolume(currVolumeUnitIdx); - volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); + currVolumeUnit = std::dynamic_pointer_cast(it->second.pVolume); + Point3f currVolUnitPos = volume.volumeUnitIdxToVolume(currVolumeUnitIdx); + volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); //! TODO: Figure out voxel interpolation TsdfVoxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); @@ -604,8 +603,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; @@ -639,9 +637,8 @@ struct HashRaycastInvoker : ParallelLoopBody const Intr::Reprojector reproj; }; -void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray _points, - cv::OutputArray _normals) const +void HashTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, + OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); @@ -660,10 +657,9 @@ void HashTSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const cv::kinfu:: struct HashFetchPointsNormalsInvoker : ParallelLoopBody { - HashFetchPointsNormalsInvoker(const HashTSDFVolumeCPU& _volume, - const std::vector& _totalVolUnits, - std::vector>& _pVecs, - std::vector>& _nVecs, bool _needNormals) + HashFetchPointsNormalsInvoker(const HashTSDFVolumeCPU& _volume, const std::vector& _totalVolUnits, + std::vector>& _pVecs, std::vector>& _nVecs, + bool _needNormals) : ParallelLoopBody(), volume(_volume), totalVolUnits(_totalVolUnits), @@ -678,21 +674,20 @@ struct HashFetchPointsNormalsInvoker : ParallelLoopBody std::vector points, normals; for (int i = range.start; i < range.end; i++) { - cv::Vec3i tsdf_idx = totalVolUnits[i]; + 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); + 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); + Vec3i voxelIdx(x, y, z); TsdfVoxel voxel = volumeUnit->at(voxelIdx); if (voxel.tsdf != -128 && voxel.weight != 0) @@ -715,7 +710,7 @@ struct HashFetchPointsNormalsInvoker : ParallelLoopBody } const HashTSDFVolumeCPU& volume; - std::vector totalVolUnits; + std::vector totalVolUnits; std::vector>& pVecs; std::vector>& nVecs; const TsdfVoxel* volDataStart; @@ -760,7 +755,7 @@ void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _nor } } -void HashTSDFVolumeCPU::fetchNormals(cv::InputArray _points, cv::OutputArray _normals) const +void HashTSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); @@ -773,8 +768,7 @@ void HashTSDFVolumeCPU::fetchNormals(cv::InputArray _points, cv::OutputArray _no Normals normals = _normals.getMat(); const HashTSDFVolumeCPU& _volume = *this; - auto HashPushNormals = [&](const ptype& point, const int* position) - { + auto HashPushNormals = [&](const ptype& point, const int* position) { const HashTSDFVolumeCPU& volume(_volume); Affine3f invPose(volume.pose.inv()); Point3f p = fromPtype(point); @@ -782,7 +776,7 @@ void HashTSDFVolumeCPU::fetchNormals(cv::InputArray _points, cv::OutputArray _no 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); }; @@ -790,13 +784,17 @@ void HashTSDFVolumeCPU::fetchNormals(cv::InputArray _points, cv::OutputArray _no } } -cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Matx44f _pose, - float _raycastStepFactor, float _truncDist, - int _maxWeight, float _truncateThreshold, - int _volumeUnitResolution) +int HashTSDFVolumeCPU::getVisibleBlocks(int currFrameId, int frameThreshold) const { - return cv::makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, - _maxWeight, _truncateThreshold, _volumeUnitResolution); + int numVisibleBlocks = 0; + //! TODO: Iterate over map parallely? + for (const auto& keyvalue : volumeUnits) + { + const VolumeUnit& volumeUnit = keyvalue.second; + if (volumeUnit.lastVisibleIndex > (currFrameId - frameThreshold)) + numVisibleBlocks++; + } + return numVisibleBlocks; } } // namespace kinfu diff --git a/modules/rgbd/src/hash_tsdf.hpp b/modules/rgbd/src/hash_tsdf.hpp index bac5ea9a4..cbe4cffea 100644 --- a/modules/rgbd/src/hash_tsdf.hpp +++ b/modules/rgbd/src/hash_tsdf.hpp @@ -15,40 +15,20 @@ namespace cv { namespace kinfu { -class HashTSDFVolume : public Volume -{ - public: - // dimension in voxels, size in meters - //! Use fixed volume cuboid - HashTSDFVolume(float _voxelSize, cv::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; -}; - struct VolumeUnit { VolumeUnit() : pVolume(nullptr){}; ~VolumeUnit() = default; - cv::Ptr pVolume; - cv::Vec3i index; + Ptr pVolume; + int lastVisibleIndex = 0; bool isActive; }; //! Spatial hashing struct tsdf_hash { - size_t operator()(const cv::Vec3i& x) const noexcept + size_t operator()(const Vec3i& x) const noexcept { size_t seed = 0; constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; @@ -60,54 +40,87 @@ struct tsdf_hash } }; -typedef std::unordered_set VolumeUnitIndexSet; -typedef std::unordered_map VolumeUnitMap; +typedef std::unordered_set VolumeUnitIndexSet; +typedef std::unordered_map VolumeUnitMap; + +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 { + public: // dimension in voxels, size in meters - HashTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, float _truncateThreshold, - int _volumeUnitRes, bool zFirstMemOrder = true); + HashTSDFVolumeCPU(float _voxelSize, const Matx44f& _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, int _volumeUnitRes, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics) override; - virtual void raycast(const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray points, - cv::OutputArray normals) const override; + HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); - virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; - virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; + void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, + const int frameId = 0) override; + void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, + OutputArray normals) const override; - virtual void reset() override; + void fetchNormals(InputArray points, OutputArray _normals) const override; + void fetchPointsNormals(OutputArray points, OutputArray normals) const override; + + void reset() override; + size_t getTotalVolumeUnits() const { return volumeUnits.size(); } + int getVisibleBlocks(int currFrameId, int frameThreshold) const; //! 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 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 Point3f& point) const; float interpolateVoxelPoint(const Point3f& point) const; - inline float interpolateVoxel(const cv::Point3f& point) const; - Point3f getNormalVoxel(cv::Point3f p) const; + float interpolateVoxel(const cv::Point3f& point) const; + Point3f getNormalVoxel(const cv::Point3f& p) const; //! Utility functions for coordinate transformations - cv::Vec3i volumeToVolumeUnitIdx(cv::Point3f point) const; - cv::Point3f volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const; + Vec3i volumeToVolumeUnitIdx(const Point3f& point) const; + Point3f volumeUnitIdxToVolume(const Vec3i& volumeUnitIdx) const; - cv::Point3f voxelCoordToVolume(cv::Vec3i voxelIdx) const; - cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const; + Point3f voxelCoordToVolume(const Vec3i& voxelIdx) const; + Vec3i volumeToVoxelCoord(const Point3f& point) const; public: //! Hashtable of individual smaller volume units VolumeUnitMap volumeUnits; }; -cv::Ptr makeHashTSDFVolume(float _voxelSize, cv::Matx44f _pose, - float _raycastStepFactor, float _truncDist, - int _maxWeight, float truncateThreshold, - int volumeUnitResolution = 16); + +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) +{ + return 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 d776f359e..1d8314aae 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -119,7 +119,6 @@ public: 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/large_kinfu.cpp b/modules/rgbd/src/large_kinfu.cpp new file mode 100644 index 000000000..dedeabb82 --- /dev/null +++ b/modules/rgbd/src/large_kinfu.cpp @@ -0,0 +1,361 @@ +// 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 "submap.hpp" +#include "tsdf.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.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 + // 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; + + int frameCounter; + Affine3f pose; +}; + +template +LargeKinfuImpl::LargeKinfuImpl(const Params& _params) + : params(_params) +{ + icp = makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh); + + submapMgr = cv::makePtr>(params.volumeParams); + reset(); + submapMgr->createNewSubmap(true); + +} + +template +void LargeKinfuImpl::reset() +{ + frameCounter = 0; + pose = Affine3f::Identity(); + submapMgr->reset(); +} + +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; + + 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); + + std::cout << "Current frameID: " << frameCounter << "\n"; + for (const auto& it : submapMgr->activeSubmaps) + { + int currTrackingId = it.first; + auto submapData = it.second; + Ptr> currTrackingSubmap = submapMgr->getSubmap(currTrackingId); + Affine3f affine; + std::cout << "Current tracking ID: " << currTrackingId << std::endl; + + if(frameCounter == 0) //! Only one current tracking map + { + currTrackingSubmap->integrate(depth, params.depthFactor, params.intr, frameCounter); + currTrackingSubmap->pyrPoints = newPoints; + currTrackingSubmap->pyrNormals = newNormals; + continue; + } + + //1. Track + bool trackingSuccess = + icp->estimateTransform(affine, currTrackingSubmap->pyrPoints, currTrackingSubmap->pyrNormals, newPoints, newNormals); + if (trackingSuccess) + currTrackingSubmap->composeCameraPose(affine); + else + { + std::cout << "Tracking failed" << std::endl; + continue; + } + + //2. Integrate + 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]); + + 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 + bool isMapUpdated = submapMgr->updateMap(frameCounter, newPoints, newNormals); + + if(isMapUpdated) + { + // TODO: Convert constraints to posegraph + PoseGraph poseGraph = submapMgr->MapToPoseGraph(); + std::cout << "Created posegraph\n"; + Optimizer::optimize(poseGraph); + submapMgr->PoseGraphToMap(poseGraph); + + } + std::cout << "Number of submaps: " << submapMgr->submapList.size() << "\n"; + + frameCounter++; + return true; +} + +template +void LargeKinfuImpl::render(OutputArray image, const Matx44f& _cameraPose) const +{ + CV_TRACE_FUNCTION(); + + 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())) + { + //! TODO: Can render be dependent on current submap + renderPointsNormals(currSubmap->pyrPoints[0], currSubmap->pyrNormals[0], image, params.lightPose); + } + else + { + MatType points, normals; + currSubmap->raycast(cameraPose, params.intr, params.frameSize, points, normals); + renderPointsNormals(points, normals, image, params.lightPose); + } +} + +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); +} + +// 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/pose_graph.cpp b/modules/rgbd/src/pose_graph.cpp new file mode 100644 index 000000000..b525e4551 --- /dev/null +++ b/modules/rgbd/src/pose_graph.cpp @@ -0,0 +1,169 @@ +// 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.hpp" + +#include +#include +#include +#include + +#if defined(CERES_FOUND) +#include +#endif + +namespace cv +{ +namespace kinfu +{ +bool PoseGraph::isValid() const +{ + int numNodes = getNumNodes(); + int numEdges = getNumEdges(); + + if (numNodes <= 0 || numEdges <= 0) + return false; + + std::unordered_set nodesVisited; + std::vector nodesToVisit; + + nodesToVisit.push_back(nodes.at(0).getId()); + + bool isGraphConnected = false; + while (!nodesToVisit.empty()) + { + 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++) + { + 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) + { + std::cout << "Next node: " << nextNodeId << " " << nodesVisited.count(nextNodeId) + << std::endl; + if (nodesVisited.count(nextNodeId) == 0) + { + nodesToVisit.push_back(nextNodeId); + } + } + } + } + + isGraphConnected = (int(nodesVisited.size()) == numNodes); + std::cout << "nodesVisited: " << nodesVisited.size() + << " IsGraphConnected: " << isGraphConnected << std::endl; + 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)) + { + invalidEdgeNode = true; + break; + } + } + return isGraphConnected && !invalidEdgeNode; +} + +#if defined(CERES_FOUND) && defined(HAVE_EIGEN) +void Optimizer::createOptimizationProblem(PoseGraph& poseGraph, ceres::Problem& problem) +{ + int numEdges = poseGraph.getNumEdges(); + int numNodes = poseGraph.getNumNodes(); + if (numEdges == 0) + { + CV_Error(Error::StsBadArg, "PoseGraph has no edges, no optimization to be done"); + return; + } + + ceres::LossFunction* lossFunction = nullptr; + // TODO: Experiment with SE3 parameterization + ceres::LocalParameterization* quatLocalParameterization = + new ceres::EigenQuaternionParameterization; + + for (int currEdgeNum = 0; currEdgeNum < numEdges; ++currEdgeNum) + { + const PoseGraphEdge& currEdge = poseGraph.edges.at(currEdgeNum); + int sourceNodeId = currEdge.getSourceNodeId(); + int targetNodeId = currEdge.getTargetNodeId(); + Pose3d& sourcePose = poseGraph.nodes.at(sourceNodeId).se3Pose; + Pose3d& targetPose = poseGraph.nodes.at(targetNodeId).se3Pose; + + const Matx66f& informationMatrix = currEdge.information; + + 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(), + targetPose.r.coeffs().data()); + problem.SetParameterization(sourcePose.r.coeffs().data(), quatLocalParameterization); + problem.SetParameterization(targetPose.r.coeffs().data(), quatLocalParameterization); + } + + for (int currNodeId = 0; currNodeId < numNodes; ++currNodeId) + { + PoseGraphNode& currNode = poseGraph.nodes.at(currNodeId); + if (currNode.isPoseFixed()) + { + problem.SetParameterBlockConstant(currNode.se3Pose.t.data()); + problem.SetParameterBlockConstant(currNode.se3Pose.r.coeffs().data()); + } + } +} +#endif + +void Optimizer::optimize(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 " << numNodes << " nodes and " << numEdges << " edges" + << std::endl; + +#if defined(CERES_FOUND) && defined(HAVE_EIGEN) + ceres::Problem problem; + createOptimizationProblem(poseGraph, problem); + + ceres::Solver::Options options; + options.max_num_iterations = 100; + options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + std::cout << summary.FullReport() << '\n'; + + std::cout << "Is solution usable: " << summary.IsSolutionUsable() << std::endl; +#else + CV_Error(Error::StsNotImplemented, "Ceres and Eigen required for Pose Graph optimization"); +#endif +} + +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/pose_graph.hpp b/modules/rgbd/src/pose_graph.hpp new file mode 100644 index 000000000..bb0164723 --- /dev/null +++ b/modules/rgbd/src/pose_graph.hpp @@ -0,0 +1,321 @@ +#ifndef OPENCV_RGBD_GRAPH_NODE_H +#define OPENCV_RGBD_GRAPH_NODE_H + +#include +#include + +#include "opencv2/core/affine.hpp" +#if defined(HAVE_EIGEN) +#include +#include +#include "opencv2/core/eigen.hpp" +#endif + +#if defined(CERES_FOUND) +#include +#endif + +namespace cv +{ +namespace kinfu +{ +/*! \class GraphNode + * \brief Defines a node/variable that is optimizable in a posegraph + * + * 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) + { +#if defined(HAVE_EIGEN) + se3Pose = Pose3d(_pose.rotation(), _pose.translation()); +#endif + } + virtual ~PoseGraphNode() = default; + + int getId() const { return nodeId; } + 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) + { + 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); + pose = poseMatrix; + } +#endif + void setFixed(bool val = true) { isFixed = val; } + bool isPoseFixed() const { return isFixed; } + + public: + int nodeId; + bool isFixed; + Affine3f pose; +#if defined(HAVE_EIGEN) + Pose3d se3Pose; +#endif +}; + +/*! \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; } + + 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; + 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) */ +/* }; */ +// clang-format on + +class PoseGraph +{ + public: + typedef std::vector NodeVector; + typedef std::vector EdgeVector; + + explicit PoseGraph(){}; + virtual ~PoseGraph() = default; + + //! PoseGraph can be copied/cloned + PoseGraph(const PoseGraph& _poseGraph) = default; + PoseGraph& operator=(const PoseGraph& _poseGraph) = default; + + void addNode(const PoseGraphNode& node) { nodes.push_back(node); } + void addEdge(const PoseGraphEdge& edge) { edges.push_back(edge); } + + bool nodeExists(int nodeId) const + { + return std::find_if(nodes.begin(), nodes.end(), [nodeId](const PoseGraphNode& currNode) { + return currNode.getId() == nodeId; + }) != nodes.end(); + } + + bool isValid() const; + + int getNumNodes() const { return int(nodes.size()); } + int getNumEdges() const { return int(edges.size()); } + + public: + NodeVector nodes; + EdgeVector edges; +}; + +namespace Optimizer +{ +void optimize(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 +{ + 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 + +} // namespace Optimizer + +} // namespace kinfu +} // namespace cv +#endif /* ifndef OPENCV_RGBD_GRAPH_NODE_H */ diff --git a/modules/rgbd/src/sparse_block_matrix.hpp b/modules/rgbd/src/sparse_block_matrix.hpp new file mode 100644 index 000000000..0e607af63 --- /dev/null +++ b/modules/rgbd/src/sparse_block_matrix.hpp @@ -0,0 +1,159 @@ +// 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 + +#include "opencv2/core/base.hpp" +#include "opencv2/core/types.hpp" + +#if defined(HAVE_EIGEN) +#include +#include +#include + +#include "opencv2/core/eigen.hpp" +#endif + +namespace cv +{ +namespace kinfu +{ +/*! + * \class BlockSparseMat + * Naive implementation of Sparse Block Matrix + */ +template +struct BlockSparseMat +{ + 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 Matx<_Tp, blockM, blockN> MatType; + typedef std::unordered_map IDtoBlockValueMap; + + BlockSparseMat(int _nBlocks) : nBlocks(_nBlocks), ijValue() {} + + MatType& 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>::zeros() }).first; + } + 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 / blockM, j / blockN), iv(i % blockM, j % blockN); + 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() * blockM * blockN); + for (auto ijv : ijValue) + { + int xb = ijv.first.x, yb = ijv.first.y; + MatType vblock = ijv.second; + for (int i = 0; i < blockM; i++) + { + for (int j = 0; j < blockN; j++) + { + float val = vblock(i, j); + if (abs(val) >= NON_ZERO_VAL_THRESHOLD) + { + tripletList.push_back(Eigen::Triplet(blockM * xb + i, blockN * yb + j, val)); + } + } + } + } + Eigen::SparseMatrix<_Tp> EigenMat(blockM * nBlocks, blockN * 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; +}; + +//! Function to solve a sparse linear system of equations HX = B +//! Requires Eigen +static bool sparseSolve(const BlockSparseMat& H, const Mat& B, Mat& X, Mat& predB) +{ + bool result = false; +#if defined(HAVE_EIGEN) + Eigen::SparseMatrix bigA = H.toEigen(); + Eigen::VectorXf bigB; + cv2eigen(B, bigB); + + Eigen::SparseMatrix bigAtranspose = bigA.transpose(); + if(!bigA.isApprox(bigAtranspose)) + { + CV_Error(Error::StsBadArg, "H matrix is not symmetrical"); + return result; + } + + Eigen::SimplicialLDLT> solver; + + solver.compute(bigA); + if (solver.info() != Eigen::Success) + { + std::cout << "failed to eigen-decompose" << std::endl; + result = false; + } + 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; + result = false; + } + else + { + eigen2cv(solutionX, X); + eigen2cv(predBEigen, predB); + 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.hpp b/modules/rgbd/src/submap.hpp new file mode 100644 index 000000000..983ee610c --- /dev/null +++ b/modules/rgbd/src/submap.hpp @@ -0,0 +1,544 @@ +// 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_HPP__ +#define __OPENCV_RGBD_SUBMAP_HPP__ + +#include + +#include +#include +#include + +#include "hash_tsdf.hpp" +#include "opencv2/core/mat.inl.hpp" +#include "pose_graph.hpp" + +namespace cv +{ +namespace kinfu +{ +template +class Submap +{ + public: + struct PoseConstraint + { + Affine3f estimatedPose; + int weight; + + PoseConstraint() : weight(0){}; + + void accumulatePose(const Affine3f& _pose, int _weight = 1) + { + Matx44f accPose = estimatedPose.matrix * weight + _pose.matrix * _weight; + 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) + { + std::cout << "Created volume\n"; + } + virtual ~Submap() = default; + + 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 int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); }; + virtual int getVisibleBlocks(int currFrameId) const + { + 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; }; + + void composeCameraPose(const cv::Affine3f& _relativePose) { cameraPose = cameraPose * _relativePose; } + PoseConstraint& getConstraint(const int _id) + { + //! Creates constraints if doesn't exist yet + return constraints[_id]; + } + + public: + const int id; + cv::Affine3f pose; + cv::Affine3f cameraPose; + Constraints constraints; + + int startFrameId; + int stopFrameId; + //! 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 + +void Submap::integrate(InputArray _depth, float depthFactor, const cv::kinfu::Intr& intrinsics, + const int currFrameId) +{ + CV_Assert(currFrameId >= startFrameId); + volume.integrate(_depth, depthFactor, cameraPose.matrix, intrinsics, currFrameId); +} + +template +void Submap::raycast(const cv::Affine3f& _cameraPose, const cv::kinfu::Intr& intrinsics, cv::Size frameSize, + OutputArray points, OutputArray normals) +{ + volume.raycast(_cameraPose.matrix, intrinsics, frameSize, points, normals); +} + +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: + 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; + + 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 activeSubmaps.size(); }; + + Ptr getSubmap(int _id) const; + Ptr getCurrentSubmap(void) const; + + int estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose); + bool updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals); + + PoseGraph MapToPoseGraph(); + void PoseGraphToMap(const PoseGraph& updatedPoseGraph); + + VolumeParams volumeParams; + + std::vector> submapList; + IdToActiveSubmaps activeSubmaps; + + PoseGraph poseGraph; +}; + +template +int SubmapManager::createNewSubmap(bool isCurrentMap, int currFrameId, const Affine3f& pose) +{ + int newId = int(submapList.size()); + + 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; + + std::cout << "Created new submap\n"; + + return newId; +} + +template +Ptr> SubmapManager::getSubmap(int _id) const +{ + CV_Assert(submapList.size() > 0); + CV_Assert(_id >= 0 && _id < int(submapList.size())); + return submapList.at(_id); +} + +template +Ptr> SubmapManager::getCurrentSubmap(void) const +{ + for (const auto& it : activeSubmaps) + { + if (it.second.type == Type::CURRENT) + return getSubmap(it.first); + } + return nullptr; +} + +template +bool SubmapManager::shouldCreateSubmap(int currFrameId) +{ + int currSubmapId = -1; + for (const auto& it : activeSubmaps) + { + auto submapData = it.second; + // No more than 1 new submap at a time! + if (submapData.type == Type::NEW) + { + return false; + } + if (submapData.type == Type::CURRENT) + { + currSubmapId = it.first; + } + } + //! TODO: This shouldn't be happening? since there should always be one active current submap + if (currSubmapId < 0) + { + return false; + } + + Ptr currSubmap = getSubmap(currSubmapId); + float ratio = currSubmap->calcVisibilityRatio(currFrameId); + + std::cout << "Ratio: " << ratio << "\n"; + + if (ratio < 0.2f) + return true; + return false; +} + +template +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 { + float rAbs = abs(residual); + if (rAbs < thresh) + return 1.0; + float numerator = sqrt(2 * thresh * rAbs - thresh * thresh); + return numerator / rAbs; + }; + + 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::vector weights(fromSubmapData.constraints.size() + 1, 1.0f); + + Affine3f prevConstraint = fromSubmap->getConstraint(toSubmap->id).estimatedPose; + int prevWeight = fromSubmap->getConstraint(toSubmap->id).weight; + + // 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++) + { + Vec6f constraintVec; + for (int j = 0; j < int(weights.size() - 1); j++) + { + Affine3f currObservation = fromSubmapData.constraints[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 /= float(sumWeight); + + float residual = 0.0f; + float diff = 0.0f; + for (int j = 0; j < int(weights.size()); j++) + { + int w; + if (j == int(weights.size() - 1)) + { + cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec); + w = prevWeight; + } + else + { + Affine3f currObservation = fromSubmapData.constraints[j]; + cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec); + w = 1; + } + + cv::Vec6f residualVec = (constraintVec - meanConstraint); + residual = float(norm(residualVec)); + float newWeight = huberWeight(residual); + diff += w * abs(newWeight - weights[j]); + weights[j] = newWeight; + } + + if (diff / (prevWeight + weights.size() - 1) < CONVERGE_WEIGHT_THRESHOLD) + break; + } + + int localInliers = 0; + Matx44f inlierConstraint; + for (int i = 0; i < int(weights.size()); i++) + { + if (weights[i] > INLIER_WEIGHT_THRESH) + { + localInliers++; + 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; + + /* std::cout << inlierPose.matrix << "\n"; */ + /* std::cout << " inliers: " << inliers << "\n"; */ + + if (inliers >= MIN_INLIERS) + { + return 1; + } + if(fromSubmapData.trackingAttempts - inliers > (MAX_TRACKING_ATTEMPTS - MIN_INLIERS)) + { + return -1; + } + + return 0; +} + +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 +bool SubmapManager::updateMap(int _frameId, std::vector _framePoints, std::vector _frameNormals) +{ + bool mapUpdated = false; + int changedCurrentMapId = -1; + + const int currSubmapId = getCurrentSubmap()->id; + + for (auto& it : activeSubmaps) + { + int submapId = it.first; + auto& submapData = it.second; + if (submapData.type == Type::NEW || submapData.type == Type::LOOP_CLOSURE) + { + // Check with previous estimate + int inliers; + Affine3f inlierPose; + int constraintUpdate = estimateConstraint(submapId, currSubmapId, inliers, inlierPose); + std::cout << "SubmapId: " << submapId << " Tracking attempts: " << submapData.trackingAttempts << "\n"; + if (constraintUpdate == 1) + { + typename SubmapT::PoseConstraint& submapConstraint = getSubmap(submapId)->getConstraint(currSubmapId); + submapConstraint.accumulatePose(inlierPose, inliers); + std::cout << "Submap constraint estimated pose: \n" << submapConstraint.estimatedPose.matrix << "\n"; + submapData.constraints.clear(); + submapData.trackingAttempts = 0; + + if (shouldChangeCurrSubmap(_frameId, submapId)) + { + std::cout << "Should change current map to the new map\n"; + changedCurrentMapId = submapId; + } + mapUpdated = true; + } + else if(constraintUpdate == -1) + { + submapData.type = Type::LOST; + } + } + } + + std::vector createNewConstraintsList; + for (auto& it : activeSubmaps) + { + int submapId = it.first; + auto& submapData = it.second; + + if (submapId == changedCurrentMapId) + { + submapData.type = Type::CURRENT; + } + if ((submapData.type == Type::CURRENT) && (changedCurrentMapId >= 0) && (submapId != changedCurrentMapId)) + { + submapData.type = Type::LOST; + createNewConstraintsList.push_back(submapId); + } + 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); + } + } + + for (typename IdToActiveSubmaps::iterator it = activeSubmaps.begin(); it != activeSubmaps.end();) + { + auto& submapData = it->second; + if (submapData.type == Type::LOST) + it = activeSubmaps.erase(it); + else + it++; + } + + 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; + } + + 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; + } + + // Debugging only + 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"; + } + } + } + + return mapUpdated; +} + +template +PoseGraph SubmapManager::MapToPoseGraph() +{ + PoseGraph localPoseGraph; + + + 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); */ + Matx66f informationMatrix = Matx66f::eye(); + PoseGraphEdge currEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix); + 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(); + std::cout << "Current node: " << currSubmap->id << " Updated Pose: \n" << currSubmap->pose.matrix << std::endl; + } +} + +} // namespace kinfu +} // namespace cv +#endif /* ifndef __OPENCV_RGBD_SUBMAP_HPP__ */ diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 947428065..ff2a42f87 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -98,7 +98,7 @@ void TSDFVolumeCPU::reset() }); } -TsdfVoxel TSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const +TsdfVoxel TSDFVolumeCPU::at(const Vec3i& volumeIdx) const { //! Out of bounds if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || @@ -121,7 +121,7 @@ TsdfVoxel TSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const #if !USE_INTRINSICS static const bool fixMissingData = false; -static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) +static inline depthType bilinearDepth(const Depth& m, Point2f pt) { const depthType defaultValue = qnan; if(pt.x < 0 || pt.x >= m.cols-1 || @@ -455,7 +455,7 @@ struct IntegrateInvoker : ParallelLoopBody const Depth& depth; const Intr& intr; const Intr::Projector proj; - const cv::Affine3f vol2cam; + const Affine3f vol2cam; const float truncDistInv; const float dfac; TsdfVoxel* volDataStart; @@ -487,11 +487,11 @@ static cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics) } // use depth instead of distance (optimization) -void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const Intr& intrinsics) +void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, + 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(); @@ -513,7 +513,7 @@ void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const cv::Ma #if USE_INTRINSICS // all coordinate checks should be done in inclosing cycle -inline float TSDFVolumeCPU::interpolateVoxel(Point3f _p) const +inline float TSDFVolumeCPU::interpolateVoxel(const Point3f& _p) const { v_float32x4 p(_p.x, _p.y, _p.z, 0); return interpolateVoxel(p); @@ -560,7 +560,7 @@ inline float TSDFVolumeCPU::interpolateVoxel(const v_float32x4& p) const return v0 + tx*(v1 - v0); } #else -inline float TSDFVolumeCPU::interpolateVoxel(Point3f p) const +inline float TSDFVolumeCPU::interpolateVoxel(const Point3f& p) const { int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; @@ -594,7 +594,7 @@ inline float TSDFVolumeCPU::interpolateVoxel(Point3f p) const #if USE_INTRINSICS //gradientDeltaFactor is fixed at 1.0 of voxel size -inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f _p) const +inline Point3f TSDFVolumeCPU::getNormalVoxel(const Point3f& _p) const { v_float32x4 p(_p.x, _p.y, _p.z, 0.f); v_float32x4 result = getNormalVoxel(p); @@ -662,7 +662,7 @@ inline v_float32x4 TSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const return Norm.get0() < 0.0001f ? nanv : n/Norm; } #else -inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const +inline Point3f TSDFVolumeCPU::getNormalVoxel(const Point3f& p) const { const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const TsdfVoxel* volData = volume.ptr(); @@ -994,8 +994,8 @@ struct RaycastInvoker : ParallelLoopBody }; -void TSDFVolumeCPU::raycast(const cv::Matx44f& cameraPose, const Intr& intrinsics, Size frameSize, - cv::OutputArray _points, cv::OutputArray _normals) const +void TSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrinsics, const Size& frameSize, + OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); @@ -1189,7 +1189,7 @@ void TSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const ///////// GPU implementation ///////// #ifdef HAVE_OPENCL -TSDFVolumeGPU::TSDFVolumeGPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, +TSDFVolumeGPU::TSDFVolumeGPU(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution) : TSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution, false) { @@ -1251,24 +1251,25 @@ static cv::UMat preCalculationPixNormGPU(int depth_rows, int depth_cols, Vec2f f // use depth instead of distance (optimization) void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, - const cv::Matx44f& cameraPose, const Intr& intrinsics) + const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); + CV_UNUSED(frameId); CV_Assert(!_depth.empty()); UMat depth = _depth.getUMat(); - cv::String errorStr; - cv::String name = "integrate"; + String errorStr; + String name = "integrate"; ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; + String options = "-cl-mad-enable"; ocl::Kernel k; k.create(name.c_str(), source, options, &errorStr); if(k.empty()) throw std::runtime_error("Failed to create kernel: " + errorStr); - cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); + Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose); float dfac = 1.f/depthFactor; Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z); Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy); @@ -1308,17 +1309,17 @@ void TSDFVolumeGPU::integrate(InputArray _depth, float depthFactor, } -void TSDFVolumeGPU::raycast(const cv::Matx44f& cameraPose, const Intr& intrinsics, Size frameSize, - cv::OutputArray _points, cv::OutputArray _normals) const +void TSDFVolumeGPU::raycast(const Matx44f& cameraPose, const Intr& intrinsics, const Size& frameSize, + OutputArray _points, OutputArray _normals) const { CV_TRACE_FUNCTION(); CV_Assert(frameSize.area() > 0); - cv::String errorStr; - cv::String name = "raycast"; + String errorStr; + String name = "raycast"; ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; + String options = "-cl-mad-enable"; ocl::Kernel k; k.create(name.c_str(), source, options, &errorStr); @@ -1384,10 +1385,10 @@ void TSDFVolumeGPU::fetchNormals(InputArray _points, OutputArray _normals) const _normals.createSameSize(_points, POINT_TYPE); UMat normals = _normals.getUMat(); - cv::String errorStr; - cv::String name = "getNormals"; + String errorStr; + String name = "getNormals"; ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; + String options = "-cl-mad-enable"; ocl::Kernel k; k.create(name.c_str(), source, options, &errorStr); @@ -1432,9 +1433,9 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) ocl::Kernel kscan; - cv::String errorStr; + String errorStr; ocl::ProgramSource source = ocl::rgbd::tsdf_oclsrc; - cv::String options = "-cl-mad-enable"; + String options = "-cl-mad-enable"; kscan.create("scanSize", source, options, &errorStr); @@ -1485,7 +1486,7 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) throw std::runtime_error("Failed to run kernel"); Mat groupedSumCpu = groupedSum.getMat(ACCESS_READ); - int gpuSum = (int)cv::sum(groupedSumCpu)[0]; + int gpuSum = (int)sum(groupedSumCpu)[0]; // should be no CPU copies when new kernel is executing groupedSumCpu.release(); @@ -1541,16 +1542,28 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray points, OutputArray normals) #endif -cv::Ptr makeTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, +Ptr makeTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution) { #ifdef HAVE_OPENCL - if (cv::ocl::useOpenCL()) - return cv::makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, - _resolution); -#endif - return cv::makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + if (ocl::useOpenCL()) + return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution); +#endif + return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, + _resolution); +} + +Ptr makeTSDFVolume(const VolumeParams& _params) +{ +#ifdef HAVE_OPENCL + if (ocl::useOpenCL()) + return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, + _params.tsdfTruncDist, _params.maxWeight, _params.resolution); +#endif + return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, + _params.tsdfTruncDist, _params.maxWeight, _params.resolution); + } } // namespace kinfu diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index a4017c73a..cd38ee80b 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/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_KINFU_TSDF_H__ #define __OPENCV_KINFU_TSDF_H__ @@ -35,7 +36,7 @@ class TSDFVolume : public Volume { public: // dimension in voxels, size in meters - TSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, + TSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true); virtual ~TSDFVolume() = default; @@ -57,20 +58,19 @@ class TSDFVolumeCPU : public TSDFVolume TSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Vec3i _resolution, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics) override; - virtual void raycast(const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray points, - cv::OutputArray normals) const override; + 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, const Size& frameSize, + OutputArray points, 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 fetchNormals(InputArray points, OutputArray _normals) const override; + virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; virtual void reset() override; - virtual TsdfVoxel at(const cv::Vec3i& volumeIdx) const; + virtual TsdfVoxel at(const Vec3i& volumeIdx) const; - float interpolateVoxel(cv::Point3f p) const; - Point3f getNormalVoxel(cv::Point3f p) const; + float interpolateVoxel(const cv::Point3f& p) const; + Point3f getNormalVoxel(const cv::Point3f& p) const; #if USE_INTRINSICS float interpolateVoxel(const v_float32x4& p) const; @@ -89,17 +89,16 @@ class TSDFVolumeGPU : public TSDFVolume { public: // dimension in voxels, size in meters - TSDFVolumeGPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, + TSDFVolumeGPU(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution); - virtual void integrate(InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics) override; - virtual void raycast(const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, - cv::Size frameSize, cv::OutputArray _points, - cv::OutputArray _normals) const override; + 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, const Size& frameSize, + OutputArray _points, 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 fetchPointsNormals(OutputArray points, OutputArray normals) const override; + virtual void fetchNormals(InputArray points, OutputArray normals) const override; virtual void reset() override; @@ -112,8 +111,9 @@ class TSDFVolumeGPU : public TSDFVolume UMat volume; }; #endif -cv::Ptr makeTSDFVolume(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, - float _truncDist, int _maxWeight, Point3i _resolution); +Ptr makeTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, Point3i _resolution); +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 b7febed57..0b9636753 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 5d83ad5c1..8177213e8 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -2,19 +2,79 @@ // 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" +#include "opencv2/core/base.hpp" +#include "precomp.hpp" +#include "tsdf.hpp" namespace cv { namespace kinfu { -cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Matx44f _pose, - float _raycastStepFactor, float _truncDist, int _maxWeight, - float _truncateThreshold, Vec3i _resolution) +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)); + if(params.type == VolumeType::TSDF) + { + 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); + } + 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) +{ + Ptr params = defaultParams(_volumeType); + + params->raycastStepFactor = 0.75f; + float volumeSize = 3.0f; + 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) + { + params->voxelSize = volumeSize / 128.f; + params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters + return params; + } + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); +} + +Ptr makeVolume(const VolumeParams& _volumeParams) +{ + 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, + float _raycastStepFactor, float _truncDist, int _maxWeight, + float _truncateThreshold, Vec3i _resolution) { Point3i _presolution = _resolution; if (_volumeType == VolumeType::TSDF) @@ -24,11 +84,10 @@ cv::Ptr makeVolume(VolumeType _volumeType, float _voxelSize, cv::Matx44f } else if (_volumeType == VolumeType::HASHTSDF) { - return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, - _truncateThreshold); + return makeHashTSDFVolume( + _voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); } - else - return nullptr; + CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } } // namespace kinfu