Merge pull request #2892 from savuor:posegraph_no_ceres

Pose Graph rewritten without Ceres

* Works well on g2o dataset, cumulative:
1. Pose3d done w/o Eigen types;
2. PoseGraph nodes vector<Node> -> map<int, Node>
3. Eigen is not used in cost function or parametrization
4. Cost function debugged & fixed (original one was wrong), rewritten from Automatic to Analytic

* g2o dataset reading added to PoseGraph

* sparse solver fixes from DynaFu draft

* Eigen cost function and parametrization removed + g2o reading fixed

* refactored: pose error, pose graph edge, pose graph node

* sparse solve: templated

* MyOptimize(): 1st version

* several fixes and TODOs for future

* sparse block matrix: val functions, template type

* works at Ceres quality (cleanup needed)

* MyOptimize() is set to default optimizer

* Ceres thrown away, PoseGraph class and header/source code reorganized

* pose, node, edge -> nested for PoseGraph

* warnings fixed

* jacobiScaling disabled for better performance + minors

* trailing whitespace fixed

* more warnings fixed

* message added: Eigen is required for build + minors

* trying to fix warning

* try to fix "unreachable code" warning

* trying to fix unreachable code, pt.3

* trying to fix unreachable code, pt. 5

* trying to fix unreachable code, pt. the worst + minors

* try to fix unreachable code, pt. the ugliest

* trying to fix unreachable code, pt. the grumpiest

* cout -> CV_LOG_INFO

* quat matrix functions moved outside cv and kinfu namespaces

* unused function fix

* pose graph made public (but in detail namespace) + test for pose graph

* minor: prints

* Pose Graph interface settled

* Pose graph interface and its use updated

* cos -> std::cos

* cout -> CV_LOG_INFO

* pose graph interface updated: implementation

* Pose Graph Node and Edge: extra fields dropped

* more minor refactor-like fixes

* return and finish condition fixed

* more updates to test

* test disabled for Debug builds because 400 sec is too much

* whitespace

* Disable pose graph test if there's no Eigen

* more unused vars

* fixing unused function warning

* less includes

* "verbose" removed

* write obj to file only when debug level is raised

* License + include guard

* skip test using tags and SkipTestException

* suppress "unused function" warning

* minor

modules/rgbd/CMakeLists.txt

@@ -1,15 +1,7 @@
 set(the_description "RGBD algorithms")
 
-find_package(Ceres QUIET)
 ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python)
 
-if(Ceres_FOUND)
-  ocv_target_compile_definitions(${the_module} PUBLIC CERES_FOUND)
-  ocv_target_link_libraries(${the_module} ${CERES_LIBRARIES})
-  if(Ceres_VERSION VERSION_LESS 2.0.0)
-    ocv_include_directories("${CERES_INCLUDE_DIRS}")
-  endif()
-  add_definitions(/DGLOG_NO_ABBREVIATED_SEVERITIES)  # avoid ERROR macro conflict in glog (ceres dependency)
-else()
-  message(STATUS "rgbd: CERES support is disabled. Ceres Solver is Required for Posegraph optimization")
+if(NOT HAVE_EIGEN)
+  message(STATUS "rgbd: Eigen support is disabled. Eigen is Required for Posegraph optimization")
 endif()

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/detail/pose_graph.hpp"
 
 
 /** @defgroup rgbd RGB-Depth Processing

modules/rgbd/include/opencv2/rgbd/detail/pose_graph.hpp

@@ -0,0 +1,62 @@
+// 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
+
+#ifndef OPENCV_RGBD_POSE_GRAPH_HPP
+#define OPENCV_RGBD_POSE_GRAPH_HPP
+
+#include "opencv2/core/affine.hpp"
+#include "opencv2/core/quaternion.hpp"
+
+namespace cv
+{
+namespace kinfu
+{
+namespace detail
+{
+
+// ATTENTION! This class is used internally in Large KinFu.
+// It has been pushed to publicly available headers for tests only.
+// Source compatibility of this API is not guaranteed in the future.
+
+// This class provides tools to solve so-called pose graph problem often arisen in SLAM problems
+// The pose graph format, cost function and optimization techniques
+// repeat the ones used in Ceres 3D Pose Graph Optimization:
+// http://ceres-solver.org/nnls_tutorial.html#other-examples, pose_graph_3d.cc bullet
+class CV_EXPORTS_W PoseGraph
+{
+public:
+    static Ptr<PoseGraph> create();
+    virtual ~PoseGraph();
+
+    // Node may have any id >= 0
+    virtual void addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) = 0;
+    virtual bool isNodeExist(size_t nodeId) const = 0;
+    virtual bool setNodeFixed(size_t nodeId, bool fixed) = 0;
+    virtual bool isNodeFixed(size_t nodeId) const = 0;
+    virtual Affine3d getNodePose(size_t nodeId) const = 0;
+    virtual std::vector<size_t> getNodesIds() const = 0;
+    virtual size_t getNumNodes() const = 0;
+
+    // Edges have consequent indices starting from 0
+    virtual void addEdge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation,
+                         const Matx66f& _information = Matx66f::eye()) = 0;
+    virtual size_t getEdgeStart(size_t i) const = 0;
+    virtual size_t getEdgeEnd(size_t i) const = 0;
+    virtual size_t getNumEdges() const = 0;
+
+    // checks if graph is connected and each edge connects exactly 2 nodes
+    virtual bool isValid() const = 0;
+
+    // Termination criteria are max number of iterations and min relative energy change to current energy
+    // Returns number of iterations elapsed or -1 if max number of iterations was reached or failed to optimize
+    virtual int optimize(const cv::TermCriteria& tc = cv::TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6)) = 0;
+
+    // calculate cost function based on current nodes parameters
+    virtual double calcEnergy() const = 0;
+};
+
+}  // namespace detail
+}  // namespace kinfu
+}  // namespace cv
+#endif /* ifndef OPENCV_RGBD_POSE_GRAPH_HPP */

modules/rgbd/src/fast_icp.cpp

@@ -504,7 +504,7 @@ void ICPImpl::getAb<Mat>(const Mat& oldPts, const Mat& oldNrm, const Mat& newPts
     const Normals np(newPts), nn(newNrm);
     GetAbInvoker invoker(sumAB, mutex, op, on, np, nn, pose,
                          intrinsics.scale(level).makeProjector(),
-                         distanceThreshold*distanceThreshold, cos(angleThreshold));
+                         distanceThreshold*distanceThreshold, std::cos(angleThreshold));
     Range range(0, newPts.rows);
     const int nstripes = -1;
     parallel_for_(range, invoker, nstripes);
@@ -590,7 +590,7 @@ void ICPImpl::getAb<UMat>(const UMat& oldPts, const UMat& oldNrm, const UMat& ne
                                     sizeof(pose.matrix.val)),
            fxy.val, cxy.val,
            distanceThreshold*distanceThreshold,
-           cos(angleThreshold),
+           std::cos(angleThreshold),
            ocl::KernelArg::Local(lsz),
            ocl::KernelArg::WriteOnlyNoSize(groupedSumGpu)
            );

modules/rgbd/src/large_kinfu.cpp

@@ -208,6 +208,7 @@ bool LargeKinfuImpl<UMat>::update(InputArray _depth)
     }
 }
 
+
 template<typename MatType>
 bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
 {
@@ -224,14 +225,14 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
                        params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size,
                        params.truncateThreshold);
 
-    std::cout << "Current frameID: " << frameCounter << "\n";
+    CV_LOG_INFO(NULL, "Current frameID: " << frameCounter);
     for (const auto& it : submapMgr->activeSubmaps)
     {
         int currTrackingId = it.first;
         auto submapData = it.second;
         Ptr<Submap<MatType>> currTrackingSubmap = submapMgr->getSubmap(currTrackingId);
         Affine3f affine;
-        std::cout << "Current tracking ID: " << currTrackingId << std::endl;
+        CV_LOG_INFO(NULL, "Current tracking ID: " << currTrackingId);
 
         if(frameCounter == 0) //! Only one current tracking map
         {
@@ -248,7 +249,7 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
             currTrackingSubmap->composeCameraPose(affine);
         else
         {
-            std::cout << "Tracking failed" << std::endl;
+            CV_LOG_INFO(NULL, "Tracking failed");
             continue;
         }
 
@@ -267,8 +268,8 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
 
         currTrackingSubmap->updatePyrPointsNormals(params.pyramidLevels);
 
-        std::cout << "Submap: " << currTrackingId << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks() << "\n";
-        std::cout << "Submap: " << currTrackingId << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter) << "\n";
+        CV_LOG_INFO(NULL, "Submap: " << currTrackingId << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks());
+        CV_LOG_INFO(NULL, "Submap: " << currTrackingId << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter));
 
     }
     //4. Update map
@@ -277,13 +278,19 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
     if(isMapUpdated)
     {
         // TODO: Convert constraints to posegraph
-        PoseGraph poseGraph = submapMgr->MapToPoseGraph();
-        std::cout << "Created posegraph\n";
-        Optimizer::optimize(poseGraph);
+        Ptr<kinfu::detail::PoseGraph> poseGraph = submapMgr->MapToPoseGraph();
+        CV_LOG_INFO(NULL, "Created posegraph");
+        int iters = poseGraph->optimize();
+        if (iters < 0)
+        {
+            CV_LOG_INFO(NULL, "Failed to perform pose graph optimization");
+            return false;
+        }
+
         submapMgr->PoseGraphToMap(poseGraph);
 
     }
-    std::cout << "Number of submaps: " << submapMgr->submapList.size() << "\n";
+    CV_LOG_INFO(NULL, "Number of submaps: " << submapMgr->submapList.size());
 
     frameCounter++;
     return true;

modules/rgbd/src/nonrigid_icp.cpp

@@ -350,7 +350,7 @@ bool ICPImpl::estimateWarpNodes(WarpField& currentWarp, const Affine3f &pose,
 
             Vec3f diff = oldPoints.at<Vec3f>(y, x) - Vec3f(newP);
             if(diff.dot(diff) > 0.0004f) continue;
-            if(abs(newN.dot(oldNormals.at<Point3f>(y, x))) < cos((float)CV_PI / 2)) continue;
+            if(abs(newN.dot(oldNormals.at<Point3f>(y, x))) < std::cos((float)CV_PI / 2)) continue;
 
             float rd = newN.dot(diff);