From 9e787377dc82038d5f8d6ccf606ecb53a2f0b7ea Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Thu, 21 Jun 2018 15:46:22 +0200 Subject: [PATCH] kinfu: allow basic wrapping for bindings for this - move Params struct out of class - use static create instead of pimpl - allow demo to be compiled without VIZ --- modules/rgbd/include/opencv2/rgbd/kinfu.hpp | 206 ++++++++++---------- modules/rgbd/samples/kinfu_demo.cpp | 70 +++---- modules/rgbd/src/fast_icp.cpp | 6 +- modules/rgbd/src/fast_icp.hpp | 2 +- modules/rgbd/src/kinfu.cpp | 103 +++------- modules/rgbd/src/kinfu_frame.cpp | 6 +- modules/rgbd/src/kinfu_frame.hpp | 2 +- modules/rgbd/src/odometry.cpp | 4 +- modules/rgbd/src/tsdf.cpp | 6 +- modules/rgbd/src/tsdf.hpp | 2 +- modules/rgbd/test/test_kinfu.cpp | 31 ++- 11 files changed, 195 insertions(+), 243 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp index 02af753cea8..31f6de5e6f9 100644 --- a/modules/rgbd/include/opencv2/rgbd/kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/kinfu.hpp @@ -15,134 +15,134 @@ namespace kinfu { //! @addtogroup kinect_fusion //! @{ -/** @brief KinectFusion implementation +struct CV_EXPORTS_W Params +{ + /** @brief Default parameters + A set of parameters which provides better model quality, can be very slow. + */ + CV_WRAP static Ptr defaultParams(); - This class implements a 3d reconstruction algorithm described in - @cite kinectfusion paper. + /** @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(); - 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. + enum PlatformType + { - An internal representation of a model is a voxel cube 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. + PLATFORM_CPU, PLATFORM_GPU + }; - This implementation is based on (kinfu-remake)[https://github.com/Nerei/kinfu_remake]. -*/ -class CV_EXPORTS KinFu -{ -public: - struct CV_EXPORTS Params - { - /** @brief Default parameters - A set of parameters which provides better model quality, can be very slow. - */ - static Params defaultParams(); + /** @brief A platform on which to run the algorithm. + * + Currently KinFu supports only one platform which is a CPU. + GPU platform is to be implemented in the future. + */ + PlatformType platform; - /** @brief Coarse parameters - A set of parameters which provides better speed, can fail to match frames - in case of rapid sensor motion. - */ - static Params coarseParams(); + /** @brief frame size in pixels */ + CV_PROP_RW Size frameSize; - enum PlatformType - { + /** @brief camera intrinsics */ + CV_PROP Matx33f intr; - PLATFORM_CPU, PLATFORM_GPU - }; + /** @brief pre-scale per 1 meter for input values - /** @brief A platform on which to run the algorithm. - * - Currently KinFu supports only one platform which is a CPU. - GPU platform is to be implemented in the future. - */ - PlatformType platform; + Typical values are: + * 5000 per 1 meter for the 16-bit PNG files of TUM database + * 1 per 1 meter for the 32-bit float images in the ROS bag files + */ + CV_PROP_RW float depthFactor; - /** @brief frame size in pixels */ - Size frameSize; + /** @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 camera intrinsics */ - Matx33f intr; + /** @brief Number of pyramid levels for ICP */ + CV_PROP_RW int pyramidLevels; - /** @brief pre-scale per 1 meter for input values + /** @brief Resolution of voxel cube - Typical values are: - * 5000 per 1 meter for the 16-bit PNG files of TUM database - * 1 per 1 meter for the 32-bit float images in the ROS bag files - */ - float depthFactor; + Number of voxels in each cube edge. + */ + CV_PROP_RW int volumeDims; + /** @brief Size of voxel cube side in meters */ + CV_PROP_RW float volumeSize; - /** @brief Depth sigma in meters for bilateral smooth */ - float bilateral_sigma_depth; - /** @brief Spatial sigma in pixels for bilateral smooth */ - float bilateral_sigma_spatial; - /** @brief Kernel size in pixels for bilateral smooth */ - int bilateral_kernel_size; + /** @brief Minimal camera movement in meters - /** @brief Number of pyramid levels for ICP */ - int pyramidLevels; + Integrate new depth frame only if camera movement exceeds this value. + */ + CV_PROP_RW float tsdf_min_camera_movement; - /** @brief Resolution of voxel cube + /** @brief initial volume pose in meters */ + Affine3f volumePose; - Number of voxels in each cube edge. - */ - int volumeDims; - /** @brief Size of voxel cube side in meters */ - float volumeSize; + /** @brief distance to truncate in meters - /** @brief Minimal camera movement in meters + Distances that exceed this value will be truncated in voxel cube values. + */ + CV_PROP_RW float tsdf_trunc_dist; - Integrate new depth frame only if camera movement exceeds this value. - */ - float tsdf_min_camera_movement; + /** @brief max number of frames per voxel - /** @brief initial volume pose in meters */ - Affine3f volumePose; + Each voxel keeps running average of distances no longer than this value. + */ + CV_PROP_RW int tsdf_max_weight; - /** @brief distance to truncate in meters + /** @brief A length of one raycast step - Distances that exceed this value will be truncated in voxel cube values. - */ - float tsdf_trunc_dist; + How much voxel sizes we skip each raycast step + */ + CV_PROP_RW float raycast_step_factor; - /** @brief max number of frames per voxel + // gradient delta in voxel sizes + // fixed at 1.0f + // float gradient_delta_factor; - Each voxel keeps running average of distances no longer than this value. - */ - int tsdf_max_weight; + /** @brief light pose for rendering in meters */ + CV_PROP Vec3f lightPose; - /** @brief A length of one raycast step + /** @brief distance theshold for ICP in meters */ + CV_PROP_RW float icpDistThresh; + /** angle threshold for ICP in radians */ + CV_PROP_RW float icpAngleThresh; + /** number of ICP iterations for each pyramid level */ + CV_PROP std::vector icpIterations; - How much voxel sizes we skip each raycast step - */ - float raycast_step_factor; + // depth truncation is not used by default + // float icp_truncate_depth_dist; //meters +}; - // gradient delta in voxel sizes - // fixed at 1.0f - // float gradient_delta_factor; +/** @brief KinectFusion implementation - /** @brief light pose for rendering in meters */ - Vec3f lightPose; + This class implements a 3d reconstruction algorithm described in + @cite kinectfusion paper. - /** @brief distance theshold for ICP in meters */ - float icpDistThresh; - /** angle threshold for ICP in radians */ - float icpAngleThresh; - /** number of ICP iterations for each pyramid level */ - std::vector icpIterations; + 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. - // depth truncation is not used by default - // float icp_truncate_depth_dist; //meters - }; + An internal representation of a model is a voxel cube 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(const Params& _params); + This implementation is based on (kinfu-remake)[https://github.com/Nerei/kinfu_remake]. +*/ +class CV_EXPORTS_W KinFu +{ +public: + CV_WRAP static Ptr create(const Ptr& _params); virtual ~KinFu(); /** @brief Get current parameters */ - const Params& getParams() const; - void setParams(const Params&); + virtual const Params& getParams() const = 0; + virtual void setParams(const Params&) = 0; /** @brief Renders a volume into an image @@ -154,7 +154,7 @@ class CV_EXPORTS KinFu which is a last frame camera pose. */ - void render(OutputArray image, const Affine3f cameraPose = Affine3f::Identity()) const; + CV_WRAP virtual void render(OutputArray image, const Matx44f& cameraPose = Matx44f::eye()) const = 0; /** @brief Gets points and normals of current 3d mesh @@ -164,7 +164,7 @@ class CV_EXPORTS KinFu @param points vector of points which are 4-float vectors @param normals vector of normals which are 4-float vectors */ - void getCloud(OutputArray points, OutputArray normals) const; + CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0; /** @brief Gets points of current 3d mesh @@ -172,22 +172,22 @@ class CV_EXPORTS KinFu @param points vector of points which are 4-float vectors */ - void getPoints(OutputArray points) const; + 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 */ - void getNormals(InputArray points, OutputArray normals) const; + CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0; /** @brief Resets the algorithm Clears current model and resets a pose. */ - void reset(); + CV_WRAP virtual void reset() = 0; /** @brief Get current pose in voxel cube space */ - const Affine3f getPose() const; + virtual const Affine3f getPose() const = 0; /** @brief Process next depth frame @@ -197,12 +197,10 @@ class CV_EXPORTS KinFu @param depth one-channel image which size and depth scale is described in algorithm's parameters @return true if succeded to align new frame with current scene, false if opposite */ - bool update(InputArray depth); + CV_WRAP virtual bool update(InputArray depth) = 0; private: - class KinFuImpl; - cv::Ptr impl; }; //! @} diff --git a/modules/rgbd/samples/kinfu_demo.cpp b/modules/rgbd/samples/kinfu_demo.cpp index b42cf46f561..55cd6989185 100644 --- a/modules/rgbd/samples/kinfu_demo.cpp +++ b/modules/rgbd/samples/kinfu_demo.cpp @@ -10,14 +10,14 @@ #include #include -#ifdef HAVE_OPENCV_VIZ - -#include - using namespace cv; using namespace cv::kinfu; using namespace std; +#ifdef HAVE_OPENCV_VIZ +#include +#endif + static vector readDepth(std::string fileList); static vector readDepth(std::string fileList) @@ -113,7 +113,7 @@ struct DepthSource return depthFileList.empty() && !(vc.isOpened()); } - void updateParams(KinFu::Params& params) + void updateParams(Params& params) { if (vc.isOpened()) { @@ -143,6 +143,7 @@ struct DepthSource bool useKinect2Workarounds; }; +#ifdef HAVE_OPENCV_VIZ const std::string vizWindowName = "cloud"; struct PauseCallbackArgs @@ -163,11 +164,12 @@ void pauseCallback(const viz::MouseEvent& me, void* args) PauseCallbackArgs pca = *((PauseCallbackArgs*)(args)); viz::Viz3d window(vizWindowName); Mat rendered; - pca.kf.render(rendered, window.getViewerPose()); + pca.kf.render(rendered, window.getViewerPose().matrix); imshow("render", rendered); waitKey(1); } } +#endif static const char* keys = { @@ -220,23 +222,26 @@ int main(int argc, char **argv) return -1; } - KinFu::Params params; + Ptr params; if(coarse) - params = KinFu::Params::coarseParams(); + params = Params::coarseParams(); else - params = KinFu::Params::defaultParams(); + params = Params::defaultParams(); // These params can be different for each depth sensor - ds.updateParams(params); + ds.updateParams(*params); // Scene-specific params should be tuned for each scene individually //params.volumePose = params.volumePose.translate(Vec3f(0.f, 0.f, 0.5f)); //params.tsdf_max_weight = 16; - KinFu kf(params); + Ptr kf = KinFu::create(params); +#ifdef HAVE_OPENCV_VIZ cv::viz::Viz3d window(vizWindowName); window.setViewerPose(Affine3f::Identity()); + bool pause = false; +#endif // TODO: can we use UMats for that? Mat rendered; @@ -245,13 +250,12 @@ int main(int argc, char **argv) int64 prevTime = getTickCount(); - bool pause = false; - for(Mat frame = ds.getDepth(); !frame.empty(); frame = ds.getDepth()) { +#ifdef HAVE_OPENCV_VIZ if(pause) { - kf.getCloud(points, normals); + kf->getCloud(points, normals); if(!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); @@ -260,9 +264,9 @@ int main(int argc, char **argv) window.showWidget("normals", cloudNormals); window.showWidget("cube", viz::WCube(Vec3d::all(0), - Vec3d::all(kf.getParams().volumeSize)), - kf.getParams().volumePose); - PauseCallbackArgs pca(kf); + Vec3d::all(kf->getParams().volumeSize)), + 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())); @@ -274,22 +278,24 @@ int main(int argc, char **argv) pause = false; } else +#endif { Mat cvt8; - float depthFactor = kf.getParams().depthFactor; + float depthFactor = kf->getParams().depthFactor; convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor); imshow("depth", cvt8); - if(!kf.update(frame)) + if(!kf->update(frame)) { - kf.reset(); + kf->reset(); std::cout << "reset" << std::endl; } +#ifdef HAVE_OPENCV_VIZ else { if(coarse) { - kf.getCloud(points, normals); + kf->getCloud(points, normals); if(!points.empty() && !normals.empty()) { viz::WCloud cloudWidget(points, viz::Color::white()); @@ -301,13 +307,14 @@ int main(int argc, char **argv) //window.showWidget("worldAxes", viz::WCoordinateSystem()); window.showWidget("cube", viz::WCube(Vec3d::all(0), - Vec3d::all(kf.getParams().volumeSize)), - kf.getParams().volumePose); - window.setViewerPose(kf.getPose()); + Vec3d::all(kf->getParams().volumeSize)), + kf->getParams().volumePose); + window.setViewerPose(kf->getPose()); window.spinOnce(1, true); } +#endif - kf.render(rendered); + kf->render(rendered); } int64 newTime = getTickCount(); @@ -322,12 +329,14 @@ int main(int argc, char **argv) switch (c) { case 'r': - kf.reset(); + kf->reset(); break; case 'q': return 0; +#ifdef HAVE_OPENCV_VIZ case 'p': pause = true; +#endif default: break; } @@ -335,12 +344,3 @@ int main(int argc, char **argv) return 0; } - -#else - -int main(int /* argc */, char ** /* argv */) -{ - std::cout << "This demo requires viz module" << std::endl; - return 0; -} -#endif diff --git a/modules/rgbd/src/fast_icp.cpp b/modules/rgbd/src/fast_icp.cpp index 342170b2705..a58205564e5 100644 --- a/modules/rgbd/src/fast_icp.cpp +++ b/modules/rgbd/src/fast_icp.cpp @@ -500,15 +500,15 @@ bool ICPGPU::estimateTransform(cv::Affine3f& /*transform*/, cv::Ptr /*_ol throw std::runtime_error("Not implemented"); } -cv::Ptr makeICP(cv::kinfu::KinFu::Params::PlatformType t, +cv::Ptr makeICP(cv::kinfu::Params::PlatformType t, const cv::kinfu::Intr _intrinsics, const std::vector &_iterations, float _angleThreshold, float _distanceThreshold) { switch (t) { - case cv::kinfu::KinFu::Params::PlatformType::PLATFORM_CPU: + case cv::kinfu::Params::PlatformType::PLATFORM_CPU: return cv::makePtr(_intrinsics, _iterations, _angleThreshold, _distanceThreshold); - case cv::kinfu::KinFu::Params::PlatformType::PLATFORM_GPU: + case cv::kinfu::Params::PlatformType::PLATFORM_GPU: return cv::makePtr(_intrinsics, _iterations, _angleThreshold, _distanceThreshold); default: return cv::Ptr(); diff --git a/modules/rgbd/src/fast_icp.hpp b/modules/rgbd/src/fast_icp.hpp index 8d31351e8af..d036e907483 100644 --- a/modules/rgbd/src/fast_icp.hpp +++ b/modules/rgbd/src/fast_icp.hpp @@ -30,7 +30,7 @@ class ICP cv::kinfu::Intr intrinsics; }; -cv::Ptr makeICP(cv::kinfu::KinFu::Params::PlatformType t, +cv::Ptr makeICP(cv::kinfu::Params::PlatformType t, const cv::kinfu::Intr _intrinsics, const std::vector &_iterations, float _angleThreshold, float _distanceThreshold); diff --git a/modules/rgbd/src/kinfu.cpp b/modules/rgbd/src/kinfu.cpp index 62ca933d325..99fa8257c81 100644 --- a/modules/rgbd/src/kinfu.cpp +++ b/modules/rgbd/src/kinfu.cpp @@ -12,29 +12,29 @@ namespace cv { namespace kinfu { -class KinFu::KinFuImpl +class KinFu::KinFuImpl : public KinFu { public: - KinFuImpl(const KinFu::Params& _params); + KinFuImpl(const Params& _params); virtual ~KinFuImpl(); - const KinFu::Params& getParams() const; - void setParams(const KinFu::Params&); + const Params& getParams() const CV_OVERRIDE; + void setParams(const Params&) CV_OVERRIDE; - void render(OutputArray image, const Affine3f cameraPose = Affine3f::Identity()) const; + void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE; - void getCloud(OutputArray points, OutputArray normals) const; - void getPoints(OutputArray points) const; - void getNormals(InputArray points, OutputArray normals) const; + 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(); + void reset() CV_OVERRIDE; - const Affine3f getPose() const; + const Affine3f getPose() const CV_OVERRIDE; - bool update(InputArray depth); + bool update(InputArray depth) CV_OVERRIDE; private: - KinFu::Params params; + Params params; cv::Ptr frameGenerator; cv::Ptr icp; @@ -45,7 +45,7 @@ class KinFu::KinFuImpl cv::Ptr frame; }; -KinFu::Params KinFu::Params::defaultParams() +Ptr Params::defaultParams() { Params p; @@ -107,36 +107,36 @@ KinFu::Params KinFu::Params::defaultParams() // depth truncation is not used by default //p.icp_truncate_depth_dist = 0.f; //meters, disabled - return p; + return makePtr(p); } -KinFu::Params KinFu::Params::coarseParams() +Ptr Params::coarseParams() { - Params p = defaultParams(); + Ptr p = defaultParams(); // first non-zero numbers are accepted const int iters[] = {5, 3, 2}; - p.icpIterations.clear(); + p->icpIterations.clear(); for(size_t i = 0; i < sizeof(iters)/sizeof(int); i++) { if(iters[i]) { - p.icpIterations.push_back(iters[i]); + p->icpIterations.push_back(iters[i]); } else break; } - p.pyramidLevels = (int)p.icpIterations.size(); + p->pyramidLevels = (int)p->icpIterations.size(); - p.volumeDims = 128; //number of voxels + p->volumeDims = 128; //number of voxels - p.raycast_step_factor = 0.75f; //in voxel sizes + p->raycast_step_factor = 0.75f; //in voxel sizes return p; } -KinFu::KinFuImpl::KinFuImpl(const KinFu::Params &_params) : +KinFu::KinFuImpl::KinFuImpl(const Params &_params) : params(_params), frameGenerator(makeFrameGenerator(params.platform)), icp(makeICP(params.platform, params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)), @@ -160,12 +160,12 @@ KinFu::KinFuImpl::~KinFuImpl() } -const KinFu::Params& KinFu::KinFuImpl::getParams() const +const Params& KinFu::KinFuImpl::getParams() const { return params; } -void KinFu::KinFuImpl::setParams(const KinFu::Params& p) +void KinFu::KinFuImpl::setParams(const Params& p) { params = p; } @@ -225,10 +225,12 @@ bool KinFu::KinFuImpl::update(InputArray _depth) } -void KinFu::KinFuImpl::render(OutputArray image, const Affine3f cameraPose) const +void KinFu::KinFuImpl::render(OutputArray image, const Matx44f& _cameraPose) const { ScopeTime st("kinfu render"); + Affine3f cameraPose(_cameraPose); + const Affine3f id = Affine3f::Identity(); if((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) || (cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation())) @@ -263,57 +265,12 @@ void KinFu::KinFuImpl::getNormals(InputArray points, OutputArray normals) const // importing class -KinFu::KinFu(const Params& _params) -{ - impl = makePtr(_params); -} - -KinFu::~KinFu() { } - -const KinFu::Params& KinFu::getParams() const +Ptr KinFu::create(const Ptr& _params) { - return impl->getParams(); + return makePtr(*_params); } -void KinFu::setParams(const Params& p) -{ - impl->setParams(p); -} - -const Affine3f KinFu::getPose() const -{ - return impl->getPose(); -} - -void KinFu::reset() -{ - impl->reset(); -} - -void KinFu::getCloud(cv::OutputArray points, cv::OutputArray normals) const -{ - impl->getCloud(points, normals); -} - -void KinFu::getPoints(OutputArray points) const -{ - impl->getPoints(points); -} - -void KinFu::getNormals(InputArray points, OutputArray normals) const -{ - impl->getNormals(points, normals); -} - -bool KinFu::update(InputArray depth) -{ - return impl->update(depth); -} - -void KinFu::render(cv::OutputArray image, const Affine3f cameraPose) const -{ - impl->render(image, cameraPose); -} +KinFu::~KinFu() {} } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/kinfu_frame.cpp b/modules/rgbd/src/kinfu_frame.cpp index 74d37b476a4..eef81b3ccbe 100644 --- a/modules/rgbd/src/kinfu_frame.cpp +++ b/modules/rgbd/src/kinfu_frame.cpp @@ -442,13 +442,13 @@ void FrameGPU::getDepth(OutputArray /* depth */) const throw std::runtime_error("Not implemented"); } -cv::Ptr makeFrameGenerator(cv::kinfu::KinFu::Params::PlatformType t) +cv::Ptr makeFrameGenerator(cv::kinfu::Params::PlatformType t) { switch (t) { - case cv::kinfu::KinFu::Params::PlatformType::PLATFORM_CPU: + case cv::kinfu::Params::PlatformType::PLATFORM_CPU: return cv::makePtr(); - case cv::kinfu::KinFu::Params::PlatformType::PLATFORM_GPU: + case cv::kinfu::Params::PlatformType::PLATFORM_GPU: return cv::makePtr(); default: return cv::Ptr(); diff --git a/modules/rgbd/src/kinfu_frame.hpp b/modules/rgbd/src/kinfu_frame.hpp index 572bf5098aa..02c281d2aea 100644 --- a/modules/rgbd/src/kinfu_frame.hpp +++ b/modules/rgbd/src/kinfu_frame.hpp @@ -115,7 +115,7 @@ struct FrameGenerator virtual ~FrameGenerator() {} }; -cv::Ptr makeFrameGenerator(cv::kinfu::KinFu::Params::PlatformType t); +cv::Ptr makeFrameGenerator(cv::kinfu::Params::PlatformType t); } // namespace kinfu } // namespace cv diff --git a/modules/rgbd/src/odometry.cpp b/modules/rgbd/src/odometry.cpp index 9270873bded..52bd3486a40 100755 --- a/modules/rgbd/src/odometry.cpp +++ b/modules/rgbd/src/odometry.cpp @@ -1482,7 +1482,7 @@ Size FastICPOdometry::prepareFrameCache(Ptr& frame, int cacheType // mask isn't used by FastICP - Ptr fg = makeFrameGenerator(KinFu::Params::PlatformType::PLATFORM_CPU); + Ptr fg = makeFrameGenerator(Params::PlatformType::PLATFORM_CPU); Ptr f = (*fg)().dynamicCast(); Intr intr(cameraMatrix); float depthFactor = 1.f; // user should rescale depth manually @@ -1517,7 +1517,7 @@ bool FastICPOdometry::computeImpl(const Ptr& srcFrame, { kinfu::Intr intr(cameraMatrix); std::vector iterations = iterCounts; - Ptr icp = kinfu::makeICP(kinfu::KinFu::Params::PlatformType::PLATFORM_CPU, + Ptr icp = kinfu::makeICP(kinfu::Params::PlatformType::PLATFORM_CPU, intr, iterations, angleThreshold, diff --git a/modules/rgbd/src/tsdf.cpp b/modules/rgbd/src/tsdf.cpp index 3840727973e..19560e985fa 100644 --- a/modules/rgbd/src/tsdf.cpp +++ b/modules/rgbd/src/tsdf.cpp @@ -1199,16 +1199,16 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray /*points*/, OutputArray /*nor throw std::runtime_error("Not implemented"); } -cv::Ptr makeTSDFVolume(cv::kinfu::KinFu::Params::PlatformType t, +cv::Ptr makeTSDFVolume(cv::kinfu::Params::PlatformType t, int _res, float _size, cv::Affine3f _pose, float _truncDist, int _maxWeight, float _raycastStepFactor) { switch (t) { - case cv::kinfu::KinFu::Params::PlatformType::PLATFORM_CPU: + case cv::kinfu::Params::PlatformType::PLATFORM_CPU: return cv::makePtr(_res, _size, _pose, _truncDist, _maxWeight, _raycastStepFactor); - case cv::kinfu::KinFu::Params::PlatformType::PLATFORM_GPU: + case cv::kinfu::Params::PlatformType::PLATFORM_GPU: return cv::makePtr(_res, _size, _pose, _truncDist, _maxWeight, _raycastStepFactor); default: diff --git a/modules/rgbd/src/tsdf.hpp b/modules/rgbd/src/tsdf.hpp index 41b4f8b376d..f4008ac1193 100644 --- a/modules/rgbd/src/tsdf.hpp +++ b/modules/rgbd/src/tsdf.hpp @@ -38,7 +38,7 @@ class TSDFVolume cv::Affine3f pose; }; -cv::Ptr makeTSDFVolume(cv::kinfu::KinFu::Params::PlatformType t, +cv::Ptr makeTSDFVolume(cv::kinfu::Params::PlatformType t, int _res, float _size, cv::Affine3f _pose, float _truncDist, int _maxWeight, float _raycastStepFactor); diff --git a/modules/rgbd/test/test_kinfu.cpp b/modules/rgbd/test/test_kinfu.cpp index 9bb104a7ee1..16bba67767b 100644 --- a/modules/rgbd/test/test_kinfu.cpp +++ b/modules/rgbd/test/test_kinfu.cpp @@ -262,12 +262,11 @@ static const bool display = false; TEST( KinectFusion, lowDense ) { - kinfu::KinFu::Params params; - params = kinfu::KinFu::Params::coarseParams(); + Ptr params = kinfu::Params::coarseParams(); - RotatingScene scene(params.frameSize, params.intr, params.depthFactor); + RotatingScene scene(params->frameSize, params->intr, params->depthFactor); - kinfu::KinFu kf(params); + Ptr kf = kinfu::KinFu::create(params); std::vector poses = scene.getPoses(); Affine3f startPoseGT = poses[0], startPoseKF; @@ -278,9 +277,9 @@ TEST( KinectFusion, lowDense ) Mat depth = scene.depth(pose); - ASSERT_TRUE(kf.update(depth)); + ASSERT_TRUE(kf->update(depth)); - kfPose = kf.getPose(); + kfPose = kf->getPose(); if(i == 0) startPoseKF = kfPose; @@ -288,9 +287,9 @@ TEST( KinectFusion, lowDense ) if(display) { - imshow("depth", depth*(1.f/params.depthFactor/4.f)); + imshow("depth", depth*(1.f/params->depthFactor/4.f)); Mat rendered; - kf.render(rendered); + kf->render(rendered); imshow("render", rendered); waitKey(10); } @@ -302,12 +301,10 @@ TEST( KinectFusion, lowDense ) TEST( KinectFusion, highDense ) { - kinfu::KinFu::Params params; + Ptr params = kinfu::Params::defaultParams(); + CubeSpheresScene scene(params->frameSize, params->intr, params->depthFactor); - params = kinfu::KinFu::Params::defaultParams(); - CubeSpheresScene scene(params.frameSize, params.intr, params.depthFactor); - - kinfu::KinFu kf(params); + Ptr kf = kinfu::KinFu::create(params); std::vector poses = scene.getPoses(); Affine3f startPoseGT = poses[0], startPoseKF; @@ -318,9 +315,9 @@ TEST( KinectFusion, highDense ) Mat depth = scene.depth(pose); - ASSERT_TRUE(kf.update(depth)); + ASSERT_TRUE(kf->update(depth)); - kfPose = kf.getPose(); + kfPose = kf->getPose(); if(i == 0) startPoseKF = kfPose; @@ -328,9 +325,9 @@ TEST( KinectFusion, highDense ) if(display) { - imshow("depth", depth*(1.f/params.depthFactor/4.f)); + imshow("depth", depth*(1.f/params->depthFactor/4.f)); Mat rendered; - kf.render(rendered); + kf->render(rendered); imshow("render", rendered); waitKey(10); }