From 5ed1b73d04483892e7c38a3321be18bf5652cda6 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 24 Feb 2021 19:37:28 +0300 Subject: [PATCH 01/55] create dummy unworked classes --- .../include/opencv2/rgbd/colored_kinfu.hpp | 252 ++++++++ modules/rgbd/samples/colored_kinfu_demo.cpp | 278 ++++++++ modules/rgbd/src/colored_kinfu.cpp | 346 ++++++++++ modules/rgbd/src/colored_tsdf.cpp | 594 ++++++++++++++++++ modules/rgbd/src/colored_tsdf.hpp | 59 ++ modules/rgbd/src/volume.cpp | 3 +- 6 files changed, 1531 insertions(+), 1 deletion(-) create mode 100644 modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp create mode 100644 modules/rgbd/samples/colored_kinfu_demo.cpp create mode 100644 modules/rgbd/src/colored_kinfu.cpp create mode 100644 modules/rgbd/src/colored_tsdf.cpp create mode 100644 modules/rgbd/src/colored_tsdf.hpp diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp new file mode 100644 index 00000000000..f55d7120fe1 --- /dev/null +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -0,0 +1,252 @@ +// 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_COLORED_KINFU_HPP__ +#define __OPENCV_RGBD_COLORED_KINFU_HPP__ + +#include "opencv2/core.hpp" +#include "opencv2/core/affine.hpp" +#include + +namespace cv { +namespace kinfu { +//! @addtogroup kinect_fusion +//! @{ + +struct CV_EXPORTS_W Params +{ + + CV_WRAP Params(){} + + /** + * @brief Constructor for Params + * Sets the initial pose of the TSDF volume. + * @param volumeInitialPoseRot rotation matrix + * @param volumeInitialPoseTransl translation vector + */ + CV_WRAP Params(Matx33f volumeInitialPoseRot, Vec3f volumeInitialPoseTransl) + { + setInitialVolumePose(volumeInitialPoseRot,volumeInitialPoseTransl); + } + + /** + * @brief Constructor for Params + * Sets the initial pose of the TSDF volume. + * @param volumeInitialPose 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume + */ + CV_WRAP Params(Matx44f volumeInitialPose) + { + setInitialVolumePose(volumeInitialPose); + } + + /** + * @brief Set Initial Volume Pose + * Sets the initial pose of the TSDF volume. + * @param R rotation matrix + * @param t translation vector + */ + CV_WRAP void setInitialVolumePose(Matx33f R, Vec3f t); + + /** + * @brief Set Initial Volume Pose + * Sets the initial pose of the TSDF volume. + * @param homogen_tf 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume + */ + CV_WRAP void setInitialVolumePose(Matx44f homogen_tf); + + /** + * @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; + + CV_PROP_RW kinfu::VolumeType volumeType; + + /** @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 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_RW 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_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 KinectFusion implementation + + This class implements a 3d reconstruction algorithm described in + @cite kinectfusion paper. + + It takes a sequence of depth images taken from depth sensor + (or any depth images source such as stereo camera matching algorithm or even raymarching renderer). + The output can be obtained as a vector of points and their normals + or can be Phong-rendered from given camera pose. + + An internal representation of a model is a voxel cuboid that keeps TSDF values + which are a sort of distances to the surface (for details read the @cite kinectfusion article about TSDF). + There is no interface to that representation yet. + + KinFu uses OpenCL acceleration automatically if available. + To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL(). + + This implementation is based on [kinfu-remake](https://github.com/Nerei/kinfu_remake). + + Note that the KinectFusion algorithm was patented and its use may be restricted by + the list of patents mentioned in README.md file in this module directory. + + That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion. +*/ +class CV_EXPORTS_W KinFu +{ +public: + CV_WRAP static Ptr create(const Ptr& _params); + virtual ~KinFu(); + + /** @brief Get current parameters */ + virtual const Params& getParams() const = 0; + + /** @brief Renders a volume into an image + + Renders a 0-surface of TSDF using Phong shading into a CV_8UC4 Mat. + Light pose is fixed in KinFu params. + + @param image resulting image + @param cameraPose pose of camera to render from. If empty then render from current pose + which is a last frame camera pose. + */ + + CV_WRAP virtual void render(OutputArray image, const Matx44f& cameraPose = Matx44f::eye()) const = 0; + + /** @brief Gets points and normals of current 3d mesh + + The order of normals corresponds to order of points. + The order of points is undefined. + + @param points vector of points which are 4-float vectors + @param normals vector of normals which are 4-float vectors + */ + CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0; + + /** @brief Gets points of current 3d mesh + + The order of points is undefined. + + @param points vector of points which are 4-float vectors + */ + CV_WRAP virtual void getPoints(OutputArray points) const = 0; + + /** @brief Calculates normals for given points + @param points input vector of points which are 4-float vectors + @param normals output vector of corresponding normals which are 4-float vectors + */ + CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0; + + /** @brief Resets the algorithm + + Clears current model and resets a pose. + */ + CV_WRAP virtual void reset() = 0; + + /** @brief Get current pose in voxel space */ + virtual const Affine3f getPose() const = 0; + + /** @brief Process next depth frame + + Integrates depth into voxel space with respect to its ICP-calculated pose. + Input image is converted to CV_32F internally if has another type. + + @param depth one-channel image which size and depth scale is described in algorithm's parameters + @return true if succeeded to align new frame with current scene, false if opposite + */ + CV_WRAP virtual bool update(InputArray depth) = 0; +}; + +//! @} +} +} +#endif diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp new file mode 100644 index 00000000000..6ae7674a37a --- /dev/null +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -0,0 +1,278 @@ +// 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::colored_kinfu; +using namespace cv::io_utils; + +#ifdef HAVE_OPENCV_VIZ +#include +#endif + +#ifdef HAVE_OPENCV_VIZ +const std::string vizWindowName = "cloud"; + +struct PauseCallbackArgs +{ + PauseCallbackArgs(KinFu& _kf) : kf(_kf) + { } + + KinFu& kf; +}; + +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.kf.render(rendered, window.getViewerPose().matrix); + imshow("render", rendered); + waitKey(1); + } +} +#endif + +static const char* keys = +{ + "{help h usage ? | | print this message }" + "{depth | | Path to depth.txt file listing a set of depth images }" + "{camera |0| Index of depth camera to be used as a depth source }" + "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," + " in coarse mode points and normals are displayed }" + "{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}" + "{idle | | Do not run KinFu, just display depth frames }" + "{record | | Write depth frames to specified file list" + " (the same format as for the 'depth' key) }" +}; + +static const std::string message = + "\nThis demo uses live depth input or RGB-D dataset taken from" + "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset" + "\nto demonstrate KinectFusion implementation \n"; + + +int main(int argc, char **argv) +{ + bool coarse = false; + bool idle = false; + bool useHashTSDF = false; + 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("useHashTSDF")) + { + useHashTSDF = true; + } + if(parser.has("idle")) + { + idle = true; + } + + Ptr ds; + if (parser.has("depth")) + ds = makePtr(parser.get("depth")); + else + ds = makePtr(parser.get("camera")); + + if (ds->empty()) + { + std::cerr << "Failed to open depth source" << std::endl; + parser.printMessage(); + return -1; + } + + Ptr depthWriter; + if(!recordPath.empty()) + depthWriter = makePtr(recordPath); + + Ptr params; + Ptr kf; + + if(coarse) + params = Params::coarseParams(); + else + params = Params::defaultParams(); + + if(useHashTSDF) + params = Params::hashTSDFParams(coarse); + + // These params can be different for each depth sensor + ds->updateParams(*params); + + // Enables OpenCL explicitly (by default can be switched-off) + cv::setUseOptimized(true); + + // Scene-specific params should be tuned for each scene individually + //float cubeSize = 1.f; + //params->voxelSize = cubeSize/params->volumeDims[0]; //meters + //params->tsdf_trunc_dist = 0.01f; //meters + //params->icpDistThresh = 0.01f; //meters + //params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters + //params->tsdf_max_weight = 16; + + if(!idle) + kf = KinFu::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 + kf->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 = kf->getParams().voxelSize*Vec3d(kf->getParams().volumeDims); + window.showWidget("cube", viz::WCube(Vec3d::all(0), + volSize), + kf->getParams().volumePose); + PauseCallbackArgs pca(*kf); + window.registerMouseCallback(pauseCallback, (void*)&pca); + window.showWidget("text", viz::WText(cv::String("Move camera in this window. " + "Close the window or press Q to resume"), Point())); + window.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(!kf->update(frame)) + { + kf->reset(); + std::cout << "reset" << std::endl; + } +#ifdef HAVE_OPENCV_VIZ + else + { + if(coarse) + { + kf->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 = kf->getParams().voxelSize*kf->getParams().volumeDims; + window.showWidget("cube", viz::WCube(Vec3d::all(0), + volSize), + kf->getParams().volumePose); + window.setViewerPose(kf->getPose()); + window.spinOnce(1, true); + } +#endif + + kf->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) + kf->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/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp new file mode 100644 index 00000000000..8755211d6b5 --- /dev/null +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -0,0 +1,346 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory + +#include "precomp.hpp" +#include "fast_icp.hpp" +#include "tsdf.hpp" +#include "hash_tsdf.hpp" +#include "colored_tsdf.hpp" +#include "kinfu_frame.hpp" + +namespace cv { +namespace kinfu { + +void Params::setInitialVolumePose(Matx33f R, Vec3f t) +{ + setInitialVolumePose(Affine3f(R,t).matrix); +} + +void Params::setInitialVolumePose(Matx44f homogen_tf) +{ + Params::volumePose.matrix = homogen_tf; +} + +Ptr Params::defaultParams() +{ + Params p; + + p.frameSize = Size(640, 480); + + p.volumeType = VolumeType::TSDF; + + 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 = 7 * p.voxelSize; // about 0.04f in 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->tsdf_trunc_dist = 2 * p->voxelSize; // 0.04f in meters + + p->raycast_step_factor = 0.75f; //in voxel sizes + + return p; +} +Ptr Params::hashTSDFParams(bool isCoarse) +{ + Ptr p; + if(isCoarse) + p = coarseParams(); + else + p = defaultParams(); + p->volumeType = VolumeType::HASHTSDF; + p->truncateThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); + return p; +} + +// MatType should be Mat or UMat +template< typename MatType> +class ColoredKinFuImpl : public KinFu +{ +public: + ColoredKinFuImpl(const Params& _params); + virtual ~ColoredKinFuImpl(); + + 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; + cv::Ptr volume; + + int frameCounter; + Matx44f pose; + std::vector pyrPoints; + std::vector pyrNormals; +}; + + +template< typename MatType > +ColoredKinFuImpl::ColoredKinFuImpl(const Params &_params) : + params(_params), + icp(makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)), + pyrPoints(), pyrNormals() +{ + volume = makeVolume(params.volumeType, params.voxelSize, params.volumePose.matrix, params.raycast_step_factor, + params.tsdf_trunc_dist, params.tsdf_max_weight, params.truncateThreshold, params.volumeDims); + reset(); +} + +template< typename MatType > +void ColoredKinFuImpl::reset() +{ + frameCounter = 0; + pose = Affine3f::Identity().matrix; + volume->reset(); +} + +template< typename MatType > +ColoredKinFuImpl::~ColoredKinFuImpl() +{ } + +template< typename MatType > +const Params& ColoredKinFuImpl::getParams() const +{ + return params; +} + +template< typename MatType > +const Affine3f ColoredKinFuImpl::getPose() const +{ + return pose; +} + + +template<> +bool ColoredKinFuImpl::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 ColoredKinFuImpl::update(InputArray _depth) +{ + CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); + + UMat depth; + if(!_depth.isUMat()) + { + _depth.copyTo(depth); + return updateT(depth); + } + else + { + return updateT(_depth.getUMat()); + } +} + + +template< typename MatType > +bool ColoredKinFuImpl::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); + if(frameCounter == 0) + { + // use depth instead of distance + volume->integrate(depth, params.depthFactor, pose, params.intr); + pyrPoints = newPoints; + pyrNormals = newNormals; + } + else + { + Affine3f affine; + bool success = icp->estimateTransform(affine, pyrPoints, pyrNormals, newPoints, newNormals); + if(!success) + return false; + + pose = (Affine3f(pose) * affine).matrix; + + float rnorm = (float)cv::norm(affine.rvec()); + float tnorm = (float)cv::norm(affine.translation()); + // We do not integrate volume if camera does not move + if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement) + { + // use depth instead of distance + volume->integrate(depth, params.depthFactor, pose, params.intr); + } + MatType& points = pyrPoints [0]; + MatType& normals = pyrNormals[0]; + volume->raycast(pose, params.intr, params.frameSize, points, normals); + buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, + params.pyramidLevels); + } + + frameCounter++; + return true; +} + + +template< typename MatType > +void ColoredKinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const +{ + CV_TRACE_FUNCTION(); + + Affine3f cameraPose(_cameraPose); + Affine3f _pose(pose); + + const Affine3f id = Affine3f::Identity(); + if((cameraPose.rotation() == _pose.rotation() && cameraPose.translation() == _pose.translation()) || + (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) + { + renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose); + } + else + { + MatType points, normals; + volume->raycast(_cameraPose, params.intr, params.frameSize, points, normals); + renderPointsNormals(points, normals, image, params.lightPose); + } +} + + +template< typename MatType > +void ColoredKinFuImpl::getCloud(OutputArray p, OutputArray n) const +{ + volume->fetchPointsNormals(p, n); +} + + +template< typename MatType > +void ColoredKinFuImpl::getPoints(OutputArray points) const +{ + volume->fetchPointsNormals(points, noArray()); +} + + +template< typename MatType > +void ColoredKinFuImpl::getNormals(InputArray points, OutputArray normals) const +{ + volume->fetchNormals(points, normals); +} + +// importing class + +#ifdef OPENCV_ENABLE_NONFREE + +Ptr KinFu::create(const Ptr& params) +{ + CV_Assert((int)params->icpIterations.size() == params->pyramidLevels); + CV_Assert(params->intr(0,1) == 0 && params->intr(1,0) == 0 && params->intr(2,0) == 0 && params->intr(2,1) == 0 && params->intr(2,2) == 1); +#ifdef HAVE_OPENCL + if(cv::ocl::useOpenCL()) + return makePtr< ColoredKinFuImpl >(*params); +#endif + return makePtr< ColoredKinFuImpl >(*params); +} + +#else +Ptr KinFu::create(const Ptr& /* params */) +{ + CV_Error(Error::StsNotImplemented, + "This algorithm is patented and is excluded in this configuration; " + "Set OPENCV_ENABLE_NONFREE CMake option and rebuild the library"); +} +#endif + +KinFu::~KinFu() {} + +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp new file mode 100644 index 00000000000..133557d3109 --- /dev/null +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -0,0 +1,594 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory + +#include "precomp.hpp" +#include "colored_tsdf.hpp" +#include "tsdf_functions.hpp" +#include "opencl_kernels_rgbd.hpp" + +namespace cv { + +namespace kinfu { + +ColoredTSDFVolume::ColoredTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, Point3i _resolution, bool zFirstMemOrder) + : Volume(_voxelSize, _pose, _raycastStepFactor), + volResolution(_resolution), + maxWeight( WeightType(_maxWeight) ) +{ + CV_Assert(_maxWeight < 255); + // Unlike original code, this should work with any volume size + // Not only when (x,y,z % 32) == 0 + volSize = Point3f(volResolution) * voxelSize; + truncDist = std::max(_truncDist, 2.1f * voxelSize); + + // (xRes*yRes*zRes) array + // Depending on zFirstMemOrder arg: + // &elem(x, y, z) = data + x*zRes*yRes + y*zRes + z; + // &elem(x, y, z) = data + x + y*xRes + z*xRes*yRes; + int xdim, ydim, zdim; + if(zFirstMemOrder) + { + xdim = volResolution.z * volResolution.y; + ydim = volResolution.z; + zdim = 1; + } + else + { + xdim = 1; + ydim = volResolution.x; + zdim = volResolution.x * volResolution.y; + } + + volDims = Vec4i(xdim, ydim, zdim); + neighbourCoords = Vec8i( + volDims.dot(Vec4i(0, 0, 0)), + volDims.dot(Vec4i(0, 0, 1)), + volDims.dot(Vec4i(0, 1, 0)), + volDims.dot(Vec4i(0, 1, 1)), + volDims.dot(Vec4i(1, 0, 0)), + volDims.dot(Vec4i(1, 0, 1)), + volDims.dot(Vec4i(1, 1, 0)), + volDims.dot(Vec4i(1, 1, 1)) + ); +} + +class ColoredTSDFVolumeCPU : public TSDFVolume +{ +public: + // dimension in voxels, size in meters + ColoredTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, Vec3i _resolution, bool zFirstMemOrder = true); + + 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(InputArray points, OutputArray _normals) const override; + virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; + + virtual void reset() override; + virtual TsdfVoxel at(const Vec3i& volumeIdx) const; + + float interpolateVoxel(const cv::Point3f& p) const; + Point3f getNormalVoxel(const cv::Point3f& p) const; + + Vec4i volStrides; + Vec6f frameParams; + Mat pixNorms; + // See zFirstMemOrder arg of parent class constructor + // for the array layout info + // Consist of Voxel elements + Mat volume; +}; + +// dimension in voxels, size in meters +ColoredTSDFVolumeCPU::ColoredTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, Vec3i _resolution, + bool zFirstMemOrder) + : TSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution, + zFirstMemOrder) +{ + int xdim, ydim, zdim; + if (zFirstMemOrder) + { + xdim = volResolution.z * volResolution.y; + ydim = volResolution.z; + zdim = 1; + } + else + { + xdim = 1; + ydim = volResolution.x; + zdim = volResolution.x * volResolution.y; + } + volStrides = Vec4i(xdim, ydim, zdim); + + volume = Mat(1, volResolution.x * volResolution.y * volResolution.z, rawType()); + + reset(); +} + +// zero volume, leave rest params the same +void ColoredTSDFVolumeCPU::reset() +{ + CV_TRACE_FUNCTION(); + + volume.forEach([](VecTsdfVoxel& vv, const int* /* position */) + { + TsdfVoxel& v = reinterpret_cast(vv); + v.tsdf = floatToTsdf(0.0f); v.weight = 0; + }); +} + +TsdfVoxel ColoredTSDFVolumeCPU::at(const Vec3i& volumeIdx) const +{ + //! Out of bounds + if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || + (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || + (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) + { + return TsdfVoxel(floatToTsdf(1.f), 0); + } + + const TsdfVoxel* volData = volume.ptr(); + int coordBase = + volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; + return volData[coordBase]; +} + +// use depth instead of distance (optimization) +void ColoredTSDFVolumeCPU::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(); + + Vec6f newParams((float)depth.rows, (float)depth.cols, + intrinsics.fx, intrinsics.fy, + intrinsics.cx, intrinsics.cy); + if (!(frameParams == newParams)) + { + frameParams = newParams; + pixNorms = preCalculationPixNorm(depth, intrinsics); + } + + integrateVolumeUnit(truncDist, voxelSize, maxWeight, (this->pose).matrix, volResolution, volStrides, depth, + depthFactor, cameraPose, intrinsics, pixNorms, volume); +} + + +inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& p) const +{ + int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; + + int ix = cvFloor(p.x); + int iy = cvFloor(p.y); + int iz = cvFloor(p.z); + + float tx = p.x - ix; + float ty = p.y - iy; + float tz = p.z - iz; + + int coordBase = ix*xdim + iy*ydim + iz*zdim; + const TsdfVoxel* volData = volume.ptr(); + + float vx[8]; + for (int i = 0; i < 8; i++) + vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase].tsdf); + + float v00 = vx[0] + tz*(vx[1] - vx[0]); + float v01 = vx[2] + tz*(vx[3] - vx[2]); + float v10 = vx[4] + tz*(vx[5] - vx[4]); + float v11 = vx[6] + tz*(vx[7] - vx[6]); + + float v0 = v00 + ty*(v01 - v00); + float v1 = v10 + ty*(v11 - v10); + + return v0 + tx*(v1 - v0); + +} + +inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& p) const +{ + const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; + const TsdfVoxel* volData = volume.ptr(); + + if(p.x < 1 || p.x >= volResolution.x - 2 || + p.y < 1 || p.y >= volResolution.y - 2 || + p.z < 1 || p.z >= volResolution.z - 2) + return nan3; + + int ix = cvFloor(p.x); + int iy = cvFloor(p.y); + int iz = cvFloor(p.z); + + float tx = p.x - ix; + float ty = p.y - iy; + float tz = p.z - iz; + + int coordBase = ix*xdim + iy*ydim + iz*zdim; + + Vec3f an; + for(int c = 0; c < 3; c++) + { + const int dim = volDims[c]; + float& nv = an[c]; + + float vx[8]; + for(int i = 0; i < 8; i++) + vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - + volData[neighbourCoords[i] + coordBase - 1*dim].tsdf); + + float v00 = vx[0] + tz*(vx[1] - vx[0]); + float v01 = vx[2] + tz*(vx[3] - vx[2]); + float v10 = vx[4] + tz*(vx[5] - vx[4]); + float v11 = vx[6] + tz*(vx[7] - vx[6]); + + float v0 = v00 + ty*(v01 - v00); + float v1 = v10 + ty*(v11 - v10); + + nv = v0 + tx*(v1 - v0); + } + + float nv = sqrt(an[0] * an[0] + + an[1] * an[1] + + an[2] * an[2]); + return nv < 0.0001f ? nan3 : an / nv; +} + + +struct RaycastInvoker : ParallelLoopBody +{ + RaycastInvoker(Points& _points, Normals& _normals, const Matx44f& cameraPose, + const Intr& intrinsics, const ColoredTSDFVolumeCPU& _volume) : + ParallelLoopBody(), + points(_points), + normals(_normals), + volume(_volume), + tstep(volume.truncDist * volume.raycastStepFactor), + // We do subtract voxel size to minimize checks after + // Note: origin of volume coordinate is placed + // in the center of voxel (0,0,0), not in the corner of the voxel! + boxMax(volume.volSize - Point3f(volume.voxelSize, + volume.voxelSize, + volume.voxelSize)), + boxMin(), + cam2vol(volume.pose.inv() * Affine3f(cameraPose)), + vol2cam(Affine3f(cameraPose.inv()) * volume.pose), + reproj(intrinsics.makeReprojector()) + { } + + virtual void operator() (const Range& range) const override + { + const Point3f camTrans = cam2vol.translation(); + const Matx33f camRot = cam2vol.rotation(); + const Matx33f volRot = vol2cam.rotation(); + + for(int y = range.start; y < range.end; y++) + { + ptype* ptsRow = points[y]; + ptype* nrmRow = normals[y]; + + for(int x = 0; x < points.cols; x++) + { + Point3f point = nan3, normal = nan3; + + Point3f orig = camTrans; + // direction through pixel in volume space + Point3f dir = normalize(Vec3f(camRot * reproj(Point3f(float(x), float(y), 1.f)))); + + // compute intersection of ray with all six bbox planes + Vec3f rayinv(1.f/dir.x, 1.f/dir.y, 1.f/dir.z); + Point3f tbottom = rayinv.mul(boxMin - orig); + Point3f ttop = rayinv.mul(boxMax - orig); + + // re-order intersections to find smallest and largest on each axis + Point3f minAx(min(ttop.x, tbottom.x), min(ttop.y, tbottom.y), min(ttop.z, tbottom.z)); + Point3f maxAx(max(ttop.x, tbottom.x), max(ttop.y, tbottom.y), max(ttop.z, tbottom.z)); + + // near clipping plane + const float clip = 0.f; + //float tmin = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip); + //float tmax = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z)); + float tmin = max({ minAx.x, minAx.y, minAx.z, clip }); + float tmax = min({ maxAx.x, maxAx.y, maxAx.z }); + + // precautions against getting coordinates out of bounds + tmin = tmin + tstep; + tmax = tmax - tstep; + + if(tmin < tmax) + { + // interpolation optimized a little + orig = orig*volume.voxelSizeInv; + dir = dir*volume.voxelSizeInv; + + Point3f rayStep = dir * tstep; + Point3f next = (orig + dir * tmin); + float f = volume.interpolateVoxel(next), fnext = f; + + //raymarch + int steps = 0; + int nSteps = int(floor((tmax - tmin)/tstep)); + for(; steps < nSteps; steps++) + { + next += rayStep; + int xdim = volume.volDims[0]; + int ydim = volume.volDims[1]; + int zdim = volume.volDims[2]; + int ix = cvRound(next.x); + int iy = cvRound(next.y); + int iz = cvRound(next.z); + fnext = tsdfToFloat(volume.volume.at(ix*xdim + iy*ydim + iz*zdim).tsdf); + if(fnext != f) + { + fnext = volume.interpolateVoxel(next); + // when ray crosses a surface + if(std::signbit(f) != std::signbit(fnext)) + break; + + f = fnext; + } + } + // if ray penetrates a surface from outside + // linearly interpolate t between two f values + if(f > 0.f && fnext < 0.f) + { + Point3f tp = next - rayStep; + float ft = volume.interpolateVoxel(tp); + float ftdt = volume.interpolateVoxel(next); + // float t = tmin + steps*tstep; + // float ts = t - tstep*ft/(ftdt - ft); + float ts = tmin + tstep*(steps - ft/(ftdt - ft)); + + // avoid division by zero + if(!cvIsNaN(ts) && !cvIsInf(ts)) + { + Point3f pv = (orig + dir*ts); + Point3f nv = volume.getNormalVoxel(pv); + + if(!isNaN(nv)) + { + //convert pv and nv to camera space + normal = volRot * nv; + // interpolation optimized a little + point = vol2cam * (pv*volume.voxelSize); + } + } + } + } + ptsRow[x] = toPtype(point); + nrmRow[x] = toPtype(normal); + } + } + } + + Points& points; + Normals& normals; + const ColoredTSDFVolumeCPU& volume; + + const float tstep; + + const Point3f boxMax; + const Point3f boxMin; + + const Affine3f cam2vol; + const Affine3f vol2cam; + const Intr::Reprojector reproj; +}; + + +void ColoredTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrinsics, const Size& frameSize, + OutputArray _points, OutputArray _normals) const +{ + CV_TRACE_FUNCTION(); + + CV_Assert(frameSize.area() > 0); + + _points.create (frameSize, POINT_TYPE); + _normals.create(frameSize, POINT_TYPE); + + Points points = _points.getMat(); + Normals normals = _normals.getMat(); + + RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); + + const int nstripes = -1; + parallel_for_(Range(0, points.rows), ri, nstripes); +} + + +struct FetchPointsNormalsInvoker : ParallelLoopBody +{ + FetchPointsNormalsInvoker(const ColoredTSDFVolumeCPU& _volume, + std::vector>& _pVecs, + std::vector>& _nVecs, + bool _needNormals) : + ParallelLoopBody(), + vol(_volume), + pVecs(_pVecs), + nVecs(_nVecs), + needNormals(_needNormals) + { + volDataStart = vol.volume.ptr(); + } + + inline void coord(std::vector& points, std::vector& normals, + int x, int y, int z, Point3f V, float v0, int axis) const + { + // 0 for x, 1 for y, 2 for z + bool limits = false; + Point3i shift; + float Vc = 0.f; + if(axis == 0) + { + shift = Point3i(1, 0, 0); + limits = (x + 1 < vol.volResolution.x); + Vc = V.x; + } + if(axis == 1) + { + shift = Point3i(0, 1, 0); + limits = (y + 1 < vol.volResolution.y); + Vc = V.y; + } + if(axis == 2) + { + shift = Point3i(0, 0, 1); + limits = (z + 1 < vol.volResolution.z); + Vc = V.z; + } + + if(limits) + { + const TsdfVoxel& voxeld = volDataStart[(x+shift.x)*vol.volDims[0] + + (y+shift.y)*vol.volDims[1] + + (z+shift.z)*vol.volDims[2]]; + float vd = tsdfToFloat(voxeld.tsdf); + + if(voxeld.weight != 0 && vd != 1.f) + { + if((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0)) + { + //linearly interpolate coordinate + float Vn = Vc + vol.voxelSize; + float dinv = 1.f/(abs(v0)+abs(vd)); + float inter = (Vc*abs(vd) + Vn*abs(v0))*dinv; + + Point3f p(shift.x ? inter : V.x, + shift.y ? inter : V.y, + shift.z ? inter : V.z); + { + points.push_back(toPtype(vol.pose * p)); + if(needNormals) + normals.push_back(toPtype(vol.pose.rotation() * + vol.getNormalVoxel(p*vol.voxelSizeInv))); + } + } + } + } + } + + virtual void operator() (const Range& range) const override + { + std::vector points, normals; + for(int x = range.start; x < range.end; x++) + { + const TsdfVoxel* volDataX = volDataStart + x*vol.volDims[0]; + for(int y = 0; y < vol.volResolution.y; y++) + { + const TsdfVoxel* volDataY = volDataX + y*vol.volDims[1]; + for(int z = 0; z < vol.volResolution.z; z++) + { + const TsdfVoxel& voxel0 = volDataY[z*vol.volDims[2]]; + float v0 = tsdfToFloat(voxel0.tsdf); + if(voxel0.weight != 0 && v0 != 1.f) + { + Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f)*vol.voxelSize); + + coord(points, normals, x, y, z, V, v0, 0); + coord(points, normals, x, y, z, V, v0, 1); + coord(points, normals, x, y, z, V, v0, 2); + + } // if voxel is not empty + } + } + } + + AutoLock al(mutex); + pVecs.push_back(points); + nVecs.push_back(normals); + } + + const ColoredTSDFVolumeCPU& vol; + std::vector>& pVecs; + std::vector>& nVecs; + const TsdfVoxel* volDataStart; + bool needNormals; + mutable Mutex mutex; +}; + +void ColoredTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const +{ + CV_TRACE_FUNCTION(); + + if(_points.needed()) + { + std::vector> pVecs, nVecs; + FetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed()); + Range range(0, volResolution.x); + const int nstripes = -1; + parallel_for_(range, fi, nstripes); + std::vector points, normals; + for(size_t i = 0; i < pVecs.size(); i++) + { + points.insert(points.end(), pVecs[i].begin(), pVecs[i].end()); + normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end()); + } + + _points.create((int)points.size(), 1, POINT_TYPE); + if(!points.empty()) + Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat()); + + if(_normals.needed()) + { + _normals.create((int)normals.size(), 1, POINT_TYPE); + if(!normals.empty()) + Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat()); + } + } +} + +void ColoredTSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const +{ + CV_TRACE_FUNCTION(); + CV_Assert(!_points.empty()); + if(_normals.needed()) + { + Points points = _points.getMat(); + CV_Assert(points.type() == POINT_TYPE); + + _normals.createSameSize(_points, _points.type()); + Normals normals = _normals.getMat(); + + const ColoredTSDFVolumeCPU& _vol = *this; + auto PushNormals = [&](const ptype& pp, const int* position) + { + const ColoredTSDFVolumeCPU& vol(_vol); + Affine3f invPose(vol.pose.inv()); + Point3f p = fromPtype(pp); + Point3f n = nan3; + if (!isNaN(p)) + { + Point3f voxPt = (invPose * p); + voxPt = voxPt * vol.voxelSizeInv; + n = vol.pose.rotation() * vol.getNormalVoxel(voxPt); + } + normals(position[0], position[1]) = toPtype(n); + }; + points.forEach(PushNormals); + } +} + +Ptr makeTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, + float _truncDist, int _maxWeight, Point3i _resolution) +{ + return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution); +} + +Ptr makeTSDFVolume(const VolumeParams& _params) +{ + return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, + _params.tsdfTruncDist, _params.maxWeight, _params.resolution); +} + +} // namespace kinfu +} // namespace cv diff --git a/modules/rgbd/src/colored_tsdf.hpp b/modules/rgbd/src/colored_tsdf.hpp new file mode 100644 index 00000000000..d6cedee1d23 --- /dev/null +++ b/modules/rgbd/src/colored_tsdf.hpp @@ -0,0 +1,59 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + +// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this +// module's directory + +#ifndef __OPENCV_KINFU_COLORED_TSDF_H__ +#define __OPENCV_KINFU_COLORED_TSDF_H__ + +#include + +#include "kinfu_frame.hpp" +#include "utils.hpp" + +namespace cv +{ +namespace kinfu +{ + +typedef int8_t TsdfType; +typedef uchar WeightType; + +struct TsdfVoxel +{ + TsdfVoxel(TsdfType _tsdf, WeightType _weight) : + tsdf(_tsdf), weight(_weight) + { } + TsdfType tsdf; + WeightType weight; +}; + +typedef Vec VecTsdfVoxel; + +class ColoredTSDFVolume : public Volume +{ + public: + // dimension in voxels, size in meters + ColoredTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, + int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true); + virtual ~ColoredTSDFVolume() = default; + + public: + + Point3i volResolution; + WeightType maxWeight; + + Point3f volSize; + float truncDist; + Vec4i volDims; + Vec8i neighbourCoords; +}; + +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/volume.cpp b/modules/rgbd/src/volume.cpp index 88c46fe4b67..427129cc132 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -4,10 +4,11 @@ #include -#include "hash_tsdf.hpp" #include "opencv2/core/base.hpp" #include "precomp.hpp" #include "tsdf.hpp" +#include "hash_tsdf.hpp" +#include "colored_tsdf.hpp" namespace cv { From 14df9f4efa5ceec720a3b1d8cafe488de6614dfb Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 25 Feb 2021 15:45:32 +0300 Subject: [PATCH 02/55] dummy class works --- modules/rgbd/include/opencv2/rgbd.hpp | 1 + .../include/opencv2/rgbd/colored_kinfu.hpp | 11 ++++-- modules/rgbd/include/opencv2/rgbd/volume.hpp | 5 ++- modules/rgbd/samples/colored_kinfu_demo.cpp | 20 ++++------ modules/rgbd/samples/io_utils.hpp | 22 +++++++++++ modules/rgbd/src/colored_kinfu.cpp | 28 +++++++++----- modules/rgbd/src/colored_tsdf.cpp | 38 +++++++++---------- modules/rgbd/src/colored_tsdf.hpp | 11 +++--- modules/rgbd/src/volume.cpp | 21 ++++++++++ 9 files changed, 104 insertions(+), 53 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd.hpp b/modules/rgbd/include/opencv2/rgbd.hpp index d4ac749c2a5..a02d7ba335e 100755 --- a/modules/rgbd/include/opencv2/rgbd.hpp +++ b/modules/rgbd/include/opencv2/rgbd.hpp @@ -14,6 +14,7 @@ #include "opencv2/rgbd/kinfu.hpp" #include "opencv2/rgbd/dynafu.hpp" #include "opencv2/rgbd/large_kinfu.hpp" +#include "opencv2/rgbd/colored_kinfu.hpp" /** @defgroup rgbd RGB-Depth Processing diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp index f55d7120fe1..42960b82d21 100644 --- a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -12,7 +12,7 @@ #include namespace cv { -namespace kinfu { +namespace colored_kinfu { //! @addtogroup kinect_fusion //! @{ @@ -74,6 +74,9 @@ struct CV_EXPORTS_W Params */ CV_WRAP static Ptr hashTSDFParams(bool isCoarse); + + CV_WRAP static Ptr coloredTSDFParams(bool isCoarse); + /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; @@ -181,11 +184,11 @@ struct CV_EXPORTS_W Params That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion. */ -class CV_EXPORTS_W KinFu +class CV_EXPORTS_W ColoredKinFu { public: - CV_WRAP static Ptr create(const Ptr& _params); - virtual ~KinFu(); + CV_WRAP static Ptr create(const Ptr& _params); + virtual ~ColoredKinFu(); /** @brief Get current parameters */ virtual const Params& getParams() const = 0; diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 33f63b1fbfb..d88579e7011 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -45,8 +45,9 @@ class CV_EXPORTS_W Volume enum class VolumeType { - TSDF = 0, - HASHTSDF = 1 + TSDF = 0, + HASHTSDF = 1, + COLOREDTSDF = 2 }; struct CV_EXPORTS_W VolumeParams diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index 6ae7674a37a..3ccb947da23 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -27,10 +27,10 @@ const std::string vizWindowName = "cloud"; struct PauseCallbackArgs { - PauseCallbackArgs(KinFu& _kf) : kf(_kf) + PauseCallbackArgs(ColoredKinFu& _kf) : kf(_kf) { } - KinFu& kf; + ColoredKinFu& kf; }; void pauseCallback(const viz::MouseEvent& me, void* args); @@ -125,16 +125,10 @@ int main(int argc, char **argv) if(!recordPath.empty()) depthWriter = makePtr(recordPath); - Ptr params; - Ptr kf; - - if(coarse) - params = Params::coarseParams(); - else - params = Params::defaultParams(); - - if(useHashTSDF) - params = Params::hashTSDFParams(coarse); + Ptr params; + Ptr kf; + + params = colored_kinfu::Params::coloredTSDFParams(coarse); // These params can be different for each depth sensor ds->updateParams(*params); @@ -151,7 +145,7 @@ int main(int argc, char **argv) //params->tsdf_max_weight = 16; if(!idle) - kf = KinFu::create(params); + kf = ColoredKinFu::create(params); #ifdef HAVE_OPENCV_VIZ cv::viz::Viz3d window(vizWindowName); diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index f0a3a1e7372..ee2c02f3be0 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -326,6 +326,28 @@ struct DepthSource } } } + + void updateParams(colored_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; diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 8755211d6b5..211f4066610 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -12,7 +12,8 @@ #include "kinfu_frame.hpp" namespace cv { -namespace kinfu { +namespace colored_kinfu { +using namespace kinfu; void Params::setInitialVolumePose(Matx33f R, Vec3f t) { @@ -108,9 +109,20 @@ Ptr Params::hashTSDFParams(bool isCoarse) return p; } +Ptr Params::coloredTSDFParams(bool isCoarse) +{ + Ptr p; + if(isCoarse) + p = coarseParams(); + else + p = defaultParams(); + p->volumeType = VolumeType::COLOREDTSDF; + return p; +} + // MatType should be Mat or UMat template< typename MatType> -class ColoredKinFuImpl : public KinFu +class ColoredKinFuImpl : public ColoredKinFu { public: ColoredKinFuImpl(const Params& _params); @@ -320,19 +332,15 @@ void ColoredKinFuImpl::getNormals(InputArray points, OutputArray normal #ifdef OPENCV_ENABLE_NONFREE -Ptr KinFu::create(const Ptr& params) +Ptr ColoredKinFu::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< ColoredKinFuImpl >(*params); -#endif - return makePtr< ColoredKinFuImpl >(*params); + return makePtr< ColoredKinFuImpl >(*params); } #else -Ptr KinFu::create(const Ptr& /* params */) +Ptr ColoredKinFu::create(const Ptr& /* params */) { CV_Error(Error::StsNotImplemented, "This algorithm is patented and is excluded in this configuration; " @@ -340,7 +348,7 @@ Ptr KinFu::create(const Ptr& /* params */) } #endif -KinFu::~KinFu() {} +ColoredKinFu::~ColoredKinFu() {} } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 133557d3109..179669bb431 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -56,7 +56,7 @@ ColoredTSDFVolume::ColoredTSDFVolume(float _voxelSize, Matx44f _pose, float _ray ); } -class ColoredTSDFVolumeCPU : public TSDFVolume +class ColoredTSDFVolumeCPU : public ColoredTSDFVolume { public: // dimension in voxels, size in meters @@ -72,7 +72,7 @@ class ColoredTSDFVolumeCPU : public TSDFVolume virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; virtual void reset() override; - virtual TsdfVoxel at(const Vec3i& volumeIdx) const; + virtual RGBTsdfVoxel at(const Vec3i& volumeIdx) const; float interpolateVoxel(const cv::Point3f& p) const; Point3f getNormalVoxel(const cv::Point3f& p) const; @@ -90,7 +90,7 @@ class ColoredTSDFVolumeCPU : public TSDFVolume ColoredTSDFVolumeCPU::ColoredTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Vec3i _resolution, bool zFirstMemOrder) - : TSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution, + : ColoredTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution, zFirstMemOrder) { int xdim, ydim, zdim; @@ -118,24 +118,24 @@ void ColoredTSDFVolumeCPU::reset() { CV_TRACE_FUNCTION(); - volume.forEach([](VecTsdfVoxel& vv, const int* /* position */) + volume.forEach([](VecRGBTsdfVoxel& vv, const int* /* position */) { - TsdfVoxel& v = reinterpret_cast(vv); + RGBTsdfVoxel& v = reinterpret_cast(vv); v.tsdf = floatToTsdf(0.0f); v.weight = 0; }); } -TsdfVoxel ColoredTSDFVolumeCPU::at(const Vec3i& volumeIdx) const +RGBTsdfVoxel ColoredTSDFVolumeCPU::at(const Vec3i& volumeIdx) const { //! Out of bounds if ((volumeIdx[0] >= volResolution.x || volumeIdx[0] < 0) || (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) { - return TsdfVoxel(floatToTsdf(1.f), 0); + return RGBTsdfVoxel(floatToTsdf(1.f), 0); } - const TsdfVoxel* volData = volume.ptr(); + const RGBTsdfVoxel* volData = volume.ptr(); int coordBase = volumeIdx[0] * volDims[0] + volumeIdx[1] * volDims[1] + volumeIdx[2] * volDims[2]; return volData[coordBase]; @@ -178,7 +178,7 @@ inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& p) const float tz = p.z - iz; int coordBase = ix*xdim + iy*ydim + iz*zdim; - const TsdfVoxel* volData = volume.ptr(); + const RGBTsdfVoxel* volData = volume.ptr(); float vx[8]; for (int i = 0; i < 8; i++) @@ -199,7 +199,7 @@ inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& p) const inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& p) const { const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; - const TsdfVoxel* volData = volume.ptr(); + const RGBTsdfVoxel* volData = volume.ptr(); if(p.x < 1 || p.x >= volResolution.x - 2 || p.y < 1 || p.y >= volResolution.y - 2 || @@ -327,7 +327,7 @@ struct RaycastInvoker : ParallelLoopBody int ix = cvRound(next.x); int iy = cvRound(next.y); int iz = cvRound(next.z); - fnext = tsdfToFloat(volume.volume.at(ix*xdim + iy*ydim + iz*zdim).tsdf); + fnext = tsdfToFloat(volume.volume.at(ix*xdim + iy*ydim + iz*zdim).tsdf); if(fnext != f) { fnext = volume.interpolateVoxel(next); @@ -418,7 +418,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody nVecs(_nVecs), needNormals(_needNormals) { - volDataStart = vol.volume.ptr(); + volDataStart = vol.volume.ptr(); } inline void coord(std::vector& points, std::vector& normals, @@ -449,7 +449,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody if(limits) { - const TsdfVoxel& voxeld = volDataStart[(x+shift.x)*vol.volDims[0] + + const RGBTsdfVoxel& voxeld = volDataStart[(x+shift.x)*vol.volDims[0] + (y+shift.y)*vol.volDims[1] + (z+shift.z)*vol.volDims[2]]; float vd = tsdfToFloat(voxeld.tsdf); @@ -482,13 +482,13 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody std::vector points, normals; for(int x = range.start; x < range.end; x++) { - const TsdfVoxel* volDataX = volDataStart + x*vol.volDims[0]; + const RGBTsdfVoxel* volDataX = volDataStart + x*vol.volDims[0]; for(int y = 0; y < vol.volResolution.y; y++) { - const TsdfVoxel* volDataY = volDataX + y*vol.volDims[1]; + const RGBTsdfVoxel* volDataY = volDataX + y*vol.volDims[1]; for(int z = 0; z < vol.volResolution.z; z++) { - const TsdfVoxel& voxel0 = volDataY[z*vol.volDims[2]]; + const RGBTsdfVoxel& voxel0 = volDataY[z*vol.volDims[2]]; float v0 = tsdfToFloat(voxel0.tsdf); if(voxel0.weight != 0 && v0 != 1.f) { @@ -511,7 +511,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody const ColoredTSDFVolumeCPU& vol; std::vector>& pVecs; std::vector>& nVecs; - const TsdfVoxel* volDataStart; + const RGBTsdfVoxel* volDataStart; bool needNormals; mutable Mutex mutex; }; @@ -578,13 +578,13 @@ void ColoredTSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals } } -Ptr makeTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, +Ptr makeColoredTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution) { return makePtr(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _resolution); } -Ptr makeTSDFVolume(const VolumeParams& _params) +Ptr makeColoredTSDFVolume(const VolumeParams& _params) { return makePtr(_params.voxelSize, _params.pose.matrix, _params.raycastStepFactor, _params.tsdfTruncDist, _params.maxWeight, _params.resolution); diff --git a/modules/rgbd/src/colored_tsdf.hpp b/modules/rgbd/src/colored_tsdf.hpp index d6cedee1d23..7aeb15e99b0 100644 --- a/modules/rgbd/src/colored_tsdf.hpp +++ b/modules/rgbd/src/colored_tsdf.hpp @@ -21,16 +21,16 @@ namespace kinfu typedef int8_t TsdfType; typedef uchar WeightType; -struct TsdfVoxel +struct RGBTsdfVoxel { - TsdfVoxel(TsdfType _tsdf, WeightType _weight) : + RGBTsdfVoxel(TsdfType _tsdf, WeightType _weight) : tsdf(_tsdf), weight(_weight) { } TsdfType tsdf; WeightType weight; }; -typedef Vec VecTsdfVoxel; +typedef Vec VecRGBTsdfVoxel; class ColoredTSDFVolume : public Volume { @@ -51,9 +51,10 @@ class ColoredTSDFVolume : public Volume Vec8i neighbourCoords; }; -Ptr makeTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, +Ptr makeColoredTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Point3i _resolution); -Ptr makeTSDFVolume(const VolumeParams& _params); +Ptr makeColoredTSDFVolume(const VolumeParams& _params); + } // namespace kinfu } // namespace cv #endif diff --git a/modules/rgbd/src/volume.cpp b/modules/rgbd/src/volume.cpp index 427129cc132..c05238f2602 100644 --- a/modules/rgbd/src/volume.cpp +++ b/modules/rgbd/src/volume.cpp @@ -39,6 +39,14 @@ Ptr VolumeParams::defaultParams(VolumeType _volumeType) params.tsdfTruncDist = 7 * params.voxelSize; //! About 0.04f in meters return makePtr(params); } + else if (params.type == VolumeType::COLOREDTSDF) + { + 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); + } CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } @@ -61,6 +69,13 @@ Ptr VolumeParams::coarseParams(VolumeType _volumeType) params->tsdfTruncDist = 2 * params->voxelSize; //! About 0.04f in meters return params; } + else if (params->type == VolumeType::COLOREDTSDF) + { + params->resolution = Vec3i::all(128); + 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"); } @@ -70,6 +85,8 @@ Ptr makeVolume(const VolumeParams& _volumeParams) return kinfu::makeTSDFVolume(_volumeParams); else if(_volumeParams.type == VolumeType::HASHTSDF) return kinfu::makeHashTSDFVolume(_volumeParams); + else if(_volumeParams.type == VolumeType::COLOREDTSDF) + return kinfu::makeColoredTSDFVolume(_volumeParams); CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } @@ -86,6 +103,10 @@ Ptr makeVolume(VolumeType _volumeType, float _voxelSize, Matx44f _pose, { return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _truncateThreshold); } + else if (_volumeType == VolumeType::COLOREDTSDF) + { + return makeColoredTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, _presolution); + } CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters"); } From 4abffa429af8a01bfea35b719697e853dc80a1cc Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 25 Feb 2021 17:45:48 +0300 Subject: [PATCH 03/55] colored kinfu demo minor changes --- modules/rgbd/samples/colored_kinfu_demo.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index 3ccb947da23..3d38f900056 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -73,7 +73,6 @@ int main(int argc, char **argv) { bool coarse = false; bool idle = false; - bool useHashTSDF = false; std::string recordPath; CommandLineParser parser(argc, argv, keys); @@ -99,10 +98,6 @@ int main(int argc, char **argv) { recordPath = parser.get("record"); } - if(parser.has("useHashTSDF")) - { - useHashTSDF = true; - } if(parser.has("idle")) { idle = true; @@ -110,7 +105,7 @@ int main(int argc, char **argv) Ptr ds; if (parser.has("depth")) - ds = makePtr(parser.get("depth")); + ds = makePtr(parser.get("depth") + "/depth.txt"); else ds = makePtr(parser.get("camera")); From 5346fb66b46333f743b951d7cf085eb28f80dbac Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 26 Feb 2021 13:20:25 +0300 Subject: [PATCH 04/55] add simple rgb reader and writer --- modules/rgbd/samples/colored_kinfu_demo.cpp | 12 +- modules/rgbd/samples/io_utils.hpp | 316 ++++++++++++++++++++ 2 files changed, 326 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index 3d38f900056..c4b593cd16c 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -104,11 +104,15 @@ int main(int argc, char **argv) } Ptr ds; + Ptr rgbs; + if (parser.has("depth")) ds = makePtr(parser.get("depth") + "/depth.txt"); else ds = makePtr(parser.get("camera")); + rgbs = makePtr(parser.get("depth") + "/rgb.txt"); + if (ds->empty()) { std::cerr << "Failed to open depth source" << std::endl; @@ -128,8 +132,10 @@ int main(int argc, char **argv) // These params can be different for each depth sensor ds->updateParams(*params); + rgbs->updateParams(*params); + // Enables OpenCL explicitly (by default can be switched-off) - cv::setUseOptimized(true); + cv::setUseOptimized(false); // Scene-specific params should be tuned for each scene individually //float cubeSize = 1.f; @@ -158,7 +164,9 @@ int main(int argc, char **argv) { if(depthWriter) depthWriter->append(frame); - + + UMat rgb_frame = rgbs->getRGB(); + imshow("rgb", rgb_frame); #ifdef HAVE_OPENCV_VIZ if(pause) { diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index ee2c02f3be0..d6f65e52962 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -12,6 +12,7 @@ #include #include #include +#include namespace cv { @@ -355,6 +356,321 @@ struct DepthSource UMat undistortMap1, undistortMap2; Type sourceType; }; + + +static std::vector readRGB(const std::string& fileList) +{ + std::vector v; + + std::fstream file(fileList); + if (!file.is_open()) + throw std::runtime_error("Failed to read rgb 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 RGBWriter +{ + RGBWriter(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; +}; + +struct RGBSource +{ + public: + enum Type + { + RGB_LIST, + RGB_KINECT2_LIST, + RGB_KINECT2, + RGB_REALSENSE, + RGB_ASTRA + }; + + RGBSource(int cam) : RGBSource("", cam) {} + + RGBSource(String fileListName) : RGBSource(fileListName, -1) {} + + RGBSource(String fileListName, int cam) + : rgbFileList(fileListName.empty() ? std::vector() + : readRGB(fileListName)), + frameIdx(0), + undistortMap1(), + undistortMap2() + { + if (cam >= 0) + { + vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam); + if (vc.isOpened()) + { + if(cam == 20) + sourceType = Type::RGB_ASTRA; + else + sourceType = Type::RGB_KINECT2; + } + else + { + vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam); + if (vc.isOpened()) + { + sourceType = Type::RGB_REALSENSE; + } + } + } + else + { + vc = VideoCapture(); + sourceType = Type::RGB_KINECT2_LIST; + } + } + + UMat getRGB() + { + UMat out; + if (!vc.isOpened()) + { + if (frameIdx < rgbFileList.size()) + { + Mat f = cv::imread(rgbFileList[frameIdx++], IMREAD_COLOR); + f.copyTo(out); + } + else + { + return UMat(); + } + } + else + { + vc.grab(); + switch (sourceType) + { + case Type::RGB_KINECT2: vc.retrieve(out, CAP_OPENNI_BGR_IMAGE); break; + case Type::RGB_REALSENSE: vc.retrieve(out, CAP_INTELPERC_IMAGE); break; + default: + // unknown rgb source + vc.retrieve(out); + } + + // workaround for Kinect 2 + if (sourceType == Type::RGB_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 rgbFileList.empty() && !(vc.isOpened()); } + + void updateIntrinsics(Matx33f& _intrinsics, Size& _frameSize, float& _depthFactor) + { + if (vc.isOpened()) + { + // this should be set in according to user's rgb 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::RGB_KINECT2) + { + fx = fy = Kinect2Params::focal; + cx = Kinect2Params::cx; + cy = Kinect2Params::cy; + + frameSize = Kinect2Params::frameSize; + } + else if (sourceType == Type::RGB_ASTRA) + { + fx = AstraParams::fx; + fy = AstraParams::fy; + cx = AstraParams::cx; + cy = AstraParams::cy; + + frameSize = AstraParams::frameSize; + } + else + { + // TODO: replace to rgb types + if (sourceType == Type::RGB_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_IMAGE_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::RGB_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::RGB_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::RGB_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::RGB_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(colored_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::RGB_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 rgbFileList; + size_t frameIdx; + VideoCapture vc; + UMat undistortMap1, undistortMap2; + Type sourceType; +}; } // namespace io_utils } // namespace cv From 9eab94ecdcd7fffad2a4058359cee427a9bf8921 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 26 Feb 2021 18:05:24 +0300 Subject: [PATCH 05/55] add rgb info to tsdf --- .../include/opencv2/rgbd/colored_kinfu.hpp | 2 +- modules/rgbd/include/opencv2/rgbd/volume.hpp | 2 + modules/rgbd/samples/colored_kinfu_demo.cpp | 12 ++++-- modules/rgbd/samples/io_utils.hpp | 37 ++++++++++++------- modules/rgbd/src/colored_kinfu.cpp | 35 ++++++++++++------ modules/rgbd/src/colored_tsdf.cpp | 6 ++- modules/rgbd/src/hash_tsdf.cpp | 4 ++ modules/rgbd/src/tsdf.hpp | 3 ++ 8 files changed, 68 insertions(+), 33 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp index 42960b82d21..74b200e5611 100644 --- a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -246,7 +246,7 @@ class CV_EXPORTS_W ColoredKinFu @param depth one-channel image which size and depth scale is described in algorithm's parameters @return true if succeeded to align new frame with current scene, false if opposite */ - CV_WRAP virtual bool update(InputArray depth) = 0; + CV_WRAP virtual bool update(InputArray depth, InputArray rgb) = 0; }; //! @} diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index d88579e7011..e7b22e7ed4e 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -30,6 +30,8 @@ class CV_EXPORTS_W Volume virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) = 0; + virtual void integrate(InputArray _depth, InputArray _rgb, 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; diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index c4b593cd16c..077de847b08 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -121,9 +121,13 @@ int main(int argc, char **argv) } Ptr depthWriter; - if(!recordPath.empty()) - depthWriter = makePtr(recordPath); + Ptr rgbWriter; + if (!recordPath.empty()) + { + depthWriter = makePtr(recordPath); + rgbWriter = makePtr(recordPath); + } Ptr params; Ptr kf; @@ -166,7 +170,7 @@ int main(int argc, char **argv) depthWriter->append(frame); UMat rgb_frame = rgbs->getRGB(); - imshow("rgb", rgb_frame); + //imshow("rgb", rgb_frame); #ifdef HAVE_OPENCV_VIZ if(pause) { @@ -206,7 +210,7 @@ int main(int argc, char **argv) { imshow("depth", cvt8); - if(!kf->update(frame)) + if(!kf->update(frame, rgb_frame)) { kf->reset(); std::cout << "reset" << std::endl; diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index d6f65e52962..d6374314870 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -88,6 +88,15 @@ static const float cy = 204.f; static const float k1 = 0.12f; static const float k2 = -0.34f; static const float k3 = 0.12f; + +static const Size rgb_frameSize = Size(640, 480); +static const float rgb_focal = 525.0f; +static const float rgb_cx = 319.5f; +static const float rgb_cy = 239.5f; +static const float rgb_k1 = 0.0f; +static const float rgb_k2 = 0.0f; +static const float rgb_k3 = 0.0f; + }; // namespace Kinect2Params namespace AstraParams @@ -496,7 +505,7 @@ struct RGBSource // workaround for Kinect 2 if (sourceType == Type::RGB_KINECT2) { - out = out(Rect(Point(), Kinect2Params::frameSize)); + out = out(Rect(Point(), Kinect2Params::rgb_frameSize)); UMat outCopy; // linear remap adds gradient between valid and invalid pixels @@ -527,11 +536,11 @@ struct RGBSource Size frameSize; if (sourceType == Type::RGB_KINECT2) { - fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; + fx = fy = Kinect2Params::rgb_focal; + cx = Kinect2Params::rgb_cx; + cy = Kinect2Params::rgb_cy; - frameSize = Kinect2Params::frameSize; + frameSize = Kinect2Params::rgb_frameSize; } else if (sourceType == Type::RGB_ASTRA) { @@ -611,9 +620,9 @@ struct RGBSource if (sourceType == Type::RGB_KINECT2) { Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; + distCoeffs(0) = Kinect2Params::rgb_k1; + distCoeffs(1) = Kinect2Params::rgb_k2; + distCoeffs(4) = Kinect2Params::rgb_k3; initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, params.frameSize, CV_16SC2, undistortMap1, undistortMap2); @@ -633,9 +642,9 @@ struct RGBSource if (sourceType == Type::RGB_KINECT2) { Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; + distCoeffs(0) = Kinect2Params::rgb_k1; + distCoeffs(1) = Kinect2Params::rgb_k2; + distCoeffs(4) = Kinect2Params::rgb_k3; initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, params.frameSize, CV_16SC2, undistortMap1, undistortMap2); @@ -655,9 +664,9 @@ struct RGBSource if (sourceType == Type::RGB_KINECT2) { Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; + distCoeffs(0) = Kinect2Params::rgb_k1; + distCoeffs(1) = Kinect2Params::rgb_k2; + distCoeffs(4) = Kinect2Params::rgb_k3; initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, params.frameSize, CV_16SC2, undistortMap1, undistortMap2); diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 211f4066610..d2415a1fe3b 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -117,6 +117,8 @@ Ptr Params::coloredTSDFParams(bool isCoarse) else p = defaultParams(); p->volumeType = VolumeType::COLOREDTSDF; + + return p; } @@ -140,9 +142,9 @@ class ColoredKinFuImpl : public ColoredKinFu const Affine3f getPose() const CV_OVERRIDE; - bool update(InputArray depth) CV_OVERRIDE; + bool update(InputArray depth, InputArray rgb) CV_OVERRIDE; - bool updateT(const MatType& depth); + bool updateT(const MatType& depth, const MatType& rgb); private: Params params; @@ -194,52 +196,61 @@ const Affine3f ColoredKinFuImpl::getPose() const template<> -bool ColoredKinFuImpl::update(InputArray _depth) +bool ColoredKinFuImpl::update(InputArray _depth, InputArray _rgb) { CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); Mat depth; + Mat rgb; if(_depth.isUMat()) { _depth.copyTo(depth); - return updateT(depth); + _rgb.copyTo(rgb); + return updateT(depth, rgb); } else { - return updateT(_depth.getMat()); + return updateT(_depth.getMat(), _rgb.getMat()); } } template<> -bool ColoredKinFuImpl::update(InputArray _depth) +bool ColoredKinFuImpl::update(InputArray _depth, InputArray _rgb) { CV_Assert(!_depth.empty() && _depth.size() == params.frameSize); UMat depth; + UMat rgb; if(!_depth.isUMat()) { _depth.copyTo(depth); - return updateT(depth); + _rgb.copyTo(rgb); + return updateT(depth, rgb); } else { - return updateT(_depth.getUMat()); + return updateT(_depth.getUMat(), _rgb.getUMat()); } } template< typename MatType > -bool ColoredKinFuImpl::updateT(const MatType& _depth) +bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _rgb) { CV_TRACE_FUNCTION(); MatType depth; + MatType rgb; if(_depth.type() != DEPTH_TYPE) _depth.convertTo(depth, DEPTH_TYPE); else depth = _depth; - + + if(_rgb.type() != POINT_TYPE) + _rgb.convertTo(rgb, POINT_TYPE); + else + rgb = _rgb; std::vector newPoints, newNormals; makeFrameFromDepth(depth, newPoints, newNormals, params.intr, @@ -252,7 +263,7 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth) if(frameCounter == 0) { // use depth instead of distance - volume->integrate(depth, params.depthFactor, pose, params.intr); + volume->integrate(depth, rgb, params.depthFactor, pose, params.intr); pyrPoints = newPoints; pyrNormals = newNormals; } @@ -271,7 +282,7 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth) if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement) { // use depth instead of distance - volume->integrate(depth, params.depthFactor, pose, params.intr); + volume->integrate(depth, rgb, params.depthFactor, pose, params.intr); } MatType& points = pyrPoints [0]; MatType& normals = pyrNormals[0]; diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 179669bb431..c439a51ebf2 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -62,9 +62,11 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume // dimension in voxels, size in meters ColoredTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Vec3i _resolution, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, + const kinfu::Intr& intrinsics, const int frameId = 0) override {}; + virtual void integrate(InputArray _depth, InputArray _rgb, 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; @@ -142,7 +144,7 @@ RGBTsdfVoxel ColoredTSDFVolumeCPU::at(const Vec3i& volumeIdx) const } // use depth instead of distance (optimization) -void ColoredTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, +void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, const Intr& intrinsics, const int frameId) { CV_TRACE_FUNCTION(); diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 08338fe83ef..6624268fd62 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -99,6 +99,8 @@ class HashTSDFVolumeCPU : public HashTSDFVolume HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); + virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, + const kinfu::Intr& intrinsics, const int frameId = 0) 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, @@ -894,6 +896,8 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void markActive(const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId); + virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, + const kinfu::Intr& intrinsics, const int frameId = 0) 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, diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 67361aa59d9..aab238576dd 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -40,6 +40,9 @@ class TSDFVolume : public Volume int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true); virtual ~TSDFVolume() = default; + virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, + const kinfu::Intr& intrinsics, const int frameId = 0) override {}; + public: Point3i volResolution; From b2d2e687bc206e7432e53c4538a60447b89017e7 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 1 Mar 2021 15:07:55 +0300 Subject: [PATCH 06/55] add new raycast --- modules/rgbd/include/opencv2/rgbd/volume.hpp | 2 ++ modules/rgbd/src/colored_tsdf.cpp | 3 ++- modules/rgbd/src/hash_tsdf.cpp | 6 ++++-- modules/rgbd/src/kinfu_frame.hpp | 1 + modules/rgbd/src/tsdf.hpp | 3 ++- 5 files changed, 11 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index e7b22e7ed4e..792a5d5ebe9 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -34,6 +34,8 @@ class CV_EXPORTS_W Volume 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 raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, + OutputArray points, OutputArray normals, OutputArray colors) const = 0; virtual void fetchNormals(InputArray points, OutputArray _normals) const = 0; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0; virtual void reset() = 0; diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index c439a51ebf2..bb9dd1f7e90 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -66,7 +66,8 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume const kinfu::Intr& intrinsics, const int frameId = 0) override {}; virtual void integrate(InputArray _depth, InputArray _rgb, 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, OutputArray colors) const override {}; virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index 6624268fd62..d06666f0b48 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -105,7 +105,8 @@ class HashTSDFVolumeCPU : public HashTSDFVolume const int frameId = 0) override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; - + void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, + OutputArray points, OutputArray normals, OutputArray colors) const override {}; void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; @@ -902,7 +903,8 @@ class HashTSDFVolumeGPU : public HashTSDFVolume const int frameId = 0) override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; - + void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, + OutputArray points, OutputArray normals, OutputArray colors) const override {}; void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; diff --git a/modules/rgbd/src/kinfu_frame.hpp b/modules/rgbd/src/kinfu_frame.hpp index 16327d36aa5..15a5347fe29 100644 --- a/modules/rgbd/src/kinfu_frame.hpp +++ b/modules/rgbd/src/kinfu_frame.hpp @@ -78,6 +78,7 @@ enum typedef cv::Mat_< ptype > Points; typedef Points Normals; +typedef Points Colors; typedef cv::Mat_< depthType > Depth; diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index aab238576dd..fd3001afb3c 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -42,7 +42,8 @@ class TSDFVolume : public Volume virtual void integrate(InputArray _depth, InputArray _rgb, 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, OutputArray colors) const override {}; public: Point3i volResolution; From 60347a759d7b7ec98484bc497491b1687a85155b Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 1 Mar 2021 16:30:05 +0300 Subject: [PATCH 07/55] replace TsdfVoxel with RGBTsdfVoxel; integrate done --- modules/rgbd/src/colored_tsdf.cpp | 7 +- modules/rgbd/src/colored_tsdf.hpp | 7 +- modules/rgbd/src/kinfu_frame.hpp | 4 +- modules/rgbd/src/tsdf_functions.cpp | 125 ++++++++++++++++++++++++++++ modules/rgbd/src/tsdf_functions.hpp | 7 ++ 5 files changed, 143 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index bb9dd1f7e90..abd860d0c85 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -111,7 +111,7 @@ ColoredTSDFVolumeCPU::ColoredTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, } volStrides = Vec4i(xdim, ydim, zdim); - volume = Mat(1, volResolution.x * volResolution.y * volResolution.z, rawType()); + volume = Mat(1, volResolution.x * volResolution.y * volResolution.z, rawType()); reset(); } @@ -135,7 +135,7 @@ RGBTsdfVoxel ColoredTSDFVolumeCPU::at(const Vec3i& volumeIdx) const (volumeIdx[1] >= volResolution.y || volumeIdx[1] < 0) || (volumeIdx[2] >= volResolution.z || volumeIdx[2] < 0)) { - return RGBTsdfVoxel(floatToTsdf(1.f), 0); + return RGBTsdfVoxel(floatToTsdf(1.f), 0, 160, 160, 160); } const RGBTsdfVoxel* volData = volume.ptr(); @@ -153,6 +153,7 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d CV_Assert(_depth.type() == DEPTH_TYPE); CV_Assert(!_depth.empty()); Depth depth = _depth.getMat(); + Rgb rgb = _rgb.getMat(); Vec6f newParams((float)depth.rows, (float)depth.cols, intrinsics.fx, intrinsics.fy, @@ -163,7 +164,7 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d pixNorms = preCalculationPixNorm(depth, intrinsics); } - integrateVolumeUnit(truncDist, voxelSize, maxWeight, (this->pose).matrix, volResolution, volStrides, depth, + integrateRGBVolumeUnit(truncDist, voxelSize, maxWeight, (this->pose).matrix, volResolution, volStrides, depth, rgb, depthFactor, cameraPose, intrinsics, pixNorms, volume); } diff --git a/modules/rgbd/src/colored_tsdf.hpp b/modules/rgbd/src/colored_tsdf.hpp index 7aeb15e99b0..d6b09b0156c 100644 --- a/modules/rgbd/src/colored_tsdf.hpp +++ b/modules/rgbd/src/colored_tsdf.hpp @@ -20,14 +20,15 @@ namespace kinfu typedef int8_t TsdfType; typedef uchar WeightType; - +typedef short int ColorType; struct RGBTsdfVoxel { - RGBTsdfVoxel(TsdfType _tsdf, WeightType _weight) : - tsdf(_tsdf), weight(_weight) + RGBTsdfVoxel(TsdfType _tsdf, WeightType _weight, ColorType _r, ColorType _g, ColorType _b) : + tsdf(_tsdf), weight(_weight), r(_r), g(_g), b(_b) { } TsdfType tsdf; WeightType weight; + ColorType r, g, b; }; typedef Vec VecRGBTsdfVoxel; diff --git a/modules/rgbd/src/kinfu_frame.hpp b/modules/rgbd/src/kinfu_frame.hpp index 15a5347fe29..b8f02248f3b 100644 --- a/modules/rgbd/src/kinfu_frame.hpp +++ b/modules/rgbd/src/kinfu_frame.hpp @@ -73,7 +73,8 @@ inline ptype toPtype(const cv::Vec3f& x) enum { DEPTH_TYPE = DataType::type, - POINT_TYPE = DataType::type + POINT_TYPE = DataType::type, + COLOR_TYPE = DataType::type }; typedef cv::Mat_< ptype > Points; @@ -81,6 +82,7 @@ typedef Points Normals; typedef Points Colors; typedef cv::Mat_< depthType > Depth; +typedef cv::Mat_< ptype > Rgb; void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, cv::Affine3f lightPose); void makeFrameFromDepth(InputArray depth, OutputArray pyrPoints, OutputArray pyrNormals, diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index b0e5276cba7..7facc1185c4 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -417,6 +417,131 @@ void integrateVolumeUnit( parallel_for_(integrateRange, IntegrateInvoker); +} +void integrateRGBVolumeUnit( + float truncDist, float voxelSize, int maxWeight, + cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, + InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose, + const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + CV_Assert(!_depth.empty()); + cv::Affine3f vpose(_pose); + Depth depth = _depth.getMat(); + Rgb color = _rgb.getMat(); + + Range integrateRange(0, volResolution.x); + + Mat volume = _volume.getMat(); + Mat pixNorms = _pixNorms.getMat(); + const Intr::Projector proj(intrinsics.makeProjector()); + const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * vpose); + const float truncDistInv(1.f / truncDist); + const float dfac(1.f / depthFactor); + RGBTsdfVoxel* volDataStart = volume.ptr();; + + auto IntegrateInvoker = [&](const Range& range) + { + for (int x = range.start; x < range.end; x++) + { + RGBTsdfVoxel* volDataX = volDataStart + x * volStrides[0]; + for (int y = 0; y < volResolution.y; y++) + { + RGBTsdfVoxel* volDataY = volDataX + y * volStrides[1]; + // optimization of camSpace transformation (vector addition instead of matmul at each z) + Point3f basePt = vol2cam * (Point3f(float(x), float(y), 0.0f) * voxelSize); + Point3f camSpacePt = basePt; + // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; + // zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize + Point3f zStep = Point3f(vol2cam.matrix(0, 2), + vol2cam.matrix(1, 2), + vol2cam.matrix(2, 2)) * voxelSize; + int startZ, endZ; + if (abs(zStep.z) > 1e-5) + { + int baseZ = int(-basePt.z / zStep.z); + if (zStep.z > 0) + { + startZ = baseZ; + endZ = volResolution.z; + } + else + { + startZ = 0; + endZ = baseZ; + } + } + else + { + if (basePt.z > 0) + { + startZ = 0; + endZ = volResolution.z; + } + else + { + // z loop shouldn't be performed + startZ = endZ = 0; + } + } + startZ = max(0, startZ); + endZ = min(int(volResolution.z), endZ); + + for (int z = startZ; z < endZ; z++) + { + // optimization of the following: + //Point3f volPt = Point3f(x, y, z)*volume.voxelSize; + //Point3f camSpacePt = vol2cam * volPt; + + camSpacePt += zStep; + if (camSpacePt.z <= 0) + continue; + + Point3f camPixVec; + Point2f projected = proj(camSpacePt, camPixVec); + + depthType v = bilinearDepth(depth, projected); + if (v == 0) { + continue; + } + + int _u = projected.x; + int _v = projected.y; + if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows)) + continue; + float pixNorm = pixNorms.at(_v, _u); + Point3i colorRGB = color.at(_v, _u); + // difference between distances of point and of surface to camera + float sdf = pixNorm * (v * dfac - camSpacePt.z); + // possible alternative is: + // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); + if (sdf >= -truncDist) + { + TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv)); + + RGBTsdfVoxel& voxel = volDataY[z * volStrides[2]]; + WeightType& weight = voxel.weight; + TsdfType& value = voxel.tsdf; + ColorType& r = voxel.r; + ColorType& g = voxel.g; + ColorType& b = voxel.b; + + // update TSDF + r = (r * weight + colorRGB.x) / (weight + 1); + g = (g * weight + colorRGB.y) / (weight + 1); + b = (b * weight + colorRGB.z) / (weight + 1); + value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); + weight = min(int(weight + 1), int(maxWeight)); + } + } + } + } + }; + + parallel_for_(integrateRange, IntegrateInvoker); + } } // namespace kinfu diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 09efecf1252..f9c34d5e290 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -9,6 +9,7 @@ #include #include "tsdf.hpp" +#include "colored_tsdf.hpp" namespace cv { @@ -45,6 +46,12 @@ void integrateVolumeUnit( InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); +void integrateRGBVolumeUnit( + float truncDist, float voxelSize, int maxWeight, + cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, + InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose, + const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); + class CustomHashSet { From db691f5965a49d724e5d1aba84dcc66139ea43d3 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 2 Mar 2021 15:17:31 +0300 Subject: [PATCH 08/55] add colors info to raycast --- modules/rgbd/src/colored_kinfu.cpp | 15 +-- modules/rgbd/src/colored_tsdf.cpp | 6 +- modules/rgbd/src/kinfu_frame.cpp | 149 +++++++++++++++++++++++++++++ modules/rgbd/src/kinfu_frame.hpp | 5 + 4 files changed, 166 insertions(+), 9 deletions(-) diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index d2415a1fe3b..a1c31503a06 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -156,6 +156,7 @@ class ColoredKinFuImpl : public ColoredKinFu Matx44f pose; std::vector pyrPoints; std::vector pyrNormals; + std::vector pyrColors; }; @@ -163,7 +164,7 @@ template< typename MatType > ColoredKinFuImpl::ColoredKinFuImpl(const Params &_params) : params(_params), icp(makeICP(params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)), - pyrPoints(), pyrNormals() + pyrPoints(), pyrNormals(), pyrColors() { volume = makeVolume(params.volumeType, params.voxelSize, params.volumePose.matrix, params.raycast_step_factor, params.tsdf_trunc_dist, params.tsdf_max_weight, params.truncateThreshold, params.volumeDims); @@ -252,8 +253,8 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r else rgb = _rgb; - std::vector newPoints, newNormals; - makeFrameFromDepth(depth, newPoints, newNormals, params.intr, + std::vector newPoints, newNormals, newColors; + makeColoredFrameFromDepth(depth, newPoints, newNormals, newColors, params.intr, params.pyramidLevels, params.depthFactor, params.bilateral_sigma_depth, @@ -266,6 +267,7 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r volume->integrate(depth, rgb, params.depthFactor, pose, params.intr); pyrPoints = newPoints; pyrNormals = newNormals; + pyrColors = newColors; } else { @@ -286,7 +288,8 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r } MatType& points = pyrPoints [0]; MatType& normals = pyrNormals[0]; - volume->raycast(pose, params.intr, params.frameSize, points, normals); + MatType& colors = pyrColors [0]; + volume->raycast(pose, params.intr, params.frameSize, points, normals, colors); buildPyramidPointsNormals(points, normals, pyrPoints, pyrNormals, params.pyramidLevels); } @@ -312,8 +315,8 @@ void ColoredKinFuImpl::render(OutputArray image, const Matx44f& _camera } else { - MatType points, normals; - volume->raycast(_cameraPose, params.intr, params.frameSize, points, normals); + MatType points, normals, colors; + volume->raycast(_cameraPose, params.intr, params.frameSize, points, normals, colors); renderPointsNormals(points, normals, image, params.lightPose); } } diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index abd860d0c85..721356516fd 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -67,9 +67,9 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume virtual void integrate(InputArray _depth, InputArray _rgb, 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, OutputArray colors) const override {}; + OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray points, OutputArray normals) const override; + OutputArray points, OutputArray normals) const override {}; virtual void fetchNormals(InputArray points, OutputArray _normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; @@ -391,7 +391,7 @@ struct RaycastInvoker : ParallelLoopBody void ColoredTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrinsics, const Size& frameSize, - OutputArray _points, OutputArray _normals) const + OutputArray _points, OutputArray _normals, OutputArray _colors) const { CV_TRACE_FUNCTION(); diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index 76d80902b7b..9ddcc19b0b5 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -275,6 +275,73 @@ struct ComputePointsNormalsInvoker : ParallelLoopBody float dfac; }; +struct ComputePointsNormalsColorsInvoker : ParallelLoopBody +{ + ComputePointsNormalsColorsInvoker(const Depth& _depth, Points& _points, Normals& _normals, Colors& _colors, + const Intr::Reprojector& _reproj, float _dfac) : + ParallelLoopBody(), + depth(_depth), + points(_points), + normals(_normals), + colors(_colors), + reproj(_reproj), + dfac(_dfac) + { } + + virtual void operator ()(const Range& range) const override + { + for(int y = range.start; y < range.end; y++) + { + const depthType* depthRow0 = depth[y]; + const depthType* depthRow1 = (y < depth.rows - 1) ? depth[y + 1] : 0; + ptype *ptsRow = points[y]; + ptype *normRow = normals[y]; + ptype *clrRow = colors[y]; + + for(int x = 0; x < depth.cols; x++) + { + depthType d00 = depthRow0[x]; + depthType z00 = d00*dfac; + Point3f v00 = reproj(Point3f((float)x, (float)y, z00)); + + Point3f p = nan3, n = nan3; + + if(x < depth.cols - 1 && y < depth.rows - 1) + { + depthType d01 = depthRow0[x+1]; + depthType d10 = depthRow1[x]; + + depthType z01 = d01*dfac; + depthType z10 = d10*dfac; + + // before it was + //if(z00*z01*z10 != 0) + if(z00 != 0 && z01 != 0 && z10 != 0) + { + Point3f v01 = reproj(Point3f((float)(x+1), (float)(y+0), z01)); + Point3f v10 = reproj(Point3f((float)(x+0), (float)(y+1), z10)); + + cv::Vec3f vec = (v01-v00).cross(v10-v00); + n = -normalize(vec); + p = v00; + } + } + + ptsRow[x] = toPtype(p); + normRow[x] = toPtype(n); + clrRow[x] = toPtype(nan3); + } + } + } + + const Depth& depth; + Points& points; + Normals& normals; + Colors& colors; + const Intr::Reprojector& reproj; + float dfac; +}; + void computePointsNormals(const Intr intr, float depthFactor, const Depth depth, Points points, Normals normals) { @@ -297,6 +364,29 @@ void computePointsNormals(const Intr intr, float depthFactor, const Depth depth, parallel_for_(range, ci, nstripes); } +void computePointsNormalsColors(const Intr intr, float depthFactor, const Depth depth, + Points points, Normals normals, Colors colors) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(!points.empty() && !normals.empty()); + CV_Assert(depth.size() == points.size()); + CV_Assert(depth.size() == normals.size()); + CV_Assert(depth.size() == colors.size()); + + // conversion to meters + // before it was: + //float dfac = 0.001f/depthFactor; + float dfac = 1.f/depthFactor; + + Intr::Reprojector reproj = intr.makeReprojector(); + + ComputePointsNormalsColorsInvoker ci(depth, points, normals, colors, reproj, dfac); + Range range(0, depth.rows); + const int nstripes = -1; + parallel_for_(range, ci, nstripes); +} + ///////// GPU implementation ///////// #ifdef HAVE_OPENCL @@ -659,6 +749,65 @@ void makeFrameFromDepth(InputArray _depth, } } +void makeColoredFrameFromDepth(InputArray _depth, + OutputArray pyrPoints, OutputArray pyrNormals, OutputArray pyrColors, + const Intr intr, int levels, float depthFactor, + float sigmaDepth, float sigmaSpatial, int kernelSize, + float truncateThreshold) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_depth.type() == DEPTH_TYPE); + + + int kp = pyrPoints.kind(), kn = pyrNormals.kind(), kc = pyrColors.kind(); + CV_Assert(kp == _InputArray::STD_ARRAY_MAT || kp == _InputArray::STD_VECTOR_MAT); + CV_Assert(kn == _InputArray::STD_ARRAY_MAT || kn == _InputArray::STD_VECTOR_MAT); + CV_Assert(kc == _InputArray::STD_ARRAY_MAT || kc == _InputArray::STD_VECTOR_MAT); + + Depth depth = _depth.getMat(); + + // looks like OpenCV's bilateral filter works the same as KinFu's + Depth smooth; + Depth depthNoNans = depth.clone(); + patchNaNs(depthNoNans); + bilateralFilter(depthNoNans, smooth, kernelSize, sigmaDepth*depthFactor, sigmaSpatial); + + // depth truncation can be used in some scenes + Depth depthThreshold; + if(truncateThreshold > 0.f) + threshold(smooth, depthThreshold, truncateThreshold * depthFactor, 0.0, THRESH_TOZERO_INV); + else + depthThreshold = smooth; + + // we don't need depth pyramid outside this method + // if we do, the code is to be refactored + + Depth scaled = depthThreshold; + Size sz = smooth.size(); + pyrPoints.create(levels, 1, POINT_TYPE); + pyrNormals.create(levels, 1, POINT_TYPE); + pyrColors.create(levels, 1, POINT_TYPE); + for(int i = 0; i < levels; i++) + { + pyrPoints .create(sz, POINT_TYPE, i); + pyrNormals.create(sz, POINT_TYPE, i); + pyrColors.create(sz, POINT_TYPE, i); + + Points p = pyrPoints. getMatRef(i); + Normals n = pyrNormals.getMatRef(i); + Colors c = pyrColors. getMatRef(i); + + computePointsNormals(intr.scale(i), depthFactor, scaled, p, n); + + if(i < levels - 1) + { + sz.width /= 2; sz.height /= 2; + scaled = pyrDownBilateral(scaled, sigmaDepth*depthFactor); + } + } +} + void buildPyramidPointsNormals(InputArray _points, InputArray _normals, OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals, diff --git a/modules/rgbd/src/kinfu_frame.hpp b/modules/rgbd/src/kinfu_frame.hpp index b8f02248f3b..61b2b528336 100644 --- a/modules/rgbd/src/kinfu_frame.hpp +++ b/modules/rgbd/src/kinfu_frame.hpp @@ -89,6 +89,11 @@ void makeFrameFromDepth(InputArray depth, OutputArray pyrPoints, OutputArray pyr const Intr intr, int levels, float depthFactor, float sigmaDepth, float sigmaSpatial, int kernelSize, float truncateThreshold); +void makeColoredFrameFromDepth(InputArray _depth, + OutputArray pyrPoints, OutputArray pyrNormals, OutputArray pyrColors, + const Intr intr, int levels, float depthFactor, + float sigmaDepth, float sigmaSpatial, int kernelSize, + float truncateThreshold); void buildPyramidPointsNormals(InputArray _points, InputArray _normals, OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals, int levels); From 7744de25ea6a79e072a952a18040e5708848d3bc Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 2 Mar 2021 16:05:50 +0300 Subject: [PATCH 09/55] just render colors --- modules/rgbd/src/colored_kinfu.cpp | 5 +- modules/rgbd/src/colored_tsdf.cpp | 62 ++++++++++++++++++++++-- modules/rgbd/src/kinfu_frame.cpp | 78 ++++++++++++++++++++++++++++++ modules/rgbd/src/kinfu_frame.hpp | 1 + 4 files changed, 139 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index a1c31503a06..8cae6e84724 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -311,13 +311,14 @@ void ColoredKinFuImpl::render(OutputArray image, const Matx44f& _camera if((cameraPose.rotation() == _pose.rotation() && cameraPose.translation() == _pose.translation()) || (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) { - renderPointsNormals(pyrPoints[0], pyrNormals[0], image, params.lightPose); + renderPointsNormalsColors(pyrPoints[0], pyrNormals[0], pyrColors[0],image, params.lightPose); } else { MatType points, normals, colors; volume->raycast(_cameraPose, params.intr, params.frameSize, points, normals, colors); - renderPointsNormals(points, normals, image, params.lightPose); + //renderPointsNormals(points, normals, image, params.lightPose); + renderPointsNormalsColors(points, normals, colors, image, params.lightPose); } } diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 721356516fd..4af308aa30e 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -79,6 +79,7 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume float interpolateVoxel(const cv::Point3f& p) const; Point3f getNormalVoxel(const cv::Point3f& p) const; + Point3f getColorVoxel(const cv::Point3f& p) const; Vec4i volStrides; Vec6f frameParams; @@ -248,14 +249,60 @@ inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& p) const return nv < 0.0001f ? nan3 : an / nv; } +inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const +{ + const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; + const RGBTsdfVoxel* volData = volume.ptr(); + + if(p.x < 1 || p.x >= volResolution.x - 2 || + p.y < 1 || p.y >= volResolution.y - 2 || + p.z < 1 || p.z >= volResolution.z - 2) + return nan3; + + int ix = cvFloor(p.x); + int iy = cvFloor(p.y); + int iz = cvFloor(p.z); + + float tx = p.x - ix; + float ty = p.y - iy; + float tz = p.z - iz; + + int coordBase = ix*xdim + iy*ydim + iz*zdim; + float r = 0, g = 0, b = 0; + /* + for(int i = 0; i < 8; i++) + for (int c = 0; c < 3; c++) + { + r = volData[neighbourCoords[i] + coordBase + 1 * volDims[c]].r; + g = volData[neighbourCoords[i] + coordBase + 1 * volDims[c]].g; + b = volData[neighbourCoords[i] + coordBase + 1 * volDims[c]].b; + } + return Point3f(r / 24.f, g / 24.f, b / 24.f); + */ + int counter = 0; + for (int i = 0; i < 8; i++) + { + if (volData[neighbourCoords[i] + coordBase].r == volData[neighbourCoords[i] + coordBase].r) + { + r += volData[neighbourCoords[i] + coordBase].r; + g += volData[neighbourCoords[i] + coordBase].g; + b += volData[neighbourCoords[i] + coordBase].b; + } + } + return Point3f(r / 24.f, g / 24.f, b / 24.f); + + +} + struct RaycastInvoker : ParallelLoopBody { - RaycastInvoker(Points& _points, Normals& _normals, const Matx44f& cameraPose, + RaycastInvoker(Points& _points, Normals& _normals, Colors& _colors, const Matx44f& cameraPose, const Intr& intrinsics, const ColoredTSDFVolumeCPU& _volume) : ParallelLoopBody(), points(_points), normals(_normals), + colors(_colors), volume(_volume), tstep(volume.truncDist * volume.raycastStepFactor), // We do subtract voxel size to minimize checks after @@ -280,10 +327,11 @@ struct RaycastInvoker : ParallelLoopBody { ptype* ptsRow = points[y]; ptype* nrmRow = normals[y]; + ptype* clrRow = colors[y]; for(int x = 0; x < points.cols; x++) { - Point3f point = nan3, normal = nan3; + Point3f point = nan3, normal = nan3, color = nan3; Point3f orig = camTrans; // direction through pixel in volume space @@ -358,11 +406,12 @@ struct RaycastInvoker : ParallelLoopBody { Point3f pv = (orig + dir*ts); Point3f nv = volume.getNormalVoxel(pv); - + Point3f cv = volume.getColorVoxel(pv); if(!isNaN(nv)) { //convert pv and nv to camera space normal = volRot * nv; + color = cv; // interpolation optimized a little point = vol2cam * (pv*volume.voxelSize); } @@ -371,12 +420,14 @@ struct RaycastInvoker : ParallelLoopBody } ptsRow[x] = toPtype(point); nrmRow[x] = toPtype(normal); + clrRow[x] = toPtype(color); } } } Points& points; Normals& normals; + Colors& colors; const ColoredTSDFVolumeCPU& volume; const float tstep; @@ -399,11 +450,12 @@ void ColoredTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrin _points.create (frameSize, POINT_TYPE); _normals.create(frameSize, POINT_TYPE); + _colors.create(frameSize, POINT_TYPE); Points points = _points.getMat(); Normals normals = _normals.getMat(); - - RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); + Colors colors = _colors.getMat(); + RaycastInvoker ri(points, normals, colors, cameraPose, intrinsics, *this); const int nstripes = -1; parallel_for_(Range(0, points.rows), ri, nstripes); diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index 9ddcc19b0b5..ff60108b4ee 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -105,6 +105,57 @@ struct RenderInvoker : ParallelLoopBody Size sz; }; +struct RenderColorInvoker : ParallelLoopBody +{ + RenderColorInvoker(const Points& _points, const Normals& _normals, const Colors& _colors, Mat_& _img, Affine3f _lightPose, Size _sz) : + ParallelLoopBody(), + points(_points), + normals(_normals), + colors(_colors), + img(_img), + lightPose(_lightPose), + sz(_sz) + { } + + virtual void operator ()(const Range& range) const override + { + for(int y = range.start; y < range.end; y++) + { + Vec4b* imgRow = img[y]; + const ptype* ptsRow = points[y]; + const ptype* nrmRow = normals[y]; + const ptype* clrRow = colors[y]; + + for(int x = 0; x < sz.width; x++) + { + Point3f p = fromPtype(ptsRow[x]); + Point3f n = fromPtype(nrmRow[x]); + Point3f c = fromPtype(clrRow[x]); + + Vec4b color; + + if(isNaN(p) || isNaN(c)) + { + color = Vec4b(0, 32, 0, 0); + } + else + { + color = Vec4b(c.x, c.y, c.z, 0); + } + + imgRow[x] = color; + } + } + } + + const Points& points; + const Normals& normals; + const Colors& colors; + Mat_& img; + Affine3f lightPose; + Size sz; +}; + void pyrDownPointsNormals(const Points p, const Normals n, Points &pdown, Normals &ndown) { @@ -688,6 +739,33 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im parallel_for_(range, ri, nstripes); } +void renderPointsNormalsColors(InputArray _points, InputArray _normals, InputArray _colors, OutputArray image, Affine3f lightPose) +{ + CV_TRACE_FUNCTION(); + + CV_Assert(_points.size().area() > 0); + CV_Assert(_points.size() == _normals.size()); + + Size sz = _points.size(); + image.create(sz, CV_8UC4); + + CV_OCL_RUN(_points.isUMat() && _normals.isUMat() && image.isUMat(), + ocl_renderPointsNormals(_points.getUMat(), + _normals.getUMat(), + image.getUMat(), lightPose)) + + Points points = _points.getMat(); + Normals normals = _normals.getMat(); + Colors colors = _colors.getMat(); + + Mat_ img = image.getMat(); + + RenderColorInvoker ri(points, normals, colors, img, lightPose, sz); + Range range(0, sz.height); + const int nstripes = -1; + parallel_for_(range, ri, nstripes); +} + void makeFrameFromDepth(InputArray _depth, OutputArray pyrPoints, OutputArray pyrNormals, diff --git a/modules/rgbd/src/kinfu_frame.hpp b/modules/rgbd/src/kinfu_frame.hpp index 61b2b528336..17f44dbafe4 100644 --- a/modules/rgbd/src/kinfu_frame.hpp +++ b/modules/rgbd/src/kinfu_frame.hpp @@ -85,6 +85,7 @@ typedef cv::Mat_< depthType > Depth; typedef cv::Mat_< ptype > Rgb; void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, cv::Affine3f lightPose); +void renderPointsNormalsColors(InputArray _points, InputArray _normals, InputArray _colors, OutputArray image, Affine3f lightPose); void makeFrameFromDepth(InputArray depth, OutputArray pyrPoints, OutputArray pyrNormals, const Intr intr, int levels, float depthFactor, float sigmaDepth, float sigmaSpatial, int kernelSize, From b9f4f85c15fe9025628907a88f97fc181a43a520 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 2 Mar 2021 18:47:14 +0300 Subject: [PATCH 10/55] color processing fix --- modules/rgbd/src/colored_tsdf.cpp | 18 +++++++++++++----- modules/rgbd/src/kinfu_frame.cpp | 1 - modules/rgbd/src/tsdf_functions.cpp | 7 ++++--- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 4af308aa30e..b16e6daac8b 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -279,17 +279,25 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const } return Point3f(r / 24.f, g / 24.f, b / 24.f); */ - int counter = 0; + /* + float counter = 0; + //std::cout << "======" << std::endl; for (int i = 0; i < 8; i++) { if (volData[neighbourCoords[i] + coordBase].r == volData[neighbourCoords[i] + coordBase].r) { - r += volData[neighbourCoords[i] + coordBase].r; - g += volData[neighbourCoords[i] + coordBase].g; - b += volData[neighbourCoords[i] + coordBase].b; + r += (float) volData[neighbourCoords[i] + coordBase].r; + g += (float) volData[neighbourCoords[i] + coordBase].g; + b += (float) volData[neighbourCoords[i] + coordBase].b; + //std::cout < Date: Wed, 3 Mar 2021 15:22:29 +0300 Subject: [PATCH 11/55] intergrate color fix --- modules/rgbd/samples/colored_kinfu_demo.cpp | 2 +- modules/rgbd/src/colored_kinfu.cpp | 7 +++-- modules/rgbd/src/colored_tsdf.cpp | 19 ++++++++++---- modules/rgbd/src/tsdf_functions.cpp | 29 ++++++++++++++++----- 4 files changed, 43 insertions(+), 14 deletions(-) diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index 077de847b08..cb9506cf6af 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -209,7 +209,7 @@ int main(int argc, char **argv) if(!idle) { imshow("depth", cvt8); - + //std::cout << rgb_frame.rows << " " << rgb_frame.cols << std::endl; if(!kf->update(frame, rgb_frame)) { kf->reset(); diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 8cae6e84724..7d84bb167eb 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -248,11 +248,14 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r else depth = _depth; - if(_rgb.type() != POINT_TYPE) - _rgb.convertTo(rgb, POINT_TYPE); + if(_rgb.type() != COLOR_TYPE) + _rgb.convertTo(rgb, COLOR_TYPE); else rgb = _rgb; + //std::cout << rgb << std::endl; + std::cout << "-rgb " << rgb.rows << " " << rgb.cols << std::endl; + std::cout << "-depth " << depth.rows << " " << depth.cols << std::endl; std::vector newPoints, newNormals, newColors; makeColoredFrameFromDepth(depth, newPoints, newNormals, newColors, params.intr, params.pyramidLevels, diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index b16e6daac8b..4b9998854f1 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -156,6 +156,11 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d Depth depth = _depth.getMat(); Rgb rgb = _rgb.getMat(); + std::cout << "+rgb " << rgb.rows << " " << rgb.cols << std::endl; + std::cout << "+depth " << depth.rows << " " << depth.cols << std::endl; + + //std::cout << rgb.rows << " " << rgb.cols << std::endl; + //std::cout << rgb << std::endl; Vec6f newParams((float)depth.rows, (float)depth.cols, intrinsics.fx, intrinsics.fy, intrinsics.cx, intrinsics.cy); @@ -279,12 +284,16 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const } return Point3f(r / 24.f, g / 24.f, b / 24.f); */ - /* + + float mainRGBsum = (float) (volData[coordBase].r + volData[coordBase].g + volData[coordBase].b); + + float counter = 0; //std::cout << "======" << std::endl; for (int i = 0; i < 8; i++) { - if (volData[neighbourCoords[i] + coordBase].r == volData[neighbourCoords[i] + coordBase].r) + float sum = (float) (volData[neighbourCoords[i] + coordBase].r + volData[neighbourCoords[i] + coordBase].g + volData[neighbourCoords[i] + coordBase].b ); + if (volData[neighbourCoords[i] + coordBase].r == volData[neighbourCoords[i] + coordBase].r && abs(sum - mainRGBsum) < 1000) { r += (float) volData[neighbourCoords[i] + coordBase].r; g += (float) volData[neighbourCoords[i] + coordBase].g; @@ -293,9 +302,9 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const counter+=1.0f; } } - */ - //Point3f res(r / counter, g / counter, b / counter); - Point3f res(volData[coordBase].r, volData[coordBase].g, volData[coordBase].b); + + Point3f res(r / counter, g / counter, b / counter); + //Point3f res(volData[coordBase].r, volData[coordBase].g, volData[coordBase].b); //std::cout << counter << " " << res << std::endl; return res; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index d60f3add387..3893bf1d97b 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -431,7 +431,7 @@ void integrateRGBVolumeUnit( cv::Affine3f vpose(_pose); Depth depth = _depth.getMat(); Rgb color = _rgb.getMat(); - + //std::cout << color << std::endl; Range integrateRange(0, volResolution.x); Mat volume = _volume.getMat(); @@ -512,7 +512,12 @@ void integrateRGBVolumeUnit( if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows)) continue; float pixNorm = pixNorms.at(_v, _u); - Point3i colorRGB = color.at(_v, _u); + if (!(_u >= 0 && _u < color.cols && _v >= 0 && _v < color.rows)) + continue; + + Vec4f colorRGB = color.at(_v, _u); + //std::cout << colorRGB << std::endl; + //std::cout << color.type() << std::endl; // difference between distances of point and of surface to camera float sdf = pixNorm * (v * dfac - camSpacePt.z); // possible alternative is: @@ -528,11 +533,23 @@ void integrateRGBVolumeUnit( ColorType& g = voxel.g; ColorType& b = voxel.b; - // update TSDF - r = (ColorType) ((int) (r * weight) + (colorRGB.x) / 65536) / (weight + 1); - g = (ColorType) ((int) (g * weight) + (colorRGB.y) / 65536) / (weight + 1); - b = (ColorType) ((int) (b * weight) + (colorRGB.z) / 65536) / (weight + 1); + /* + if (abs(((float)(r + g + b)) - (colorRGB.x + colorRGB.y + colorRGB.z)) < 1000 || weight < 1) + { + r = (ColorType)((int)(r * weight) + (colorRGB.x) / 65536) / (weight + 1); + g = (ColorType)((int)(g * weight) + (colorRGB.y) / 65536) / (weight + 1); + b = (ColorType)((int)(b * weight) + (colorRGB.z) / 65536) / (weight + 1); + } + */ + if (abs(((float)(r + g + b)) - (colorRGB[0] + colorRGB[1] + colorRGB[2])) < 1000 || weight < 1) + { + r = (ColorType)((float)(r * weight) + (colorRGB[0])) / (weight + 1); + g = (ColorType)((float)(g * weight) + (colorRGB[1])) / (weight + 1); + b = (ColorType)((float)(b * weight) + (colorRGB[2])) / (weight + 1); + } //std::cout<<"("< Date: Thu, 4 Mar 2021 12:22:24 +0300 Subject: [PATCH 12/55] add simple shift; minor fixes --- modules/rgbd/src/colored_kinfu.cpp | 4 ++-- modules/rgbd/src/colored_tsdf.cpp | 9 +++++---- modules/rgbd/src/tsdf_functions.cpp | 11 +++++++---- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 7d84bb167eb..0473ee5c55a 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -254,8 +254,8 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r rgb = _rgb; //std::cout << rgb << std::endl; - std::cout << "-rgb " << rgb.rows << " " << rgb.cols << std::endl; - std::cout << "-depth " << depth.rows << " " << depth.cols << std::endl; + //std::cout << "-rgb " << rgb.rows << " " << rgb.cols << std::endl; + //std::cout << "-depth " << depth.rows << " " << depth.cols << std::endl; std::vector newPoints, newNormals, newColors; makeColoredFrameFromDepth(depth, newPoints, newNormals, newColors, params.intr, params.pyramidLevels, diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 4b9998854f1..07b76fb48a7 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -156,8 +156,8 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d Depth depth = _depth.getMat(); Rgb rgb = _rgb.getMat(); - std::cout << "+rgb " << rgb.rows << " " << rgb.cols << std::endl; - std::cout << "+depth " << depth.rows << " " << depth.cols << std::endl; + //std::cout << "+rgb " << rgb.rows << " " << rgb.cols << std::endl; + //std::cout << "+depth " << depth.rows << " " << depth.cols << std::endl; //std::cout << rgb.rows << " " << rgb.cols << std::endl; //std::cout << rgb << std::endl; @@ -284,7 +284,7 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const } return Point3f(r / 24.f, g / 24.f, b / 24.f); */ - + /* float mainRGBsum = (float) (volData[coordBase].r + volData[coordBase].g + volData[coordBase].b); @@ -304,7 +304,8 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const } Point3f res(r / counter, g / counter, b / counter); - //Point3f res(volData[coordBase].r, volData[coordBase].g, volData[coordBase].b); + */ + Point3f res(volData[coordBase].r, volData[coordBase].g, volData[coordBase].b); //std::cout << counter << " " << res << std::endl; return res; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 3893bf1d97b..df42b26b8da 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -509,13 +509,16 @@ void integrateRGBVolumeUnit( int _u = projected.x; int _v = projected.y; - if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows)) + int _uShift = 5; + int _vShift = -5; + if (!(_v >= 0 && _v < depth.rows && _u >= 0 && _u < depth.cols && + (_v + _vShift) >= 0 && (_v + _vShift) < depth.rows && (_u + _uShift) >= 0 && (_u + _uShift) < depth.cols)) continue; float pixNorm = pixNorms.at(_v, _u); - if (!(_u >= 0 && _u < color.cols && _v >= 0 && _v < color.rows)) - continue; + //if (!(_u >= 0 && _u < color.cols && _v >= 0 && _v < color.rows)) + // continue; - Vec4f colorRGB = color.at(_v, _u); + Vec3f colorRGB = color.at(_v + _vShift, _u + _uShift); //std::cout << colorRGB << std::endl; //std::cout << color.type() << std::endl; // difference between distances of point and of surface to camera From aea0de9b94dd6b8e974ca1339da26b1eb0a3ccd9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 4 Mar 2021 12:39:17 +0300 Subject: [PATCH 13/55] add cv_errors for not implemented functions --- modules/rgbd/src/colored_tsdf.cpp | 6 ++++-- modules/rgbd/src/hash_tsdf.cpp | 6 ++++-- modules/rgbd/src/precomp.hpp | 1 + modules/rgbd/src/tsdf.hpp | 6 ++++-- 4 files changed, 13 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 07b76fb48a7..c0e524003cb 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -63,13 +63,15 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume ColoredTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Vec3i _resolution, bool zFirstMemOrder = true); virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) override {}; + const kinfu::Intr& intrinsics, const int frameId = 0) override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void integrate(InputArray _depth, InputArray _rgb, 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, OutputArray colors) const override; virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray points, OutputArray normals) const override {}; + OutputArray points, OutputArray normals) const override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void fetchNormals(InputArray points, OutputArray _normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index d06666f0b48..b8e936cd7f0 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -898,13 +898,15 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void markActive(const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId); virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) override {}; + const kinfu::Intr& intrinsics, const int frameId = 0) override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray points, OutputArray normals, OutputArray colors) const override {}; + OutputArray points, OutputArray normals, OutputArray colors) const override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; diff --git a/modules/rgbd/src/precomp.hpp b/modules/rgbd/src/precomp.hpp index bad630705d7..04d09aeca4e 100644 --- a/modules/rgbd/src/precomp.hpp +++ b/modules/rgbd/src/precomp.hpp @@ -19,6 +19,7 @@ #include "opencv2/core/private.hpp" #include "opencv2/core/hal/intrin.hpp" #include "opencv2/core/ocl.hpp" +#include "opencv2/core.hpp" #include "opencv2/imgproc.hpp" #include "opencv2/calib3d.hpp" #include "opencv2/rgbd.hpp" diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index fd3001afb3c..4985de66eda 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -41,9 +41,11 @@ class TSDFVolume : public Volume virtual ~TSDFVolume() = default; virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) override {}; + const kinfu::Intr& intrinsics, const int frameId = 0) override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray points, OutputArray normals, OutputArray colors) const override {}; + OutputArray points, OutputArray normals, OutputArray colors) const override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; public: Point3i volResolution; From e04a6ef7819f3555290d4820d8471520732b8838 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 4 Mar 2021 17:53:20 +0300 Subject: [PATCH 14/55] minor fixes --- .../include/opencv2/rgbd/colored_kinfu.hpp | 1 + modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 2 +- .../rgbd/include/opencv2/rgbd/large_kinfu.hpp | 2 +- modules/rgbd/include/opencv2/rgbd/volume.hpp | 2 +- modules/rgbd/samples/colored_kinfu_demo.cpp | 4 +- modules/rgbd/samples/io_utils.hpp | 42 ++++++++++++++----- modules/rgbd/src/colored_kinfu.cpp | 23 +++++++++- modules/rgbd/src/colored_tsdf.cpp | 6 +-- modules/rgbd/src/hash_tsdf.cpp | 8 ++-- modules/rgbd/src/tsdf.hpp | 2 +- modules/rgbd/src/tsdf_functions.cpp | 16 ++++++- modules/rgbd/src/tsdf_functions.hpp | 2 +- 12 files changed, 84 insertions(+), 26 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp index 74b200e5611..84b56d8b7b5 100644 --- a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -84,6 +84,7 @@ struct CV_EXPORTS_W Params /** @brief camera intrinsics */ CV_PROP_RW Matx33f intr; + CV_PROP_RW Matx33f rgb_intr; /** @brief pre-scale per 1 meter for input values diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 95f732b6769..e3793bc5bb7 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -81,7 +81,7 @@ struct CV_EXPORTS_W Params /** @brief camera intrinsics */ CV_PROP_RW Matx33f intr; - + CV_PROP_RW Matx33f rgb_intr; /** @brief pre-scale per 1 meter for input values Typical values are: diff --git a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp index 7f1428c6d51..a5de2dbed02 100644 --- a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp @@ -40,7 +40,7 @@ struct CV_EXPORTS_W Params /** @brief camera intrinsics */ CV_PROP_RW Matx33f intr; - + CV_PROP_RW Matx33f rgb_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 diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 792a5d5ebe9..84861b59e61 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -31,7 +31,7 @@ class CV_EXPORTS_W Volume virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) = 0; virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) = 0; + const kinfu::Intr& intrinsics, const Intr& rgb_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 raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index cb9506cf6af..986c3a562f4 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -168,7 +168,7 @@ int main(int argc, char **argv) { if(depthWriter) depthWriter->append(frame); - + //std::cout << << std::endl; UMat rgb_frame = rgbs->getRGB(); //imshow("rgb", rgb_frame); #ifdef HAVE_OPENCV_VIZ @@ -213,7 +213,7 @@ int main(int argc, char **argv) if(!kf->update(frame, rgb_frame)) { kf->reset(); - std::cout << "reset" << std::endl; + //std::cout << "reset" << std::endl; } #ifdef HAVE_OPENCV_VIZ else diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index d6374314870..293b352af49 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -110,6 +110,15 @@ static const float cy = 247.6f; static const float k1 = 0.0f; static const float k2 = 0.0f; static const float k3 = 0.0f; + +static const Size rgb_frameSize = Size(640, 480); +static const float rgb_focal = 525.0f; +static const float rgb_cx = 319.5f; +static const float rgb_cy = 239.5f; +static const float rgb_k1 = 0.0f; +static const float rgb_k2 = 0.0f; +static const float rgb_k3 = 0.0f; + }; // namespace Kinect2Params struct DepthSource @@ -522,7 +531,7 @@ struct RGBSource bool empty() { return rgbFileList.empty() && !(vc.isOpened()); } - void updateIntrinsics(Matx33f& _intrinsics, Size& _frameSize, float& _depthFactor) + void updateIntrinsics(Matx33f& _intrinsics, Matx33f& _rgb_intrinsics, Size& _frameSize, float& _depthFactor) { if (vc.isOpened()) { @@ -532,15 +541,20 @@ struct RGBSource // it's recommended to calibrate sensor to obtain its intrinsics float fx, fy, cx, cy; + float rgb_fx, rgb_fy, rgb_cx, rgb_cy; float depthFactor = 1000.f; - Size frameSize; + Size frameSize, rgb_frameSize; if (sourceType == Type::RGB_KINECT2) { - fx = fy = Kinect2Params::rgb_focal; - cx = Kinect2Params::rgb_cx; - cy = Kinect2Params::rgb_cy; - - frameSize = Kinect2Params::rgb_frameSize; + fx = fy = Kinect2Params::focal; + cx = Kinect2Params::cx; + cy = Kinect2Params::cy; + + rgb_fx = rgb_fy = Kinect2Params::rgb_focal; + rgb_cx = Kinect2Params::rgb_cx; + rgb_cy = Kinect2Params::rgb_cy; + + rgb_frameSize = Kinect2Params::rgb_frameSize; } else if (sourceType == Type::RGB_ASTRA) { @@ -550,6 +564,12 @@ struct RGBSource cy = AstraParams::cy; frameSize = AstraParams::frameSize; + + rgb_fx = rgb_fy = AstraParams::rgb_focal; + rgb_cx = AstraParams::rgb_cx; + rgb_cy = AstraParams::rgb_cy; + + rgb_frameSize = AstraParams::rgb_frameSize; } else { @@ -573,7 +593,9 @@ struct RGBSource } Matx33f camMatrix = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); + Matx33f rgb_camMatrix = Matx33f(rgb_fx, 0, rgb_cx, 0, rgb_fy, rgb_cy, 0, 0, 1); _intrinsics = camMatrix; + _rgb_intrinsics = rgb_camMatrix; _frameSize = frameSize; _depthFactor = depthFactor; } @@ -611,7 +633,7 @@ struct RGBSource { if (vc.isOpened()) { - updateIntrinsics(params.intr, params.frameSize, params.depthFactor); + updateIntrinsics(params.intr, params.rgb_intr, params.frameSize, params.depthFactor); updateVolumeParams(params.volumeParams.resolution, params.volumeParams.voxelSize, params.volumeParams.tsdfTruncDist, params.volumeParams.pose, params.truncateThreshold); @@ -634,7 +656,7 @@ struct RGBSource { if (vc.isOpened()) { - updateIntrinsics(params.intr, params.frameSize, params.depthFactor); + updateIntrinsics(params.intr, params.rgb_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); @@ -656,7 +678,7 @@ struct RGBSource { if (vc.isOpened()) { - updateIntrinsics(params.intr, params.frameSize, params.depthFactor); + updateIntrinsics(params.intr, params.rgb_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); diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 0473ee5c55a..bfad0651339 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -41,6 +41,24 @@ Ptr Params::defaultParams() 0, fy, cy, 0, 0, 1); + float rgb_fx, rgb_fy, rgb_cx, rgb_cy; + rgb_fx = rgb_fy = 525.f; + //rgb_cx = p.frameSize.width / 2 - 0.5f; + //rgb_cy = p.frameSize.height / 2 - 0.5f; + rgb_cx = 319.5f; + rgb_cy = 239.5f; + p.rgb_intr = Matx33f(rgb_fx, 0, rgb_cx, 0, rgb_fy, rgb_cy, 0, 0, 1); + + /* + static const Size rgb_frameSize = Size(640, 480); +static const float rgb_focal = 525.0f; +static const float rgb_cx = 319.5f; +static const float rgb_cy = 239.5f; +static const float rgb_k1 = 0.0f; +static const float rgb_k2 = 0.0f; +static const float rgb_k3 = 0.0f + */ + // 5000 for the 16-bit PNG files // 1 for the 32-bit float images in the ROS bag files p.depthFactor = 5000; @@ -256,6 +274,7 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r //std::cout << rgb << std::endl; //std::cout << "-rgb " << rgb.rows << " " << rgb.cols << std::endl; //std::cout << "-depth " << depth.rows << " " << depth.cols << std::endl; + std::vector newPoints, newNormals, newColors; makeColoredFrameFromDepth(depth, newPoints, newNormals, newColors, params.intr, params.pyramidLevels, @@ -267,7 +286,7 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r if(frameCounter == 0) { // use depth instead of distance - volume->integrate(depth, rgb, params.depthFactor, pose, params.intr); + volume->integrate(depth, rgb, params.depthFactor, pose, params.intr, params.rgb_intr); pyrPoints = newPoints; pyrNormals = newNormals; pyrColors = newColors; @@ -287,7 +306,7 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement) { // use depth instead of distance - volume->integrate(depth, rgb, params.depthFactor, pose, params.intr); + volume->integrate(depth, rgb, params.depthFactor, pose, params.intr, params.rgb_intr); } MatType& points = pyrPoints [0]; MatType& normals = pyrNormals[0]; diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index c0e524003cb..f29ea3dcaba 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -66,7 +66,7 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume const kinfu::Intr& intrinsics, const int frameId = 0) override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) override; + const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override; virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, @@ -149,7 +149,7 @@ RGBTsdfVoxel ColoredTSDFVolumeCPU::at(const Vec3i& volumeIdx) const // use depth instead of distance (optimization) void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const Intr& intrinsics, const int frameId) + const Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId) { CV_TRACE_FUNCTION(); CV_UNUSED(frameId); @@ -173,7 +173,7 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d } integrateRGBVolumeUnit(truncDist, voxelSize, maxWeight, (this->pose).matrix, volResolution, volStrides, depth, rgb, - depthFactor, cameraPose, intrinsics, pixNorms, volume); + depthFactor, cameraPose, intrinsics, rgb_intrinsics, pixNorms, volume); } diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index b8e936cd7f0..badd3672517 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -100,13 +100,15 @@ class HashTSDFVolumeCPU : public HashTSDFVolume HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) override {}; + const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray points, OutputArray normals, OutputArray colors) const override {}; + OutputArray points, OutputArray normals, OutputArray colors) const override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; @@ -898,7 +900,7 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void markActive(const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId); virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) override + const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 4985de66eda..0222378458a 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -41,7 +41,7 @@ class TSDFVolume : public Volume virtual ~TSDFVolume() = default; virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) override + const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals, OutputArray colors) const override diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index df42b26b8da..5a52d9c6658 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -422,7 +422,7 @@ void integrateRGBVolumeUnit( float truncDist, float voxelSize, int maxWeight, cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume) + const cv::kinfu::Intr& intrinsics, const cv::kinfu::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume) { CV_TRACE_FUNCTION(); @@ -437,6 +437,9 @@ void integrateRGBVolumeUnit( Mat volume = _volume.getMat(); Mat pixNorms = _pixNorms.getMat(); const Intr::Projector proj(intrinsics.makeProjector()); + + const Intr::Projector projRGB(rgb_intrinsics); + //std::cout << rgb_intrinsics.cx << " " << rgb_intrinsics.cy << " " << rgb_intrinsics.fx << " " << rgb_intrinsics.fy << " " << std::endl; const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * vpose); const float truncDistInv(1.f / truncDist); const float dfac(1.f / depthFactor); @@ -501,6 +504,8 @@ void integrateRGBVolumeUnit( Point3f camPixVec; Point2f projected = proj(camSpacePt, camPixVec); + //Point2f projectedRGB = projRGB(camSpacePt, camPixVec); + depthType v = bilinearDepth(depth, projected); if (v == 0) { @@ -511,14 +516,23 @@ void integrateRGBVolumeUnit( int _v = projected.y; int _uShift = 5; int _vShift = -5; + + //int rgb_u = projectedRGB.x; + //int rgb_v = projectedRGB.y; + if (!(_v >= 0 && _v < depth.rows && _u >= 0 && _u < depth.cols && (_v + _vShift) >= 0 && (_v + _vShift) < depth.rows && (_u + _uShift) >= 0 && (_u + _uShift) < depth.cols)) + //rgb_v >= 0 && rgb_v < depth.rows && rgb_u >= 0 && rgb_u < depth.cols)) continue; + + //std::cout << "pix " << _v << " " << _u << " | rgb " << rgb_v << " "<< rgb_u << std::endl; + float pixNorm = pixNorms.at(_v, _u); //if (!(_u >= 0 && _u < color.cols && _v >= 0 && _v < color.rows)) // continue; Vec3f colorRGB = color.at(_v + _vShift, _u + _uShift); + //Vec3f colorRGB = color.at(rgb_v, rgb_u); //std::cout << colorRGB << std::endl; //std::cout << color.type() << std::endl; // difference between distances of point and of surface to camera diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index f9c34d5e290..75e0e930d71 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -50,7 +50,7 @@ void integrateRGBVolumeUnit( float truncDist, float voxelSize, int maxWeight, cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); + const cv::kinfu::Intr& intrinsics, const cv::kinfu::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume); class CustomHashSet From e7b70d515a9a9a23bbadabba93b0f3c99d5c4684 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 5 Mar 2021 12:58:52 +0300 Subject: [PATCH 15/55] made calibration --- modules/rgbd/src/colored_kinfu.cpp | 8 +++++--- modules/rgbd/src/tsdf_functions.cpp | 19 ++++++++++--------- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index bfad0651339..6411da33b21 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -42,9 +42,11 @@ Ptr Params::defaultParams() 0, 0, 1); float rgb_fx, rgb_fy, rgb_cx, rgb_cy; - rgb_fx = rgb_fy = 525.f; - //rgb_cx = p.frameSize.width / 2 - 0.5f; - //rgb_cy = p.frameSize.height / 2 - 0.5f; + //rgb_fx = rgb_fy = 525.f; + //rgb_cx = 319.5f; + //rgb_cy = 239.5f; + rgb_fx = 515.0f; + rgb_fy = 550.0f; rgb_cx = 319.5f; rgb_cy = 239.5f; p.rgb_intr = Matx33f(rgb_fx, 0, rgb_cx, 0, rgb_fy, rgb_cy, 0, 0, 1); diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 5a52d9c6658..b6b170e9f6b 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -439,7 +439,7 @@ void integrateRGBVolumeUnit( const Intr::Projector proj(intrinsics.makeProjector()); const Intr::Projector projRGB(rgb_intrinsics); - //std::cout << rgb_intrinsics.cx << " " << rgb_intrinsics.cy << " " << rgb_intrinsics.fx << " " << rgb_intrinsics.fy << " " << std::endl; + std::cout << rgb_intrinsics.cx << " " << rgb_intrinsics.cy << " " << rgb_intrinsics.fx << " " << rgb_intrinsics.fy << " " << std::endl; const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * vpose); const float truncDistInv(1.f / truncDist); const float dfac(1.f / depthFactor); @@ -504,7 +504,7 @@ void integrateRGBVolumeUnit( Point3f camPixVec; Point2f projected = proj(camSpacePt, camPixVec); - //Point2f projectedRGB = projRGB(camSpacePt, camPixVec); + Point2f projectedRGB = projRGB(camSpacePt, camPixVec); depthType v = bilinearDepth(depth, projected); @@ -517,12 +517,13 @@ void integrateRGBVolumeUnit( int _uShift = 5; int _vShift = -5; - //int rgb_u = projectedRGB.x; - //int rgb_v = projectedRGB.y; - + int rgb_u = projectedRGB.x; + int rgb_v = projectedRGB.y; + //std::cout <<_u << " " << _v << " | " << rgb_u << " " << rgb_v << std::endl; + if (!(_v >= 0 && _v < depth.rows && _u >= 0 && _u < depth.cols && - (_v + _vShift) >= 0 && (_v + _vShift) < depth.rows && (_u + _uShift) >= 0 && (_u + _uShift) < depth.cols)) - //rgb_v >= 0 && rgb_v < depth.rows && rgb_u >= 0 && rgb_u < depth.cols)) + //(_v + _vShift) >= 0 && (_v + _vShift) < depth.rows && (_u + _uShift) >= 0 && (_u + _uShift) < depth.cols)) + rgb_v >= 0 && rgb_v < depth.rows && rgb_u >= 0 && rgb_u < depth.cols)) continue; //std::cout << "pix " << _v << " " << _u << " | rgb " << rgb_v << " "<< rgb_u << std::endl; @@ -531,8 +532,8 @@ void integrateRGBVolumeUnit( //if (!(_u >= 0 && _u < color.cols && _v >= 0 && _v < color.rows)) // continue; - Vec3f colorRGB = color.at(_v + _vShift, _u + _uShift); - //Vec3f colorRGB = color.at(rgb_v, rgb_u); + //Vec3f colorRGB = color.at(_v + _vShift, _u + _uShift); + Vec3f colorRGB = color.at(rgb_v, rgb_u); //std::cout << colorRGB << std::endl; //std::cout << color.type() << std::endl; // difference between distances of point and of surface to camera From a1290c7664d5b165c6378a1eb01f2f4168b8ef39 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 5 Mar 2021 14:53:53 +0300 Subject: [PATCH 16/55] makeColoredFrameFromDepth works --- modules/rgbd/src/colored_kinfu.cpp | 4 +++- modules/rgbd/src/kinfu_frame.cpp | 28 +++++++++++++++++----------- modules/rgbd/src/kinfu_frame.hpp | 4 ++-- modules/rgbd/src/tsdf_functions.cpp | 2 +- 4 files changed, 23 insertions(+), 15 deletions(-) diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 6411da33b21..807a960fa2b 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -278,7 +278,9 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r //std::cout << "-depth " << depth.rows << " " << depth.cols << std::endl; std::vector newPoints, newNormals, newColors; - makeColoredFrameFromDepth(depth, newPoints, newNormals, newColors, params.intr, + makeColoredFrameFromDepth(depth, rgb, + newPoints, newNormals, newColors, + params.intr, params.rgb_intr, params.pyramidLevels, params.depthFactor, params.bilateral_sigma_depth, diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index d7951284211..c3cfc24cfab 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -327,14 +327,16 @@ struct ComputePointsNormalsInvoker : ParallelLoopBody struct ComputePointsNormalsColorsInvoker : ParallelLoopBody { - ComputePointsNormalsColorsInvoker(const Depth& _depth, Points& _points, Normals& _normals, Colors& _colors, - const Intr::Reprojector& _reproj, float _dfac) : + ComputePointsNormalsColorsInvoker(const Depth& _depth, const Colors& _rgb, Points& _points, Normals& _normals, Colors& _colors, + const Intr::Reprojector& _reproj, const Intr::Reprojector& _rgb_reproj, float _dfac) : ParallelLoopBody(), depth(_depth), + rgb(_rgb), points(_points), normals(_normals), colors(_colors), reproj(_reproj), + rgb_reproj(_rgb_reproj), dfac(_dfac) { } @@ -354,7 +356,7 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody depthType z00 = d00*dfac; Point3f v00 = reproj(Point3f((float)x, (float)y, z00)); - Point3f p = nan3, n = nan3; + Point3f p = nan3, n = nan3, c = nan3; if(x < depth.cols - 1 && y < depth.rows - 1) { @@ -374,21 +376,24 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody cv::Vec3f vec = (v01-v00).cross(v10-v00); n = -normalize(vec); p = v00; + c = rgb.at(y, x); } } ptsRow[x] = toPtype(p); normRow[x] = toPtype(n); - clrRow[x] = toPtype(nan3); + clrRow[x] = toPtype(c); } } } const Depth& depth; + const Colors& rgb; Points& points; Normals& normals; Colors& colors; const Intr::Reprojector& reproj; + const Intr::Reprojector& rgb_reproj; float dfac; }; @@ -414,8 +419,8 @@ void computePointsNormals(const Intr intr, float depthFactor, const Depth depth, parallel_for_(range, ci, nstripes); } -void computePointsNormalsColors(const Intr intr, float depthFactor, const Depth depth, - Points points, Normals normals, Colors colors) +void computePointsNormalsColors(const Intr intr, const Intr rgb_intr, float depthFactor, + const Depth depth, const Colors rgb, Points points, Normals normals, Colors colors) { CV_TRACE_FUNCTION(); @@ -430,8 +435,9 @@ void computePointsNormalsColors(const Intr intr, float depthFactor, const Depth float dfac = 1.f/depthFactor; Intr::Reprojector reproj = intr.makeReprojector(); + Intr::Reprojector reprojRGB = rgb_intr.makeReprojector(); - ComputePointsNormalsColorsInvoker ci(depth, points, normals, colors, reproj, dfac); + ComputePointsNormalsColorsInvoker ci(depth, rgb, points, normals, colors, reproj, reprojRGB, dfac); Range range(0, depth.rows); const int nstripes = -1; parallel_for_(range, ci, nstripes); @@ -826,9 +832,9 @@ void makeFrameFromDepth(InputArray _depth, } } -void makeColoredFrameFromDepth(InputArray _depth, +void makeColoredFrameFromDepth(InputArray _depth, InputArray _rgb, OutputArray pyrPoints, OutputArray pyrNormals, OutputArray pyrColors, - const Intr intr, int levels, float depthFactor, + const Intr intr, const Intr rgb_intr, int levels, float depthFactor, float sigmaDepth, float sigmaSpatial, int kernelSize, float truncateThreshold) { @@ -843,7 +849,7 @@ void makeColoredFrameFromDepth(InputArray _depth, CV_Assert(kc == _InputArray::STD_ARRAY_MAT || kc == _InputArray::STD_VECTOR_MAT); Depth depth = _depth.getMat(); - + Colors rgb = _rgb.getMat(); // looks like OpenCV's bilateral filter works the same as KinFu's Depth smooth; Depth depthNoNans = depth.clone(); @@ -875,7 +881,7 @@ void makeColoredFrameFromDepth(InputArray _depth, Normals n = pyrNormals.getMatRef(i); Colors c = pyrColors. getMatRef(i); - computePointsNormals(intr.scale(i), depthFactor, scaled, p, n); + computePointsNormalsColors(intr.scale(i), rgb_intr.scale(i), depthFactor, scaled, rgb, p, n, c); if(i < levels - 1) { diff --git a/modules/rgbd/src/kinfu_frame.hpp b/modules/rgbd/src/kinfu_frame.hpp index 17f44dbafe4..4ba61748891 100644 --- a/modules/rgbd/src/kinfu_frame.hpp +++ b/modules/rgbd/src/kinfu_frame.hpp @@ -90,9 +90,9 @@ void makeFrameFromDepth(InputArray depth, OutputArray pyrPoints, OutputArray pyr const Intr intr, int levels, float depthFactor, float sigmaDepth, float sigmaSpatial, int kernelSize, float truncateThreshold); -void makeColoredFrameFromDepth(InputArray _depth, +void makeColoredFrameFromDepth(InputArray _depth, InputArray _rgb, OutputArray pyrPoints, OutputArray pyrNormals, OutputArray pyrColors, - const Intr intr, int levels, float depthFactor, + const Intr intr, const Intr rgb_intr, int levels, float depthFactor, float sigmaDepth, float sigmaSpatial, int kernelSize, float truncateThreshold); void buildPyramidPointsNormals(InputArray _points, InputArray _normals, diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index b6b170e9f6b..8678e0964ae 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -439,7 +439,7 @@ void integrateRGBVolumeUnit( const Intr::Projector proj(intrinsics.makeProjector()); const Intr::Projector projRGB(rgb_intrinsics); - std::cout << rgb_intrinsics.cx << " " << rgb_intrinsics.cy << " " << rgb_intrinsics.fx << " " << rgb_intrinsics.fy << " " << std::endl; + //std::cout << rgb_intrinsics.cx << " " << rgb_intrinsics.cy << " " << rgb_intrinsics.fx << " " << rgb_intrinsics.fy << " " << std::endl; const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * vpose); const float truncDistInv(1.f / truncDist); const float dfac(1.f / depthFactor); From 0479f0edae939607f0d443bcf89431eba8e09a94 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 5 Mar 2021 17:29:16 +0300 Subject: [PATCH 17/55] remove comments --- modules/rgbd/include/opencv2/rgbd/volume.hpp | 5 +++-- modules/rgbd/samples/colored_kinfu_demo.cpp | 6 ++--- modules/rgbd/src/colored_kinfu.cpp | 12 +++------- modules/rgbd/src/colored_tsdf.cpp | 23 ++------------------ modules/rgbd/src/tsdf_functions.cpp | 22 +------------------ 5 files changed, 11 insertions(+), 57 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 84861b59e61..e4c14d37b55 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -30,8 +30,9 @@ class CV_EXPORTS_W Volume virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) = 0; - virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) = 0; + virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, + const Matx44f& cameraPose, const kinfu::Intr& intrinsics, + const Intr& rgb_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 raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index 986c3a562f4..a007f5cc436 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -168,9 +168,8 @@ int main(int argc, char **argv) { if(depthWriter) depthWriter->append(frame); - //std::cout << << std::endl; UMat rgb_frame = rgbs->getRGB(); - //imshow("rgb", rgb_frame); + #ifdef HAVE_OPENCV_VIZ if(pause) { @@ -209,11 +208,10 @@ int main(int argc, char **argv) if(!idle) { imshow("depth", cvt8); - //std::cout << rgb_frame.rows << " " << rgb_frame.cols << std::endl; + imshow("rgb", rgb_frame); if(!kf->update(frame, rgb_frame)) { kf->reset(); - //std::cout << "reset" << std::endl; } #ifdef HAVE_OPENCV_VIZ else diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 807a960fa2b..6ff1475aae8 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -42,14 +42,13 @@ Ptr Params::defaultParams() 0, 0, 1); float rgb_fx, rgb_fy, rgb_cx, rgb_cy; - //rgb_fx = rgb_fy = 525.f; - //rgb_cx = 319.5f; - //rgb_cy = 239.5f; rgb_fx = 515.0f; rgb_fy = 550.0f; rgb_cx = 319.5f; rgb_cy = 239.5f; - p.rgb_intr = Matx33f(rgb_fx, 0, rgb_cx, 0, rgb_fy, rgb_cy, 0, 0, 1); + p.rgb_intr = Matx33f(rgb_fx, 0, rgb_cx, + 0, rgb_fy, rgb_cy, + 0, 0, 1); /* static const Size rgb_frameSize = Size(640, 480); @@ -272,10 +271,6 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r _rgb.convertTo(rgb, COLOR_TYPE); else rgb = _rgb; - - //std::cout << rgb << std::endl; - //std::cout << "-rgb " << rgb.rows << " " << rgb.cols << std::endl; - //std::cout << "-depth " << depth.rows << " " << depth.cols << std::endl; std::vector newPoints, newNormals, newColors; makeColoredFrameFromDepth(depth, rgb, @@ -343,7 +338,6 @@ void ColoredKinFuImpl::render(OutputArray image, const Matx44f& _camera { MatType points, normals, colors; volume->raycast(_cameraPose, params.intr, params.frameSize, points, normals, colors); - //renderPointsNormals(points, normals, image, params.lightPose); renderPointsNormalsColors(points, normals, colors, image, params.lightPose); } } diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index f29ea3dcaba..c63f4ee5d5c 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -158,11 +158,6 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d Depth depth = _depth.getMat(); Rgb rgb = _rgb.getMat(); - //std::cout << "+rgb " << rgb.rows << " " << rgb.cols << std::endl; - //std::cout << "+depth " << depth.rows << " " << depth.cols << std::endl; - - //std::cout << rgb.rows << " " << rgb.cols << std::endl; - //std::cout << rgb << std::endl; Vec6f newParams((float)depth.rows, (float)depth.cols, intrinsics.fx, intrinsics.fy, intrinsics.cx, intrinsics.cy); @@ -276,22 +271,11 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const int coordBase = ix*xdim + iy*ydim + iz*zdim; float r = 0, g = 0, b = 0; - /* - for(int i = 0; i < 8; i++) - for (int c = 0; c < 3; c++) - { - r = volData[neighbourCoords[i] + coordBase + 1 * volDims[c]].r; - g = volData[neighbourCoords[i] + coordBase + 1 * volDims[c]].g; - b = volData[neighbourCoords[i] + coordBase + 1 * volDims[c]].b; - } - return Point3f(r / 24.f, g / 24.f, b / 24.f); - */ + + // TODO: create better interpolation or remove this simple version /* float mainRGBsum = (float) (volData[coordBase].r + volData[coordBase].g + volData[coordBase].b); - - float counter = 0; - //std::cout << "======" << std::endl; for (int i = 0; i < 8; i++) { float sum = (float) (volData[neighbourCoords[i] + coordBase].r + volData[neighbourCoords[i] + coordBase].g + volData[neighbourCoords[i] + coordBase].b ); @@ -300,15 +284,12 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const r += (float) volData[neighbourCoords[i] + coordBase].r; g += (float) volData[neighbourCoords[i] + coordBase].g; b += (float) volData[neighbourCoords[i] + coordBase].b; - //std::cout <= 0 && _v < depth.rows && _u >= 0 && _u < depth.cols && - //(_v + _vShift) >= 0 && (_v + _vShift) < depth.rows && (_u + _uShift) >= 0 && (_u + _uShift) < depth.cols)) rgb_v >= 0 && rgb_v < depth.rows && rgb_u >= 0 && rgb_u < depth.cols)) continue; - //std::cout << "pix " << _v << " " << _u << " | rgb " << rgb_v << " "<< rgb_u << std::endl; - float pixNorm = pixNorms.at(_v, _u); - //if (!(_u >= 0 && _u < color.cols && _v >= 0 && _v < color.rows)) - // continue; - - //Vec3f colorRGB = color.at(_v + _vShift, _u + _uShift); Vec3f colorRGB = color.at(rgb_v, rgb_u); - //std::cout << colorRGB << std::endl; - //std::cout << color.type() << std::endl; // difference between distances of point and of surface to camera float sdf = pixNorm * (v * dfac - camSpacePt.z); // possible alternative is: @@ -551,21 +539,13 @@ void integrateRGBVolumeUnit( ColorType& g = voxel.g; ColorType& b = voxel.b; - /* - if (abs(((float)(r + g + b)) - (colorRGB.x + colorRGB.y + colorRGB.z)) < 1000 || weight < 1) - { - r = (ColorType)((int)(r * weight) + (colorRGB.x) / 65536) / (weight + 1); - g = (ColorType)((int)(g * weight) + (colorRGB.y) / 65536) / (weight + 1); - b = (ColorType)((int)(b * weight) + (colorRGB.z) / 65536) / (weight + 1); - } - */ + // update RGB if (abs(((float)(r + g + b)) - (colorRGB[0] + colorRGB[1] + colorRGB[2])) < 1000 || weight < 1) { r = (ColorType)((float)(r * weight) + (colorRGB[0])) / (weight + 1); g = (ColorType)((float)(g * weight) + (colorRGB[1])) / (weight + 1); b = (ColorType)((float)(b * weight) + (colorRGB[2])) / (weight + 1); } - //std::cout<<"("< Date: Tue, 9 Mar 2021 15:22:36 +0300 Subject: [PATCH 18/55] makeColoredFrameFromDepth fix --- modules/rgbd/src/kinfu_frame.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index c3cfc24cfab..4274198d6fb 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -328,7 +328,7 @@ struct ComputePointsNormalsInvoker : ParallelLoopBody struct ComputePointsNormalsColorsInvoker : ParallelLoopBody { ComputePointsNormalsColorsInvoker(const Depth& _depth, const Colors& _rgb, Points& _points, Normals& _normals, Colors& _colors, - const Intr::Reprojector& _reproj, const Intr::Reprojector& _rgb_reproj, float _dfac) : + const Intr::Reprojector& _reproj, const Intr::Projector& _rgb_reproj, float _dfac) : ParallelLoopBody(), depth(_depth), rgb(_rgb), @@ -336,7 +336,7 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody normals(_normals), colors(_colors), reproj(_reproj), - rgb_reproj(_rgb_reproj), + rgb_proj(_rgb_reproj), dfac(_dfac) { } @@ -355,10 +355,12 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody depthType d00 = depthRow0[x]; depthType z00 = d00*dfac; Point3f v00 = reproj(Point3f((float)x, (float)y, z00)); + Point2f proj = rgb_proj(v00); + int rgb_u = proj.x, rgb_v = proj.y; Point3f p = nan3, n = nan3, c = nan3; - - if(x < depth.cols - 1 && y < depth.rows - 1) + if(x < depth.cols - 1 && y < depth.rows - 1 && + rgb_v >= 0 && rgb_v < depth.rows && rgb_u >= 0 && rgb_u < depth.cols) { depthType d01 = depthRow0[x+1]; depthType d10 = depthRow1[x]; @@ -376,7 +378,7 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody cv::Vec3f vec = (v01-v00).cross(v10-v00); n = -normalize(vec); p = v00; - c = rgb.at(y, x); + c = rgb.at(rgb_v, rgb_u); } } @@ -393,7 +395,7 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody Normals& normals; Colors& colors; const Intr::Reprojector& reproj; - const Intr::Reprojector& rgb_reproj; + const Intr::Projector& rgb_proj; float dfac; }; @@ -435,9 +437,9 @@ void computePointsNormalsColors(const Intr intr, const Intr rgb_intr, float dept float dfac = 1.f/depthFactor; Intr::Reprojector reproj = intr.makeReprojector(); - Intr::Reprojector reprojRGB = rgb_intr.makeReprojector(); + Intr::Projector projRGB = rgb_intr.makeProjector(); - ComputePointsNormalsColorsInvoker ci(depth, rgb, points, normals, colors, reproj, reprojRGB, dfac); + ComputePointsNormalsColorsInvoker ci(depth, rgb, points, normals, colors, reproj, projRGB, dfac); Range range(0, depth.rows); const int nstripes = -1; parallel_for_(range, ci, nstripes); From 748d8c3cbb277323cb6808c6a2845f505e9971a8 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 9 Mar 2021 17:17:54 +0300 Subject: [PATCH 19/55] RGBSoure remove extra code --- modules/rgbd/samples/colored_kinfu_demo.cpp | 1 - modules/rgbd/samples/io_utils.hpp | 51 ++++++--------------- 2 files changed, 15 insertions(+), 37 deletions(-) diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index a007f5cc436..f543f7c032a 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -57,7 +57,6 @@ static const char* keys = "{camera |0| Index of depth camera to be used as a depth source }" "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better)," " in coarse mode points and normals are displayed }" - "{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}" "{idle | | Do not run KinFu, just display depth frames }" "{record | | Write depth frames to specified file list" " (the same format as for the 'depth' key) }" diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index 293b352af49..9814bb8ed17 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -531,7 +531,7 @@ struct RGBSource bool empty() { return rgbFileList.empty() && !(vc.isOpened()); } - void updateIntrinsics(Matx33f& _intrinsics, Matx33f& _rgb_intrinsics, Size& _frameSize, float& _depthFactor) + void updateIntrinsics(Matx33f& _intrinsics, Matx33f& _rgb_intrinsics, Size& _frameSize) { if (vc.isOpened()) { @@ -542,7 +542,6 @@ struct RGBSource // it's recommended to calibrate sensor to obtain its intrinsics float fx, fy, cx, cy; float rgb_fx, rgb_fy, rgb_cx, rgb_cy; - float depthFactor = 1000.f; Size frameSize, rgb_frameSize; if (sourceType == Type::RGB_KINECT2) { @@ -576,9 +575,9 @@ struct RGBSource // TODO: replace to rgb types if (sourceType == Type::RGB_REALSENSE) { + // TODO: find correct rgd focal length 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 { @@ -597,47 +596,27 @@ struct RGBSource _intrinsics = camMatrix; _rgb_intrinsics = rgb_camMatrix; _frameSize = frameSize; - _depthFactor = depthFactor; } } - void updateVolumeParams(const Vec3i& _resolution, float& _voxelSize, float& _tsdfTruncDist, - Affine3f& _volumePose, float& _depthTruncateThreshold) + void updateVolumeParams(const Vec3i& _resolution, float& _voxelSize, float& _tsdfTruncDist, Affine3f& _volumePose) { - float volumeSize = 3.0f; - _depthTruncateThreshold = 0.0f; - // RealSense has shorter depth range, some params should be tuned - if (sourceType == Type::RGB_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)); + // TODO: do this settings for rgb image } - void updateICPParams(float& _icpDistThresh, float& _bilateralSigmaDepth) + void updateICPParams(float& _icpDistThresh) { - _icpDistThresh = 0.1f; - _bilateralSigmaDepth = 0.04f; - // RealSense has shorter depth range, some params should be tuned - if (sourceType == Type::RGB_REALSENSE) - { - _icpDistThresh = 0.01f; - _bilateralSigmaDepth = 0.01f; - } + // TODO: do this settings for rgb image icp } void updateParams(large_kinfu::Params& params) { if (vc.isOpened()) { - updateIntrinsics(params.intr, params.rgb_intr, params.frameSize, params.depthFactor); + updateIntrinsics(params.intr, params.rgb_intr, params.frameSize); updateVolumeParams(params.volumeParams.resolution, params.volumeParams.voxelSize, - params.volumeParams.tsdfTruncDist, params.volumeParams.pose, - params.truncateThreshold); - updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth); + params.volumeParams.tsdfTruncDist, params.volumeParams.pose ); + updateICPParams(params.icpDistThresh); if (sourceType == Type::RGB_KINECT2) { @@ -656,10 +635,10 @@ struct RGBSource { if (vc.isOpened()) { - updateIntrinsics(params.intr, params.rgb_intr, params.frameSize, params.depthFactor); + updateIntrinsics(params.intr, params.rgb_intr, params.frameSize); updateVolumeParams(params.volumeDims, params.voxelSize, - params.tsdf_trunc_dist, params.volumePose, params.truncateThreshold); - updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth); + params.tsdf_trunc_dist, params.volumePose); + updateICPParams(params.icpDistThresh); if (sourceType == Type::RGB_KINECT2) { @@ -678,10 +657,10 @@ struct RGBSource { if (vc.isOpened()) { - updateIntrinsics(params.intr, params.rgb_intr, params.frameSize, params.depthFactor); + updateIntrinsics(params.intr, params.rgb_intr, params.frameSize); updateVolumeParams(params.volumeDims, params.voxelSize, - params.tsdf_trunc_dist, params.volumePose, params.truncateThreshold); - updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth); + params.tsdf_trunc_dist, params.volumePose); + updateICPParams(params.icpDistThresh); if (sourceType == Type::RGB_KINECT2) { From a47372bbdf15c582c89bd11595d69c17e38b5e9d Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 9 Mar 2021 17:33:16 +0300 Subject: [PATCH 20/55] RGBSoure remove extra code 1 --- .../include/opencv2/rgbd/colored_kinfu.hpp | 1 + modules/rgbd/samples/io_utils.hpp | 84 ++----------------- 2 files changed, 6 insertions(+), 79 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp index 84b56d8b7b5..186aad4f6b6 100644 --- a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -79,6 +79,7 @@ struct CV_EXPORTS_W Params /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; + CV_PROP_RW Size rgb_frameSize; CV_PROP_RW kinfu::VolumeType volumeType; diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index 9814bb8ed17..ad7364deb6e 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -531,7 +531,7 @@ struct RGBSource bool empty() { return rgbFileList.empty() && !(vc.isOpened()); } - void updateIntrinsics(Matx33f& _intrinsics, Matx33f& _rgb_intrinsics, Size& _frameSize) + void updateIntrinsics(Matx33f& _rgb_intrinsics, Size& _rgb_frameSize) { if (vc.isOpened()) { @@ -540,15 +540,10 @@ struct RGBSource 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 rgb_fx, rgb_fy, rgb_cx, rgb_cy; - Size frameSize, rgb_frameSize; + Size rgb_frameSize; if (sourceType == Type::RGB_KINECT2) { - fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; - rgb_fx = rgb_fy = Kinect2Params::rgb_focal; rgb_cx = Kinect2Params::rgb_cx; rgb_cy = Kinect2Params::rgb_cy; @@ -557,13 +552,6 @@ struct RGBSource } else if (sourceType == Type::RGB_ASTRA) { - fx = AstraParams::fx; - fy = AstraParams::fy; - cx = AstraParams::cx; - cy = AstraParams::cy; - - frameSize = AstraParams::frameSize; - rgb_fx = rgb_fy = AstraParams::rgb_focal; rgb_cx = AstraParams::rgb_cx; rgb_cy = AstraParams::rgb_cy; @@ -573,29 +561,11 @@ struct RGBSource else { // TODO: replace to rgb types - if (sourceType == Type::RGB_REALSENSE) - { - // TODO: find correct rgd focal length - fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ); - fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT); - } - else - { - fx = fy = - (float)vc.get(CAP_OPENNI_IMAGE_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); Matx33f rgb_camMatrix = Matx33f(rgb_fx, 0, rgb_cx, 0, rgb_fy, rgb_cy, 0, 0, 1); - _intrinsics = camMatrix; - _rgb_intrinsics = rgb_camMatrix; - _frameSize = frameSize; + _rgb_intrinsics = rgb_camMatrix; + _rgb_frameSize = rgb_frameSize; } } @@ -609,55 +579,11 @@ struct RGBSource // TODO: do this settings for rgb image icp } - void updateParams(large_kinfu::Params& params) - { - if (vc.isOpened()) - { - updateIntrinsics(params.intr, params.rgb_intr, params.frameSize); - updateVolumeParams(params.volumeParams.resolution, params.volumeParams.voxelSize, - params.volumeParams.tsdfTruncDist, params.volumeParams.pose ); - updateICPParams(params.icpDistThresh); - - if (sourceType == Type::RGB_KINECT2) - { - Matx distCoeffs; - distCoeffs(0) = Kinect2Params::rgb_k1; - distCoeffs(1) = Kinect2Params::rgb_k2; - distCoeffs(4) = Kinect2Params::rgb_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.rgb_intr, params.frameSize); - updateVolumeParams(params.volumeDims, params.voxelSize, - params.tsdf_trunc_dist, params.volumePose); - updateICPParams(params.icpDistThresh); - - if (sourceType == Type::RGB_KINECT2) - { - Matx distCoeffs; - distCoeffs(0) = Kinect2Params::rgb_k1; - distCoeffs(1) = Kinect2Params::rgb_k2; - distCoeffs(4) = Kinect2Params::rgb_k3; - - initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, - params.frameSize, CV_16SC2, undistortMap1, undistortMap2); - } - } - } - void updateParams(colored_kinfu::Params& params) { if (vc.isOpened()) { - updateIntrinsics(params.intr, params.rgb_intr, params.frameSize); + updateIntrinsics(params.rgb_intr, params.rgb_frameSize); updateVolumeParams(params.volumeDims, params.voxelSize, params.tsdf_trunc_dist, params.volumePose); updateICPParams(params.icpDistThresh); From a2c223d144117250dd2d2a40c711b14c6a195b68 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 10 Mar 2021 16:00:26 +0300 Subject: [PATCH 21/55] simple fix of bug with rgb size --- modules/rgbd/samples/colored_kinfu_demo.cpp | 2 +- modules/rgbd/src/colored_kinfu.cpp | 38 ++++++++------------- modules/rgbd/src/colored_tsdf.cpp | 3 +- modules/rgbd/src/kinfu_frame.cpp | 2 +- modules/rgbd/src/kinfu_frame.hpp | 1 - modules/rgbd/src/tsdf_functions.cpp | 4 +-- 6 files changed, 19 insertions(+), 31 deletions(-) diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index f543f7c032a..68436fd9ff3 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -110,6 +110,7 @@ int main(int argc, char **argv) else ds = makePtr(parser.get("camera")); + //TODO: intrinsics for camera rgbs = makePtr(parser.get("depth") + "/rgb.txt"); if (ds->empty()) @@ -168,7 +169,6 @@ int main(int argc, char **argv) if(depthWriter) depthWriter->append(frame); UMat rgb_frame = rgbs->getRGB(); - #ifdef HAVE_OPENCV_VIZ if(pause) { diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 6ff1475aae8..4883af2259e 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -50,16 +50,6 @@ Ptr Params::defaultParams() 0, rgb_fy, rgb_cy, 0, 0, 1); - /* - static const Size rgb_frameSize = Size(640, 480); -static const float rgb_focal = 525.0f; -static const float rgb_cx = 319.5f; -static const float rgb_cy = 239.5f; -static const float rgb_k1 = 0.0f; -static const float rgb_k2 = 0.0f; -static const float rgb_k3 = 0.0f - */ - // 5000 for the 16-bit PNG files // 1 for the 32-bit float images in the ROS bag files p.depthFactor = 5000; @@ -116,17 +106,6 @@ Ptr Params::coarseParams() return p; } -Ptr Params::hashTSDFParams(bool isCoarse) -{ - Ptr p; - if(isCoarse) - p = coarseParams(); - else - p = defaultParams(); - p->volumeType = VolumeType::HASHTSDF; - p->truncateThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); - return p; -} Ptr Params::coloredTSDFParams(bool isCoarse) { @@ -137,7 +116,6 @@ Ptr Params::coloredTSDFParams(bool isCoarse) p = defaultParams(); p->volumeType = VolumeType::COLOREDTSDF; - return p; } @@ -262,16 +240,27 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r MatType depth; MatType rgb; + if(_depth.type() != DEPTH_TYPE) _depth.convertTo(depth, DEPTH_TYPE); else depth = _depth; - if(_rgb.type() != COLOR_TYPE) - _rgb.convertTo(rgb, COLOR_TYPE); + if (_rgb.type() != COLOR_TYPE) + { + cv::Mat rgb_tmp, rgbchannel[3], z; + std::vector channels; + _rgb.convertTo(rgb_tmp, COLOR_TYPE); + cv::split(rgb_tmp, rgbchannel); + z = cv::Mat::zeros(rgbchannel[0].size(), CV_32F); + channels.push_back(rgbchannel[0]); channels.push_back(rgbchannel[1]); + channels.push_back(rgbchannel[2]); channels.push_back(z); + merge(channels, rgb); + } else rgb = _rgb; + std::vector newPoints, newNormals, newColors; makeColoredFrameFromDepth(depth, rgb, newPoints, newNormals, newColors, @@ -282,6 +271,7 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r params.bilateral_sigma_spatial, params.bilateral_kernel_size, params.truncateThreshold); + if(frameCounter == 0) { // use depth instead of distance diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index c63f4ee5d5c..a0f5ca67eb2 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -156,8 +156,7 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d CV_Assert(_depth.type() == DEPTH_TYPE); CV_Assert(!_depth.empty()); Depth depth = _depth.getMat(); - Rgb rgb = _rgb.getMat(); - + Colors rgb = _rgb.getMat(); Vec6f newParams((float)depth.rows, (float)depth.cols, intrinsics.fx, intrinsics.fy, intrinsics.cx, intrinsics.cy); diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index 4274198d6fb..e0e4c1e338e 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -378,7 +378,7 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody cv::Vec3f vec = (v01-v00).cross(v10-v00); n = -normalize(vec); p = v00; - c = rgb.at(rgb_v, rgb_u); + c = fromPtype(rgb.at(rgb_v, rgb_u)); } } diff --git a/modules/rgbd/src/kinfu_frame.hpp b/modules/rgbd/src/kinfu_frame.hpp index 4ba61748891..199cfcf2c03 100644 --- a/modules/rgbd/src/kinfu_frame.hpp +++ b/modules/rgbd/src/kinfu_frame.hpp @@ -82,7 +82,6 @@ typedef Points Normals; typedef Points Colors; typedef cv::Mat_< depthType > Depth; -typedef cv::Mat_< ptype > Rgb; void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, cv::Affine3f lightPose); void renderPointsNormalsColors(InputArray _points, InputArray _normals, InputArray _colors, OutputArray image, Affine3f lightPose); diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 2bce0df4735..035d90eccf0 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -430,7 +430,7 @@ void integrateRGBVolumeUnit( CV_Assert(!_depth.empty()); cv::Affine3f vpose(_pose); Depth depth = _depth.getMat(); - Rgb color = _rgb.getMat(); + Colors color = _rgb.getMat(); Range integrateRange(0, volResolution.x); Mat volume = _volume.getMat(); @@ -523,7 +523,7 @@ void integrateRGBVolumeUnit( continue; float pixNorm = pixNorms.at(_v, _u); - Vec3f colorRGB = color.at(rgb_v, rgb_u); + Vec4f colorRGB = color.at(rgb_v, rgb_u); // difference between distances of point and of surface to camera float sdf = pixNorm * (v * dfac - camSpacePt.z); // possible alternative is: From 7e55474aef392c490bf80752a59f5fb26c8a1892 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 10 Mar 2021 16:29:08 +0300 Subject: [PATCH 22/55] minor fix --- modules/rgbd/src/kinfu_frame.cpp | 2 +- modules/rgbd/src/tsdf_functions.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index e0e4c1e338e..9a6c29b90b0 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -360,7 +360,7 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody Point3f p = nan3, n = nan3, c = nan3; if(x < depth.cols - 1 && y < depth.rows - 1 && - rgb_v >= 0 && rgb_v < depth.rows && rgb_u >= 0 && rgb_u < depth.cols) + rgb_v >= 0 && rgb_v < rgb.rows && rgb_u >= 0 && rgb_u < rgb.cols) { depthType d01 = depthRow0[x+1]; depthType d10 = depthRow1[x]; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 035d90eccf0..9d1f1b3cf63 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -512,14 +512,12 @@ void integrateRGBVolumeUnit( int _u = projected.x; int _v = projected.y; - int _uShift = 5; - int _vShift = -5; int rgb_u = projectedRGB.x; int rgb_v = projectedRGB.y; if (!(_v >= 0 && _v < depth.rows && _u >= 0 && _u < depth.cols && - rgb_v >= 0 && rgb_v < depth.rows && rgb_u >= 0 && rgb_u < depth.cols)) + rgb_v >= 0 && rgb_v < color.rows && rgb_u >= 0 && rgb_u < color.cols)) continue; float pixNorm = pixNorms.at(_v, _u); From baaeb77a9b597b0a97c247480722053e5df9c828 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 10 Mar 2021 16:58:08 +0300 Subject: [PATCH 23/55] docs fix --- modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp | 2 ++ modules/rgbd/include/opencv2/rgbd/volume.hpp | 2 +- modules/rgbd/samples/colored_kinfu_demo.cpp | 2 +- modules/rgbd/samples/io_utils.hpp | 2 +- modules/rgbd/src/colored_kinfu.cpp | 8 ++++---- modules/rgbd/src/colored_tsdf.cpp | 6 ++---- modules/rgbd/src/tsdf_functions.cpp | 4 ++-- 7 files changed, 13 insertions(+), 13 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp index 186aad4f6b6..82069e6199f 100644 --- a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -241,6 +241,8 @@ class CV_EXPORTS_W ColoredKinFu virtual const Affine3f getPose() const = 0; /** @brief Process next depth frame + @param depth input Mat of depth frame + @param rgb input Mat of rgb (colored) frame Integrates depth into voxel space with respect to its ICP-calculated pose. Input image is converted to CV_32F internally if has another type. diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index e4c14d37b55..3fb1d98ef4c 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/modules/rgbd/include/opencv2/rgbd/volume.hpp @@ -31,7 +31,7 @@ class CV_EXPORTS_W Volume virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) = 0; virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, - const Matx44f& cameraPose, const kinfu::Intr& intrinsics, + const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Intr& rgb_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; diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index 68436fd9ff3..c18a7c1103d 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -130,7 +130,7 @@ int main(int argc, char **argv) } Ptr params; Ptr kf; - + params = colored_kinfu::Params::coloredTSDFParams(coarse); // These params can be different for each depth sensor diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index ad7364deb6e..20f0a832f0f 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -345,7 +345,7 @@ struct DepthSource } } } - + void updateParams(colored_kinfu::Params& params) { if (vc.isOpened()) diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 4883af2259e..4c801f57f96 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -245,7 +245,7 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r _depth.convertTo(depth, DEPTH_TYPE); else depth = _depth; - + if (_rgb.type() != COLOR_TYPE) { cv::Mat rgb_tmp, rgbchannel[3], z; @@ -259,11 +259,11 @@ bool ColoredKinFuImpl::updateT(const MatType& _depth, const MatType& _r } else rgb = _rgb; - + std::vector newPoints, newNormals, newColors; - makeColoredFrameFromDepth(depth, rgb, - newPoints, newNormals, newColors, + makeColoredFrameFromDepth(depth, rgb, + newPoints, newNormals, newColors, params.intr, params.rgb_intr, params.pyramidLevels, params.depthFactor, diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index a0f5ca67eb2..2959d414058 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -63,7 +63,7 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume ColoredTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Vec3i _resolution, bool zFirstMemOrder = true); virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) override + const kinfu::Intr& intrinsics, const int frameId = 0) override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override; @@ -270,7 +270,7 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const int coordBase = ix*xdim + iy*ydim + iz*zdim; float r = 0, g = 0, b = 0; - + // TODO: create better interpolation or remove this simple version /* float mainRGBsum = (float) (volData[coordBase].r + volData[coordBase].g + volData[coordBase].b); @@ -290,8 +290,6 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const */ Point3f res(volData[coordBase].r, volData[coordBase].g, volData[coordBase].b); return res; - - } diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 9d1f1b3cf63..12767c63bae 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -512,14 +512,14 @@ void integrateRGBVolumeUnit( int _u = projected.x; int _v = projected.y; - + int rgb_u = projectedRGB.x; int rgb_v = projectedRGB.y; if (!(_v >= 0 && _v < depth.rows && _u >= 0 && _u < depth.cols && rgb_v >= 0 && rgb_v < color.rows && rgb_u >= 0 && rgb_u < color.cols)) continue; - + float pixNorm = pixNorms.at(_v, _u); Vec4f colorRGB = color.at(rgb_v, rgb_u); // difference between distances of point and of surface to camera From 07edbc5b6c91e738441f0f6c5dcebeb12d237075 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 10 Mar 2021 17:47:54 +0300 Subject: [PATCH 24/55] docs fix --- modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp index 82069e6199f..c3e7a1ffbac 100644 --- a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -244,11 +244,7 @@ class CV_EXPORTS_W ColoredKinFu @param depth input Mat of depth frame @param rgb input Mat of rgb (colored) frame - Integrates depth into voxel space with respect to its ICP-calculated pose. - Input image is converted to CV_32F internally if has another type. - - @param depth one-channel image which size and depth scale is described in algorithm's parameters - @return true if succeeded to align new frame with current scene, false if opposite + @return true if succeeded to align new frame with current scene, false if opposite */ CV_WRAP virtual bool update(InputArray depth, InputArray rgb) = 0; }; From bcaefe5c4074b6138f170e0d017d2946b6927123 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 10 Mar 2021 20:39:23 +0300 Subject: [PATCH 25/55] unused parameter fix --- modules/rgbd/src/colored_tsdf.cpp | 6 ++---- modules/rgbd/src/hash_tsdf.cpp | 12 ++++-------- modules/rgbd/src/tsdf.hpp | 6 ++---- 3 files changed, 8 insertions(+), 16 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 2959d414058..656630f8afe 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -62,15 +62,13 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume // dimension in voxels, size in meters ColoredTSDFVolumeCPU(float _voxelSize, cv::Matx44f _pose, float _raycastStepFactor, float _truncDist, int _maxWeight, Vec3i _resolution, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const int frameId = 0) override + virtual void integrate(InputArray, float, const Matx44f&, const kinfu::Intr&, const int) override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override; virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals, OutputArray colors) const override; - virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray points, OutputArray normals) const override + virtual void raycast(const Matx44f&, const kinfu::Intr&, const Size&, OutputArray, OutputArray) const override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void fetchNormals(InputArray points, OutputArray _normals) const override; diff --git a/modules/rgbd/src/hash_tsdf.cpp b/modules/rgbd/src/hash_tsdf.cpp index badd3672517..4e492064a44 100644 --- a/modules/rgbd/src/hash_tsdf.cpp +++ b/modules/rgbd/src/hash_tsdf.cpp @@ -99,15 +99,13 @@ class HashTSDFVolumeCPU : public HashTSDFVolume HashTSDFVolumeCPU(const VolumeParams& _volumeParams, bool zFirstMemOrder = true); - virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override + virtual void integrate(InputArray, InputArray, float, const Matx44f&, const kinfu::Intr&, const Intr&, const int) override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; - void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray points, OutputArray normals, OutputArray colors) const override + void raycast(const Matx44f&, const kinfu::Intr&, const Size&, OutputArray, OutputArray, OutputArray) const override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; void fetchNormals(InputArray points, OutputArray _normals) const override; void fetchPointsNormals(OutputArray points, OutputArray normals) const override; @@ -899,15 +897,13 @@ class HashTSDFVolumeGPU : public HashTSDFVolume void markActive(const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId); - virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override + virtual void integrate(InputArray, InputArray, float, const Matx44f&, const kinfu::Intr&, const Intr&, const int) override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const int frameId = 0) override; void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, OutputArray points, OutputArray normals) const override; - void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray points, OutputArray normals, OutputArray colors) const override + void raycast(const Matx44f&, const kinfu::Intr&, const Size&, OutputArray, OutputArray, OutputArray) const override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; void fetchNormals(InputArray points, OutputArray _normals) const override; diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 0222378458a..511a061a4ce 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -40,11 +40,9 @@ class TSDFVolume : public Volume int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true); virtual ~TSDFVolume() = default; - virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override + virtual void integrate(InputArray, InputArray, float, const Matx44f&, const kinfu::Intr&, const Intr&, const int) override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; - virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, - OutputArray points, OutputArray normals, OutputArray colors) const override + virtual void raycast(const Matx44f&, const kinfu::Intr&, const Size&, OutputArray, OutputArray, OutputArray) const override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; public: From 68e9c42ea39ef0a349a5767801277c79b3b764cf Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 12:23:07 +0300 Subject: [PATCH 26/55] warnings fix --- modules/rgbd/samples/io_utils.hpp | 5 +++-- modules/rgbd/src/colored_tsdf.cpp | 6 +----- modules/rgbd/src/kinfu_frame.cpp | 9 +++------ 3 files changed, 7 insertions(+), 13 deletions(-) diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index 20f0a832f0f..5d477e64870 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -561,6 +561,7 @@ struct RGBSource else { // TODO: replace to rgb types + rgb_frameSize = Size(w, h); } Matx33f rgb_camMatrix = Matx33f(rgb_fx, 0, rgb_cx, 0, rgb_fy, rgb_cy, 0, 0, 1); @@ -569,12 +570,12 @@ struct RGBSource } } - void updateVolumeParams(const Vec3i& _resolution, float& _voxelSize, float& _tsdfTruncDist, Affine3f& _volumePose) + void updateVolumeParams(const Vec3i&, float&, float&, Affine3f&) { // TODO: do this settings for rgb image } - void updateICPParams(float& _icpDistThresh) + void updateICPParams(float&) { // TODO: do this settings for rgb image icp } diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 656630f8afe..c182347e5d4 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -262,15 +262,11 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const int iy = cvFloor(p.y); int iz = cvFloor(p.z); - float tx = p.x - ix; - float ty = p.y - iy; - float tz = p.z - iz; - int coordBase = ix*xdim + iy*ydim + iz*zdim; - float r = 0, g = 0, b = 0; // TODO: create better interpolation or remove this simple version /* + float r = 0, g = 0, b = 0; float mainRGBsum = (float) (volData[coordBase].r + volData[coordBase].g + volData[coordBase].b); float counter = 0; for (int i = 0; i < 8; i++) diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index 9a6c29b90b0..8e7524384d3 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -12,6 +12,7 @@ namespace cv { namespace kinfu { static void computePointsNormals(const cv::kinfu::Intr, float depthFactor, const Depth, Points, Normals ); +void computePointsNormalsColors(const Intr, const Intr, float, const Depth, const Colors, Points, Normals, Colors); static Depth pyrDownBilateral(const Depth depth, float sigma); static void pyrDownPointsNormals(const Points p, const Normals n, Points& pdown, Normals& ndown); @@ -107,10 +108,9 @@ struct RenderInvoker : ParallelLoopBody struct RenderColorInvoker : ParallelLoopBody { - RenderColorInvoker(const Points& _points, const Normals& _normals, const Colors& _colors, Mat_& _img, Affine3f _lightPose, Size _sz) : + RenderColorInvoker(const Points& _points, const Colors& _colors, Mat_& _img, Affine3f _lightPose, Size _sz) : ParallelLoopBody(), points(_points), - normals(_normals), colors(_colors), img(_img), lightPose(_lightPose), @@ -123,13 +123,11 @@ struct RenderColorInvoker : ParallelLoopBody { Vec4b* imgRow = img[y]; const ptype* ptsRow = points[y]; - const ptype* nrmRow = normals[y]; const ptype* clrRow = colors[y]; for(int x = 0; x < sz.width; x++) { Point3f p = fromPtype(ptsRow[x]); - Point3f n = fromPtype(nrmRow[x]); Point3f c = fromPtype(clrRow[x]); Vec4b color; @@ -148,7 +146,6 @@ struct RenderColorInvoker : ParallelLoopBody } const Points& points; - const Normals& normals; const Colors& colors; Mat_& img; Affine3f lightPose; @@ -767,7 +764,7 @@ void renderPointsNormalsColors(InputArray _points, InputArray _normals, InputArr Mat_ img = image.getMat(); - RenderColorInvoker ri(points, normals, colors, img, lightPose, sz); + RenderColorInvoker ri(points, colors, img, lightPose, sz); Range range(0, sz.height); const int nstripes = -1; parallel_for_(range, ri, nstripes); From 4997b87897b4502636bf40332ff850126104f9e0 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 13:55:58 +0300 Subject: [PATCH 27/55] fix tsdf errors; rgbd_perf warnings --- modules/rgbd/perf/perf_tsdf.cpp | 8 +++++++- modules/rgbd/src/colored_tsdf.cpp | 6 +++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/perf/perf_tsdf.cpp b/modules/rgbd/perf/perf_tsdf.cpp index ebca91587d6..20aeb01a222 100644 --- a/modules/rgbd/perf/perf_tsdf.cpp +++ b/modules/rgbd/perf/perf_tsdf.cpp @@ -95,7 +95,7 @@ struct Scene struct SemisphereScene : Scene { - const int framesPerCycle = 72; + const int framesPerCycle = 18; const float nCycles = 1.0f; const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -1.5f)); @@ -203,6 +203,7 @@ PERF_TEST(Perf_TSDF, integrate) startTimer(); settings.volume->integrate(depth, settings._params->depthFactor, pose, settings._params->intr); stopTimer(); + depth.release(); } SANITY_CHECK_NOTHING(); } @@ -220,6 +221,8 @@ PERF_TEST(Perf_TSDF, raycast) startTimer(); settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals); stopTimer(); + depth.release(); + _points.release(); _normals.release(); } SANITY_CHECK_NOTHING(); } @@ -235,6 +238,7 @@ PERF_TEST(Perf_HashTSDF, integrate) startTimer(); settings.volume->integrate(depth, settings._params->depthFactor, pose, settings._params->intr); stopTimer(); + depth.release(); } SANITY_CHECK_NOTHING(); } @@ -252,6 +256,8 @@ PERF_TEST(Perf_HashTSDF, raycast) startTimer(); settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals); stopTimer(); + depth.release(); + _points.release(); _normals.release(); } SANITY_CHECK_NOTHING(); } diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index c182347e5d4..28380e25af2 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -287,9 +287,9 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const } -struct RaycastInvoker : ParallelLoopBody +struct ColorRaycastInvoker : ParallelLoopBody { - RaycastInvoker(Points& _points, Normals& _normals, Colors& _colors, const Matx44f& cameraPose, + ColorRaycastInvoker(Points& _points, Normals& _normals, Colors& _colors, const Matx44f& cameraPose, const Intr& intrinsics, const ColoredTSDFVolumeCPU& _volume) : ParallelLoopBody(), points(_points), @@ -447,7 +447,7 @@ void ColoredTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrin Points points = _points.getMat(); Normals normals = _normals.getMat(); Colors colors = _colors.getMat(); - RaycastInvoker ri(points, normals, colors, cameraPose, intrinsics, *this); + ColorRaycastInvoker ri(points, normals, colors, cameraPose, intrinsics, *this); const int nstripes = -1; parallel_for_(Range(0, points.rows), ri, nstripes); From 71056f24dbe4c965975d6db1c4f2176a1333324c Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 15:02:17 +0300 Subject: [PATCH 28/55] fix errors --- modules/rgbd/src/colored_kinfu.cpp | 12 ++++++++++++ modules/rgbd/src/kinfu_frame.cpp | 4 ++-- modules/rgbd/src/tsdf_functions.cpp | 10 +++++----- 3 files changed, 19 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 4c801f57f96..dc87be6b39a 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -107,6 +107,18 @@ Ptr Params::coarseParams() return p; } +Ptr Params::hashTSDFParams(bool isCoarse) +{ + Ptr p; + if (isCoarse) + p = coarseParams(); + else + p = defaultParams(); + p->volumeType = VolumeType::HASHTSDF; + p->truncateThreshold = rgbd::Odometry::DEFAULT_MAX_DEPTH(); + return p; +} + Ptr Params::coloredTSDFParams(bool isCoarse) { Ptr p; diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index 8e7524384d3..46423a98387 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -137,7 +137,7 @@ struct RenderColorInvoker : ParallelLoopBody } else { - color = Vec4b(c.x, c.y, c.z, 0); + color = Vec4b((uchar)c.x, (uchar)c.y, (uchar)c.z, (uchar)0); } imgRow[x] = color; @@ -353,7 +353,7 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody depthType z00 = d00*dfac; Point3f v00 = reproj(Point3f((float)x, (float)y, z00)); Point2f proj = rgb_proj(v00); - int rgb_u = proj.x, rgb_v = proj.y; + int rgb_u = (int) proj.x, rgb_v = (int) proj.y; Point3f p = nan3, n = nan3, c = nan3; if(x < depth.cols - 1 && y < depth.rows - 1 && diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 12767c63bae..30d5cbf9121 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -510,11 +510,11 @@ void integrateRGBVolumeUnit( continue; } - int _u = projected.x; - int _v = projected.y; + int _u = (int) projected.x; + int _v = (int) projected.y; - int rgb_u = projectedRGB.x; - int rgb_v = projectedRGB.y; + int rgb_u = (int) projectedRGB.x; + int rgb_v = (int) projectedRGB.y; if (!(_v >= 0 && _v < depth.rows && _u >= 0 && _u < depth.cols && rgb_v >= 0 && rgb_v < color.rows && rgb_u >= 0 && rgb_u < color.cols)) @@ -547,7 +547,7 @@ void integrateRGBVolumeUnit( // update TSDF value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); - weight = min(int(weight + 1), int(maxWeight)); + weight = WeightType( min(int(weight + 1), int(maxWeight)) ); } } } From d2832fa14bf400af97d0446236268e46307bf907 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 16:02:26 +0300 Subject: [PATCH 29/55] minor fix --- modules/rgbd/samples/io_utils.hpp | 3 +++ modules/rgbd/src/tsdf.hpp | 12 ++++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index 5d477e64870..0acc1830fdf 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -561,6 +561,9 @@ struct RGBSource else { // TODO: replace to rgb types + rgb_fx = rgb_fy = Kinect2Params::rgb_focal; + rgb_cx = Kinect2Params::rgb_cx; + rgb_cy = Kinect2Params::rgb_cy; rgb_frameSize = Size(w, h); } diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 511a061a4ce..a3f561fbe4a 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -40,10 +40,6 @@ class TSDFVolume : public Volume int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true); virtual ~TSDFVolume() = default; - virtual void integrate(InputArray, InputArray, float, const Matx44f&, const kinfu::Intr&, const Intr&, const int) override - { CV_Error(Error::StsNotImplemented, "Not implemented"); }; - virtual void raycast(const Matx44f&, const kinfu::Intr&, const Size&, OutputArray, OutputArray, OutputArray) const override - { CV_Error(Error::StsNotImplemented, "Not implemented"); }; public: Point3i volResolution; @@ -66,6 +62,10 @@ class TSDFVolumeCPU : public TSDFVolume 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 integrate(InputArray, InputArray, float, const Matx44f&, const kinfu::Intr&, const Intr&, const int) override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; + virtual void raycast(const Matx44f&, const kinfu::Intr&, const Size&, OutputArray, OutputArray, OutputArray) const override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void fetchNormals(InputArray points, OutputArray _normals) const override; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; @@ -101,6 +101,10 @@ class TSDFVolumeGPU : public TSDFVolume 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 integrate(InputArray, InputArray, float, const Matx44f&, const kinfu::Intr&, const Intr&, const int) override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; + virtual void raycast(const Matx44f&, const kinfu::Intr&, const Size&, OutputArray, OutputArray, OutputArray) const override + { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override; virtual void fetchNormals(InputArray points, OutputArray normals) const override; From c8f965965bdaa1a3f3f251c8e06098a3e6a24adb Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 16:04:07 +0300 Subject: [PATCH 30/55] debug print --- modules/rgbd/test/test_tsdf.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 5a9b23afb95..4052feeb2bd 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -333,7 +333,9 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo } if (isFetchPointsNormals) { + std::cout << "1" << std::endl; volume->fetchPointsNormals(_points, _normals); + std::cout << "2" << std::endl; } if (isFetchNormals) { From 7fa4a6a8d51ee647999d2a5f8aaa5861803f59e7 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 16:27:21 +0300 Subject: [PATCH 31/55] debug print 2 --- modules/rgbd/src/tsdf.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 0a0fcfacf05..d0ca8307de7 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -756,28 +756,31 @@ void TSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals if(_points.needed()) { + std::cout << "2" << std::endl; std::vector> pVecs, nVecs; FetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed()); Range range(0, volResolution.x); const int nstripes = -1; parallel_for_(range, fi, nstripes); std::vector points, normals; + std::cout << "2" << std::endl; for(size_t i = 0; i < pVecs.size(); i++) { points.insert(points.end(), pVecs[i].begin(), pVecs[i].end()); normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end()); } - + std::cout << "2" << std::endl; _points.create((int)points.size(), 1, POINT_TYPE); if(!points.empty()) Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat()); - + std::cout << "2" << std::endl; if(_normals.needed()) { _normals.create((int)normals.size(), 1, POINT_TYPE); if(!normals.empty()) Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat()); } + std::cout << "2" << std::endl; } } From e895ef7511399d2990b33ab3434276cd3cf447fb Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 16:59:40 +0300 Subject: [PATCH 32/55] debug print 3 --- modules/rgbd/src/tsdf.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index d0ca8307de7..a8b12379fbb 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -658,6 +658,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody inline void coord(std::vector& points, std::vector& normals, int x, int y, int z, Point3f V, float v0, int axis) const { + std::cout << "4" << std::endl; // 0 for x, 1 for y, 2 for z bool limits = false; Point3i shift; @@ -680,14 +681,14 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody limits = (z + 1 < vol.volResolution.z); Vc = V.z; } - + std::cout << "4" << std::endl; if(limits) { const TsdfVoxel& voxeld = volDataStart[(x+shift.x)*vol.volDims[0] + (y+shift.y)*vol.volDims[1] + (z+shift.z)*vol.volDims[2]]; float vd = tsdfToFloat(voxeld.tsdf); - + std::cout << "4" << std::endl; if(voxeld.weight != 0 && vd != 1.f) { if((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0)) @@ -700,8 +701,10 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody Point3f p(shift.x ? inter : V.x, shift.y ? inter : V.y, shift.z ? inter : V.z); + std::cout << "4" << std::endl; { points.push_back(toPtype(vol.pose * p)); + std::cout << "4" << std::endl; if(needNormals) normals.push_back(toPtype(vol.pose.rotation() * vol.getNormalVoxel(p*vol.voxelSizeInv))); @@ -713,6 +716,7 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody virtual void operator() (const Range& range) const override { + std::cout << "3" << std::endl; std::vector points, normals; for(int x = range.start; x < range.end; x++) { From 92e40507e6abf2f4cfd83f330fbdcd3364a0a3ef Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 17:33:56 +0300 Subject: [PATCH 33/55] invoker fix --- modules/rgbd/src/colored_tsdf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 28380e25af2..bcc7c6d1290 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -454,9 +454,9 @@ void ColoredTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrin } -struct FetchPointsNormalsInvoker : ParallelLoopBody +struct ColorFetchPointsNormalsInvoker : ParallelLoopBody { - FetchPointsNormalsInvoker(const ColoredTSDFVolumeCPU& _volume, + ColorFetchPointsNormalsInvoker(const ColoredTSDFVolumeCPU& _volume, std::vector>& _pVecs, std::vector>& _nVecs, bool _needNormals) : From 2e78bee9ec36debc1726078e0fc103448f2f7f74 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 17:38:18 +0300 Subject: [PATCH 34/55] minor fix --- modules/rgbd/src/colored_tsdf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index bcc7c6d1290..aa3946e5aa0 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -571,7 +571,7 @@ void ColoredTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _ if(_points.needed()) { std::vector> pVecs, nVecs; - FetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed()); + ColorFetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed()); Range range(0, volResolution.x); const int nstripes = -1; parallel_for_(range, fi, nstripes); From 63821a5007cf1a143c761609e3fdc4527cc82ffd Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Thu, 11 Mar 2021 17:56:15 +0300 Subject: [PATCH 35/55] remove debug cout --- modules/rgbd/src/tsdf.cpp | 15 ++++----------- modules/rgbd/test/test_tsdf.cpp | 2 -- 2 files changed, 4 insertions(+), 13 deletions(-) diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index a8b12379fbb..c647e3ab2c1 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -658,7 +658,6 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody inline void coord(std::vector& points, std::vector& normals, int x, int y, int z, Point3f V, float v0, int axis) const { - std::cout << "4" << std::endl; // 0 for x, 1 for y, 2 for z bool limits = false; Point3i shift; @@ -681,14 +680,13 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody limits = (z + 1 < vol.volResolution.z); Vc = V.z; } - std::cout << "4" << std::endl; + if(limits) { const TsdfVoxel& voxeld = volDataStart[(x+shift.x)*vol.volDims[0] + (y+shift.y)*vol.volDims[1] + (z+shift.z)*vol.volDims[2]]; float vd = tsdfToFloat(voxeld.tsdf); - std::cout << "4" << std::endl; if(voxeld.weight != 0 && vd != 1.f) { if((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0)) @@ -701,10 +699,8 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody Point3f p(shift.x ? inter : V.x, shift.y ? inter : V.y, shift.z ? inter : V.z); - std::cout << "4" << std::endl; { points.push_back(toPtype(vol.pose * p)); - std::cout << "4" << std::endl; if(needNormals) normals.push_back(toPtype(vol.pose.rotation() * vol.getNormalVoxel(p*vol.voxelSizeInv))); @@ -716,7 +712,6 @@ struct FetchPointsNormalsInvoker : ParallelLoopBody virtual void operator() (const Range& range) const override { - std::cout << "3" << std::endl; std::vector points, normals; for(int x = range.start; x < range.end; x++) { @@ -760,31 +755,29 @@ void TSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals if(_points.needed()) { - std::cout << "2" << std::endl; std::vector> pVecs, nVecs; FetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed()); Range range(0, volResolution.x); const int nstripes = -1; parallel_for_(range, fi, nstripes); + std::vector points, normals; - std::cout << "2" << std::endl; for(size_t i = 0; i < pVecs.size(); i++) { points.insert(points.end(), pVecs[i].begin(), pVecs[i].end()); normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end()); } - std::cout << "2" << std::endl; + _points.create((int)points.size(), 1, POINT_TYPE); if(!points.empty()) Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat()); - std::cout << "2" << std::endl; + if(_normals.needed()) { _normals.create((int)normals.size(), 1, POINT_TYPE); if(!normals.empty()) Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat()); } - std::cout << "2" << std::endl; } } diff --git a/modules/rgbd/test/test_tsdf.cpp b/modules/rgbd/test/test_tsdf.cpp index 4052feeb2bd..5a9b23afb95 100644 --- a/modules/rgbd/test/test_tsdf.cpp +++ b/modules/rgbd/test/test_tsdf.cpp @@ -333,9 +333,7 @@ void normal_test(bool isHashTSDF, bool isRaycast, bool isFetchPointsNormals, boo } if (isFetchPointsNormals) { - std::cout << "1" << std::endl; volume->fetchPointsNormals(_points, _normals); - std::cout << "2" << std::endl; } if (isFetchNormals) { From a1ed445df67c869d0dad25ffbc5be46f3368cab0 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 12 Mar 2021 14:39:48 +0300 Subject: [PATCH 36/55] add simple tests --- modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 5 + modules/rgbd/src/kinfu.cpp | 12 + modules/rgbd/test/test_colored_kinfu.cpp | 414 ++++++++++++++++++++ 3 files changed, 431 insertions(+) create mode 100644 modules/rgbd/test/test_colored_kinfu.cpp diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index e3793bc5bb7..0c849e07092 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -73,6 +73,11 @@ struct CV_EXPORTS_W Params A set of parameters suitable for use with HashTSDFVolume */ CV_WRAP static Ptr hashTSDFParams(bool isCoarse); + /** @brief ColoredTSDF parameters + A set of parameters suitable for use with ColoredTSDFVolume + */ + CV_WRAP static Ptr coloredTSDFParams(bool isCoarse); + /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index 1d8314aae6f..7f9f1b32361 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -107,6 +107,18 @@ Ptr Params::hashTSDFParams(bool isCoarse) return p; } +Ptr Params::coloredTSDFParams(bool isCoarse) +{ + Ptr p; + if (isCoarse) + p = coarseParams(); + else + p = defaultParams(); + p->volumeType = VolumeType::COLOREDTSDF; + + return p; +} + // MatType should be Mat or UMat template< typename MatType> class KinFuImpl : public KinFu diff --git a/modules/rgbd/test/test_colored_kinfu.cpp b/modules/rgbd/test/test_colored_kinfu.cpp new file mode 100644 index 00000000000..efad131efe9 --- /dev/null +++ b/modules/rgbd/test/test_colored_kinfu.cpp @@ -0,0 +1,414 @@ +// 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 "test_precomp.hpp" + +// Inspired by Inigo Quilez' raymarching guide: +// http://iquilezles.org/www/articles/distfunctions/distfunctions.htm + +namespace opencv_test { namespace { + +using namespace cv; + +/** Reprojects screen point to camera space given z coord. */ +struct Reprojector +{ + Reprojector() {} + inline Reprojector(Matx33f intr) + { + fxinv = 1.f/intr(0, 0), fyinv = 1.f/intr(1, 1); + cx = intr(0, 2), cy = intr(1, 2); + } + template + inline cv::Point3_ operator()(cv::Point3_ p) const + { + T x = p.z * (p.x - cx) * fxinv; + T y = p.z * (p.y - cy) * fyinv; + return cv::Point3_(x, y, p.z); + } + + float fxinv, fyinv, cx, cy; +}; + +template +struct RenderInvoker : ParallelLoopBody +{ + RenderInvoker(Mat_& _frame, Affine3f _pose, + Reprojector _reproj, + float _depthFactor) : ParallelLoopBody(), + frame(_frame), + pose(_pose), + reproj(_reproj), + depthFactor(_depthFactor) + { } + + virtual void operator ()(const cv::Range& r) const + { + for(int y = r.start; y < r.end; y++) + { + float* frameRow = frame[y]; + for(int x = 0; x < frame.cols; x++) + { + float pix = 0; + + Point3f orig = pose.translation(); + // direction through pixel + Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f)); + float xyt = 1.f/(screenVec.x*screenVec.x + + screenVec.y*screenVec.y + 1.f); + Point3f dir = normalize(Vec3f(pose.rotation() * screenVec)); + // screen space axis + dir.y = - dir.y; + + const float maxDepth = 20.f; + const float maxSteps = 256; + float t = 0.f; + for(int step = 0; step < maxSteps && t < maxDepth; step++) + { + Point3f p = orig + dir*t; + float d = Scene::map(p); + if(d < 0.000001f) + { + float depth = std::sqrt(t*t*xyt); + pix = depth*depthFactor; + break; + } + t += d; + } + + frameRow[x] = pix; + } + } + } + + Mat_& frame; + Affine3f pose; + Reprojector reproj; + float depthFactor; +}; + +struct Scene +{ + virtual ~Scene() {} + static Ptr create(int nScene, Size sz, Matx33f _intr, float _depthFactor); + virtual Mat depth(Affine3f pose) = 0; + virtual std::vector getPoses() = 0; +}; + +struct CubeSpheresScene : Scene +{ + const int framesPerCycle = 32; + const float nCycles = 0.25f; + const Affine3f startPose = Affine3f(Vec3f(-0.5f, 0.f, 0.f), Vec3f(2.1f, 1.4f, -2.1f)); + + CubeSpheresScene(Size sz, Matx33f _intr, float _depthFactor) : + frameSize(sz), intr(_intr), depthFactor(_depthFactor) + { } + + static float map(Point3f p) + { + float plane = p.y + 0.5f; + + Point3f boxPose = p - Point3f(-0.0f, 0.3f, 0.0f); + float boxSize = 0.5f; + float roundness = 0.08f; + Point3f boxTmp; + boxTmp.x = max(abs(boxPose.x) - boxSize, 0.0f); + boxTmp.y = max(abs(boxPose.y) - boxSize, 0.0f); + boxTmp.z = max(abs(boxPose.z) - boxSize, 0.0f); + float roundBox = (float)cv::norm(boxTmp) - roundness; + + float sphereRadius = 0.7f; + float sphere = (float)cv::norm(boxPose) - sphereRadius; + + float boxMinusSphere = max(roundBox, -sphere); + + float sphere2 = (float)cv::norm(p - Point3f(0.3f, 1.f, 0.f)) - 0.1f; + float sphere3 = (float)cv::norm(p - Point3f(0.0f, 1.f, 0.f)) - 0.2f; + float res = min(min(plane, boxMinusSphere), min(sphere2, sphere3)); + + return res; + } + + Mat depth(Affine3f pose) override + { + Mat_ frame(frameSize); + Reprojector reproj(intr); + + Range range(0, frame.rows); + parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor)); + + return std::move(frame); + } + + std::vector getPoses() override + { + std::vector poses; + for(int i = 0; i < (int)(framesPerCycle*nCycles); i++) + { + float angle = (float)(CV_2PI*i/framesPerCycle); + Affine3f pose; + pose = pose.rotate(startPose.rotation()); + pose = pose.rotate(Vec3f(0.f, -1.f, 0.f)*angle); + pose = pose.translate(Vec3f(startPose.translation()[0]*sin(angle), + startPose.translation()[1], + startPose.translation()[2]*cos(angle))); + poses.push_back(pose); + } + + return poses; + } + + Size frameSize; + Matx33f intr; + float depthFactor; +}; + + +struct RotatingScene : Scene +{ + const int framesPerCycle = 32; + const float nCycles = 0.5f; + const Affine3f startPose = Affine3f(Vec3f(-1.f, 0.f, 0.f), Vec3f(1.5f, 2.f, -1.5f)); + + RotatingScene(Size sz, Matx33f _intr, float _depthFactor) : + frameSize(sz), intr(_intr), depthFactor(_depthFactor) + { + cv::RNG rng(0); + rng.fill(randTexture, cv::RNG::UNIFORM, 0.f, 1.f); + } + + static float noise(Point2f pt) + { + pt.x = abs(pt.x - (int)pt.x); + pt.y = abs(pt.y - (int)pt.y); + pt *= 256.f; + + int xi = cvFloor(pt.x), yi = cvFloor(pt.y); + + const float* row0 = randTexture[(yi+0)%256]; + const float* row1 = randTexture[(yi+1)%256]; + + float v00 = row0[(xi+0)%256]; + float v01 = row0[(xi+1)%256]; + float v10 = row1[(xi+0)%256]; + float v11 = row1[(xi+1)%256]; + + float tx = pt.x - xi, ty = pt.y - yi; + float v0 = v00 + tx*(v01 - v00); + float v1 = v10 + tx*(v11 - v10); + return v0 + ty*(v1 - v0); + } + + static float map(Point3f p) + { + const Point3f torPlace(0.f, 0.f, 0.f); + Point3f torPos(p - torPlace); + const Point2f torusParams(1.f, 0.2f); + Point2f torq(std::sqrt(torPos.x*torPos.x + torPos.z*torPos.z) - torusParams.x, torPos.y); + float torus = (float)cv::norm(torq) - torusParams.y; + + const Point3f cylShift(0.25f, 0.25f, 0.25f); + + Point3f cylPos = Point3f(abs(std::fmod(p.x-0.1f, cylShift.x)), + p.y, + abs(std::fmod(p.z-0.2f, cylShift.z))) - cylShift*0.5f; + + const Point2f cylParams(0.1f, + 0.1f+0.1f*sin(p.x*p.y*5.f /* +std::log(1.f+abs(p.x*0.1f)) */)); + Point2f cyld = Point2f(abs(std::sqrt(cylPos.x*cylPos.x + cylPos.z*cylPos.z)), abs(cylPos.y)) - cylParams; + float pins = min(max(cyld.x, cyld.y), 0.0f) + (float)cv::norm(Point2f(max(cyld.x, 0.f), max(cyld.y, 0.f))); + + float terrain = p.y + 0.25f*noise(Point2f(p.x, p.z)*0.01f); + + float res = min(terrain, max(-pins, torus)); + + return res; + } + + Mat depth(Affine3f pose) override + { + Mat_ frame(frameSize); + Reprojector reproj(intr); + + Range range(0, frame.rows); + parallel_for_(range, RenderInvoker(frame, pose, reproj, depthFactor)); + + return std::move(frame); + } + + std::vector getPoses() override + { + std::vector poses; + for(int i = 0; i < framesPerCycle*nCycles; i++) + { + float angle = (float)(CV_2PI*i/framesPerCycle); + Affine3f pose; + pose = pose.rotate(startPose.rotation()); + pose = pose.rotate(Vec3f(0.f, -1.f, 0.f)*angle); + pose = pose.translate(Vec3f(startPose.translation()[0]*sin(angle), + startPose.translation()[1], + startPose.translation()[2]*cos(angle))); + poses.push_back(pose); + } + + return poses; + } + + Size frameSize; + Matx33f intr; + float depthFactor; + static cv::Mat_ randTexture; +}; + +Mat_ RotatingScene::randTexture(256, 256); + +Ptr Scene::create(int nScene, Size sz, Matx33f _intr, float _depthFactor) +{ + if(nScene == 0) + return makePtr(sz, _intr, _depthFactor); + else + return makePtr(sz, _intr, _depthFactor); +} + +Mat_ creareRGBframe(Size s) +{ + Mat rgb(s.height, s.width, CV_8UC3, Scalar(0, 0, 0)); + int ci = s.height / 2; + int cj = s.width / 2; + for (int i = 0; i < s.height; i++) + { + for (int j = 0; j < s.width; j++) + { + + if (i < ci && j < cj) // red + rgb.at(i, j) = Vec3b(200, 0, 0); + if (i < ci && j > cj) // green + rgb.at(i, j) = Vec3b(0, 200, 0); + if (i > ci && j < cj) // blue + rgb.at(i, j) = Vec3b(0, 0, 200); + if (i > ci && j > cj) // mixed + rgb.at(i, j) = Vec3b(100, 100, 100); + + } + } + return rgb; +} + +void CheckFrequency(Mat image) +{ + float all = image.size().height * image.size().width; + int cc1 = 0, cc2 = 0, cc3 = 0, cc4 = 0; + Vec3b c1 = Vec3b(200, 0, 0), c2 = Vec3b(0, 200, 0); + Vec3b c3 = Vec3b(0, 0, 200), c4 = Vec3b(100, 100, 100); + float fc1, fc2, fc3, fc4; + for (int i = 0; i < image.size().height; i++) + { + for (int j = 0; j < image.size().width; j++) + { + Vec3b color = image.at(i, j); + if (color == c1) cc1++; + if (color == c2) cc2++; + if (color == c3) cc3++; + if (color == c4) cc4++; + } + } + ASSERT_LT(float(cc1) / all, 0.2); + ASSERT_LT(float(cc2) / all, 0.2); + ASSERT_LT(float(cc3) / all, 0.2); + ASSERT_LT(float(cc4) / all, 0.2); +} + +static const bool display = false; + +void flyTest(bool hiDense, bool test_colors) +{ + Ptr params; + params = colored_kinfu::Params::coloredTSDFParams(!hiDense); + Ptr scene = Scene::create(false, params->frameSize, params->intr, params->depthFactor); + + Ptr kf = colored_kinfu::ColoredKinFu::create(params); + + std::vector poses = scene->getPoses(); + Affine3f startPoseGT = poses[0], startPoseKF; + Affine3f pose, kfPose; + for(size_t i = 0; i < poses.size(); i++) + { + pose = poses[i]; + + Mat depth = scene->depth(pose); + Mat rgb = creareRGBframe(depth.size()); + + ASSERT_TRUE(kf->update(depth, rgb)); + + kfPose = kf->getPose(); + if(i == 0) + startPoseKF = kfPose; + + pose = ( startPoseGT.inv() * pose )*startPoseKF; + + if(display) + { + imshow("depth", depth*(1.f/params->depthFactor/4.f)); + imshow("rgb", rgb); + Mat rendered; + kf->render(rendered); + imshow("render", rendered); + waitKey(10); + } + + if (test_colors) + { + Mat rendered; + kf->render(rendered); + return; + } + } + + double rvecThreshold = hiDense ? 0.01 : 0.02; + ASSERT_LT(cv::norm(kfPose.rvec() - pose.rvec()), rvecThreshold); + double poseThreshold = hiDense ? 0.03 : 0.1; + ASSERT_LT(cv::norm(kfPose.translation() - pose.translation()), poseThreshold); +} + + +#ifdef OPENCV_ENABLE_NONFREE +TEST( ColoredKinectFusion, lowDense ) +#else +TEST(ColoredKinectFusion, DISABLED_lowDense) +#endif +{ + flyTest(false, false); +} + +#ifdef OPENCV_ENABLE_NONFREE +TEST(ColoredKinectFusion, highDense) +#else +TEST(KinectFusion, DISABLED_highDense) +#endif +{ + flyTest(true, false); +} + +#ifdef OPENCV_ENABLE_NONFREE +TEST( ColoredKinectFusion, color_lowDense ) +#else +TEST(ColoredKinectFusion, DISABLED_color_lowDense) +#endif +{ + flyTest(false, true); +} + +#ifdef OPENCV_ENABLE_NONFREE +TEST(ColoredKinectFusion, color_highDense) +#else +TEST(KinectFusion, DISABLED_color_highDense) +#endif +{ + flyTest(true, true); +} + +}} // namespace From e74a1543d33faa09d9b50837452f8f559344c641 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 12 Mar 2021 14:54:40 +0300 Subject: [PATCH 37/55] docs fix --- modules/rgbd/test/test_colored_kinfu.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/modules/rgbd/test/test_colored_kinfu.cpp b/modules/rgbd/test/test_colored_kinfu.cpp index efad131efe9..d432223cf89 100644 --- a/modules/rgbd/test/test_colored_kinfu.cpp +++ b/modules/rgbd/test/test_colored_kinfu.cpp @@ -283,7 +283,6 @@ Mat_ creareRGBframe(Size s) { for (int j = 0; j < s.width; j++) { - if (i < ci && j < cj) // red rgb.at(i, j) = Vec3b(200, 0, 0); if (i < ci && j > cj) // green @@ -359,7 +358,7 @@ void flyTest(bool hiDense, bool test_colors) imshow("render", rendered); waitKey(10); } - + if (test_colors) { Mat rendered; From 67658db5d61ea55cc2d65420f8dc466fbe868475 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 12 Mar 2021 15:20:19 +0300 Subject: [PATCH 38/55] warnings fix --- modules/rgbd/test/test_colored_kinfu.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/modules/rgbd/test/test_colored_kinfu.cpp b/modules/rgbd/test/test_colored_kinfu.cpp index d432223cf89..f6516627951 100644 --- a/modules/rgbd/test/test_colored_kinfu.cpp +++ b/modules/rgbd/test/test_colored_kinfu.cpp @@ -299,11 +299,10 @@ Mat_ creareRGBframe(Size s) void CheckFrequency(Mat image) { - float all = image.size().height * image.size().width; + float all = (float) image.size().height * image.size().width; int cc1 = 0, cc2 = 0, cc3 = 0, cc4 = 0; Vec3b c1 = Vec3b(200, 0, 0), c2 = Vec3b(0, 200, 0); Vec3b c3 = Vec3b(0, 0, 200), c4 = Vec3b(100, 100, 100); - float fc1, fc2, fc3, fc4; for (int i = 0; i < image.size().height; i++) { for (int j = 0; j < image.size().width; j++) From cf20c8e39b5874e43bebdde38a41f4c1563aa153 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 12 Mar 2021 15:52:13 +0300 Subject: [PATCH 39/55] minor fix --- modules/rgbd/test/test_colored_kinfu.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/rgbd/test/test_colored_kinfu.cpp b/modules/rgbd/test/test_colored_kinfu.cpp index f6516627951..70688b4e412 100644 --- a/modules/rgbd/test/test_colored_kinfu.cpp +++ b/modules/rgbd/test/test_colored_kinfu.cpp @@ -362,6 +362,7 @@ void flyTest(bool hiDense, bool test_colors) { Mat rendered; kf->render(rendered); + CheckFrequency(rendered); return; } } From 1cf064376b71c13b248f685d2ff7eaa01d1264c5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 12 Mar 2021 16:22:18 +0300 Subject: [PATCH 40/55] function warning fix --- modules/rgbd/test/test_colored_kinfu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/test/test_colored_kinfu.cpp b/modules/rgbd/test/test_colored_kinfu.cpp index 70688b4e412..0cd88bfd54f 100644 --- a/modules/rgbd/test/test_colored_kinfu.cpp +++ b/modules/rgbd/test/test_colored_kinfu.cpp @@ -297,7 +297,7 @@ Mat_ creareRGBframe(Size s) return rgb; } -void CheckFrequency(Mat image) +static inline void CheckFrequency(Mat image) { float all = (float) image.size().height * image.size().width; int cc1 = 0, cc2 = 0, cc3 = 0, cc4 = 0; From 5eeeef241ac01ae929219c61b1b91cf429b5f7b5 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 15 Mar 2021 12:46:38 +0300 Subject: [PATCH 41/55] add vectorized code --- modules/rgbd/src/colored_tsdf.cpp | 321 +++++++++++++++++++++++++++- modules/rgbd/src/tsdf_functions.cpp | 173 ++++++++++++++- 2 files changed, 490 insertions(+), 4 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index aa3946e5aa0..42e23902523 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -81,6 +81,12 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume Point3f getNormalVoxel(const cv::Point3f& p) const; Point3f getColorVoxel(const cv::Point3f& p) const; +#if USE_INTRINSICS + float interpolateVoxel(const v_float32x4& p) const; + v_float32x4 getNormalVoxel(const v_float32x4& p) const; + v_float32x4 getColorVoxel(const v_float32x4& p) const; +#endif + Vec4i volStrides; Vec6f frameParams; Mat pixNorms; @@ -168,7 +174,55 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d depthFactor, cameraPose, intrinsics, rgb_intrinsics, pixNorms, volume); } +#if USE_INTRINSICS +// all coordinate checks should be done in inclosing cycle +inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& _p) const +{ + v_float32x4 p(_p.x, _p.y, _p.z, 0); + return interpolateVoxel(p); +} + +inline float ColoredTSDFVolumeCPU::interpolateVoxel(const v_float32x4& p) const +{ + // tx, ty, tz = floor(p) + v_int32x4 ip = v_floor(p); + v_float32x4 t = p - v_cvt_f32(ip); + float tx = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float ty = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float tz = t.get0(); + + int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; + const RGBTsdfVoxel* volData = volume.ptr(); + + int ix = ip.get0(); + ip = v_rotate_right<1>(ip); + int iy = ip.get0(); + ip = v_rotate_right<1>(ip); + int iz = ip.get0(); + + int coordBase = ix * xdim + iy * ydim + iz * zdim; + + TsdfType vx[8]; + for (int i = 0; i < 8; i++) + vx[i] = volData[neighbourCoords[i] + coordBase].tsdf; + + v_float32x4 v0246 = tsdfToFloat_INTR(v_int32x4(vx[0], vx[2], vx[4], vx[6])); + v_float32x4 v1357 = tsdfToFloat_INTR(v_int32x4(vx[1], vx[3], vx[5], vx[7])); + v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); + + v_float32x4 v00_10 = vxx; + v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx))); + + v_float32x4 v0_1 = v00_10 + v_setall_f32(ty) * (v01_11 - v00_10); + float v0 = v0_1.get0(); + v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); + float v1 = v0_1.get0(); + return v0 + tx * (v1 - v0); +} +#else inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& p) const { int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; @@ -197,9 +251,80 @@ inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& p) const float v1 = v10 + ty*(v11 - v10); return v0 + tx*(v1 - v0); +} +#endif + +#if USE_INTRINSICS +//gradientDeltaFactor is fixed at 1.0 of voxel size +inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& _p) const +{ + v_float32x4 p(_p.x, _p.y, _p.z, 0.f); + v_float32x4 result = getNormalVoxel(p); + float CV_DECL_ALIGNED(16) ares[4]; + v_store_aligned(ares, result); + return Point3f(ares[0], ares[1], ares[2]); } +inline v_float32x4 ColoredTSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const +{ + if (v_check_any(p < v_float32x4(1.f, 1.f, 1.f, 0.f)) || + v_check_any(p >= v_float32x4((float)(volResolution.x - 2), + (float)(volResolution.y - 2), + (float)(volResolution.z - 2), 1.f)) + ) + return nanv; + + v_int32x4 ip = v_floor(p); + v_float32x4 t = p - v_cvt_f32(ip); + float tx = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float ty = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float tz = t.get0(); + + const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; + const RGBTsdfVoxel* volData = volume.ptr(); + + int ix = ip.get0(); ip = v_rotate_right<1>(ip); + int iy = ip.get0(); ip = v_rotate_right<1>(ip); + int iz = ip.get0(); + + int coordBase = ix * xdim + iy * ydim + iz * zdim; + + float CV_DECL_ALIGNED(16) an[4]; + an[0] = an[1] = an[2] = an[3] = 0.f; + for (int c = 0; c < 3; c++) + { + const int dim = volDims[c]; + float& nv = an[c]; + + TsdfType vx[8]; + for (int i = 0; i < 8; i++) + vx[i] = volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf - + volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf; + + v_float32x4 v0246 = tsdfToFloat_INTR(v_int32x4(vx[0], vx[2], vx[4], vx[6])); + v_float32x4 v1357 = tsdfToFloat_INTR(v_int32x4(vx[1], vx[3], vx[5], vx[7])); + v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); + + v_float32x4 v00_10 = vxx; + v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx))); + + v_float32x4 v0_1 = v00_10 + v_setall_f32(ty) * (v01_11 - v00_10); + float v0 = v0_1.get0(); + v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); + float v1 = v0_1.get0(); + + nv = v0 + tx * (v1 - v0); + } + + v_float32x4 n = v_load_aligned(an); + v_float32x4 Norm = v_sqrt(v_setall_f32(v_reduce_sum(n * n))); + + return Norm.get0() < 0.0001f ? nanv : n / Norm; +} +#else inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& p) const { const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; @@ -247,7 +372,53 @@ inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& p) const an[2] * an[2]); return nv < 0.0001f ? nan3 : an / nv; } +#endif +#if USE_INTRINSICS +//gradientDeltaFactor is fixed at 1.0 of voxel size +inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& _p) const +{ + v_float32x4 p(_p.x, _p.y, _p.z, 0.f); + v_float32x4 result = getColorVoxel(p); + float CV_DECL_ALIGNED(16) ares[4]; + v_store_aligned(ares, result); + return Point3f(ares[0], ares[1], ares[2]); +} +inline v_float32x4 ColoredTSDFVolumeCPU::getColorVoxel(const v_float32x4& p) const +{ + if (v_check_any(p < v_float32x4(1.f, 1.f, 1.f, 0.f)) || + v_check_any(p >= v_float32x4((float)(volResolution.x - 2), + (float)(volResolution.y - 2), + (float)(volResolution.z - 2), 1.f)) + ) + return nanv; + + v_int32x4 ip = v_floor(p); + v_float32x4 t = p - v_cvt_f32(ip); + float tx = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float ty = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + float tz = t.get0(); + + const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; + const RGBTsdfVoxel* volData = volume.ptr(); + + int ix = ip.get0(); ip = v_rotate_right<1>(ip); + int iy = ip.get0(); ip = v_rotate_right<1>(ip); + int iz = ip.get0(); + + int coordBase = ix * xdim + iy * ydim + iz * zdim; + float CV_DECL_ALIGNED(16) rgb[4]; + rgb[0] = volData[coordBase].r; + rgb[1] = volData[coordBase].g; + rgb[2] = volData[coordBase].b; + rgb[3] = 0.f; + + v_float32x4 res = v_load_aligned(rgb); + return res; +} +#else inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const { const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; @@ -285,7 +456,7 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const Point3f res(volData[coordBase].r, volData[coordBase].g, volData[coordBase].b); return res; } - +#endif struct ColorRaycastInvoker : ParallelLoopBody { @@ -308,7 +479,154 @@ struct ColorRaycastInvoker : ParallelLoopBody vol2cam(Affine3f(cameraPose.inv()) * volume.pose), reproj(intrinsics.makeReprojector()) { } +#if USE_INTRINSICS + virtual void operator() (const Range& range) const override + { + const v_float32x4 vfxy(reproj.fxinv, reproj.fyinv, 0, 0); + const v_float32x4 vcxy(reproj.cx, reproj.cy, 0, 0); + + const float(&cm)[16] = cam2vol.matrix.val; + const v_float32x4 camRot0(cm[0], cm[4], cm[8], 0); + const v_float32x4 camRot1(cm[1], cm[5], cm[9], 0); + const v_float32x4 camRot2(cm[2], cm[6], cm[10], 0); + const v_float32x4 camTrans(cm[3], cm[7], cm[11], 0); + + const v_float32x4 boxDown(boxMin.x, boxMin.y, boxMin.z, 0.f); + const v_float32x4 boxUp(boxMax.x, boxMax.y, boxMax.z, 0.f); + + const v_float32x4 invVoxelSize = v_float32x4(volume.voxelSizeInv, + volume.voxelSizeInv, + volume.voxelSizeInv, 1.f); + + const float(&vm)[16] = vol2cam.matrix.val; + const v_float32x4 volRot0(vm[0], vm[4], vm[8], 0); + const v_float32x4 volRot1(vm[1], vm[5], vm[9], 0); + const v_float32x4 volRot2(vm[2], vm[6], vm[10], 0); + const v_float32x4 volTrans(vm[3], vm[7], vm[11], 0); + + for (int y = range.start; y < range.end; y++) + { + ptype* ptsRow = points[y]; + ptype* nrmRow = normals[y]; + ptype* clrRow = colors[y]; + + for (int x = 0; x < points.cols; x++) + { + v_float32x4 point = nanv, normal = nanv, color = nanv; + + v_float32x4 orig = camTrans; + + // get direction through pixel in volume space: + + // 1. reproject (x, y) on projecting plane where z = 1.f + v_float32x4 planed = (v_float32x4((float)x, (float)y, 0.f, 0.f) - vcxy) * vfxy; + planed = v_combine_low(planed, v_float32x4(1.f, 0.f, 0.f, 0.f)); + + // 2. rotate to volume space + planed = v_matmuladd(planed, camRot0, camRot1, camRot2, v_setzero_f32()); + + // 3. normalize + v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(planed * planed))); + v_float32x4 dir = planed * invNorm; + + // compute intersection of ray with all six bbox planes + v_float32x4 rayinv = v_setall_f32(1.f) / dir; + // div by zero should be eliminated by these products + v_float32x4 tbottom = rayinv * (boxDown - orig); + v_float32x4 ttop = rayinv * (boxUp - orig); + + // re-order intersections to find smallest and largest on each axis + v_float32x4 minAx = v_min(ttop, tbottom); + v_float32x4 maxAx = v_max(ttop, tbottom); + + // near clipping plane + const float clip = 0.f; + float _minAx[4], _maxAx[4]; + v_store(_minAx, minAx); + v_store(_maxAx, maxAx); + float tmin = max({ _minAx[0], _minAx[1], _minAx[2], clip }); + float tmax = min({ _maxAx[0], _maxAx[1], _maxAx[2] }); + + // precautions against getting coordinates out of bounds + tmin = tmin + tstep; + tmax = tmax - tstep; + + if (tmin < tmax) + { + // interpolation optimized a little + orig *= invVoxelSize; + dir *= invVoxelSize; + + int xdim = volume.volDims[0]; + int ydim = volume.volDims[1]; + int zdim = volume.volDims[2]; + v_float32x4 rayStep = dir * v_setall_f32(tstep); + v_float32x4 next = (orig + dir * v_setall_f32(tmin)); + float f = volume.interpolateVoxel(next), fnext = f; + + //raymarch + int steps = 0; + int nSteps = cvFloor((tmax - tmin) / tstep); + for (; steps < nSteps; steps++) + { + next += rayStep; + v_int32x4 ip = v_round(next); + int ix = ip.get0(); ip = v_rotate_right<1>(ip); + int iy = ip.get0(); ip = v_rotate_right<1>(ip); + int iz = ip.get0(); + int coord = ix * xdim + iy * ydim + iz * zdim; + + fnext = tsdfToFloat(volume.volume.at(coord).tsdf); + if (fnext != f) + { + fnext = volume.interpolateVoxel(next); + + // when ray crosses a surface + if (std::signbit(f) != std::signbit(fnext)) + break; + + f = fnext; + } + } + + // if ray penetrates a surface from outside + // linearly interpolate t between two f values + if (f > 0.f && fnext < 0.f) + { + v_float32x4 tp = next - rayStep; + float ft = volume.interpolateVoxel(tp); + float ftdt = volume.interpolateVoxel(next); + float ts = tmin + tstep * (steps - ft / (ftdt - ft)); + // avoid division by zero + if (!cvIsNaN(ts) && !cvIsInf(ts)) + { + v_float32x4 pv = (orig + dir * v_setall_f32(ts)); + v_float32x4 nv = volume.getNormalVoxel(pv); + v_float32x4 cl = volume.getColorVoxel(pv); + + if (!isNaN(nv)) + { + color = cl; + //convert pv and nv to camera space + normal = v_matmuladd(nv, volRot0, volRot1, volRot2, v_setzero_f32()); + // interpolation optimized a little + point = v_matmuladd(pv * v_float32x4(volume.voxelSize, + volume.voxelSize, + volume.voxelSize, 1.f), + volRot0, volRot1, volRot2, volTrans); + } + } + } + } + + v_store((float*)(&ptsRow[x]), point); + v_store((float*)(&nrmRow[x]), normal); + v_store((float*)(&clrRow[x]), color); + } + } + } +#else virtual void operator() (const Range& range) const override { const Point3f camTrans = cam2vol.translation(); @@ -416,6 +734,7 @@ struct ColorRaycastInvoker : ParallelLoopBody } } } +#endif Points& points; Normals& normals; diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 30d5cbf9121..b048820c7c9 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -441,8 +441,176 @@ void integrateRGBVolumeUnit( const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * vpose); const float truncDistInv(1.f / truncDist); const float dfac(1.f / depthFactor); - RGBTsdfVoxel* volDataStart = volume.ptr();; + RGBTsdfVoxel* volDataStart = volume.ptr(); +#if USE_INTRINSICS + auto IntegrateInvoker = [&](const Range& range) + { + // zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt; + Point3f zStepPt = Point3f(vol2cam.matrix(0, 2), + vol2cam.matrix(1, 2), + vol2cam.matrix(2, 2)) * voxelSize; + + v_float32x4 zStep(zStepPt.x, zStepPt.y, zStepPt.z, 0); + v_float32x4 vfxy(proj.fx, proj.fy, 0.f, 0.f), vcxy(proj.cx, proj.cy, 0.f, 0.f); + v_float32x4 rgb_vfxy(projRGB.fx, projRGB.fy, 0.f, 0.f), rgb_vcxy(projRGB.cx, projRGB.cy, 0.f, 0.f); + const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols - 1, depth.rows - 1, 0, 0)); + + for (int x = range.start; x < range.end; x++) + { + RGBTsdfVoxel* volDataX = volDataStart + x * volStrides[0]; + for (int y = 0; y < volResolution.y; y++) + { + RGBTsdfVoxel* volDataY = volDataX + y * volStrides[1]; + // optimization of camSpace transformation (vector addition instead of matmul at each z) + Point3f basePt = vol2cam * (Point3f((float)x, (float)y, 0) * voxelSize); + v_float32x4 camSpacePt(basePt.x, basePt.y, basePt.z, 0); + + int startZ, endZ; + if (abs(zStepPt.z) > 1e-5) + { + int baseZ = (int)(-basePt.z / zStepPt.z); + if (zStepPt.z > 0) + { + startZ = baseZ; + endZ = volResolution.z; + } + else + { + startZ = 0; + endZ = baseZ; + } + } + else + { + if (basePt.z > 0) + { + startZ = 0; + endZ = volResolution.z; + } + else + { + // z loop shouldn't be performed + startZ = endZ = 0; + } + } + startZ = max(0, startZ); + endZ = min(int(volResolution.z), endZ); + for (int z = startZ; z < endZ; z++) + { + // optimization of the following: + //Point3f volPt = Point3f(x, y, z)*voxelSize; + //Point3f camSpacePt = vol2cam * volPt; + camSpacePt += zStep; + + float zCamSpace = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(camSpacePt))).get0(); + if (zCamSpace <= 0.f) + continue; + + v_float32x4 camPixVec = camSpacePt / v_setall_f32(zCamSpace); + v_float32x4 projected = v_muladd(camPixVec, vfxy, vcxy); + // leave only first 2 lanes + projected = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & + v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); + + v_float32x4 projectedRGB = v_muladd(camPixVec, rgb_vfxy, rgb_vcxy); + // leave only first 2 lanes + projectedRGB = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & + v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); + + + + depthType v; + // bilinearly interpolate depth at projected + { + const v_float32x4& pt = projected; + // check coords >= 0 and < imgSize + v_uint32x4 limits = v_reinterpret_as_u32(pt < v_setzero_f32()) | + v_reinterpret_as_u32(pt >= upLimits); + limits = limits | v_rotate_right<1>(limits); + if (limits.get0()) + continue; + + // xi, yi = floor(pt) + v_int32x4 ip = v_floor(pt); + v_int32x4 ipshift = ip; + int xi = ipshift.get0(); + ipshift = v_rotate_right<1>(ipshift); + int yi = ipshift.get0(); + + const depthType* row0 = depth[yi + 0]; + const depthType* row1 = depth[yi + 1]; + + // v001 = [v(xi + 0, yi + 0), v(xi + 1, yi + 0)] + v_float32x4 v001 = v_load_low(row0 + xi); + // v101 = [v(xi + 0, yi + 1), v(xi + 1, yi + 1)] + v_float32x4 v101 = v_load_low(row1 + xi); + + v_float32x4 vall = v_combine_low(v001, v101); + + // assume correct depth is positive + // don't fix missing data + if (v_check_all(vall > v_setzero_f32())) + { + v_float32x4 t = pt - v_cvt_f32(ip); + float tx = t.get0(); + t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); + v_float32x4 ty = v_setall_f32(t.get0()); + // vx is y-interpolated between rows 0 and 1 + v_float32x4 vx = v001 + ty * (v101 - v001); + float v0 = vx.get0(); + vx = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vx))); + float v1 = vx.get0(); + v = v0 + tx * (v1 - v0); + } + else + continue; + } + + // norm(camPixVec) produces double which is too slow + int _u = (int)projected.get0(); + int _v = (int)v_rotate_right<1>(projected).get0(); + int rgb_u = (int)projected.get0(); + int rgb_v = (int)v_rotate_right<1>(projected).get0(); + + if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows && + rgb_v >= 0 && rgb_v < color.rows && rgb_u >= 0 && rgb_u < color.cols)) + continue; + float pixNorm = pixNorms.at(_v, _u); + Vec4f colorRGB = color.at(rgb_v, rgb_u); + //float pixNorm = sqrt(v_reduce_sum(camPixVec*camPixVec)); + // difference between distances of point and of surface to camera + float sdf = pixNorm * (v * dfac - zCamSpace); + // possible alternative is: + // kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1); + if (sdf >= -truncDist) + { + TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv)); + + RGBTsdfVoxel& voxel = volDataY[z * volStrides[2]]; + WeightType& weight = voxel.weight; + TsdfType& value = voxel.tsdf; + ColorType& r = voxel.r; + ColorType& g = voxel.g; + ColorType& b = voxel.b; + + // update RGB + if (abs(((float)(r + g + b)) - (colorRGB[0] + colorRGB[1] + colorRGB[2])) < 1000 || weight < 1) + { + r = (ColorType)((float)(r * weight) + (colorRGB[0])) / (weight + 1); + g = (ColorType)((float)(g * weight) + (colorRGB[1])) / (weight + 1); + b = (ColorType)((float)(b * weight) + (colorRGB[2])) / (weight + 1); + } + + // update TSDF + value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); + weight = WeightType(min(int(weight + 1), int(maxWeight))); + } + } + } + } + }; +#else auto IntegrateInvoker = [&](const Range& range) { for (int x = range.start; x < range.end; x++) @@ -553,9 +721,8 @@ void integrateRGBVolumeUnit( } } }; - +#endif parallel_for_(integrateRange, IntegrateInvoker); - } } // namespace kinfu From 59e60c22436455df7082fcb2ec0d38391f656b28 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 15 Mar 2021 16:04:25 +0300 Subject: [PATCH 42/55] minor fixes --- modules/rgbd/src/colored_tsdf.cpp | 6 ------ modules/rgbd/src/tsdf_functions.cpp | 2 +- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 42e23902523..88f8a8fad3f 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -394,12 +394,6 @@ inline v_float32x4 ColoredTSDFVolumeCPU::getColorVoxel(const v_float32x4& p) con return nanv; v_int32x4 ip = v_floor(p); - v_float32x4 t = p - v_cvt_f32(ip); - float tx = t.get0(); - t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); - float ty = t.get0(); - t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t))); - float tz = t.get0(); const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const RGBTsdfVoxel* volData = volume.ptr(); diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index b048820c7c9..08fe416c41b 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -512,7 +512,7 @@ void integrateRGBVolumeUnit( // leave only first 2 lanes projected = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); - + v_float32x4 projectedRGB = v_muladd(camPixVec, rgb_vfxy, rgb_vcxy); // leave only first 2 lanes projectedRGB = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & From 21666b7f66a94fc90440145c33783d8a30826f51 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 15 Mar 2021 16:06:47 +0300 Subject: [PATCH 43/55] minor speed up --- modules/rgbd/src/tsdf_functions.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 08fe416c41b..3031c12dca0 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -513,13 +513,6 @@ void integrateRGBVolumeUnit( projected = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); - v_float32x4 projectedRGB = v_muladd(camPixVec, rgb_vfxy, rgb_vcxy); - // leave only first 2 lanes - projectedRGB = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & - v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); - - - depthType v; // bilinearly interpolate depth at projected { @@ -567,6 +560,11 @@ void integrateRGBVolumeUnit( continue; } + v_float32x4 projectedRGB = v_muladd(camPixVec, rgb_vfxy, rgb_vcxy); + // leave only first 2 lanes + projectedRGB = v_reinterpret_as_f32(v_reinterpret_as_u32(projected) & + v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)); + // norm(camPixVec) produces double which is too slow int _u = (int)projected.get0(); int _v = (int)v_rotate_right<1>(projected).get0(); From 53eeb475d9d33a7b71307b4b2e8740a9bed54c28 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Mar 2021 11:56:42 +0300 Subject: [PATCH 44/55] minor fixes; renaming --- .../include/opencv2/rgbd/colored_kinfu.hpp | 8 +- modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 5 +- .../rgbd/include/opencv2/rgbd/large_kinfu.hpp | 3 + modules/rgbd/samples/io_utils.hpp | 106 +++++++++--------- 4 files changed, 67 insertions(+), 55 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp index c3e7a1ffbac..427f830b972 100644 --- a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -74,17 +74,23 @@ struct CV_EXPORTS_W Params */ CV_WRAP static Ptr hashTSDFParams(bool isCoarse); - + /** @brief ColoredTSDF parameters + A set of parameters suitable for use with HashTSDFVolume + */ CV_WRAP static Ptr coloredTSDFParams(bool isCoarse); /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; + + /** @brief rgb frame size in pixels */ CV_PROP_RW Size rgb_frameSize; CV_PROP_RW kinfu::VolumeType volumeType; /** @brief camera intrinsics */ CV_PROP_RW Matx33f intr; + + /** @brief rgb camera intrinsics */ CV_PROP_RW Matx33f rgb_intr; /** @brief pre-scale per 1 meter for input values diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 0c849e07092..924314eddbe 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -73,19 +73,22 @@ struct CV_EXPORTS_W Params A set of parameters suitable for use with HashTSDFVolume */ CV_WRAP static Ptr hashTSDFParams(bool isCoarse); + /** @brief ColoredTSDF parameters A set of parameters suitable for use with ColoredTSDFVolume */ CV_WRAP static Ptr coloredTSDFParams(bool isCoarse); - /** @brief frame size in pixels */ CV_PROP_RW Size frameSize; + /** @brief rgb frame size in pixels */ CV_PROP_RW kinfu::VolumeType volumeType; /** @brief camera intrinsics */ CV_PROP_RW Matx33f intr; + + /** @brief rgb camera intrinsics */ CV_PROP_RW Matx33f rgb_intr; /** @brief pre-scale per 1 meter for input values diff --git a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp index a5de2dbed02..2f2d510e8e5 100644 --- a/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp @@ -40,7 +40,10 @@ struct CV_EXPORTS_W Params /** @brief camera intrinsics */ CV_PROP_RW Matx33f intr; + + /** @brief rgb camera intrinsics */ CV_PROP_RW Matx33f rgb_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 diff --git a/modules/rgbd/samples/io_utils.hpp b/modules/rgbd/samples/io_utils.hpp index 0acc1830fdf..56170b4d42b 100644 --- a/modules/rgbd/samples/io_utils.hpp +++ b/modules/rgbd/samples/io_utils.hpp @@ -80,44 +80,44 @@ struct DepthWriter namespace Kinect2Params { -static const Size frameSize = Size(512, 424); +static const Size depth_frameSize = Size(512, 424); // approximate values, no guarantee to be correct -static const float focal = 366.1f; -static const float cx = 258.2f; -static const float cy = 204.f; -static const float k1 = 0.12f; -static const float k2 = -0.34f; -static const float k3 = 0.12f; +static const float depth_focal = 366.1f; +static const float depth_cx = 258.2f; +static const float depth_cy = 204.f; +static const float depth_k1 = 0.12f; +static const float depth_k2 = -0.34f; +static const float depth_k3 = 0.12f; static const Size rgb_frameSize = Size(640, 480); static const float rgb_focal = 525.0f; -static const float rgb_cx = 319.5f; -static const float rgb_cy = 239.5f; -static const float rgb_k1 = 0.0f; -static const float rgb_k2 = 0.0f; -static const float rgb_k3 = 0.0f; +static const float rgb_cx = 319.5f; +static const float rgb_cy = 239.5f; +static const float rgb_k1 = 0.0f; +static const float rgb_k2 = 0.0f; +static const float rgb_k3 = 0.0f; }; // namespace Kinect2Params namespace AstraParams { -static const Size frameSize = Size(640, 480); +static const Size depth_frameSize = Size(640, 480); // approximate values, no guarantee to be correct -static const float fx = 535.4f; -static const float fy = 539.2f; -static const float cx = 320.1f; -static const float cy = 247.6f; -static const float k1 = 0.0f; -static const float k2 = 0.0f; -static const float k3 = 0.0f; +static const float depth_fx = 535.4f; +static const float depth_fy = 539.2f; +static const float depth_cx = 320.1f; +static const float depth_cy = 247.6f; +static const float depth_k1 = 0.0f; +static const float depth_k2 = 0.0f; +static const float depth_k3 = 0.0f; static const Size rgb_frameSize = Size(640, 480); static const float rgb_focal = 525.0f; -static const float rgb_cx = 319.5f; -static const float rgb_cy = 239.5f; -static const float rgb_k1 = 0.0f; -static const float rgb_k2 = 0.0f; -static const float rgb_k3 = 0.0f; +static const float rgb_cx = 319.5f; +static const float rgb_cy = 239.5f; +static const float rgb_k1 = 0.0f; +static const float rgb_k2 = 0.0f; +static const float rgb_k3 = 0.0f; }; // namespace Kinect2Params @@ -200,7 +200,7 @@ struct DepthSource // workaround for Kinect 2 if (sourceType == Type::DEPTH_KINECT2) { - out = out(Rect(Point(), Kinect2Params::frameSize)); + out = out(Rect(Point(), Kinect2Params::depth_frameSize)); UMat outCopy; // linear remap adds gradient between valid and invalid pixels @@ -231,20 +231,20 @@ struct DepthSource Size frameSize; if (sourceType == Type::DEPTH_KINECT2) { - fx = fy = Kinect2Params::focal; - cx = Kinect2Params::cx; - cy = Kinect2Params::cy; + fx = fy = Kinect2Params::depth_focal; + cx = Kinect2Params::depth_cx; + cy = Kinect2Params::depth_cy; - frameSize = Kinect2Params::frameSize; + frameSize = Kinect2Params::depth_frameSize; } else if (sourceType == Type::DEPTH_ASTRA) { - fx = AstraParams::fx; - fy = AstraParams::fy; - cx = AstraParams::cx; - cy = AstraParams::cy; + fx = AstraParams::depth_fx; + fy = AstraParams::depth_fy; + cx = AstraParams::depth_cx; + cy = AstraParams::depth_cy; - frameSize = AstraParams::frameSize; + frameSize = AstraParams::depth_frameSize; } else { @@ -314,9 +314,9 @@ struct DepthSource if (sourceType == Type::DEPTH_KINECT2) { Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; + distCoeffs(0) = Kinect2Params::depth_k1; + distCoeffs(1) = Kinect2Params::depth_k2; + distCoeffs(4) = Kinect2Params::depth_k3; initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, params.frameSize, CV_16SC2, undistortMap1, undistortMap2); @@ -336,9 +336,9 @@ struct DepthSource if (sourceType == Type::DEPTH_KINECT2) { Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; + distCoeffs(0) = Kinect2Params::depth_k1; + distCoeffs(1) = Kinect2Params::depth_k2; + distCoeffs(4) = Kinect2Params::depth_k3; initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, params.frameSize, CV_16SC2, undistortMap1, undistortMap2); @@ -358,9 +358,9 @@ struct DepthSource if (sourceType == Type::DEPTH_KINECT2) { Matx distCoeffs; - distCoeffs(0) = Kinect2Params::k1; - distCoeffs(1) = Kinect2Params::k2; - distCoeffs(4) = Kinect2Params::k3; + distCoeffs(0) = Kinect2Params::depth_k1; + distCoeffs(1) = Kinect2Params::depth_k2; + distCoeffs(4) = Kinect2Params::depth_k3; initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr, params.frameSize, CV_16SC2, undistortMap1, undistortMap2); @@ -414,20 +414,20 @@ struct RGBWriter dir = fileList.substr(0, slashIdx); if (!file.is_open()) - throw std::runtime_error("Failed to write depth list"); + throw std::runtime_error("Failed to write rgb list"); - file << "# depth maps saved from device" << std::endl; + file << "# rgb maps saved from device" << std::endl; file << "# useless_number filename" << std::endl; } - void append(InputArray _depth) + void append(InputArray _rgb) { - 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; + Mat rgb = _rgb.getMat(); + std::string rgbFname = cv::format("%04d.png", count); + std::string fullRGBFname = dir + '/' + rgbFname; + if (!imwrite(fullRGBFname, rgb)) + throw std::runtime_error("Failed to write rgb to file " + fullRGBFname); + file << count++ << " " << rgbFname << std::endl; } std::fstream file; From ecd8bd5e9b1dba185f2272902e94c24210f0dd28 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 17 Mar 2021 13:30:18 +0300 Subject: [PATCH 45/55] new minor fixes --- modules/rgbd/src/colored_tsdf.cpp | 34 ++++++++++++++--------------- modules/rgbd/src/tsdf_functions.cpp | 9 ++++---- modules/rgbd/src/tsdf_functions.hpp | 2 +- 3 files changed, 22 insertions(+), 23 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 88f8a8fad3f..87ef1b1e37d 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -65,8 +65,8 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume virtual void integrate(InputArray, float, const Matx44f&, const kinfu::Intr&, const int) override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const kinfu::Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override; - virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& intrinsics, const Size& frameSize, + const kinfu::Intr& depth_intrinsics, const Intr& rgb_intrinsics, const int frameId = 0) override; + virtual void raycast(const Matx44f& cameraPose, const kinfu::Intr& depth_intrinsics, const Size& frameSize, OutputArray points, OutputArray normals, OutputArray colors) const override; virtual void raycast(const Matx44f&, const kinfu::Intr&, const Size&, OutputArray, OutputArray) const override { CV_Error(Error::StsNotImplemented, "Not implemented"); }; @@ -153,7 +153,7 @@ RGBTsdfVoxel ColoredTSDFVolumeCPU::at(const Vec3i& volumeIdx) const // use depth instead of distance (optimization) void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float depthFactor, const Matx44f& cameraPose, - const Intr& intrinsics, const Intr& rgb_intrinsics, const int frameId) + const Intr& depth_intrinsics, const Intr& rgb_intrinsics, const int frameId) { CV_TRACE_FUNCTION(); CV_UNUSED(frameId); @@ -162,16 +162,16 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d Depth depth = _depth.getMat(); Colors rgb = _rgb.getMat(); Vec6f newParams((float)depth.rows, (float)depth.cols, - intrinsics.fx, intrinsics.fy, - intrinsics.cx, intrinsics.cy); + depth_intrinsics.fx, depth_intrinsics.fy, + depth_intrinsics.cx, depth_intrinsics.cy); if (!(frameParams == newParams)) { frameParams = newParams; - pixNorms = preCalculationPixNorm(depth, intrinsics); + pixNorms = preCalculationPixNorm(depth, depth_intrinsics); } integrateRGBVolumeUnit(truncDist, voxelSize, maxWeight, (this->pose).matrix, volResolution, volStrides, depth, rgb, - depthFactor, cameraPose, intrinsics, rgb_intrinsics, pixNorms, volume); + depthFactor, cameraPose, depth_intrinsics, rgb_intrinsics, pixNorms, volume); } #if USE_INTRINSICS @@ -455,7 +455,7 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const struct ColorRaycastInvoker : ParallelLoopBody { ColorRaycastInvoker(Points& _points, Normals& _normals, Colors& _colors, const Matx44f& cameraPose, - const Intr& intrinsics, const ColoredTSDFVolumeCPU& _volume) : + const Intr& depth_intrinsics, const ColoredTSDFVolumeCPU& _volume) : ParallelLoopBody(), points(_points), normals(_normals), @@ -471,13 +471,13 @@ struct ColorRaycastInvoker : ParallelLoopBody boxMin(), cam2vol(volume.pose.inv() * Affine3f(cameraPose)), vol2cam(Affine3f(cameraPose.inv()) * volume.pose), - reproj(intrinsics.makeReprojector()) + reprojDepth(depth_intrinsics.makeReprojector()) { } #if USE_INTRINSICS virtual void operator() (const Range& range) const override { - const v_float32x4 vfxy(reproj.fxinv, reproj.fyinv, 0, 0); - const v_float32x4 vcxy(reproj.cx, reproj.cy, 0, 0); + const v_float32x4 vfxy(reprojDepth.fxinv, reprojDepth.fyinv, 0, 0); + const v_float32x4 vcxy(reprojDepth.cx, reprojDepth.cy, 0, 0); const float(&cm)[16] = cam2vol.matrix.val; const v_float32x4 camRot0(cm[0], cm[4], cm[8], 0); @@ -597,11 +597,11 @@ struct ColorRaycastInvoker : ParallelLoopBody { v_float32x4 pv = (orig + dir * v_setall_f32(ts)); v_float32x4 nv = volume.getNormalVoxel(pv); - v_float32x4 cl = volume.getColorVoxel(pv); + v_float32x4 cv = volume.getColorVoxel(pv); if (!isNaN(nv)) { - color = cl; + color = cv; //convert pv and nv to camera space normal = v_matmuladd(nv, volRot0, volRot1, volRot2, v_setzero_f32()); // interpolation optimized a little @@ -639,7 +639,7 @@ struct ColorRaycastInvoker : ParallelLoopBody Point3f orig = camTrans; // direction through pixel in volume space - Point3f dir = normalize(Vec3f(camRot * reproj(Point3f(float(x), float(y), 1.f)))); + Point3f dir = normalize(Vec3f(camRot * reprojDepth(Point3f(float(x), float(y), 1.f)))); // compute intersection of ray with all six bbox planes Vec3f rayinv(1.f/dir.x, 1.f/dir.y, 1.f/dir.z); @@ -742,11 +742,11 @@ struct ColorRaycastInvoker : ParallelLoopBody const Affine3f cam2vol; const Affine3f vol2cam; - const Intr::Reprojector reproj; + const Intr::Reprojector reprojDepth; }; -void ColoredTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrinsics, const Size& frameSize, +void ColoredTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& depth_intrinsics, const Size& frameSize, OutputArray _points, OutputArray _normals, OutputArray _colors) const { CV_TRACE_FUNCTION(); @@ -760,7 +760,7 @@ void ColoredTSDFVolumeCPU::raycast(const Matx44f& cameraPose, const Intr& intrin Points points = _points.getMat(); Normals normals = _normals.getMat(); Colors colors = _colors.getMat(); - ColorRaycastInvoker ri(points, normals, colors, cameraPose, intrinsics, *this); + ColorRaycastInvoker ri(points, normals, colors, cameraPose, depth_intrinsics, *this); const int nstripes = -1; parallel_for_(Range(0, points.rows), ri, nstripes); diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 3031c12dca0..158aa86cfed 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -422,7 +422,7 @@ void integrateRGBVolumeUnit( float truncDist, float voxelSize, int maxWeight, cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics, const cv::kinfu::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume) + const cv::kinfu::Intr& depth_intrinsics, const cv::kinfu::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume) { CV_TRACE_FUNCTION(); @@ -435,8 +435,7 @@ void integrateRGBVolumeUnit( Mat volume = _volume.getMat(); Mat pixNorms = _pixNorms.getMat(); - const Intr::Projector proj(intrinsics.makeProjector()); - + const Intr::Projector projDepth(depth_intrinsics.makeProjector()); const Intr::Projector projRGB(rgb_intrinsics); const cv::Affine3f vol2cam(Affine3f(cameraPose.inv()) * vpose); const float truncDistInv(1.f / truncDist); @@ -452,7 +451,7 @@ void integrateRGBVolumeUnit( vol2cam.matrix(2, 2)) * voxelSize; v_float32x4 zStep(zStepPt.x, zStepPt.y, zStepPt.z, 0); - v_float32x4 vfxy(proj.fx, proj.fy, 0.f, 0.f), vcxy(proj.cx, proj.cy, 0.f, 0.f); + v_float32x4 vfxy(projDepth.fx, projDepth.fy, 0.f, 0.f), vcxy(projDepth.cx, projDepth.cy, 0.f, 0.f); v_float32x4 rgb_vfxy(projRGB.fx, projRGB.fy, 0.f, 0.f), rgb_vcxy(projRGB.cx, projRGB.cy, 0.f, 0.f); const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols - 1, depth.rows - 1, 0, 0)); @@ -667,7 +666,7 @@ void integrateRGBVolumeUnit( continue; Point3f camPixVec; - Point2f projected = proj(camSpacePt, camPixVec); + Point2f projected = projDepth(camSpacePt, camPixVec); Point2f projectedRGB = projRGB(camSpacePt, camPixVec); diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 75e0e930d71..9b11bf0e855 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -50,7 +50,7 @@ void integrateRGBVolumeUnit( float truncDist, float voxelSize, int maxWeight, cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose, - const cv::kinfu::Intr& intrinsics, const cv::kinfu::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume); + const cv::kinfu::Intr& depth_intrinsics, const cv::kinfu::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume); class CustomHashSet From 8b24c4160ae0f34beed246f29cc6e40c1dd4039e Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 2 Apr 2021 19:38:21 +0300 Subject: [PATCH 46/55] bug fix --- modules/rgbd/src/colored_tsdf.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 87ef1b1e37d..628b01ce0e7 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -301,11 +301,11 @@ inline v_float32x4 ColoredTSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) co TsdfType vx[8]; for (int i = 0; i < 8; i++) - vx[i] = volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf - - volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf; + vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf) - + tsdfToFloat(volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf); - v_float32x4 v0246 = tsdfToFloat_INTR(v_int32x4(vx[0], vx[2], vx[4], vx[6])); - v_float32x4 v1357 = tsdfToFloat_INTR(v_int32x4(vx[1], vx[3], vx[5], vx[7])); + v_float32x4 v0246(vx[0], vx[2], vx[4], vx[6]); + v_float32x4 v1357(vx[1], vx[3], vx[5], vx[7]); v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); v_float32x4 v00_10 = vxx; @@ -353,8 +353,8 @@ inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& p) const float vx[8]; for(int i = 0; i < 8; i++) - vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1*dim].tsdf - - volData[neighbourCoords[i] + coordBase - 1*dim].tsdf); + vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf) - + tsdfToFloat(volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf); float v00 = vx[0] + tz*(vx[1] - vx[0]); float v01 = vx[2] + tz*(vx[3] - vx[2]); From 20443bd1156ed303fd7f5b0c703ecf2302a76090 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Fri, 2 Apr 2021 22:55:42 +0300 Subject: [PATCH 47/55] minor fix --- modules/rgbd/src/colored_tsdf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 628b01ce0e7..80917d4128c 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -299,7 +299,7 @@ inline v_float32x4 ColoredTSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) co const int dim = volDims[c]; float& nv = an[c]; - TsdfType vx[8]; + float vx[8]; for (int i = 0; i < 8; i++) vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf) - tsdfToFloat(volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf); From 5a2f4853c38b997e9c198113358edec21a758bfa Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 5 Apr 2021 10:25:46 +0300 Subject: [PATCH 48/55] test update --- modules/rgbd/test/test_colored_kinfu.cpp | 199 +++++++++++++++++------ 1 file changed, 145 insertions(+), 54 deletions(-) diff --git a/modules/rgbd/test/test_colored_kinfu.cpp b/modules/rgbd/test/test_colored_kinfu.cpp index 0cd88bfd54f..1baac464258 100644 --- a/modules/rgbd/test/test_colored_kinfu.cpp +++ b/modules/rgbd/test/test_colored_kinfu.cpp @@ -9,7 +9,8 @@ // Inspired by Inigo Quilez' raymarching guide: // http://iquilezles.org/www/articles/distfunctions/distfunctions.htm -namespace opencv_test { namespace { +namespace opencv_test { +namespace { using namespace cv; @@ -19,7 +20,7 @@ struct Reprojector Reprojector() {} inline Reprojector(Matx33f intr) { - fxinv = 1.f/intr(0, 0), fyinv = 1.f/intr(1, 1); + fxinv = 1.f / intr(0, 0), fyinv = 1.f / intr(1, 1); cx = intr(0, 2), cy = intr(1, 2); } template @@ -37,8 +38,8 @@ template struct RenderInvoker : ParallelLoopBody { RenderInvoker(Mat_& _frame, Affine3f _pose, - Reprojector _reproj, - float _depthFactor) : ParallelLoopBody(), + Reprojector _reproj, + float _depthFactor) : ParallelLoopBody(), frame(_frame), pose(_pose), reproj(_reproj), @@ -47,33 +48,33 @@ struct RenderInvoker : ParallelLoopBody virtual void operator ()(const cv::Range& r) const { - for(int y = r.start; y < r.end; y++) + for (int y = r.start; y < r.end; y++) { float* frameRow = frame[y]; - for(int x = 0; x < frame.cols; x++) + for (int x = 0; x < frame.cols; x++) { float pix = 0; Point3f orig = pose.translation(); // direction through pixel Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f)); - float xyt = 1.f/(screenVec.x*screenVec.x + - screenVec.y*screenVec.y + 1.f); + float xyt = 1.f / (screenVec.x * screenVec.x + + screenVec.y * screenVec.y + 1.f); Point3f dir = normalize(Vec3f(pose.rotation() * screenVec)); // screen space axis - dir.y = - dir.y; + dir.y = -dir.y; const float maxDepth = 20.f; const float maxSteps = 256; float t = 0.f; - for(int step = 0; step < maxSteps && t < maxDepth; step++) + for (int step = 0; step < maxSteps && t < maxDepth; step++) { - Point3f p = orig + dir*t; + Point3f p = orig + dir * t; float d = Scene::map(p); - if(d < 0.000001f) + if (d < 0.000001f) { - float depth = std::sqrt(t*t*xyt); - pix = depth*depthFactor; + float depth = std::sqrt(t * t * xyt); + pix = depth * depthFactor; break; } t += d; @@ -90,11 +91,77 @@ struct RenderInvoker : ParallelLoopBody float depthFactor; }; +template +struct RenderColorInvoker : ParallelLoopBody +{ + RenderColorInvoker(Mat_& _frame, Affine3f _pose, + Reprojector _reproj, + float _depthFactor) : ParallelLoopBody(), + frame(_frame), + pose(_pose), + reproj(_reproj), + depthFactor(_depthFactor) + { } + + virtual void operator ()(const cv::Range& r) const + { + for (int y = r.start; y < r.end; y++) + { + Vec3f* frameRow = frame[y]; + for (int x = 0; x < frame.cols; x++) + { + Vec3f pix = 0; + + Point3f orig = pose.translation(); + // direction through pixel + Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f)); + float xyt = 1.f / (screenVec.x * screenVec.x + + screenVec.y * screenVec.y + 1.f); + Point3f dir = normalize(Vec3f(pose.rotation() * screenVec)); + // screen space axis + dir.y = -dir.y; + + const float maxDepth = 20.f; + const float maxSteps = 256; + float t = 0.f; + for (int step = 0; step < maxSteps && t < maxDepth; step++) + { + Point3f p = orig + dir * t; + float d = Scene::map(p); + if (d < 0.000001f) + { + float m = 0.25f; + float p0 = float(abs(fmod(p.x, m)) > m / 2.f); + float p1 = float(abs(fmod(p.y, m)) > m / 2.f); + float p2 = float(abs(fmod(p.z, m)) > m / 2.f); + + pix[0] = p0 + p1; + pix[1] = p1 + p2; + pix[2] = p0 + p2; + + pix *= 128.f; + break; + } + t += d; + } + + frameRow[x] = pix; + } + } + } + + Mat_& frame; + Affine3f pose; + Reprojector reproj; + float depthFactor; +}; + struct Scene { virtual ~Scene() {} static Ptr create(int nScene, Size sz, Matx33f _intr, float _depthFactor); virtual Mat depth(Affine3f pose) = 0; + virtual Mat rgb(Affine3f pose) = 0; virtual std::vector getPoses() = 0; }; @@ -144,18 +211,29 @@ struct CubeSpheresScene : Scene return std::move(frame); } + Mat rgb(Affine3f pose) override + { + Mat_ frame(frameSize); + Reprojector reproj(intr); + + Range range(0, frame.rows); + parallel_for_(range, RenderColorInvoker(frame, pose, reproj, depthFactor)); + + return std::move(frame); + } + std::vector getPoses() override { std::vector poses; - for(int i = 0; i < (int)(framesPerCycle*nCycles); i++) + for (int i = 0; i < (int)(framesPerCycle * nCycles); i++) { - float angle = (float)(CV_2PI*i/framesPerCycle); + float angle = (float)(CV_2PI * i / framesPerCycle); Affine3f pose; pose = pose.rotate(startPose.rotation()); - pose = pose.rotate(Vec3f(0.f, -1.f, 0.f)*angle); - pose = pose.translate(Vec3f(startPose.translation()[0]*sin(angle), - startPose.translation()[1], - startPose.translation()[2]*cos(angle))); + pose = pose.rotate(Vec3f(0.f, -1.f, 0.f) * angle); + pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), + startPose.translation()[1], + startPose.translation()[2] * cos(angle))); poses.push_back(pose); } @@ -189,18 +267,18 @@ struct RotatingScene : Scene int xi = cvFloor(pt.x), yi = cvFloor(pt.y); - const float* row0 = randTexture[(yi+0)%256]; - const float* row1 = randTexture[(yi+1)%256]; + const float* row0 = randTexture[(yi + 0) % 256]; + const float* row1 = randTexture[(yi + 1) % 256]; - float v00 = row0[(xi+0)%256]; - float v01 = row0[(xi+1)%256]; - float v10 = row1[(xi+0)%256]; - float v11 = row1[(xi+1)%256]; + float v00 = row0[(xi + 0) % 256]; + float v01 = row0[(xi + 1) % 256]; + float v10 = row1[(xi + 0) % 256]; + float v11 = row1[(xi + 1) % 256]; float tx = pt.x - xi, ty = pt.y - yi; - float v0 = v00 + tx*(v01 - v00); - float v1 = v10 + tx*(v11 - v10); - return v0 + ty*(v1 - v0); + float v0 = v00 + tx * (v01 - v00); + float v1 = v10 + tx * (v11 - v10); + return v0 + ty * (v1 - v0); } static float map(Point3f p) @@ -208,21 +286,21 @@ struct RotatingScene : Scene const Point3f torPlace(0.f, 0.f, 0.f); Point3f torPos(p - torPlace); const Point2f torusParams(1.f, 0.2f); - Point2f torq(std::sqrt(torPos.x*torPos.x + torPos.z*torPos.z) - torusParams.x, torPos.y); + Point2f torq(std::sqrt(torPos.x * torPos.x + torPos.z * torPos.z) - torusParams.x, torPos.y); float torus = (float)cv::norm(torq) - torusParams.y; const Point3f cylShift(0.25f, 0.25f, 0.25f); - Point3f cylPos = Point3f(abs(std::fmod(p.x-0.1f, cylShift.x)), - p.y, - abs(std::fmod(p.z-0.2f, cylShift.z))) - cylShift*0.5f; + Point3f cylPos = Point3f(abs(std::fmod(p.x - 0.1f, cylShift.x)), + p.y, + abs(std::fmod(p.z - 0.2f, cylShift.z))) - cylShift * 0.5f; const Point2f cylParams(0.1f, - 0.1f+0.1f*sin(p.x*p.y*5.f /* +std::log(1.f+abs(p.x*0.1f)) */)); - Point2f cyld = Point2f(abs(std::sqrt(cylPos.x*cylPos.x + cylPos.z*cylPos.z)), abs(cylPos.y)) - cylParams; + 0.1f + 0.1f * sin(p.x * p.y * 5.f /* +std::log(1.f+abs(p.x*0.1f)) */)); + Point2f cyld = Point2f(abs(std::sqrt(cylPos.x * cylPos.x + cylPos.z * cylPos.z)), abs(cylPos.y)) - cylParams; float pins = min(max(cyld.x, cyld.y), 0.0f) + (float)cv::norm(Point2f(max(cyld.x, 0.f), max(cyld.y, 0.f))); - float terrain = p.y + 0.25f*noise(Point2f(p.x, p.z)*0.01f); + float terrain = p.y + 0.25f * noise(Point2f(p.x, p.z) * 0.01f); float res = min(terrain, max(-pins, torus)); @@ -240,18 +318,29 @@ struct RotatingScene : Scene return std::move(frame); } + Mat rgb(Affine3f pose) override + { + Mat_ frame(frameSize); + Reprojector reproj(intr); + + Range range(0, frame.rows); + parallel_for_(range, RenderColorInvoker(frame, pose, reproj, depthFactor)); + + return std::move(frame); + } + std::vector getPoses() override { std::vector poses; - for(int i = 0; i < framesPerCycle*nCycles; i++) + for (int i = 0; i < framesPerCycle * nCycles; i++) { - float angle = (float)(CV_2PI*i/framesPerCycle); + float angle = (float)(CV_2PI * i / framesPerCycle); Affine3f pose; pose = pose.rotate(startPose.rotation()); - pose = pose.rotate(Vec3f(0.f, -1.f, 0.f)*angle); - pose = pose.translate(Vec3f(startPose.translation()[0]*sin(angle), - startPose.translation()[1], - startPose.translation()[2]*cos(angle))); + pose = pose.rotate(Vec3f(0.f, -1.f, 0.f) * angle); + pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle), + startPose.translation()[1], + startPose.translation()[2] * cos(angle))); poses.push_back(pose); } @@ -268,7 +357,7 @@ Mat_ RotatingScene::randTexture(256, 256); Ptr Scene::create(int nScene, Size sz, Matx33f _intr, float _depthFactor) { - if(nScene == 0) + if (nScene == 0) return makePtr(sz, _intr, _depthFactor); else return makePtr(sz, _intr, _depthFactor); @@ -299,7 +388,7 @@ Mat_ creareRGBframe(Size s) static inline void CheckFrequency(Mat image) { - float all = (float) image.size().height * image.size().width; + float all = (float)image.size().height * image.size().width; int cc1 = 0, cc2 = 0, cc3 = 0, cc4 = 0; Vec3b c1 = Vec3b(200, 0, 0), c2 = Vec3b(0, 200, 0); Vec3b c3 = Vec3b(0, 0, 200), c4 = Vec3b(100, 100, 100); @@ -333,25 +422,26 @@ void flyTest(bool hiDense, bool test_colors) std::vector poses = scene->getPoses(); Affine3f startPoseGT = poses[0], startPoseKF; Affine3f pose, kfPose; - for(size_t i = 0; i < poses.size(); i++) + for (size_t i = 0; i < poses.size(); i++) { pose = poses[i]; Mat depth = scene->depth(pose); - Mat rgb = creareRGBframe(depth.size()); + //DEBUG + Mat rgb = scene->rgb(pose);// creareRGBframe(depth.size()); ASSERT_TRUE(kf->update(depth, rgb)); kfPose = kf->getPose(); - if(i == 0) + if (i == 0) startPoseKF = kfPose; - pose = ( startPoseGT.inv() * pose )*startPoseKF; + pose = (startPoseGT.inv() * pose) * startPoseKF; - if(display) + if (display) { - imshow("depth", depth*(1.f/params->depthFactor/4.f)); - imshow("rgb", rgb); + imshow("depth", depth * (1.f / params->depthFactor / 4.f)); + imshow("rgb", rgb * (1.f / 255.f)); Mat rendered; kf->render(rendered); imshow("render", rendered); @@ -375,7 +465,7 @@ void flyTest(bool hiDense, bool test_colors) #ifdef OPENCV_ENABLE_NONFREE -TEST( ColoredKinectFusion, lowDense ) +TEST(ColoredKinectFusion, lowDense) #else TEST(ColoredKinectFusion, DISABLED_lowDense) #endif @@ -393,7 +483,7 @@ TEST(KinectFusion, DISABLED_highDense) } #ifdef OPENCV_ENABLE_NONFREE -TEST( ColoredKinectFusion, color_lowDense ) +TEST(ColoredKinectFusion, color_lowDense) #else TEST(ColoredKinectFusion, DISABLED_color_lowDense) #endif @@ -410,4 +500,5 @@ TEST(KinectFusion, DISABLED_color_highDense) flyTest(true, true); } -}} // namespace +} +} // namespace From d1e5f3b0db9ffd6eef4a33d1b250b874e4fd31ad Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 5 Apr 2021 10:31:42 +0300 Subject: [PATCH 49/55] extra code removed --- modules/rgbd/src/tsdf_functions.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 158aa86cfed..f0964a7ddca 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -592,7 +592,7 @@ void integrateRGBVolumeUnit( ColorType& b = voxel.b; // update RGB - if (abs(((float)(r + g + b)) - (colorRGB[0] + colorRGB[1] + colorRGB[2])) < 1000 || weight < 1) + if (weight < 1) { r = (ColorType)((float)(r * weight) + (colorRGB[0])) / (weight + 1); g = (ColorType)((float)(g * weight) + (colorRGB[1])) / (weight + 1); @@ -703,7 +703,7 @@ void integrateRGBVolumeUnit( ColorType& b = voxel.b; // update RGB - if (abs(((float)(r + g + b)) - (colorRGB[0] + colorRGB[1] + colorRGB[2])) < 1000 || weight < 1) + if (weight < 1) { r = (ColorType)((float)(r * weight) + (colorRGB[0])) / (weight + 1); g = (ColorType)((float)(g * weight) + (colorRGB[1])) / (weight + 1); From 32cae64c9e3b0a3069ea308549af60b616029e84 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 5 Apr 2021 10:46:37 +0300 Subject: [PATCH 50/55] bugfix; warning fix --- modules/rgbd/src/tsdf_functions.cpp | 4 ++-- modules/rgbd/test/test_colored_kinfu.cpp | 25 ------------------------ 2 files changed, 2 insertions(+), 27 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index f0964a7ddca..292b1d19734 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -567,8 +567,8 @@ void integrateRGBVolumeUnit( // norm(camPixVec) produces double which is too slow int _u = (int)projected.get0(); int _v = (int)v_rotate_right<1>(projected).get0(); - int rgb_u = (int)projected.get0(); - int rgb_v = (int)v_rotate_right<1>(projected).get0(); + int rgb_u = (int)projectedRGB.get0(); + int rgb_v = (int)v_rotate_right<1>(projectedRGB).get0(); if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows && rgb_v >= 0 && rgb_v < color.rows && rgb_u >= 0 && rgb_u < color.cols)) diff --git a/modules/rgbd/test/test_colored_kinfu.cpp b/modules/rgbd/test/test_colored_kinfu.cpp index 1baac464258..4303b260e3e 100644 --- a/modules/rgbd/test/test_colored_kinfu.cpp +++ b/modules/rgbd/test/test_colored_kinfu.cpp @@ -115,8 +115,6 @@ struct RenderColorInvoker : ParallelLoopBody Point3f orig = pose.translation(); // direction through pixel Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f)); - float xyt = 1.f / (screenVec.x * screenVec.x + - screenVec.y * screenVec.y + 1.f); Point3f dir = normalize(Vec3f(pose.rotation() * screenVec)); // screen space axis dir.y = -dir.y; @@ -363,29 +361,6 @@ Ptr Scene::create(int nScene, Size sz, Matx33f _intr, float _depthFactor) return makePtr(sz, _intr, _depthFactor); } -Mat_ creareRGBframe(Size s) -{ - Mat rgb(s.height, s.width, CV_8UC3, Scalar(0, 0, 0)); - int ci = s.height / 2; - int cj = s.width / 2; - for (int i = 0; i < s.height; i++) - { - for (int j = 0; j < s.width; j++) - { - if (i < ci && j < cj) // red - rgb.at(i, j) = Vec3b(200, 0, 0); - if (i < ci && j > cj) // green - rgb.at(i, j) = Vec3b(0, 200, 0); - if (i > ci && j < cj) // blue - rgb.at(i, j) = Vec3b(0, 0, 200); - if (i > ci && j > cj) // mixed - rgb.at(i, j) = Vec3b(100, 100, 100); - - } - } - return rgb; -} - static inline void CheckFrequency(Mat image) { float all = (float)image.size().height * image.size().width; From 0fadae244f6176b34cfc3a3a56c1db30f914ca75 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 5 Apr 2021 15:00:39 +0300 Subject: [PATCH 51/55] simple interpolation --- modules/rgbd/src/colored_tsdf.cpp | 62 +++++++++++++++++++++---------- 1 file changed, 42 insertions(+), 20 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 80917d4128c..9f7cfa6a1fe 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -9,6 +9,8 @@ #include "tsdf_functions.hpp" #include "opencl_kernels_rgbd.hpp" +#define USE_INTERPOLATION_IN_GETNORMAL 1 + namespace cv { namespace kinfu { @@ -79,9 +81,10 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume float interpolateVoxel(const cv::Point3f& p) const; Point3f getNormalVoxel(const cv::Point3f& p) const; + float interpolateColor(float tx, float ty, float tz, float vx[8]) const; Point3f getColorVoxel(const cv::Point3f& p) const; -#if USE_INTRINSICS +#if USE_INTRINSICS && 0 float interpolateVoxel(const v_float32x4& p) const; v_float32x4 getNormalVoxel(const v_float32x4& p) const; v_float32x4 getColorVoxel(const v_float32x4& p) const; @@ -174,7 +177,7 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d depthFactor, cameraPose, depth_intrinsics, rgb_intrinsics, pixNorms, volume); } -#if USE_INTRINSICS +#if USE_INTRINSICS && 0 // all coordinate checks should be done in inclosing cycle inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& _p) const { @@ -255,7 +258,7 @@ inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& p) const #endif -#if USE_INTRINSICS +#if USE_INTRINSICS && 0 //gradientDeltaFactor is fixed at 1.0 of voxel size inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& _p) const { @@ -374,7 +377,20 @@ inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& p) const } #endif -#if USE_INTRINSICS +inline float ColoredTSDFVolumeCPU::interpolateColor(float tx, float ty, float tz, float vx[8]) const +{ + float v00 = vx[0] + tz * (vx[1] - vx[0]); + float v01 = vx[2] + tz * (vx[3] - vx[2]); + float v10 = vx[4] + tz * (vx[5] - vx[4]); + float v11 = vx[6] + tz * (vx[7] - vx[6]); + + float v0 = v00 + ty * (v01 - v00); + float v1 = v10 + ty * (v11 - v10); + + return v0 + tx * (v1 - v0); +} + +#if USE_INTRINSICS && 0 //gradientDeltaFactor is fixed at 1.0 of voxel size inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& _p) const { @@ -418,6 +434,8 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2]; const RGBTsdfVoxel* volData = volume.ptr(); + + if(p.x < 1 || p.x >= volResolution.x - 2 || p.y < 1 || p.y >= volResolution.y - 2 || p.z < 1 || p.z >= volResolution.z - 2) @@ -428,26 +446,30 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const int iz = cvFloor(p.z); int coordBase = ix*xdim + iy*ydim + iz*zdim; + Point3f res; +#if USE_INTERPOLATION_IN_GETNORMAL // TODO: create better interpolation or remove this simple version - /* - float r = 0, g = 0, b = 0; - float mainRGBsum = (float) (volData[coordBase].r + volData[coordBase].g + volData[coordBase].b); - float counter = 0; + float r[8], g[8], b[8]; for (int i = 0; i < 8; i++) { - float sum = (float) (volData[neighbourCoords[i] + coordBase].r + volData[neighbourCoords[i] + coordBase].g + volData[neighbourCoords[i] + coordBase].b ); - if (volData[neighbourCoords[i] + coordBase].r == volData[neighbourCoords[i] + coordBase].r && abs(sum - mainRGBsum) < 1000) - { - r += (float) volData[neighbourCoords[i] + coordBase].r; - g += (float) volData[neighbourCoords[i] + coordBase].g; - b += (float) volData[neighbourCoords[i] + coordBase].b; - counter+=1.0f; - } + r[i] = (float) volData[neighbourCoords[i] + coordBase].r; + g[i] = (float) volData[neighbourCoords[i] + coordBase].g; + b[i] = (float) volData[neighbourCoords[i] + coordBase].b; } - Point3f res(r / counter, g / counter, b / counter); - */ - Point3f res(volData[coordBase].r, volData[coordBase].g, volData[coordBase].b); + + Point3f ptVox = p * voxelSizeInv; + Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z)); + float tx = ptVox.x - iptVox[0]; + float ty = ptVox.y - iptVox[1]; + float tz = ptVox.z - iptVox[2]; + + res=Point3f(interpolateColor(tx, ty, tz, r), + interpolateColor(tx, ty, tz, g), + interpolateColor(tx, ty, tz, b)); +#else + res=Point3f(volData[coordBase].r, volData[coordBase].g, volData[coordBase].b); +#endif return res; } #endif @@ -473,7 +495,7 @@ struct ColorRaycastInvoker : ParallelLoopBody vol2cam(Affine3f(cameraPose.inv()) * volume.pose), reprojDepth(depth_intrinsics.makeReprojector()) { } -#if USE_INTRINSICS +#if USE_INTRINSICS && 0 virtual void operator() (const Range& range) const override { const v_float32x4 vfxy(reprojDepth.fxinv, reprojDepth.fyinv, 0, 0); From 9b030eabd3cc313d9a764a69f4044ded5d040e25 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Mon, 5 Apr 2021 15:28:54 +0300 Subject: [PATCH 52/55] minor fix --- modules/rgbd/src/tsdf_functions.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index 292b1d19734..d75bdbba51b 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -592,12 +592,9 @@ void integrateRGBVolumeUnit( ColorType& b = voxel.b; // update RGB - if (weight < 1) - { - r = (ColorType)((float)(r * weight) + (colorRGB[0])) / (weight + 1); - g = (ColorType)((float)(g * weight) + (colorRGB[1])) / (weight + 1); - b = (ColorType)((float)(b * weight) + (colorRGB[2])) / (weight + 1); - } + r = (ColorType)((float)(r * weight) + (colorRGB[0])) / (weight + 1); + g = (ColorType)((float)(g * weight) + (colorRGB[1])) / (weight + 1); + b = (ColorType)((float)(b * weight) + (colorRGB[2])) / (weight + 1); // update TSDF value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); From bc4be4146c5d3f114412f27161c86b7e9d9510c9 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 6 Apr 2021 11:09:00 +0300 Subject: [PATCH 53/55] color work minor fix --- modules/rgbd/src/colored_tsdf.cpp | 1 + modules/rgbd/src/kinfu_frame.cpp | 8 ++++++++ modules/rgbd/src/tsdf_functions.cpp | 2 +- modules/rgbd/src/tsdf_functions.hpp | 14 ++++++++++++++ 4 files changed, 24 insertions(+), 1 deletion(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 9f7cfa6a1fe..070e147aeb0 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -470,6 +470,7 @@ inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& p) const #else res=Point3f(volData[coordBase].r, volData[coordBase].g, volData[coordBase].b); #endif + colorFix(res); return res; } #endif diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index 46423a98387..b8df7c1f5a7 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -43,6 +43,13 @@ inline float specPow<1>(float x) return x; } +inline void colorFix(Point3f& c) +{ + if (c.x > 255) c.x = 255; + if (c.y > 255) c.y = 255; + if (c.z > 255) c.z = 255; +} + struct RenderInvoker : ParallelLoopBody { RenderInvoker(const Points& _points, const Normals& _normals, Mat_& _img, Affine3f _lightPose, Size _sz) : @@ -376,6 +383,7 @@ struct ComputePointsNormalsColorsInvoker : ParallelLoopBody n = -normalize(vec); p = v00; c = fromPtype(rgb.at(rgb_v, rgb_u)); + colorFix(c); } } diff --git a/modules/rgbd/src/tsdf_functions.cpp b/modules/rgbd/src/tsdf_functions.cpp index d75bdbba51b..3f8bc26f011 100644 --- a/modules/rgbd/src/tsdf_functions.cpp +++ b/modules/rgbd/src/tsdf_functions.cpp @@ -595,7 +595,7 @@ void integrateRGBVolumeUnit( r = (ColorType)((float)(r * weight) + (colorRGB[0])) / (weight + 1); g = (ColorType)((float)(g * weight) + (colorRGB[1])) / (weight + 1); b = (ColorType)((float)(b * weight) + (colorRGB[2])) / (weight + 1); - + colorFix(r, g, b); // update TSDF value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1)); weight = WeightType(min(int(weight + 1), int(maxWeight))); diff --git a/modules/rgbd/src/tsdf_functions.hpp b/modules/rgbd/src/tsdf_functions.hpp index 9b11bf0e855..1031474d0b7 100644 --- a/modules/rgbd/src/tsdf_functions.hpp +++ b/modules/rgbd/src/tsdf_functions.hpp @@ -35,6 +35,20 @@ inline float tsdfToFloat(TsdfType num) return float(num) * (-1.f / 128.f); } +inline void colorFix(ColorType& r, ColorType& g, ColorType&b) +{ + if (r > 255) r = 255; + if (g > 255) g = 255; + if (b > 255) b = 255; +} + +inline void colorFix(Point3f& c) +{ + if (c.x > 255) c.x = 255; + if (c.y > 255) c.y = 255; + if (c.z > 255) c.z = 255; +} + cv::Mat preCalculationPixNorm(Depth depth, const Intr& intrinsics); cv::UMat preCalculationPixNormGPU(const UMat& depth, const Intr& intrinsics); From ba9e2fdae247fd264488e0c962e5bfc0b467b663 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Tue, 6 Apr 2021 13:58:11 +0300 Subject: [PATCH 54/55] vectorized interpolation --- modules/rgbd/src/colored_tsdf.cpp | 54 +++++++++++++++++++++++++++---- 1 file changed, 48 insertions(+), 6 deletions(-) diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 070e147aeb0..116136a7f21 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -84,7 +84,7 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume float interpolateColor(float tx, float ty, float tz, float vx[8]) const; Point3f getColorVoxel(const cv::Point3f& p) const; -#if USE_INTRINSICS && 0 +#if USE_INTRINSICS float interpolateVoxel(const v_float32x4& p) const; v_float32x4 getNormalVoxel(const v_float32x4& p) const; v_float32x4 getColorVoxel(const v_float32x4& p) const; @@ -177,7 +177,7 @@ void ColoredTSDFVolumeCPU::integrate(InputArray _depth, InputArray _rgb, float d depthFactor, cameraPose, depth_intrinsics, rgb_intrinsics, pixNorms, volume); } -#if USE_INTRINSICS && 0 +#if USE_INTRINSICS // all coordinate checks should be done in inclosing cycle inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& _p) const { @@ -258,7 +258,7 @@ inline float ColoredTSDFVolumeCPU::interpolateVoxel(const Point3f& p) const #endif -#if USE_INTRINSICS && 0 +#if USE_INTRINSICS //gradientDeltaFactor is fixed at 1.0 of voxel size inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& _p) const { @@ -377,6 +377,25 @@ inline Point3f ColoredTSDFVolumeCPU::getNormalVoxel(const Point3f& p) const } #endif +#if USE_INTRINSICS +inline float ColoredTSDFVolumeCPU::interpolateColor(float tx, float ty, float tz, float vx[8]) const +{ + v_float32x4 v0246, v1357; + v_load_deinterleave(vx, v0246, v1357); + + v_float32x4 vxx = v0246 + v_setall_f32(tz) * (v1357 - v0246); + + v_float32x4 v00_10 = vxx; + v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx))); + + v_float32x4 v0_1 = v00_10 + v_setall_f32(ty) * (v01_11 - v00_10); + float v0 = v0_1.get0(); + v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1))); + float v1 = v0_1.get0(); + + return v0 + tx * (v1 - v0); +} +#else inline float ColoredTSDFVolumeCPU::interpolateColor(float tx, float ty, float tz, float vx[8]) const { float v00 = vx[0] + tz * (vx[1] - vx[0]); @@ -389,8 +408,9 @@ inline float ColoredTSDFVolumeCPU::interpolateColor(float tx, float ty, float tz return v0 + tx * (v1 - v0); } +#endif -#if USE_INTRINSICS && 0 +#if USE_INTRINSICS //gradientDeltaFactor is fixed at 1.0 of voxel size inline Point3f ColoredTSDFVolumeCPU::getColorVoxel(const Point3f& _p) const { @@ -420,11 +440,33 @@ inline v_float32x4 ColoredTSDFVolumeCPU::getColorVoxel(const v_float32x4& p) con int coordBase = ix * xdim + iy * ydim + iz * zdim; float CV_DECL_ALIGNED(16) rgb[4]; + +#if USE_INTERPOLATION_IN_GETNORMAL + float r[8], g[8], b[8]; + for (int i = 0; i < 8; i++) + { + r[i] = (float)volData[neighbourCoords[i] + coordBase].r; + g[i] = (float)volData[neighbourCoords[i] + coordBase].g; + b[i] = (float)volData[neighbourCoords[i] + coordBase].b; + } + + v_float32x4 vsi(voxelSizeInv, voxelSizeInv, voxelSizeInv, voxelSizeInv); + v_float32x4 ptVox = p * vsi; + v_int32x4 iptVox = v_floor(ptVox); + v_float32x4 t = ptVox - v_cvt_f32(iptVox); + float tx = t.get0(); t = v_rotate_right<1>(t); + float ty = t.get0(); t = v_rotate_right<1>(t); + float tz = t.get0(); + rgb[0] = interpolateColor(tx, ty, tz, r); + rgb[1] = interpolateColor(tx, ty, tz, g); + rgb[2] = interpolateColor(tx, ty, tz, b); + rgb[3] = 0.f; +#else rgb[0] = volData[coordBase].r; rgb[1] = volData[coordBase].g; rgb[2] = volData[coordBase].b; rgb[3] = 0.f; - +#endif v_float32x4 res = v_load_aligned(rgb); return res; } @@ -496,7 +538,7 @@ struct ColorRaycastInvoker : ParallelLoopBody vol2cam(Affine3f(cameraPose.inv()) * volume.pose), reprojDepth(depth_intrinsics.makeReprojector()) { } -#if USE_INTRINSICS && 0 +#if USE_INTRINSICS virtual void operator() (const Range& range) const override { const v_float32x4 vfxy(reprojDepth.fxinv, reprojDepth.fyinv, 0, 0); From 8f40b486f885c5e7b4a18f57a7ac52d36a74c447 Mon Sep 17 00:00:00 2001 From: arsaratovtsev Date: Wed, 7 Apr 2021 12:01:35 +0300 Subject: [PATCH 55/55] minor fix --- modules/rgbd/samples/colored_kinfu_demo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/rgbd/samples/colored_kinfu_demo.cpp b/modules/rgbd/samples/colored_kinfu_demo.cpp index c18a7c1103d..5500c9dc1aa 100644 --- a/modules/rgbd/samples/colored_kinfu_demo.cpp +++ b/modules/rgbd/samples/colored_kinfu_demo.cpp @@ -53,7 +53,7 @@ void pauseCallback(const viz::MouseEvent& me, void* args) static const char* keys = { "{help h usage ? | | print this message }" - "{depth | | Path to depth.txt file listing a set of depth images }" + "{depth | | Path to folder with depth.txt and rgb.txt files listing a set of depth and rgb 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 }"