Add node marginalization support

slam_toolbox/config/mapper_params_lifelong.yaml

@@ -22,6 +22,7 @@ lifelong_overlap_score_scale: 0.06
 lifelong_constraint_multiplier: 0.08
 lifelong_nearby_penalty: 0.001
 lifelong_candidates_scale: 0.03
+lifelong_node_marginalization: true
 
 # if you'd like to immediately start continuing a map at a given pose
 # or at the dock, but they are mutually exclusive, if pose is given

slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp

@@ -63,6 +63,7 @@ class LifelongSlamToolbox : public SlamToolbox
   double candidates_scale_;
   double iou_match_;
   double nearby_penalty_;
+  bool node_marginalization_;
 };
 
 }

slam_toolbox/lib/karto_sdk/Authors

@@ -4,4 +4,5 @@ Contributors:
 -------------------------------
 Michael A. Eriksen ([email protected])
 Benson Limketkai ([email protected])
-Steven Macenski ([email protected])
+Steven Macenski ([email protected])
+Michel Hidalgo ([email protected])

slam_toolbox/lib/karto_sdk/CMakeLists.txt

@@ -25,10 +25,17 @@ catkin_package(
 add_definitions(${EIGEN3_DEFINITIONS})
 
 include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
-add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp)
+add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp src/contrib/ChowLiuTreeApprox.cpp)
 target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} ${TBB_LIBRARIES})
 
 install(DIRECTORY include/ DESTINATION include)
 install(TARGETS kartoSlamToolbox
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
+
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(eigenExtensionsTest test/eigen_extensions_test.cpp)
+  target_link_libraries(eigenExtensionsTest kartoSlamToolbox ${catkin_LIBRARIES})
+  catkin_add_gtest(chowLiuTreeApproxTest test/chow_liu_tree_approx_test.cpp)
+  target_link_libraries(chowLiuTreeApproxTest kartoSlamToolbox ${catkin_LIBRARIES})
+endif()

slam_toolbox/lib/karto_sdk/include/karto_sdk/Karto.h

@@ -50,6 +50,8 @@
 #include <boost/serialization/array.hpp>
 #include <boost/version.hpp>
 
+#include <Eigen/Core>
+
 #ifdef USE_POCO
 #include <Poco/Mutex.h>
 #endif
@@ -2459,6 +2461,18 @@ namespace karto
       memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
     }
 
+   /**
+    * Copy constructor for equivalent Eigen type
+    */
+    inline Matrix3(const Eigen::Matrix3d & rOther)
+    {
+      for (Eigen::Index i = 0; i < rOther.rows(); ++i) {
+        for (Eigen::Index j = 0; j < rOther.cols(); ++j) {
+          m_Matrix[i][j] = rOther(i, j);
+        }
+      }
+    }
+
   public:
     /**
      * Sets this matrix to identity matrix
@@ -2611,6 +2625,16 @@ namespace karto
       return converter.str();
     }
 
+    inline Eigen::Matrix3d ToEigen() const
+    {
+      Eigen::Matrix3d matrix;
+      matrix <<
+          m_Matrix[0][0], m_Matrix[0][1], m_Matrix[0][2],
+          m_Matrix[1][0], m_Matrix[1][1], m_Matrix[1][2],
+          m_Matrix[2][0], m_Matrix[2][1], m_Matrix[2][2];
+      return matrix;
+    }
+
   public:
     /**
      * Assignment operator

slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h

@@ -24,6 +24,7 @@
 #include <queue>
 
 #include <Eigen/Core>
+#include <Eigen/Sparse>
 
 #include "tbb/parallel_for.h"
 #include "tbb/parallel_do.h"
@@ -141,20 +142,37 @@ namespace karto
   class LinkInfo : public EdgeLabel
   {
   public:
+    LinkInfo()
+    {
+    }
+
     /**
      * Constructs a link between the given poses
      * @param rPose1
      * @param rPose2
      * @param rCovariance
      */
-    LinkInfo()
-    {
-    }
     LinkInfo(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
     {
       Update(rPose1, rPose2, rCovariance);
     }
 
+    /**
+     * Constructs a link
+     * @param rPose1
+     * @param rPose2
+     * @param rPoseDifference
+     * @param rCovariance
+     */
+    LinkInfo(const Pose2 & rPose1, const Pose2 & rPose2,
+             const Pose2 & rPoseDifference,
+             const Matrix3 & rCovariance)
+      : m_Pose1(rPose1), m_Pose2(rPose2),
+        m_PoseDifference(rPoseDifference),
+        m_Covariance(rCovariance)
+    {
+    }
+
     /**
      * Destructor
      */
@@ -294,6 +312,19 @@ namespace karto
       return;
     }
 
+    /**
+     * Removes an edge
+     */
+    inline void RemoveEdge(Edge<T> * pEdge)
+    {
+      auto it = std::find(m_Edges.begin(), m_Edges.end(), pEdge);
+      if (it == m_Edges.end()) {
+        std::cout << "Edge not connected to vertex!" << std::endl;
+        return;
+      }
+      RemoveEdge(std::distance(m_Edges.begin(), it));
+    }
+
     /**
      * Gets score for vertex
      * @return score
@@ -632,6 +663,19 @@ namespace karto
       m_Edges.erase(m_Edges.begin() + idx);
     }
 
+    /**
+     * Removes an edge of the graph
+     * @param pEdge
+     */
+    inline void RemoveEdge(Edge<T>* pEdge)
+    {
+      auto it = std::find(m_Edges.begin(), m_Edges.end(), pEdge);
+      if (it == m_Edges.end()) {
+        std::cout << "Edge not found in graph!" << std::endl;
+        return;
+      }
+      RemoveEdge(std::distance(m_Edges.begin(), it));
+    }
 
     /**
      * Deletes the graph data
@@ -746,6 +790,11 @@ namespace karto
                                       LocalizedRangeScan* pTargetScan,
                                       kt_bool& rIsNewEdge);
 
+    /**
+     * Adds an edge to the graph, as-is.
+     */
+    kt_bool AddEdge(Edge<LocalizedRangeScan> * edge);
+
     /**
      * Link scan to last scan and nearby chains of scans
      * @param pScan
@@ -1023,12 +1072,18 @@ namespace karto
     /**
      * Get graph stored
      */
-    virtual std::unordered_map<int, Eigen::Vector3d>* getGraph()
+    virtual std::unordered_map<int, Eigen::Map<Eigen::Vector3d>> GetGraph()
     {
       std::cout << "getGraph method not implemented for this solver type. Graph visualization unavailable." << std::endl;
-      return nullptr;
+      return {};
     }
 
+    /**
+     * Get information matrix associated with the graph
+     */
+    virtual Eigen::SparseMatrix<double> GetInformationMatrix(
+        std::unordered_map<int, Eigen::Index> * /* ordering */) const = 0;
+
     /**
      * Modify a node's pose
      */
@@ -1996,6 +2051,7 @@ namespace karto
     kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan, kt_bool addScanToLocalizationBuffer = false);
     kt_bool ProcessLocalization(LocalizedRangeScan* pScan);
     kt_bool RemoveNodeFromGraph(Vertex<LocalizedRangeScan>*);
+    kt_bool MarginalizeNodeFromGraph(Vertex<LocalizedRangeScan>*);
     void AddScanToLocalizationBuffer(LocalizedRangeScan* pScan, Vertex<LocalizedRangeScan>* scan_vertex);
     void ClearLocalizationBuffer();
 
@@ -2085,6 +2141,8 @@ namespace karto
      */
     kt_bool HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const;
 
+    kt_bool RemoveEdgeFromGraph(Edge<LocalizedRangeScan> *);
+
   public:
     /////////////////////////////////////////////
     // fire information for listeners!!

slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h

@@ -0,0 +1,54 @@
+/*
+ * Copyright 2022 Ekumen Inc.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 2.1 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef KARTO_SDK__CHOW_LIU_TREE_APPROX_H_
+#define KARTO_SDK__CHOW_LIU_TREE_APPROX_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Sparse>
+
+#include "karto_sdk/Karto.h"
+#include "karto_sdk/Mapper.h"
+#include "karto_sdk/Types.h"
+
+namespace karto
+{
+  namespace contrib
+  {
+
+    /** Marginalizes a variable from a sparse information matrix. */
+    Eigen::SparseMatrix<double> ComputeMarginalInformationMatrix(
+        const Eigen::SparseMatrix<double> & information_matrix,
+        const Eigen::Index discarded_variable_index,
+        const Eigen::Index variables_dimension);
+
+    /**
+     * Computes a Chow Liu tree approximation to a given pose graph clique.
+     *
+     * Currently, this function only performs linear approximations to full
+     * rank constraints (i.e. constraints with full rank covariance matrices).
+     */
+    std::vector<Edge<LocalizedRangeScan> *> ComputeChowLiuTreeApproximation(
+        const std::vector<Vertex<LocalizedRangeScan> *> & clique,
+        const Eigen::MatrixXd & covariance_matrix);
+
+  }  // namespace contrib
+}  // namespace karto
+
+#endif // KARTO_SDK__CHOW_LIU_TREE_APPROX_H_