diff --git a/lib/karto_sdk/CMakeLists.txt b/lib/karto_sdk/CMakeLists.txt
index be14006e..ecbe7bea 100644
--- a/lib/karto_sdk/CMakeLists.txt
+++ b/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})
 
diff --git a/lib/karto_sdk/include/karto_sdk/Karto.h b/lib/karto_sdk/include/karto_sdk/Karto.h
index c03f6757..54a08d22 100644
--- a/lib/karto_sdk/include/karto_sdk/Karto.h
+++ b/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
diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h
index ae6c8cad..a7ebc873 100644
--- a/lib/karto_sdk/include/karto_sdk/Mapper.h
+++ b/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,7 +1071,7 @@ 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." <<
@@ -1040,6 +1079,12 @@ class ScanSolver
     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!!
diff --git a/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h b/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h
new file mode 100644
index 00000000..7ddcd922
--- /dev/null
+++ b/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_
diff --git a/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h b/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h
new file mode 100644
index 00000000..d076b5f8
--- /dev/null
+++ b/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h
@@ -0,0 +1,689 @@
+#ifndef KARTO_SDK__EIGENEXTENSIONS_H_
+#define KARTO_SDK__EIGENEXTENSIONS_H_
+
+#include <Eigen/Core>
+#include <Eigen/Sparse>
+#include <Eigen/SparseLU>
+
+namespace Eigen {
+
+namespace internal {
+
+// Returns (a + b) or Dynamic if either operand is.
+template<typename A, typename B>
+inline constexpr int size_sum_prefer_dynamic(A a, B b) {
+  static_assert(
+      std::is_enum<A>::value || std::is_integral<A>::value,
+      "Argument a must be an integer or enum");
+  static_assert(
+      std::is_enum<B>::value || std::is_integral<B>::value,
+      "Argument b must be an integer or enum");
+  if ((int) a == Dynamic || (int) b == Dynamic) return Dynamic;
+  return (int) a + (int) b;
+}
+
+// Returns max(a, b) or Dynamic if either operand is.
+template<typename A, typename B>
+inline constexpr int max_size_prefer_dynamic(A a, B b) {
+  static_assert(
+      std::is_enum<A>::value || std::is_integral<A>::value,
+      "Argument a must be an integer or enum");
+  static_assert(
+      std::is_enum<B>::value || std::is_integral<B>::value,
+      "Argument b must be an integer or enum");
+  if ((int) a == Dynamic || (int) b == Dynamic) return Dynamic;
+  return std::max((int) a, (int) b);
+}
+
+template<bool Condition, typename ThenT, typename ElseT>
+struct constexpr_conditional_impl;
+
+template<typename ThenT, typename ElseT>
+struct constexpr_conditional_impl<true, ThenT, ElseT> {
+  constexpr_conditional_impl(ThenT&& some_value, ElseT&&)
+    : value(some_value)
+  {
+  }
+
+  ThenT value;
+};
+
+template<typename ThenT, typename ElseT>
+struct constexpr_conditional_impl<false, ThenT, ElseT> {
+  constexpr_conditional_impl(ThenT&&, ElseT&& other_value)
+    : value(other_value)
+  {
+  }
+
+  ElseT value;
+};
+
+// Returns `some_value` if `Condition`, else `other_value`.
+//
+// Compile-time if-then-else expression where `some_value`
+// and `other_value` types need not match.
+template<bool Condition, typename ThenT, typename ElseT>
+inline constexpr auto
+constexpr_conditional(ThenT&& some_value, ElseT&& other_value)
+{
+  return constexpr_conditional_impl<Condition, ThenT, ElseT>{
+    std::forward<ThenT>(some_value),
+    std::forward<ElseT>(other_value)}.value;
+}
+
+}  // namespace internal
+
+// Forward declaration.
+template<typename LhsType, typename RhsType>
+class HorizontalStack;
+
+// Forward declaration.
+template<typename LhsType, typename RhsType>
+class VerticalStack;
+
+// Forward declaration.
+template<typename XprType, typename RowIndices, typename ColIndices>
+class View;
+
+namespace internal {
+
+template <typename A, typename B>
+struct promote_storage_kind;
+
+template <typename A>
+struct promote_storage_kind<A, Sparse> { using type = Sparse; };
+
+template <typename B>
+struct promote_storage_kind<Sparse, B> { using type = Sparse; };
+
+template <>
+struct promote_storage_kind<Sparse, Sparse> { using type = Sparse; };
+
+template <typename A, typename B>
+struct promote_scalar {
+  static_assert(
+      std::is_convertible<A, B>::value ||
+      std::is_convertible<B, A>::value,
+      "Scalar types are incommensurable");
+
+  using type = typename std::conditional<
+    std::is_convertible<A, B>::value, B, A>::type;
+};
+
+template<typename LhsType, typename RhsType>
+struct traits<HorizontalStack<LhsType, RhsType>>
+{
+  using XprKind = typename traits<LhsType>::XprKind;
+  using StorageKind = typename promote_storage_kind<
+    typename traits<LhsType>::StorageKind,
+    typename traits<RhsType>::StorageKind>::type;
+  using StorageIndex = typename promote_index_type<
+    typename traits<LhsType>::StorageIndex,
+    typename traits<RhsType>::StorageIndex>::type;
+  using Scalar = typename promote_scalar<
+    typename traits<LhsType>::Scalar,
+    typename traits<RhsType>::Scalar>::type;
+  enum {
+    RowsAtCompileTime = (
+        traits<LhsType>::RowsAtCompileTime == Dynamic ?
+        traits<RhsType>::RowsAtCompileTime :
+        traits<LhsType>::RowsAtCompileTime),
+    ColsAtCompileTime = internal::size_sum_prefer_dynamic(
+        traits<LhsType>::ColsAtCompileTime,
+        traits<RhsType>::ColsAtCompileTime),
+    MaxRowsAtCompileTime = internal::max_size_prefer_dynamic(
+        traits<LhsType>::MaxRowsAtCompileTime,
+        traits<RhsType>::MaxRowsAtCompileTime),
+    MaxColsAtCompileTime = internal::size_sum_prefer_dynamic(
+        traits<LhsType>::MaxColsAtCompileTime,
+        traits<RhsType>::MaxColsAtCompileTime),
+    Flags = int(traits<LhsType>::Flags) & RowMajorBit
+  };
+};
+
+template<typename LhsType, typename RhsType>
+struct traits<VerticalStack<LhsType, RhsType>>
+{
+  using XprKind = typename traits<LhsType>::XprKind;
+  using StorageKind = typename promote_storage_kind<
+    typename traits<LhsType>::StorageKind,
+    typename traits<RhsType>::StorageKind>::type;
+  using StorageIndex = typename promote_index_type<
+    typename traits<LhsType>::StorageIndex,
+    typename traits<RhsType>::StorageIndex>::type;
+  using Scalar = typename promote_scalar<
+    typename traits<LhsType>::Scalar,
+    typename traits<RhsType>::Scalar>::type;
+  enum {
+    RowsAtCompileTime = internal::size_sum_prefer_dynamic(
+        traits<LhsType>::RowsAtCompileTime,
+        traits<RhsType>::RowsAtCompileTime),
+    ColsAtCompileTime = (
+        traits<LhsType>::ColsAtCompileTime == Dynamic ?
+        traits<RhsType>::ColsAtCompileTime :
+        traits<LhsType>::ColsAtCompileTime),
+    MaxRowsAtCompileTime = internal::size_sum_prefer_dynamic(
+        traits<LhsType>::MaxRowsAtCompileTime,
+        traits<RhsType>::MaxRowsAtCompileTime),
+    MaxColsAtCompileTime = internal::max_size_prefer_dynamic(
+        traits<LhsType>::MaxColsAtCompileTime,
+        traits<RhsType>::MaxColsAtCompileTime),
+    Flags = int(traits<LhsType>::Flags) & RowMajorBit
+  };
+};
+
+template<typename XprType, typename RowIndices, typename ColIndices>
+struct traits<View<XprType, RowIndices, ColIndices>> : traits<XprType>
+{
+  enum {
+    RowsAtCompileTime = Dynamic,
+    ColsAtCompileTime = Dynamic,
+    MaxRowsAtCompileTime = RowsAtCompileTime,
+    MaxColsAtCompileTime = ColsAtCompileTime,
+    IsRowMajor = (int(traits<XprType>::Flags) & RowMajorBit) != 0,
+    FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
+    Flags = int(traits<XprType>::Flags) & RowMajorBit,
+  };
+};
+
+}  // namespace internal
+
+/**
+ * Expression of a column by column concatenation (i.e. horizontal stacking)
+ * of two matrix (or array) expressions.
+ *
+ * Only sparse expressions are supported.
+ */
+template<typename LhsType, typename RhsType>
+class HorizontalStack : public internal::generic_xpr_base<
+  HorizontalStack<LhsType, RhsType>>::type, internal::no_assignment_operator
+{
+ public:
+  EIGEN_STATIC_ASSERT_SAME_XPR_KIND(LhsType, RhsType)
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(LhsType, RhsType)
+  EIGEN_STATIC_ASSERT((
+      ((internal::traits<LhsType>::Flags & RowMajorBit) ==
+       (internal::traits<RhsType>::Flags & RowMajorBit))),
+      THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH)
+
+  using Lhs = typename internal::remove_all<LhsType>::type;
+  using Rhs = typename internal::remove_all<RhsType>::type;
+  using Base = typename internal::generic_xpr_base<
+    HorizontalStack<LhsType, RhsType>>::type;
+
+  EIGEN_GENERIC_PUBLIC_INTERFACE(HorizontalStack)
+
+  HorizontalStack(const LhsType& lhs, const RhsType& rhs)
+  : m_lhs(lhs), m_rhs(rhs)
+  {
+    eigen_assert(lhs.rows() == rhs.rows());
+  }
+
+  constexpr Index rows() const noexcept
+  {
+    constexpr auto LhsRowsAtCompileTime =
+        internal::traits<LhsType>::RowsAtCompileTime;
+    return LhsRowsAtCompileTime == Dynamic ? m_rhs.rows() : m_lhs.rows();
+  }
+
+  constexpr Index cols() const noexcept
+  {
+    return m_lhs.cols() + m_rhs.cols();
+  }
+
+  using LhsTypeNested = typename internal::ref_selector<LhsType>::type;
+  using RhsTypeNested = typename internal::ref_selector<RhsType>::type;
+  using LhsTypeNestedNoRef =
+      typename internal::remove_reference<LhsTypeNested>::type;
+  using RhsTypeNestedNoRef =
+      typename internal::remove_reference<RhsTypeNested>::type;
+
+  const LhsTypeNestedNoRef& lhs() const { return m_lhs; }
+
+  const RhsTypeNestedNoRef& rhs() const { return m_rhs; }
+
+ protected:
+  LhsTypeNested m_lhs;
+  RhsTypeNested m_rhs;
+};
+
+
+/**
+ * Expression of a row by row concatenation (i.e. vertical stacking)
+ * of two matrix (or array) expressions.
+ *
+ * Only sparse expressions are supported.
+ */
+template<typename LhsType, typename RhsType>
+class VerticalStack : public internal::generic_xpr_base<
+  VerticalStack<LhsType, RhsType>>::type, internal::no_assignment_operator
+{
+ public:
+  EIGEN_STATIC_ASSERT_SAME_XPR_KIND(LhsType, RhsType)
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(LhsType, RhsType)
+  EIGEN_STATIC_ASSERT((
+      ((internal::traits<LhsType>::Flags & RowMajorBit) ==
+       (internal::traits<RhsType>::Flags & RowMajorBit))),
+      THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH)
+
+  using Lhs = typename internal::remove_all<LhsType>::type;
+  using Rhs = typename internal::remove_all<RhsType>::type;
+  using Base = typename internal::generic_xpr_base<
+    VerticalStack<LhsType, RhsType>>::type;
+
+  EIGEN_GENERIC_PUBLIC_INTERFACE(VerticalStack)
+
+  VerticalStack(const LhsType& lhs, const RhsType& rhs)
+  : m_lhs(lhs), m_rhs(rhs)
+  {
+    eigen_assert(lhs.cols() == rhs.cols());
+  }
+
+  constexpr Index rows() const noexcept
+  {
+    return m_lhs.rows() + m_rhs.rows();
+  }
+
+  constexpr Index cols() const noexcept
+  {
+    constexpr auto LhsColsAtCompileTime =
+        internal::traits<LhsType>::ColsAtCompileTime;
+    return LhsColsAtCompileTime == Dynamic ? m_rhs.cols() : m_lhs.cols();
+  }
+
+  using LhsTypeNested = typename internal::ref_selector<LhsType>::type;
+  using RhsTypeNested = typename internal::ref_selector<RhsType>::type;
+  using LhsTypeNestedNoRef =
+      typename internal::remove_reference<LhsTypeNested>::type;
+  using RhsTypeNestedNoRef =
+      typename internal::remove_reference<RhsTypeNested>::type;
+
+  const LhsTypeNestedNoRef& lhs() const { return m_lhs; }
+
+  const RhsTypeNestedNoRef& rhs() const { return m_rhs; }
+
+ protected:
+  LhsTypeNested m_lhs;
+  RhsTypeNested m_rhs;
+};
+
+/**
+ * Expression of a non-sequential sub-matrix defined by arbitrary sequences
+ * of row and column indices.
+ *
+ * Only sparse expressions are supported.
+ */
+// NOTE(hidmic): this a *much* simplified equivalent to IndexedView in Eigen 3.4
+template<typename XprType, typename RowIndices, typename ColIndices>
+class View : public internal::generic_xpr_base<
+  View<XprType, RowIndices, ColIndices>>::type, internal::no_assignment_operator
+{
+ public:
+  using Base = typename internal::generic_xpr_base<
+    View<XprType, RowIndices, ColIndices>>::type;
+  using NestedExpression = typename internal::remove_all<XprType>::type;
+
+  EIGEN_GENERIC_PUBLIC_INTERFACE(View)
+
+  View(XprType& xpr, const RowIndices& rowIndices, const ColIndices& colIndices)
+  : m_xpr(xpr), m_rowIndices(rowIndices), m_colIndices(colIndices)
+  {
+  }
+
+  constexpr Index rows() const noexcept { return m_rowIndices.size(); }
+
+  constexpr Index cols() const noexcept { return m_colIndices.size(); }
+
+  const typename internal::remove_all<XprType>::type&
+  nestedExpression() const { return m_xpr; }
+
+  typename internal::remove_reference<XprType>::type&
+  nestedExpression() { return m_xpr; }
+
+  const RowIndices& rowIndices() const { return m_rowIndices; }
+
+  const ColIndices& colIndices() const { return m_colIndices; }
+
+ protected:
+  using XprTypeNested =
+      typename internal::ref_selector<XprType>::non_const_type;
+  XprTypeNested m_xpr;
+  RowIndices m_rowIndices;
+  ColIndices m_colIndices;
+};
+
+namespace internal {
+
+template<typename LhsType, typename RhsType>
+struct evaluator<HorizontalStack<LhsType, RhsType>>
+    : public binary_evaluator<HorizontalStack<LhsType, RhsType>>
+{
+  using XprType = HorizontalStack<LhsType, RhsType>;
+  using Base = binary_evaluator<HorizontalStack<LhsType, RhsType>>;
+
+  explicit evaluator(const XprType& xpr) : Base(xpr) {}
+};
+
+template<typename LhsType, typename RhsType>
+struct binary_evaluator<
+  HorizontalStack<LhsType, RhsType>, IteratorBased, IteratorBased
+> : public evaluator_base<HorizontalStack<LhsType, RhsType>>
+{
+  using XprType = HorizontalStack<LhsType, RhsType>;
+  using LhsIteratorType = typename evaluator<LhsType>::InnerIterator;
+  using RhsIteratorType = typename evaluator<RhsType>::InnerIterator;
+  using StorageIndex = typename traits<XprType>::StorageIndex;
+  using Scalar = typename traits<XprType>::Scalar;
+
+ public:
+  class InnerIterator
+  {
+    enum {
+      IsRowMajor = int(traits<XprType>::Flags) & RowMajorBit
+    };
+
+   public:
+    InnerIterator(const binary_evaluator& eval, Index outer)
+      : m_useLhsIter(IsRowMajor || outer < eval.m_lhsCols),
+        m_lhsIter(eval.m_lhsEval, m_useLhsIter ? outer : 0),
+        m_useRhsIter(IsRowMajor || outer >= eval.m_lhsCols),
+        m_rhsIter(eval.m_rhsEval, m_useRhsIter ? (
+            ! IsRowMajor ? outer - eval.m_lhsCols : outer) : 0),
+        m_rhsOffset(IsRowMajor ? eval.m_lhsCols : 0),
+        m_outer(outer)
+    {
+      this->operator++();
+    }
+
+    InnerIterator& operator++()
+    {
+      if (m_useLhsIter && m_lhsIter) {
+        m_value = m_lhsIter.value();
+        m_inner = m_lhsIter.index();
+        ++m_lhsIter;
+      } else if (m_useRhsIter && m_rhsIter) {
+        m_value = m_rhsIter.value();
+        m_inner = m_rhsOffset + m_rhsIter.index();
+        ++m_rhsIter;
+      } else {
+        m_value = Scalar(0);
+        m_inner = -1;
+      }
+      return *this;
+    }
+
+    Scalar value() const { return m_value; }
+    Index index() const { return m_inner; }
+    Index row() const { return IsRowMajor ? m_outer : index(); }
+    Index col() const { return IsRowMajor ? index() : m_outer; }
+
+    operator bool() const { return m_inner >= 0; }
+
+   protected:
+    bool m_useLhsIter;
+    LhsIteratorType m_lhsIter;
+    bool m_useRhsIter;
+    RhsIteratorType m_rhsIter;
+    StorageIndex m_rhsOffset;
+
+    StorageIndex m_outer;
+    StorageIndex m_inner;
+    Scalar m_value;
+  };
+
+  enum {
+    CoeffReadCost = (int(evaluator<LhsType>::CoeffReadCost) +
+                     int(evaluator<RhsType>::CoeffReadCost)),
+    Flags = int(evaluator<LhsType>::Flags) & RowMajorBit,
+  };
+
+  explicit binary_evaluator(const XprType& xpr)
+    : m_lhsEval(xpr.lhs()),
+      m_rhsEval(xpr.rhs()),
+      m_lhsCols(xpr.lhs().cols())
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+ protected:
+  evaluator<LhsType> m_lhsEval;
+  evaluator<RhsType> m_rhsEval;
+  StorageIndex m_lhsCols;
+};
+
+template<typename LhsType, typename RhsType>
+struct evaluator<VerticalStack<LhsType, RhsType>>
+    : public binary_evaluator<VerticalStack<LhsType, RhsType>>
+{
+  using XprType = VerticalStack<LhsType, RhsType>;
+  using Base = binary_evaluator<VerticalStack<LhsType, RhsType>>;
+
+  explicit evaluator(const XprType& xpr) : Base(xpr) {}
+};
+
+template<typename LhsType, typename RhsType>
+struct binary_evaluator<
+  VerticalStack<LhsType, RhsType>, IteratorBased, IteratorBased
+> : public evaluator_base<VerticalStack<LhsType, RhsType>>
+{
+  using XprType = VerticalStack<LhsType, RhsType>;
+  using LhsIteratorType = typename evaluator<LhsType>::InnerIterator;
+  using RhsIteratorType = typename evaluator<RhsType>::InnerIterator;
+  using StorageIndex = typename traits<XprType>::StorageIndex;
+  using Scalar = typename traits<XprType>::Scalar;
+
+ public:
+  class InnerIterator
+  {
+    enum {
+      IsRowMajor = int(traits<XprType>::Flags) & RowMajorBit
+    };
+
+   public:
+    InnerIterator(const binary_evaluator& eval, Index outer)
+      : m_useLhsIter(!IsRowMajor || outer < eval.m_lhsRows),
+        m_lhsIter(eval.m_lhsEval, m_useLhsIter ? outer : 0),
+        m_useRhsIter(!IsRowMajor || outer >= eval.m_lhsRows),
+        m_rhsIter(eval.m_rhsEval, m_useRhsIter ? (
+            IsRowMajor ? outer - eval.m_lhsRows : outer) : 0),
+        m_rhsOffset(!IsRowMajor ? eval.m_lhsRows : 0),
+        m_outer(outer)
+    {
+      this->operator++();
+    }
+
+    InnerIterator& operator++()
+    {
+      if (m_useLhsIter && m_lhsIter) {
+        m_value = m_lhsIter.value();
+        m_inner = m_lhsIter.index();
+        ++m_lhsIter;
+      } else if (m_useRhsIter && m_rhsIter) {
+        m_value = m_rhsIter.value();
+        m_inner = m_rhsOffset + m_rhsIter.index();
+        ++m_rhsIter;
+      } else {
+        m_value = Scalar(0);
+        m_inner = -1;
+      }
+      return *this;
+    }
+
+    Scalar value() const { return m_value; }
+    Index index() const { return m_inner; }
+    Index row() const { return IsRowMajor ? m_outer : index(); }
+    Index col() const { return IsRowMajor ? index() : m_outer; }
+
+    operator bool() const { return m_inner >= 0; }
+
+   protected:
+    bool m_useLhsIter;
+    LhsIteratorType m_lhsIter;
+    bool m_useRhsIter;
+    RhsIteratorType m_rhsIter;
+    StorageIndex m_rhsOffset;
+
+    StorageIndex m_outer;
+    StorageIndex m_inner;
+    Scalar m_value;
+  };
+
+  enum {
+    CoeffReadCost = (int(evaluator<LhsType>::CoeffReadCost) +
+                     int(evaluator<RhsType>::CoeffReadCost)),
+    Flags = int(evaluator<LhsType>::Flags) & RowMajorBit,
+  };
+
+  explicit binary_evaluator(const XprType& xpr)
+    : m_lhsEval(xpr.lhs()),
+      m_rhsEval(xpr.rhs()),
+      m_lhsRows(xpr.lhs().rows())
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+ protected:
+  evaluator<LhsType> m_lhsEval;
+  evaluator<RhsType> m_rhsEval;
+  StorageIndex m_lhsRows;
+};
+
+template<typename ArgType, typename RowIndices, typename ColIndices>
+struct unary_evaluator<View<ArgType, RowIndices, ColIndices>, IteratorBased>
+  : evaluator_base<View<ArgType, RowIndices, ColIndices>>
+{
+  using XprType = View<ArgType, RowIndices, ColIndices>;
+
+  class InnerIterator
+  {
+    enum {
+      IsRowMajor = traits<XprType>::IsRowMajor
+    };
+   public:
+    using Scalar = typename traits<XprType>::Scalar;
+    using StorageIndex = typename traits<XprType>::StorageIndex;
+
+    InnerIterator(const unary_evaluator& eval, const Index outer)
+    {
+      const auto & outerIndices = constexpr_conditional<IsRowMajor>(
+          eval.m_xpr.rowIndices(), eval.m_xpr.colIndices());
+      const auto & innerIndices = constexpr_conditional<IsRowMajor>(
+          eval.m_xpr.colIndices(), eval.m_xpr.rowIndices());
+      using ArgIteratorType = typename evaluator<ArgType>::InnerIterator;
+      for (ArgIteratorType it(eval.m_argImpl, outerIndices[outer]); it; ++it) {
+        auto found = std::find(innerIndices.begin(), innerIndices.end(), it.index());
+        if (found == innerIndices.end()) { continue; }
+        const StorageIndex inner = std::distance(innerIndices.begin(), found);
+        const StorageIndex row = IsRowMajor ? outer : inner;
+        const StorageIndex col = IsRowMajor ? inner : outer;
+        m_triplets.emplace_back(row, col, it.value());
+      }
+      std::sort(
+          m_triplets.begin(), m_triplets.end(),
+          [](const TripletType& a, const TripletType& b) {
+            return IsRowMajor ? a.col() < b.col() : a.row() < b.row();
+          });
+      m_tripletsIter = m_triplets.begin();
+    }
+
+    InnerIterator& operator++()
+    {
+      ++m_tripletsIter;
+      return *this;
+    }
+
+    Scalar value() const { return m_tripletsIter->value(); }
+    StorageIndex index() const { return IsRowMajor? col() : row(); }
+    StorageIndex row() const { return m_tripletsIter->row(); }
+    StorageIndex col() const { return m_tripletsIter->col(); }
+
+    operator bool() const { return m_tripletsIter != m_triplets.end(); }
+
+   protected:
+    using TripletType = Triplet<Scalar, StorageIndex>;
+    std::vector<TripletType> m_triplets;
+    typename std::vector<TripletType>::iterator m_tripletsIter;
+  };
+
+  enum {
+    CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+
+    FlagsRowMajorBit = traits<XprType>::FlagsRowMajorBit,
+
+    Flags = evaluator<ArgType>::Flags & RowMajorBit,
+  };
+
+  explicit unary_evaluator(const XprType& xpr)
+      : m_argImpl(xpr.nestedExpression()), m_xpr(xpr)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+protected:
+  evaluator<ArgType> m_argImpl;
+  const XprType& m_xpr;
+};
+
+}  // namespace internal
+}  // namespace Eigen
+
+namespace karto
+{
+namespace contrib
+{
+
+/**
+ * Stacks two matrix expressions horizontally
+ * i.e. from left to right, column by column.
+ */
+template <typename LhsType, typename RhsType>
+Eigen::HorizontalStack<LhsType, RhsType>
+StackHorizontally(const LhsType& lhs, const RhsType& rhs)
+{
+  return Eigen::HorizontalStack<LhsType, RhsType>(lhs, rhs);
+}
+
+/**
+ * Stacks two matrix expressions vertically
+ * i.e. from left to right, row by row.
+ */
+template <typename LhsType, typename RhsType>
+Eigen::VerticalStack<LhsType, RhsType>
+StackVertically(const LhsType& lhs, const RhsType& rhs)
+{
+  return Eigen::VerticalStack<LhsType, RhsType>(lhs, rhs);
+}
+
+/**
+ * Arranges a view of a matrix expression defined by
+ * arbitrary sequences of row and column indices.
+ */
+template<typename XprType, typename RowIndices, typename ColIndices>
+Eigen::View<XprType, RowIndices, ColIndices>
+ArrangeView(XprType & xpr, const RowIndices & rowIndices, const ColIndices & colIndices)
+{
+  return Eigen::View<XprType, RowIndices, ColIndices>(xpr, rowIndices, colIndices);
+}
+
+/** Computes the inverse of a sparse matrix. */
+template <typename Scalar, int Options, typename StorageIndex,
+          typename SolverType = Eigen::SparseLU<
+            Eigen::SparseMatrix<Scalar, Options, StorageIndex>>>
+Eigen::SparseMatrix<Scalar, Options, StorageIndex>
+ComputeSparseInverse(const Eigen::SparseMatrix<Scalar, Options, StorageIndex>& matrix)
+{
+  using MatrixType = Eigen::SparseMatrix<Scalar, Options, StorageIndex>;
+
+  const int dimension = matrix.outerSize();
+  assert(dimension == matrix.innerSize());  // must be square
+  SolverType solver;
+  solver.compute(matrix);
+  MatrixType I(dimension, dimension);
+  I.setIdentity();
+  return solver.solve(I);
+}
+
+}  // namespace contrib
+}  // namespace karto
+
+#endif // KARTO_SDK__EIGENEXTENSIONS_H_
diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp
index 27c02930..9d0ef86c 100644
--- a/lib/karto_sdk/src/Mapper.cpp
+++ b/lib/karto_sdk/src/Mapper.cpp
@@ -20,6 +20,7 @@
 #include <math.h>
 #include <assert.h>
 #include <boost/serialization/vector.hpp>
+#include <Eigen/Core>
 #include <sstream>
 #include <fstream>
 #include <stdexcept>
@@ -28,12 +29,15 @@
 #include <list>
 #include <iterator>
 #include <map>
+#include <unordered_map>
 #include <vector>
 #include <utility>
 #include <algorithm>
 #include <string>
 
 #include "karto_sdk/Mapper.h"
+#include "karto_sdk/contrib/ChowLiuTreeApprox.h"
+#include "karto_sdk/contrib/EigenExtensions.h"
 
 BOOST_CLASS_EXPORT(karto::MapperGraph);
 BOOST_CLASS_EXPORT(karto::Graph<karto::LocalizedRangeScan>);
@@ -1616,6 +1620,40 @@ Edge<LocalizedRangeScan> * MapperGraph::AddEdge(
   return pEdge;
 }
 
+kt_bool MapperGraph::AddEdge(Edge<LocalizedRangeScan> * pEdge)
+{
+  using IteratorT = std::map<int, Vertex<LocalizedRangeScan> *>::iterator;
+
+  LocalizedRangeScan * pSourceScan = pEdge->GetSource()->GetObject();
+  LocalizedRangeScan * pTargetScan = pEdge->GetTarget()->GetObject();
+  IteratorT v1 = m_Vertices[pSourceScan->GetSensorName()]
+                 .find(pSourceScan->GetStateId());
+  IteratorT v2 = m_Vertices[pTargetScan->GetSensorName()]
+                 .find(pTargetScan->GetStateId());
+
+  if (v1 == m_Vertices[pSourceScan->GetSensorName()].end() ||
+    v2 == m_Vertices[pSourceScan->GetSensorName()].end())
+  {
+    std::cout << "AddEdge: At least one vertex is invalid." << std::endl;
+    return false;
+  }
+
+  // see if edge already exists
+  for (Edge<LocalizedRangeScan> * pOtherEdge : m_Edges) {
+    if (pEdge->GetSource() == pOtherEdge->GetSource() &&
+        pEdge->GetTarget() == pOtherEdge->GetTarget()) {
+      return false;
+    }
+    if (pEdge->GetTarget() == pOtherEdge->GetSource() &&
+        pEdge->GetSource() == pOtherEdge->GetTarget()) {
+      return false;
+    }
+  }
+
+  Graph<LocalizedRangeScan>::AddEdge(pEdge);
+  return true;
+}
+
 void MapperGraph::LinkScans(
   LocalizedRangeScan * pFromScan, LocalizedRangeScan * pToScan,
   const Pose2 & rMean, const Matrix3 & rCovariance)
@@ -2951,35 +2989,111 @@ void Mapper::ClearLocalizationBuffer()
   return;
 }
 
+kt_bool Mapper::MarginalizeNodeFromGraph(
+    Vertex<LocalizedRangeScan> * vertex_to_marginalize)
+{
+  // Marginalization is carried out as proposed in section 5 of:
+  //
+  //   Kretzschmar, Henrik, and Cyrill Stachniss. “Information-Theoretic
+  //   Compression of Pose Graphs for Laser-Based SLAM.” The International
+  //   Journal of Robotics Research, vol. 31, no. 11, Sept. 2012,
+  //   pp. 1219–1230, doi:10.1177/0278364912455072.
+
+  // (1) Fetch information matrix from solver.
+  std::unordered_map<int, Eigen::Index> ordering;
+  const Eigen::SparseMatrix<double> information_matrix =
+      m_pScanOptimizer->GetInformationMatrix(&ordering);
+  // (2) Marginalize variable from information matrix.
+  constexpr Eigen::Index block_size = 3;
+  auto block_index_of = [&](Vertex<LocalizedRangeScan> * vertex) {
+    return ordering[vertex->GetObject()->GetUniqueId()];
+  };
+  const Eigen::Index marginalized_block_index =
+      block_index_of(vertex_to_marginalize);
+  const Eigen::SparseMatrix<double> marginal_information_matrix =
+      contrib::ComputeMarginalInformationMatrix(
+          information_matrix, marginalized_block_index, block_size);
+  // (3) Compute marginal covariance *local* to the elimination clique
+  // i.e. by only inverting the relevant marginal information submatrix.
+  // This is an approximation for the sake of performance.
+  std::vector<Vertex<LocalizedRangeScan> *> elimination_clique =
+      vertex_to_marginalize->GetAdjacentVertices();
+  std::vector<Eigen::Index> elimination_clique_indices;  // need all indices
+  elimination_clique_indices.reserve(elimination_clique.size() * block_size);
+  for (Vertex<LocalizedRangeScan> * vertex : elimination_clique) {
+    Eigen::Index block_index = block_index_of(vertex);
+    if (block_index > marginalized_block_index) {
+      block_index -= block_size;
+    }
+    for (Eigen::Index offset = 0; offset < block_size; ++offset) {
+      elimination_clique_indices.push_back(block_index + offset);
+    }
+  }
+  const Eigen::SparseMatrix<double> local_marginal_covariance_matrix =
+      contrib::ComputeSparseInverse(
+          contrib::ArrangeView(marginal_information_matrix,
+                               elimination_clique_indices,
+                               elimination_clique_indices).eval());
+  // (4) Remove node for marginalized variable.
+  RemoveNodeFromGraph(vertex_to_marginalize);
+  // (5) Remove all edges in the subgraph induced by the elimination clique.
+  for (Vertex<LocalizedRangeScan> * vertex : elimination_clique) {
+    for (Edge<LocalizedRangeScan> * edge : vertex->GetEdges()) {
+      Vertex<LocalizedRangeScan> * other_vertex =
+          edge->GetSource() == vertex ?
+          edge->GetTarget() : edge->GetSource();
+      const auto it = std::find(
+          elimination_clique.begin(),
+          elimination_clique.end(),
+          other_vertex);
+      if (it != elimination_clique.end()) {
+        RemoveEdgeFromGraph(edge);
+      }
+    }
+  }
+  // (6) Compute Chow-Liu tree approximation to the elimination clique.
+  std::vector<Edge<LocalizedRangeScan> *> chow_liu_tree_approximation =
+      contrib::ComputeChowLiuTreeApproximation(
+          elimination_clique, local_marginal_covariance_matrix);
+  // (7) Push tree edges to graph and solver (as constraints).
+  for (Edge<LocalizedRangeScan> * edge : chow_liu_tree_approximation) {
+    assert(m_pGraph->AddEdge(edge));
+    m_pScanOptimizer->AddConstraint(edge);
+  }
+  return true;
+}
+
+kt_bool Mapper::RemoveEdgeFromGraph(Edge<LocalizedRangeScan> * edge_to_remove)
+{
+  Vertex<LocalizedRangeScan> * source = edge_to_remove->GetSource();
+  Vertex<LocalizedRangeScan> * target = edge_to_remove->GetTarget();
+  source->RemoveEdge(edge_to_remove);
+  target->RemoveEdge(edge_to_remove);
+  m_pScanOptimizer->RemoveConstraint(
+      source->GetObject()->GetUniqueId(),
+      target->GetObject()->GetUniqueId());
+  m_pGraph->RemoveEdge(edge_to_remove);
+  delete edge_to_remove;
+  return true;
+}
+
 kt_bool Mapper::RemoveNodeFromGraph(Vertex<LocalizedRangeScan> * vertex_to_remove)
 {
   // 1) delete edges in adjacent vertices, graph, and optimizer
-  std::vector<Vertex<LocalizedRangeScan> *> adjVerts =
-    vertex_to_remove->GetAdjacentVertices();
-  for (int i = 0; i != adjVerts.size(); i++) {
-    std::vector<Edge<LocalizedRangeScan> *> adjEdges = adjVerts[i]->GetEdges();
+  std::vector<Vertex<LocalizedRangeScan> *> vertices =
+      vertex_to_remove->GetAdjacentVertices();
+  for (Vertex<LocalizedRangeScan> * vertex : vertices) {
     bool found = false;
-    for (int j = 0; j != adjEdges.size(); j++) {
-      if (adjEdges[j]->GetTarget() == vertex_to_remove ||
-        adjEdges[j]->GetSource() == vertex_to_remove)
+    for (Edge<LocalizedRangeScan> * edge : vertex->GetEdges()) {
+      if (edge->GetTarget() == vertex_to_remove ||
+          edge->GetSource() == vertex_to_remove)
       {
-        adjVerts[i]->RemoveEdge(j);
+        vertex->RemoveEdge(edge);
         m_pScanOptimizer->RemoveConstraint(
-          adjEdges[j]->GetSource()->GetObject()->GetUniqueId(),
-          adjEdges[j]->GetTarget()->GetObject()->GetUniqueId());
-        std::vector<Edge<LocalizedRangeScan> *> edges = m_pGraph->GetEdges();
-        std::vector<Edge<LocalizedRangeScan> *>::iterator edgeGraphIt =
-          std::find(edges.begin(), edges.end(), adjEdges[j]);
-
-        if (edgeGraphIt == edges.end()) {
-          std::cout << "Edge not found in graph to remove!" << std::endl;
-          continue;
-        }
-
-        int posEdge = edgeGraphIt - edges.begin();
-        m_pGraph->RemoveEdge(posEdge);   // remove from graph
-        delete *edgeGraphIt;   // free hat!
-        *edgeGraphIt = NULL;
+          edge->GetSource()->GetObject()->GetUniqueId(),
+          edge->GetTarget()->GetObject()->GetUniqueId());
+        m_pGraph->RemoveEdge(edge);
+        delete edge;
         found = true;
       }
     }
diff --git a/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp b/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp
new file mode 100644
index 00000000..2924ee62
--- /dev/null
+++ b/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp
@@ -0,0 +1,214 @@
+#include "karto_sdk/contrib/ChowLiuTreeApprox.h"
+#include "karto_sdk/contrib/EigenExtensions.h"
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/kruskal_min_spanning_tree.hpp>
+
+namespace karto
+{
+namespace contrib
+{
+
+Eigen::SparseMatrix<double> ComputeMarginalInformationMatrix(
+    const Eigen::SparseMatrix<double> & information_matrix,
+    const Eigen::Index discarded_variable_index,
+    const Eigen::Index variables_dimension)
+{
+  const Eigen::Index dimension = information_matrix.outerSize();
+  assert(dimension == information_matrix.innerSize());  // must be square
+  const Eigen::Index marginal_dimension = dimension - variables_dimension;
+  const Eigen::Index last_variable_index = dimension - variables_dimension;
+  // (1) Break up information matrix based on which are the variables
+  // kept (a) and which is the variable discarded (b).
+  Eigen::SparseMatrix<double>
+      information_submatrix_aa, information_submatrix_ab,
+      information_submatrix_ba, information_submatrix_bb;
+  if (discarded_variable_index == 0) {
+    information_submatrix_aa =
+        information_matrix.bottomRightCorner(
+            marginal_dimension, marginal_dimension);
+    information_submatrix_ab =
+        information_matrix.bottomLeftCorner(
+            marginal_dimension, variables_dimension);
+    information_submatrix_ba =
+        information_matrix.topRightCorner(
+            variables_dimension, marginal_dimension);
+    information_submatrix_bb =
+        information_matrix.topLeftCorner(
+            variables_dimension, variables_dimension);
+  } else if (discarded_variable_index == last_variable_index) {
+    information_submatrix_aa =
+        information_matrix.topLeftCorner(
+            marginal_dimension, marginal_dimension);
+    information_submatrix_ab =
+        information_matrix.topRightCorner(
+            marginal_dimension, variables_dimension);
+    information_submatrix_ba =
+        information_matrix.bottomLeftCorner(
+            variables_dimension, marginal_dimension);
+    information_submatrix_bb =
+        information_matrix.bottomRightCorner(
+            variables_dimension, variables_dimension);
+  } else {
+    const Eigen::Index next_variable_index =
+        discarded_variable_index + variables_dimension;
+    information_submatrix_aa = StackVertically(
+        StackHorizontally(
+            information_matrix.topLeftCorner(
+                discarded_variable_index,
+                discarded_variable_index),
+            information_matrix.topRightCorner(
+                discarded_variable_index,
+                dimension - next_variable_index)),
+        StackHorizontally(
+            information_matrix.bottomLeftCorner(
+                dimension - next_variable_index,
+                discarded_variable_index),
+            information_matrix.bottomRightCorner(
+                dimension - next_variable_index,
+                dimension - next_variable_index)));
+    information_submatrix_ab = StackVertically(
+        information_matrix.block(
+            0,
+            discarded_variable_index,
+            discarded_variable_index,
+            variables_dimension),
+        information_matrix.block(
+            next_variable_index,
+            discarded_variable_index,
+            dimension - next_variable_index,
+            variables_dimension));
+    information_submatrix_ba = StackHorizontally(
+        information_matrix.block(
+            discarded_variable_index,
+            0,
+            variables_dimension,
+            discarded_variable_index),
+        information_matrix.block(
+            discarded_variable_index,
+            next_variable_index,
+            variables_dimension,
+            dimension - next_variable_index));
+    information_submatrix_bb =
+        information_matrix.block(
+            discarded_variable_index,
+            discarded_variable_index,
+            variables_dimension,
+            variables_dimension);
+  }
+
+  // (2) Compute Schur's complement over the variables that are kept.
+  return (information_submatrix_aa - information_submatrix_ba *
+          ComputeSparseInverse(information_submatrix_bb) *
+          information_submatrix_ab);
+}
+
+UncertainPose2 ComputeRelativePose2(
+    Pose2 source_pose, Pose2 target_pose,
+    Eigen::Matrix<double, 6, 6> joint_pose_covariance)
+{
+  // Computation is carried out as proposed in section 3.2 of:
+  //
+  //    R. Smith, M. Self and P. Cheeseman, "Estimating uncertain spatial
+  //    relationships in robotics," Proceedings. 1987 IEEE International
+  //    Conference on Robotics and Automation, 1987, pp. 850-850,
+  //    doi: 10.1109/ROBOT.1987.1087846.
+  //
+  // In particular, this is a case of tail-tail composition of two spatial
+  // relationships p_ij and p_ik as in: p_jk = ⊖ p_ij ⊕ p_ik
+  UncertainPose2 relative_pose;
+  // (1) Compute mean relative pose by simply
+  // transforming mean source and target poses.
+  Transform source_transform(source_pose);
+  relative_pose.mean =
+      source_transform.InverseTransformPose(target_pose);
+  // (2) Compute relative pose covariance by linearizing
+  // the transformation around mean source and target
+  // poses.
+  Eigen::Matrix<double, 3, 6> transform_jacobian;
+  const double x_jk = relative_pose.mean.GetX();
+  const double y_jk = relative_pose.mean.GetY();
+  const double theta_ij = source_pose.GetHeading();
+  transform_jacobian <<
+      -cos(theta_ij), -sin(theta_ij),  y_jk,  cos(theta_ij), sin(theta_ij), 0.0,
+       sin(theta_ij), -cos(theta_ij), -x_jk, -sin(theta_ij), cos(theta_ij), 0.0,
+                 0.0,            0.0,  -1.0,            0.0,           0.0, 1.0;
+  relative_pose.covariance = Matrix3(
+      transform_jacobian * joint_pose_covariance *
+      transform_jacobian.transpose());
+  return relative_pose;
+}
+
+std::vector<Edge<LocalizedRangeScan> *> ComputeChowLiuTreeApproximation(
+  const std::vector<Vertex<LocalizedRangeScan> *> & clique,
+  const Eigen::SparseMatrix<double> & covariance_matrix)
+{
+  // (1) Build clique subgraph, weighting edges by the *negated* mutual
+  // information between corresponding variables (so as to apply
+  // Kruskal's minimum spanning tree algorithm down below).
+  using WeightedGraphT = boost::adjacency_list<
+    boost::vecS, boost::vecS, boost::undirectedS, boost::no_property,
+    boost::property<boost::edge_weight_t, double>>;
+  WeightedGraphT clique_subgraph(clique.size());
+  for (size_t i = 0; i < clique.size() - 1; ++i) {
+    for (size_t j = i + 1; j < clique.size(); ++j) {
+      const auto covariance_submatrix_ii =
+          Eigen::Matrix3d{covariance_matrix.block(i, i, 3, 3)};
+      const auto covariance_submatrix_ij =
+          Eigen::Matrix3d{covariance_matrix.block(i, j, 3, 3)};
+      const auto covariance_submatrix_ji =
+          Eigen::Matrix3d{covariance_matrix.block(j, i, 3, 3)};
+      const auto covariance_submatrix_jj =
+          Eigen::Matrix3d{covariance_matrix.block(j, j, 3, 3)};
+      const double mutual_information =
+        0.5 * std::log2(covariance_submatrix_ii.determinant() / (
+            covariance_submatrix_ii - covariance_submatrix_ij *
+            covariance_submatrix_jj.inverse() *
+            covariance_submatrix_ji).determinant());
+      boost::add_edge(i, j, -mutual_information, clique_subgraph);
+    }
+  }
+  // (2) Find maximum mutual information spanning tree in the clique subgraph
+  // (which best approximates the underlying joint probability distribution as
+  // proved by Chow & Liu).
+  using EdgeDescriptorT =
+      boost::graph_traits<WeightedGraphT>::edge_descriptor;
+  std::vector<EdgeDescriptorT> minimum_spanning_tree_edges;
+  boost::kruskal_minimum_spanning_tree(
+      clique_subgraph, std::back_inserter(minimum_spanning_tree_edges));
+  using VertexDescriptorT =
+      boost::graph_traits<WeightedGraphT>::vertex_descriptor;
+  // (3) Build tree approximation as an edge list, using the mean and
+  // covariance of the marginal joint distribution between each variable
+  // to recompute the nonlinear constraint (i.e. a 2D isometry) between them.
+  std::vector<Edge<LocalizedRangeScan> *> chow_liu_tree_approximation;
+  for (const EdgeDescriptorT & edge_descriptor : minimum_spanning_tree_edges) {
+    const VertexDescriptorT i = boost::source(edge_descriptor, clique_subgraph);
+    const VertexDescriptorT j = boost::target(edge_descriptor, clique_subgraph);
+    auto * edge = new Edge<LocalizedRangeScan>(clique[i], clique[j]);
+    Eigen::Matrix<double, 6, 6> joint_pose_covariance_matrix;
+    joint_pose_covariance_matrix <<  // marginalized from the larger matrix
+        Eigen::Matrix3d{covariance_matrix.block(i, i, 3, 3)},
+        Eigen::Matrix3d{covariance_matrix.block(i, j, 3, 3)},
+        Eigen::Matrix3d{covariance_matrix.block(j, i, 3, 3)},
+        Eigen::Matrix3d{covariance_matrix.block(j, j, 3, 3)};
+    LocalizedRangeScan * source_scan = edge->GetSource()->GetObject();
+    LocalizedRangeScan * target_scan = edge->GetTarget()->GetObject();
+    const UncertainPose2 relative_pose =
+        ComputeRelativePose2(source_scan->GetCorrectedPose(),
+                             target_scan->GetCorrectedPose(),
+                             joint_pose_covariance_matrix);
+    edge->SetLabel(new LinkInfo(
+        source_scan->GetCorrectedPose(),
+        target_scan->GetCorrectedPose(),
+        relative_pose.mean, relative_pose.covariance));
+    chow_liu_tree_approximation.push_back(edge);
+  }
+  return chow_liu_tree_approximation;
+}
+
+}  // namespace contrib
+}  // namespace karto
diff --git a/solvers/ceres_solver.cpp b/solvers/ceres_solver.cpp
index 0e49352e..f9357c24 100644
--- a/solvers/ceres_solver.cpp
+++ b/solvers/ceres_solver.cpp
@@ -46,7 +46,9 @@ void CeresSolver::Configure(rclcpp::Node::SharedPtr node)
   first_node_ = nodes_->end();
 
   // formulate problem
-  angle_local_parameterization_ = AngleLocalParameterization::Create();
+  local_parameterization_ = new ceres::ProductParameterization(
+      new ceres::IdentityParameterization(2),
+      AngleLocalParameterization::Create());
 
   // choose loss function default squared loss (NULL)
   loss_function_ = NULL;
@@ -182,9 +184,7 @@ void CeresSolver::Compute()
       "CeresSolver: Setting first node as a constant pose:"
       "%0.2f, %0.2f, %0.2f.", first_node_->second(0),
       first_node_->second(1), first_node_->second(2));
-    problem_->SetParameterBlockConstant(&first_node_->second(0));
-    problem_->SetParameterBlockConstant(&first_node_->second(1));
-    problem_->SetParameterBlockConstant(&first_node_->second(2));
+    problem_->SetParameterBlockConstant(first_node_->second.data());
     was_constant_set_ = !was_constant_set_;
   }
 
@@ -222,6 +222,30 @@ const karto::ScanSolver::IdPoseVector & CeresSolver::GetCorrections() const
   return corrections_;
 }
 
+/*****************************************************************************/
+Eigen::SparseMatrix<double> CeresSolver::GetInformationMatrix(
+    std::unordered_map<int, Eigen::Index> * ordering) const
+/****************************************************************************/
+{
+  if (ordering) {
+    Eigen::Index index = 0u;
+    std::vector<double*> parameter_blocks;
+    problem_->GetParameterBlocks(&parameter_blocks);
+    for (auto * block : parameter_blocks) {
+      (*ordering)[(*nodes_inverted_)[block]] = index++;
+    }
+  }
+  ceres::CRSMatrix jacobian_data;
+  problem_->Evaluate(ceres::Problem::EvaluateOptions(),
+                     nullptr, nullptr, nullptr, &jacobian_data);
+  const Eigen::Index dimension = problem_->NumParameters();
+  Eigen::SparseMatrix<double> jacobian(dimension, dimension);
+  jacobian.setFromTriplets(
+      CRSMatrixIterator::begin(jacobian_data),
+      CRSMatrixIterator::end(jacobian_data));
+  return jacobian.transpose() * jacobian;
+}
+
 /*****************************************************************************/
 void CeresSolver::Clear()
 /*****************************************************************************/
@@ -242,6 +266,10 @@ void CeresSolver::Reset()
     delete problem_;
   }
 
+  if (nodes_inverted_) {
+    delete nodes_inverted_;
+  }
+
   if (nodes_) {
     delete nodes_;
   }
@@ -251,11 +279,14 @@ void CeresSolver::Reset()
   }
 
   nodes_ = new std::unordered_map<int, Eigen::Vector3d>();
+  nodes_inverted_ = new std::unordered_map<double *, int>();
   blocks_ = new std::unordered_map<std::size_t, ceres::ResidualBlockId>();
   problem_ = new ceres::Problem(options_problem_);
   first_node_ = nodes_->end();
 
-  angle_local_parameterization_ = AngleLocalParameterization::Create();
+  local_parameterization_ = new ceres::ProductParameterization(
+      new ceres::IdentityParameterization(2),
+      AngleLocalParameterization::Create());
 }
 
 /*****************************************************************************/
@@ -267,13 +298,16 @@ void CeresSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan> * pVertex)
     return;
   }
 
-  karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
-  Eigen::Vector3d pose2d(pose.GetX(), pose.GetY(), pose.GetHeading());
+  const karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
+  const Eigen::Vector3d pose2d(pose.GetX(), pose.GetY(), pose.GetHeading());
 
   const int id = pVertex->GetObject()->GetUniqueId();
 
   boost::mutex::scoped_lock lock(nodes_mutex_);
-  nodes_->insert(std::pair<int, Eigen::Vector3d>(id, pose2d));
+
+  nodes_->insert(std::make_pair(id, pose2d));
+  nodes_inverted_->insert(
+    std::make_pair((*nodes_)[id].data(), id));
 
   if (nodes_->size() == 1) {
     first_node_ = nodes_->find(id);
@@ -307,31 +341,20 @@ void CeresSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> * pEdge)
   // extract transformation
   karto::LinkInfo * pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel());
   karto::Pose2 diff = pLinkInfo->GetPoseDifference();
-  Eigen::Vector3d pose2d(diff.GetX(), diff.GetY(), diff.GetHeading());
-
-  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
-  Eigen::Matrix3d sqrt_information;
-  sqrt_information(0, 0) = precisionMatrix(0, 0);
-  sqrt_information(0, 1) = sqrt_information(1, 0) = precisionMatrix(0, 1);
-  sqrt_information(0, 2) = sqrt_information(2, 0) = precisionMatrix(0, 2);
-  sqrt_information(1, 1) = precisionMatrix(1, 1);
-  sqrt_information(1, 2) = sqrt_information(2, 1) = precisionMatrix(1, 2);
-  sqrt_information(2, 2) = precisionMatrix(2, 2);
-
   // populate residual and parameterization for heading normalization
-  ceres::CostFunction * cost_function = PoseGraph2dErrorTerm::Create(pose2d(0),
-      pose2d(1), pose2d(2), sqrt_information);
+  Eigen::Matrix3d sqrt_information =
+      pLinkInfo->GetCovariance().Inverse().ToEigen().llt().matrixL();
+  ceres::CostFunction * cost_function = PoseGraph2dErrorTerm::Create(
+      diff.GetX(), diff.GetY(), diff.GetHeading(), sqrt_information);
   ceres::ResidualBlockId block = problem_->AddResidualBlock(
     cost_function, loss_function_,
-    &node1it->second(0), &node1it->second(1), &node1it->second(2),
-    &node2it->second(0), &node2it->second(1), &node2it->second(2));
-  problem_->SetParameterization(&node1it->second(2),
-    angle_local_parameterization_);
-  problem_->SetParameterization(&node2it->second(2),
-    angle_local_parameterization_);
-
-  blocks_->insert(std::pair<std::size_t, ceres::ResidualBlockId>(
-      GetHash(node1, node2), block));
+    node1it->second.data(), node2it->second.data());
+  problem_->SetParameterization(
+    node1it->second.data(), local_parameterization_);
+  problem_->SetParameterization(
+    node1it->second.data(), local_parameterization_);
+
+  blocks_->insert(std::make_pair(GetHash(node1, node2), block));
 }
 
 /*****************************************************************************/
@@ -341,6 +364,8 @@ void CeresSolver::RemoveNode(kt_int32s id)
   boost::mutex::scoped_lock lock(nodes_mutex_);
   GraphIterator nodeit = nodes_->find(id);
   if (nodeit != nodes_->end()) {
+    problem_->RemoveParameterBlock(nodeit->second.data());
+    nodes_inverted_->erase(nodes_inverted_->find(nodeit->second.data()));
     nodes_->erase(nodeit);
   } else {
     RCLCPP_ERROR(node_->get_logger(), "RemoveNode: Failed to find node matching id %i",
@@ -395,10 +420,10 @@ void CeresSolver::GetNodeOrientation(const int & unique_id, double & pose)
 }
 
 /*****************************************************************************/
-std::unordered_map<int, Eigen::Vector3d> * CeresSolver::getGraph()
+const std::unordered_map<int, Eigen::Vector3d> * CeresSolver::getGraph()
 /*****************************************************************************/
 {
-  boost::mutex::scoped_lock lock(nodes_mutex_);
+  boost::mutex::scoped_lock lock(nodes_mutex_);  // useless?
   return nodes_;
 }
 
diff --git a/solvers/ceres_solver.hpp b/solvers/ceres_solver.hpp
index b2757414..c76c741b 100644
--- a/solvers/ceres_solver.hpp
+++ b/solvers/ceres_solver.hpp
@@ -46,7 +46,10 @@ class CeresSolver : public karto::ScanSolver
   // Adds a constraint to the solver
   virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan> * pEdge);
   // Get graph stored
-  virtual std::unordered_map<int, Eigen::Vector3d> * getGraph();
+  virtual const std::unordered_map<int, Eigen::Vector3d> * getGraph();
+  // Get information matrix associated with the graph
+  virtual Eigen::SparseMatrix<double> GetInformationMatrix(
+      std::unordered_map<int, Eigen::Index> * ordering) const;
   // Removes a node from the solver correction table
   virtual void RemoveNode(kt_int32s id);
   // Removes constraints from the optimization problem
@@ -66,11 +69,12 @@ class CeresSolver : public karto::ScanSolver
   ceres::Problem::Options options_problem_;
   ceres::LossFunction * loss_function_;
   ceres::Problem * problem_;
-  ceres::LocalParameterization * angle_local_parameterization_;
+  ceres::LocalParameterization * local_parameterization_;
   bool was_constant_set_, debug_logging_;
 
   // graph
   std::unordered_map<int, Eigen::Vector3d> * nodes_;
+  std::unordered_map<double *, int> * nodes_inverted_;
   std::unordered_map<size_t, ceres::ResidualBlockId> * blocks_;
   std::unordered_map<int, Eigen::Vector3d>::iterator first_node_;
   boost::mutex nodes_mutex_;
diff --git a/solvers/ceres_utils.h b/solvers/ceres_utils.h
index 42c2fad8..24eb396a 100644
--- a/solvers/ceres_utils.h
+++ b/solvers/ceres_utils.h
@@ -9,8 +9,11 @@
 #include <ceres/ceres.h>
 #include <ceres/local_parameterization.h>
 #include <cmath>
+#include <iterator>
 #include <utility>
 
+#include <Eigen/SparseCore>
+
 /*****************************************************************************/
 /*****************************************************************************/
 /*****************************************************************************/
@@ -23,6 +26,86 @@ inline std::size_t GetHash(const int & x, const int & y)
 /*****************************************************************************/
 /*****************************************************************************/
 
+class CRSMatrixIterator : public std::iterator<
+  std::input_iterator_tag, Eigen::Triplet<double>>
+{
+public:
+  static CRSMatrixIterator begin(const ceres::CRSMatrix & matrix)
+  {
+    return CRSMatrixIterator(matrix);
+  }
+
+  static CRSMatrixIterator end(const ceres::CRSMatrix & matrix)
+  {
+    return CRSMatrixIterator(matrix, matrix.num_rows + 1);
+  }
+
+  CRSMatrixIterator& operator++()
+  {
+    if (++data_index_ == matrix_.rows[row_index_ + 1])
+    {
+      ++row_index_;
+    }
+    current_triplet_ = MakeTriplet();
+    return *this;
+  }
+
+  CRSMatrixIterator operator++(int)
+  {
+    CRSMatrixIterator it = *this;
+    ++(*this);
+    return it;
+  }
+
+  bool operator==(const CRSMatrixIterator & other) const
+  {
+    return &matrix_ == &other.matrix_ &&
+        row_index_ == other.row_index_ &&
+        data_index_ == other.data_index_;
+  }
+
+  bool operator!=(const CRSMatrixIterator & other) const
+  {
+    return !(*this == other);
+  }
+
+  pointer operator->() {
+    return &current_triplet_;
+  }
+
+  reference operator*() {
+    return current_triplet_;
+  }
+
+ private:
+  explicit CRSMatrixIterator(
+      const ceres::CRSMatrix & matrix,
+      size_t row_index = 0)
+  : matrix_(matrix),
+    row_index_(row_index),
+    data_index_(matrix.rows[row_index])
+  {
+    current_triplet_ = MakeTriplet();
+  }
+
+  Eigen::Triplet<double> MakeTriplet() const
+  {
+    return Eigen::Triplet<double>(
+      row_index_, matrix_.cols[data_index_],
+      matrix_.values[data_index_]);
+  }
+
+  const ceres::CRSMatrix & matrix_;
+  size_t row_index_;
+  size_t data_index_;
+
+  Eigen::Triplet<double> current_triplet_;
+};
+
+/*****************************************************************************/
+/*****************************************************************************/
+/*****************************************************************************/
+
 // Normalizes the angle in radians between [-pi and pi).
 template<typename T>
 inline T NormalizeAngle(const T & angle_radians)
@@ -75,38 +158,37 @@ class PoseGraph2dErrorTerm
 {
 public:
   PoseGraph2dErrorTerm(
-    double x_ab, double y_ab, double yaw_ab_radians,
-    const Eigen::Matrix3d & sqrt_information)
-  : p_ab_(x_ab, y_ab), yaw_ab_radians_(yaw_ab_radians), sqrt_information_(sqrt_information)
+      double x_ab, double y_ab, double yaw_ab,
+      const Eigen::Matrix3d & sqrt_information)
+  : p_ab_(x_ab, y_ab), yaw_ab_(yaw_ab), sqrt_information_(sqrt_information)
   {
   }
 
   template<typename T>
-  bool operator()(
-    const T * const x_a, const T * const y_a, const T * const yaw_a,
-    const T * const x_b, const T * const y_b, const T * const yaw_b,
-    T * residuals_ptr) const
+  bool operator()(const T * pose_a, const T * pose_b, T * residuals_ptr) const
   {
-    const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
-    const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
-    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
-    residuals_map.template head<2>() = RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) -
-      p_ab_.cast<T>();
-    residuals_map(2) = NormalizeAngle((*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
+    const Eigen::Matrix<T, 2, 1> p_a(pose_a);
+    const Eigen::Matrix<T, 2, 1> p_b(pose_b);
+    const T yaw_a = pose_a[2];
+    const T yaw_b = pose_b[2];
+
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals(residuals_ptr);
+
+    residuals.template head<2>() =
+        RotationMatrix2D(yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
+    residuals(2) = NormalizeAngle((yaw_b - yaw_a) - static_cast<T>(yaw_ab_));
     // Scale the residuals by the square root information
     // matrix to account for the measurement uncertainty.
-    residuals_map = sqrt_information_.template cast<T>() * residuals_map;
+    residuals = sqrt_information_.template cast<T>() * residuals;
     return true;
   }
 
   static ceres::CostFunction * Create(
-    double x_ab, double y_ab, double yaw_ab_radians,
+    double x_ab, double y_ab, double yaw_ab,
     const Eigen::Matrix3d & sqrt_information)
   {
-    return new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
-      new PoseGraph2dErrorTerm(
-        x_ab, y_ab, yaw_ab_radians,
-        sqrt_information));
+    return new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 3, 3>(
+      new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab, sqrt_information));
   }
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -115,7 +197,7 @@ class PoseGraph2dErrorTerm
   // The position of B relative to A in the A frame.
   const Eigen::Vector2d p_ab_;
   // The orientation of frame B relative to frame A.
-  const double yaw_ab_radians_;
+  const double yaw_ab_;
   // The inverse square root of the measurement covariance matrix.
   const Eigen::Matrix3d sqrt_information_;
 };
diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp
index a56dc59c..d4355056 100644
--- a/src/experimental/slam_toolbox_lifelong.cpp
+++ b/src/experimental/slam_toolbox_lifelong.cpp
@@ -298,14 +298,12 @@ void LifelongSlamToolbox::removeFromSlamGraph(
   Vertex<LocalizedRangeScan> * vertex)
 /*****************************************************************************/
 {
-  smapper_->getMapper()->RemoveNodeFromGraph(vertex);
+  smapper_->getMapper()->MarginalizeNodeFromGraph(vertex);
   smapper_->getMapper()->GetMapperSensorManager()->RemoveScan(
     vertex->GetObject());
   dataset_->RemoveData(vertex->GetObject());
   vertex->RemoveObject();
   delete vertex;
-  vertex = nullptr;
-  // LTS what do we do about the contraints that node had about it?Nothing?Transfer?
 }
 
 /*****************************************************************************/
diff --git a/src/loop_closure_assistant.cpp b/src/loop_closure_assistant.cpp
index f606a7dd..4de4cd86 100644
--- a/src/loop_closure_assistant.cpp
+++ b/src/loop_closure_assistant.cpp
@@ -144,7 +144,7 @@ void LoopClosureAssistant::publishGraph()
 /*****************************************************************************/
 {
   interactive_server_->clear();
-  std::unordered_map<int, Eigen::Vector3d> * graph = solver_->getGraph();
+  const std::unordered_map<int, Eigen::Vector3d> * graph = solver_->getGraph();
 
   if (graph->size() == 0) {
     return;
@@ -161,7 +161,7 @@ void LoopClosureAssistant::publishGraph()
   visualization_msgs::msg::Marker m = vis_utils::toMarker(map_frame_,
       "slam_toolbox", 0.1, node_);
 
-  for (ConstGraphIterator it = graph->begin(); it != graph->end(); ++it) {
+  for (ConstGraphIterator it = graph->cbegin(); it != graph->cend(); ++it) {
     m.id = it->first + 1;
     m.pose.position.x = it->second(0);
     m.pose.position.y = it->second(1);