debugging code for Large Scale Depth Fusion

Author: savuor

modules/rgbd/samples/large_kinfu_LCD_demo.cpp

@@ -173,6 +173,15 @@ int main(int argc, char** argv)
     // These params can be different for each depth sensor
     ds->updateParams(*params);
 
+    //DEBUG for copyroom dataset
+    if (0)
+    {
+        params->frameSize = Size(640, 480);
+        params->intr = Matx33f{ 583, 0, 320, /**/ 0, 583, 240, /**/ 0, 0, 1 };
+        params->depthFactor = 1000.f;
+        params->rgb_intr = Matx33f{ 582.871f, 0, 320, /**/ 0, 582.871f, 240, /**/ 0, 0, 1 };
+    }
+
     cv::setUseOptimized(true);
 
     if (!idle)
@@ -181,7 +190,7 @@ int main(int argc, char** argv)
     const auto& volParams = largeKinfu->getParams().volumeParams;
 
     if (!modelBin.empty())
-        LargeKinfu->setModelForLCD(modelBin, modelTxt, inputSize, backend, target);
+        largeKinfu->setModelForLCD(modelBin, modelTxt, inputSize, backend, target);
 
 #ifdef HAVE_OPENCV_VIZ
     cv::viz::Viz3d window(vizWindowName);

modules/rgbd/src/large_kinfu.cpp

@@ -8,6 +8,9 @@
 #include "precomp.hpp"
 #include "loop_closure_detection.hpp"
 
+//DEBUG
+#include "opencv2/viz.hpp"
+
 namespace cv
 {
 namespace large_kinfu
@@ -335,8 +338,51 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth, const Mat& _img)
         }
     }
 
+    //DEBUG
+    auto drawScene = [&]()
+    {
+        viz::Viz3d view("debug");
+        std::vector<viz::Color> colors = { viz::Color::amethyst(), viz::Color::apricot(), viz::Color::azure(),
+                                           viz::Color::bluberry(), viz::Color::blue(), viz::Color::brown(),
+                                           viz::Color::celestial_blue(), viz::Color::chartreuse(), viz::Color::cherry() };
+        for (int i = 0; i < submapMgr->submapList.size(); i++)
+        {
+            Ptr<detail::Submap<MatType>> ps = submapMgr->submapList[i];
+            MatType p;
+            ps->volume->fetchPointsNormals(p, noArray());
+
+            Mat pmat; p.copyTo(pmat);
+            float step = 0.03f;
+            typedef std::tuple<int, int, int> int3;
+            std::set<int3> dset;
+            for (int j = 0; j < pmat.rows; j++)
+            {
+                Vec4f pp = pmat.at<Vec4f>(j);
+                dset.insert(int3(pp[0] / step, pp[1] / step, pp[2] / step));
+            }
+            std::vector<Point3f> downsampled;
+            for (auto ix : dset)
+            {
+                int x, y, z;
+                std::tie(x, y, z) = ix;
+                downsampled.push_back(Point3f(x * step, y * step, z * step));
+            }
+            Mat pp(downsampled);
+            viz::WCloud c(pp, colors[i % colors.size()]);
+            Affine3f pm = ps->pose;
+            //pm = Affine3f(pm.rotation().t(), pm.translation());
+            view.showWidget(cv::format("d%d", i), c, pm);
+        }
+        view.showWidget("3d", viz::WCoordinateSystem());
+        view.showWidget("plane", viz::WGrid());
+        view.spin();
+    };
+
     if(isMapUpdated)
     {
+        //DEBUG
+        drawScene();
+
         // TODO: Convert constraints to posegraph
         Ptr<detail::PoseGraph> poseGraph = submapMgr->MapToPoseGraph();
         CV_LOG_INFO(NULL, "Created posegraph");
@@ -349,6 +395,9 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth, const Mat& _img)
 
         submapMgr->PoseGraphToMap(poseGraph);
 
+        //DEBUG
+        drawScene();
+
     }
     CV_LOG_INFO(NULL, "Number of submaps: " << submapMgr->submapList.size());
 
@@ -361,12 +410,34 @@ template<typename MatType>
 void LargeKinfuImpl<MatType>::render(OutputArray image) const
 {
     CV_TRACE_FUNCTION();
+    //DEBUG
+    /*
     auto currSubmap = submapMgr->getCurrentSubmap();
     //! TODO: Can render be dependent on current submap
     MatType pts, nrm;
     currSubmap->frame->getPyramidAt(pts, OdometryFrame::PYR_CLOUD, 0);
     currSubmap->frame->getPyramidAt(nrm, OdometryFrame::PYR_NORM,  0);
     detail::renderPointsNormals(pts, nrm, image, params.lightPose);
+    */
+    Mat places(Size(1280, 900), CV_8UC4);
+    size_t sz = submapMgr->submapList.size();
+    for (int i = 0; i < sz; i++)
+    {
+        auto s = submapMgr->submapList[i];
+        MatType pts, nrm, img;
+        s->frame->getPyramidAt(pts, OdometryFrame::PYR_CLOUD, 0);
+        s->frame->getPyramidAt(nrm, OdometryFrame::PYR_NORM, 0);
+        detail::renderPointsNormals(pts, nrm, img, params.lightPose);
+        MatType smallImg;
+        resize(img, smallImg, Size(320, 240));
+        smallImg.copyTo(
+            places(
+            Rect(Point(320*(i%4), 240*(i/4)), Size(320, 240))
+            )
+        );
+    }
+    image.createSameSize(places, CV_8UC4);
+    places.copyTo(image);
 }