Merge pull request #1669 from paroj:kinfupy

modules/rgbd/samples/kinfu_demo.cpp

@@ -10,14 +10,14 @@
 #include <opencv2/highgui.hpp>
 #include <opencv2/rgbd/kinfu.hpp>
 
-#ifdef HAVE_OPENCV_VIZ
-
-#include <opencv2/viz.hpp>
-
 using namespace cv;
 using namespace cv::kinfu;
 using namespace std;
 
+#ifdef HAVE_OPENCV_VIZ
+#include <opencv2/viz.hpp>
+#endif
+
 static vector<string> readDepth(std::string fileList);
 
 static vector<string> readDepth(std::string fileList)
@@ -113,7 +113,7 @@ public:
         return depthFileList.empty() && !(vc.isOpened());
     }
 
-    void updateParams(KinFu::Params& params)
+    void updateParams(Params& params)
     {
         if (vc.isOpened())
         {
@@ -143,6 +143,7 @@ public:
     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> 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<KinFu> 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

modules/rgbd/src/fast_icp.cpp

@@ -500,15 +500,15 @@ bool ICPGPU::estimateTransform(cv::Affine3f& /*transform*/, cv::Ptr<Frame> /*_ol
     throw std::runtime_error("Not implemented");
 }
 
-cv::Ptr<ICP> makeICP(cv::kinfu::KinFu::Params::PlatformType t,
+cv::Ptr<ICP> makeICP(cv::kinfu::Params::PlatformType t,
                      const cv::kinfu::Intr _intrinsics, const std::vector<int> &_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<ICPCPU>(_intrinsics, _iterations, _angleThreshold, _distanceThreshold);
-    case cv::kinfu::KinFu::Params::PlatformType::PLATFORM_GPU:
+    case cv::kinfu::Params::PlatformType::PLATFORM_GPU:
         return cv::makePtr<ICPGPU>(_intrinsics, _iterations, _angleThreshold, _distanceThreshold);
     default:
         return cv::Ptr<ICP>();

modules/rgbd/src/fast_icp.hpp

@@ -30,7 +30,7 @@ protected:
     cv::kinfu::Intr intrinsics;
 };
 
-cv::Ptr<ICP> makeICP(cv::kinfu::KinFu::Params::PlatformType t,
+cv::Ptr<ICP> makeICP(cv::kinfu::Params::PlatformType t,
                      const cv::kinfu::Intr _intrinsics, const std::vector<int> &_iterations,
                      float _angleThreshold, float _distanceThreshold);
 

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> frameGenerator;
     cv::Ptr<ICP> icp;
@@ -45,7 +45,7 @@ private:
     cv::Ptr<Frame> frame;
 };
 
-KinFu::Params KinFu::Params::defaultParams()
+Ptr<Params> 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<Params>(p);
 }
 
-KinFu::Params KinFu::Params::coarseParams()
+Ptr<Params> Params::coarseParams()
 {
-    Params p = defaultParams();
+    Ptr<Params> 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<KinFu::KinFuImpl>(_params);
-}
-
-KinFu::~KinFu() { }
-
-const KinFu::Params& KinFu::getParams() const
+Ptr<KinFu> KinFu::create(const Ptr<Params>& _params)
 {
-    return impl->getParams();
+    return makePtr<KinFu::KinFuImpl>(*_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

modules/rgbd/src/kinfu_frame.cpp

@@ -442,13 +442,13 @@ void FrameGPU::getDepth(OutputArray /* depth */) const
     throw std::runtime_error("Not implemented");
 }
 
-cv::Ptr<FrameGenerator> makeFrameGenerator(cv::kinfu::KinFu::Params::PlatformType t)
+cv::Ptr<FrameGenerator> 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<FrameGeneratorCPU>();
-    case cv::kinfu::KinFu::Params::PlatformType::PLATFORM_GPU:
+    case cv::kinfu::Params::PlatformType::PLATFORM_GPU:
         return cv::makePtr<FrameGeneratorGPU>();
     default:
         return cv::Ptr<FrameGenerator>();

modules/rgbd/src/kinfu_frame.hpp

@@ -115,7 +115,7 @@ public:
     virtual ~FrameGenerator() {}
 };
 
-cv::Ptr<FrameGenerator> makeFrameGenerator(cv::kinfu::KinFu::Params::PlatformType t);
+cv::Ptr<FrameGenerator> makeFrameGenerator(cv::kinfu::Params::PlatformType t);
 
 } // namespace kinfu
 } // namespace cv

modules/rgbd/src/odometry.cpp

@@ -1482,7 +1482,7 @@ Size FastICPOdometry::prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType
 
     // mask isn't used by FastICP
 
-    Ptr<FrameGenerator> fg = makeFrameGenerator(KinFu::Params::PlatformType::PLATFORM_CPU);
+    Ptr<FrameGenerator> fg = makeFrameGenerator(Params::PlatformType::PLATFORM_CPU);
     Ptr<FrameCPU> f = (*fg)().dynamicCast<FrameCPU>();
     Intr intr(cameraMatrix);
     float depthFactor = 1.f; // user should rescale depth manually
@@ -1517,7 +1517,7 @@ bool FastICPOdometry::computeImpl(const Ptr<OdometryFrame>& srcFrame,
 {
     kinfu::Intr intr(cameraMatrix);
     std::vector<int> iterations = iterCounts;
-    Ptr<kinfu::ICP> icp = kinfu::makeICP(kinfu::KinFu::Params::PlatformType::PLATFORM_CPU,
+    Ptr<kinfu::ICP> icp = kinfu::makeICP(kinfu::Params::PlatformType::PLATFORM_CPU,
                                          intr,
                                          iterations,
                                          angleThreshold,

modules/rgbd/src/tsdf.cpp

@@ -1199,16 +1199,16 @@ void TSDFVolumeGPU::fetchPointsNormals(OutputArray /*points*/, OutputArray /*nor
     throw std::runtime_error("Not implemented");
 }
 
-cv::Ptr<TSDFVolume> makeTSDFVolume(cv::kinfu::KinFu::Params::PlatformType t,
+cv::Ptr<TSDFVolume> 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<TSDFVolumeCPU>(_res, _size, _pose, _truncDist, _maxWeight,
                                           _raycastStepFactor);
-    case cv::kinfu::KinFu::Params::PlatformType::PLATFORM_GPU:
+    case cv::kinfu::Params::PlatformType::PLATFORM_GPU:
         return cv::makePtr<TSDFVolumeGPU>(_res, _size, _pose, _truncDist, _maxWeight,
                                           _raycastStepFactor);
     default:

modules/rgbd/src/tsdf.hpp

@@ -38,7 +38,7 @@ public:
     cv::Affine3f pose;
 };
 
-cv::Ptr<TSDFVolume> makeTSDFVolume(cv::kinfu::KinFu::Params::PlatformType t,
+cv::Ptr<TSDFVolume> makeTSDFVolume(cv::kinfu::Params::PlatformType t,
                                    int _res, float _size, cv::Affine3f _pose, float _truncDist, int _maxWeight,
                                    float _raycastStepFactor);
 

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<kinfu::Params> 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<kinfu::KinFu> kf = kinfu::KinFu::create(params);
 
     std::vector<Affine3f> 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<kinfu::Params> 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<kinfu::KinFu> kf = kinfu::KinFu::create(params);
 
     std::vector<Affine3f> 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);
         }