Test code for DBoW

modules/rgbd/CMakeLists.txt

@@ -1,3 +1,3 @@
 set(the_description "RGBD algorithms")
 
-ocv_define_module(rgbd opencv_core opencv_3d opencv_imgproc OPTIONAL opencv_viz WRAP python)
+ocv_define_module(rgbd opencv_core opencv_3d opencv_imgproc OPTIONAL opencv_features2d opencv_viz WRAP python)

modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp

@@ -138,7 +138,29 @@ class CV_EXPORTS_W LargeKinfu
 
     virtual const Affine3f getPose() const = 0;
 
-    CV_WRAP virtual bool update(InputArray depth) = 0;
+    CV_WRAP virtual bool update(InputArray depth, InputArray img=noArray()) = 0;
+
+    CV_WRAP virtual void setDBOW(const String& dbowPath, double simThreshold=3.0) = 0;
+};
+
+/** @brief Loop Closing Detection implementation
+
+  This class implements a Loop Closing Detection of 3d reconstruction algorithm for
+  larger environments using Spatially hashed TSDF volume "Submaps".
+  @see DBOWTrainer for more information.
+*/
+class CV_EXPORTS_W LoopClosureDetection {
+public:
+    // Load DBoW from the given path and set similarity threshold.
+    CV_WRAP static Ptr<LoopClosureDetection> create(const String& dbowPath, double simThreshold=3.0);
+
+    virtual ~LoopClosureDetection() = default;
+
+    // Add frame and check if a loop exist, set tarSubmapID is so.
+    CV_WRAP virtual bool addFrame(InputArray img, const int frameID, const int submapID, CV_OUT int& tarSubmapID) = 0;
+
+    // Stop run loop closing detection.
+    CV_WRAP virtual void reset() = 0;
 };
 
 }  // namespace large_kinfu

modules/rgbd/samples/large_kinfu_dbow_demo.cpp

@@ -0,0 +1,293 @@
+// 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 <fstream>
+#include <iostream>
+#include <opencv2/3d.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/rgbd/large_kinfu.hpp>
+
+#include "io_utils.hpp"
+
+using namespace cv;
+using namespace cv::kinfu;
+using namespace cv::large_kinfu;
+using namespace cv::io_utils;
+
+#ifdef HAVE_OPENCV_VIZ
+#include <opencv2/viz.hpp>
+#endif
+
+#ifdef HAVE_OPENCV_VIZ
+const std::string vizWindowName = "cloud";
+
+struct PauseCallbackArgs
+{
+    PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) {}
+
+    LargeKinfu& largeKinfu;
+};
+
+void pauseCallback(const viz::MouseEvent& me, void* args);
+void pauseCallback(const viz::MouseEvent& me, void* args)
+{
+    if (me.type == viz::MouseEvent::Type::MouseMove ||
+        me.type == viz::MouseEvent::Type::MouseScrollDown ||
+        me.type == viz::MouseEvent::Type::MouseScrollUp)
+    {
+        PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
+        viz::Viz3d window(vizWindowName);
+        UMat rendered;
+        pca.largeKinfu.render(rendered, window.getViewerPose().matrix);
+        imshow("render", rendered);
+        waitKey(1);
+    }
+}
+#endif
+
+static const char* keys = {
+    "{help h usage ? | | print this message   }"
+    "{depth  | | Path to depth.txt file listing a set of depth images }"
+    "{camera |0| Index of depth camera to be used as a depth source }"
+    "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
+    " in coarse mode points and normals are displayed }"
+    "{idle   | | Do not run LargeKinfu, just display depth frames }"
+    "{record | | Write depth frames to specified file list"
+    " (the same format as for the 'depth' key) }"
+    "{rgb    | | Path to rgb.txt file listing a set of RGB images }"
+    "{dbow   | | Path to trained DBoW file }"
+};
+
+static const std::string message =
+    "\nThis demo uses live depth input or RGB-D dataset taken from"
+    "\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
+    "\nto demonstrate Submap based large environment reconstruction"
+    "\nThis module uses the newer hashtable based TSDFVolume (relatively fast) for larger "
+    "reconstructions by default\n";
+
+int main(int argc, char** argv)
+{
+    bool coarse = false;
+    bool idle   = false;
+    std::string recordPath, dbowPath;
+
+    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<String>("record");
+    }
+    if (parser.has("idle"))
+    {
+        idle = true;
+    }
+    if (parser.has("dbow"))
+    {
+        dbowPath = parser.get<String>("dbow");
+    }
+
+    Ptr<DepthSource> ds;
+    Ptr<RGBSource> rgb;
+
+    if (parser.has("depth"))
+        ds = makePtr<DepthSource>(parser.get<String>("depth"));
+    else
+        ds = makePtr<DepthSource>(parser.get<int>("camera"));
+
+    if (parser.has("rgb"))
+        rgb = makePtr<RGBSource>(parser.get<String>("rgb"));
+    else
+        rgb = makePtr<RGBSource>(parser.get<int>("camera"));
+
+    if (ds->empty())
+    {
+        std::cerr << "Failed to open depth source\n";
+        parser.printMessage();
+        return -1;
+    }
+
+    if (rgb->empty())
+    {
+        std::cerr << "Failed to open RGB source\n";
+        parser.printMessage();
+        return -1;
+    }
+
+
+    Ptr<DepthWriter> depthWriter;
+    Ptr<RGBWriter> rgbWriter;
+
+    if (!recordPath.empty())
+    {
+        depthWriter = makePtr<DepthWriter>(recordPath);
+        rgbWriter = makePtr<RGBWriter>(recordPath);
+    }
+
+    Ptr<large_kinfu::Params> params;
+    Ptr<LargeKinfu> largeKinfu;
+
+    params = large_kinfu::Params::hashTSDFParams(coarse);
+
+    // These params can be different for each depth sensor
+    ds->updateParams(*params);
+
+    cv::setUseOptimized(true);
+
+    if (!idle)
+        largeKinfu = LargeKinfu::create(params);
+
+    const auto& volParams = largeKinfu->getParams().volumeParams;
+
+    if (!dbowPath.empty())
+        largeKinfu->setDBOW(dbowPath);
+
+#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(), rgb_frame = rgb->getRGB(); !frame.empty() && !rgb_frame.empty(); frame = ds->getDepth(), rgb_frame = rgb->getRGB())
+    {
+        if (depthWriter)
+            depthWriter->append(frame);
+
+        Vec3i volResolution(volParams.resolutionX,
+                            volParams.resolutionY,
+                            volParams.resolutionZ);
+        Affine3f volPose(Matx44f(volParams.pose));
+
+#ifdef HAVE_OPENCV_VIZ
+        if (pause)
+        {
+            // doesn't happen in idle mode
+            largeKinfu->getCloud(points, normals);
+            if (!points.empty() && !normals.empty())
+            {
+                viz::WCloud cloudWidget(points, viz::Color::white());
+                viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05,
+                                                viz::Color::gray());
+                window.showWidget("cloud", cloudWidget);
+                window.showWidget("normals", cloudNormals);
+
+                Vec3d volSize = volParams.voxelSize * Vec3d(volResolution);
+                window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose);
+                PauseCallbackArgs pca(*largeKinfu);
+                window.registerMouseCallback(pauseCallback, (void*)&pca);
+                window.showWidget("text",
+                                  viz::WText(cv::String("Move camera in this window. "
+                                                        "Close the window or press Q to resume"),
+                                             Point()));
+                window.spin();
+                window.removeWidget("text");
+                window.removeWidget("cloud");
+                window.removeWidget("normals");
+                window.registerMouseCallback(0);
+            }
+
+            pause = false;
+        }
+        else
+#endif
+        {
+            UMat cvt8;
+            float depthFactor = params->depthFactor;
+            convertScaleAbs(frame, cvt8, 0.25 * 256. / depthFactor);
+            if (!idle)
+            {
+                imshow("depth", cvt8);
+                imshow("RGB", rgb_frame);
+
+                if (!largeKinfu->update(frame, rgb_frame))
+                {
+                    largeKinfu->reset();
+                    std::cout << "reset\n";
+                }
+#ifdef HAVE_OPENCV_VIZ
+                else
+                {
+                    if (coarse)
+                    {
+                        largeKinfu->getCloud(points, normals);
+                        if (!points.empty() && !normals.empty())
+                        {
+                            viz::WCloud cloudWidget(points, viz::Color::white());
+                            viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1,
+                                                            /*scale*/ 0.05, viz::Color::gray());
+                            window.showWidget("cloud", cloudWidget);
+                            window.showWidget("normals", cloudNormals);
+                        }
+                    }
+
+                    // window.showWidget("worldAxes", viz::WCoordinateSystem());
+                    Vec3d volSize = volParams.voxelSize * volResolution;
+                    window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose);
+                    window.setViewerPose(largeKinfu->getPose());
+                    window.spinOnce(1, true);
+                }
+#endif
+
+                largeKinfu->render(rendered);
+            }
+            else
+            {
+                rendered = cvt8;
+            }
+        }
+
+        int64 newTime = getTickCount();
+        putText(rendered,
+                cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
+                           (int)(getTickFrequency() / (newTime - prevTime))),
+                Point(0, rendered.rows - 1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
+        prevTime = newTime;
+        imshow("render", rendered);
+
+        int c = waitKey(1);
+        switch (c)
+        {
+            case 'r':
+                if (!idle)
+                    largeKinfu->reset();
+                break;
+            case 'q': return 0;
+#ifdef HAVE_OPENCV_VIZ
+            case 'p':
+                if (!idle)
+                    pause = true;
+#endif
+            default: break;
+        }
+    }
+
+    return 0;
+}