[Rolling] Implement pose graph node marginalization

lib/karto_sdk/CMakeLists.txt

@@ -28,7 +28,7 @@ include_directories(include ${catkin_INCLUDE_DIRS}
 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)
 ament_target_dependencies(kartoSlamToolbox ${dependencies})
 target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} ${TBB_LIBRARIES})
 

lib/karto_sdk/include/karto_sdk/Karto.h

@@ -1,3 +1,4 @@
+
 /*
  * Copyright 2010 SRI International
  *
@@ -38,6 +39,8 @@
 #include <boost/serialization/array.hpp>
 #include <boost/version.hpp>
 
+#include <Eigen/Core>
+
 #include <string>
 #include <fstream>
 #include <limits>
@@ -2450,6 +2453,18 @@ class Matrix3
     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
@@ -2593,6 +2608,16 @@ class Matrix3
     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

lib/karto_sdk/include/karto_sdk/Mapper.h

@@ -32,6 +32,7 @@
 #include "tbb/blocked_range.h"
 
 #include "Eigen/Core"
+#include "Eigen/SparseCore"
 #include "rclcpp/rclcpp.hpp"
 #include "Karto.h"  // NOLINT
 #include "nanoflann_adaptors.h"  // NOLINT
@@ -143,20 +144,32 @@ class EdgeLabel
 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);
   }
 
+  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
    */
@@ -296,6 +309,18 @@ class Vertex
     m_Edges.erase(m_Edges.begin() + idx);
   }
 
+  /**
+   * 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 found!" << std::endl;
+    }
+    RemoveEdge(std::distance(m_Edges.begin(), it));
+  }
+
   /**
    * Gets score for vertex
    * @return score
@@ -629,6 +654,18 @@ class Graph
     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!" << std::endl;
+    }
+    RemoveEdge(std::distance(m_Edges.begin(), it));
+  }
 
   /**
    * Deletes the graph data
@@ -743,6 +780,8 @@ class KARTO_EXPORT MapperGraph : public Graph<LocalizedRangeScan>
     LocalizedRangeScan * pTargetScan,
     kt_bool & rIsNewEdge);
 
+  kt_bool AddEdge(Edge<LocalizedRangeScan> * edge);
+
   /**
    * Link scan to last scan and nearby chains of scans
    * @param pScan
@@ -1032,14 +1071,20 @@ class ScanSolver
   /**
    * Get graph stored
    */
-  virtual std::unordered_map<int, Eigen::Vector3d> * getGraph()
+  virtual const std::unordered_map<int, Eigen::Vector3d> * getGraph()
   {
     std::cout <<
       "getGraph method not implemented for this solver type. Graph visualization unavailable." <<
       std::endl;
     return nullptr;
   }
 
+  /**
+   * 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
    */
@@ -2009,6 +2054,7 @@ class KARTO_EXPORT Mapper : public Module
   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();
 
@@ -2098,6 +2144,8 @@ class KARTO_EXPORT Mapper : public Module
    */
   kt_bool HasMovedEnough(LocalizedRangeScan * pScan, LocalizedRangeScan * pLastScan) const;
 
+  kt_bool RemoveEdgeFromGraph(Edge<LocalizedRangeScan> *);
+
 public:
   /////////////////////////////////////////////
   // fire information for listeners!!

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

@@ -0,0 +1,50 @@
+#ifndef KARTO_SDK__CHOWLIUTREEAPPROX_H_
+#define KARTO_SDK__CHOWLIUTREEAPPROX_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
+{
+
+/** An uncertain, gaussian-distributed 2D pose. */
+struct UncertainPose2
+{
+  Pose2 mean;
+  Matrix3 covariance;
+};
+
+/**
+ * Returns the target pose relative to the source pose,
+ * accounting for their joint distribution covariance.
+ */
+UncertainPose2 ComputeRelativePose2(
+    Pose2 source_pose, Pose2 target_pose,
+    Eigen::Matrix<double, 6, 6> joint_pose_covariance);
+
+/** 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 clique
+ * (i.e. a graphical representation of joint probability distribution).
+ */
+std::vector<Edge<LocalizedRangeScan> *> ComputeChowLiuTreeApproximation(
+  const std::vector<Vertex<LocalizedRangeScan> *> & clique,
+  const Eigen::SparseMatrix<double> & covariance_matrix);
+
+}  // namespace contrib
+}  // namespace karto
+
+#endif // KARTO_SDK__CHOWLIUTREEAPPROX_H_