From 54111364b5090f15b29d6c7be0f7a71d830456e7 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 13 Jan 2022 12:35:42 -0300 Subject: [PATCH 1/6] Add node marginalization support. Based on a Chow-Liu tree approximation to Karto's graph upon marginalization, using Karto's solver estimate of the full joint information matrix to build new nonlinear constraints. Signed-off-by: Michel Hidalgo --- .../config/mapper_params_lifelong.yaml | 1 + .../experimental/slam_toolbox_lifelong.hpp | 1 + slam_toolbox/lib/karto_sdk/Authors | 3 +- slam_toolbox/lib/karto_sdk/CMakeLists.txt | 9 +- .../lib/karto_sdk/include/karto_sdk/Karto.h | 24 + .../lib/karto_sdk/include/karto_sdk/Mapper.h | 64 +- .../karto_sdk/contrib/ChowLiuTreeApprox.h | 54 ++ .../karto_sdk/contrib/EigenExtensions.h | 717 ++++++++++++++++++ slam_toolbox/lib/karto_sdk/src/Mapper.cpp | 165 +++- .../src/contrib/ChowLiuTreeApprox.cpp | 249 ++++++ .../test/chow_liu_tree_approx_test.cpp | 86 +++ .../karto_sdk/test/eigen_extensions_test.cpp | 139 ++++ slam_toolbox/solvers/ceres_solver.cpp | 88 ++- slam_toolbox/solvers/ceres_solver.hpp | 5 +- slam_toolbox/solvers/ceres_utils.h | 106 ++- .../experimental/slam_toolbox_lifelong.cpp | 11 +- 16 files changed, 1652 insertions(+), 70 deletions(-) create mode 100644 slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h create mode 100644 slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h create mode 100644 slam_toolbox/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp create mode 100644 slam_toolbox/lib/karto_sdk/test/chow_liu_tree_approx_test.cpp create mode 100644 slam_toolbox/lib/karto_sdk/test/eigen_extensions_test.cpp diff --git a/slam_toolbox/config/mapper_params_lifelong.yaml b/slam_toolbox/config/mapper_params_lifelong.yaml index 29986882f..9c2851f40 100644 --- a/slam_toolbox/config/mapper_params_lifelong.yaml +++ b/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 diff --git a/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 5d4c458dd..3a435cea7 100644 --- a/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/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_; }; } diff --git a/slam_toolbox/lib/karto_sdk/Authors b/slam_toolbox/lib/karto_sdk/Authors index 380058050..008bf0ae6 100644 --- a/slam_toolbox/lib/karto_sdk/Authors +++ b/slam_toolbox/lib/karto_sdk/Authors @@ -4,4 +4,5 @@ Contributors: ------------------------------- Michael A. Eriksen (eriksen@ai.sri.com) Benson Limketkai (bensonl@ai.sri.com) -Steven Macenski (steven.macenski@simberobotics.com) \ No newline at end of file +Steven Macenski (steven.macenski@simberobotics.com) +Michel Hidalgo (michel@ekumenlabs.com) diff --git a/slam_toolbox/lib/karto_sdk/CMakeLists.txt b/slam_toolbox/lib/karto_sdk/CMakeLists.txt index ad6ef0248..5b8b7f976 100644 --- a/slam_toolbox/lib/karto_sdk/CMakeLists.txt +++ b/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() diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Karto.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Karto.h index c4c4f270b..8e1022686 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Karto.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Karto.h @@ -50,6 +50,8 @@ #include #include +#include + #ifdef USE_POCO #include #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 diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h index 2a439c6fc..2e9a4e770 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -24,6 +24,7 @@ #include #include +#include #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 * 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* 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 * edge); + /** * Link scan to last scan and nearby chains of scans * @param pScan @@ -1029,6 +1078,12 @@ namespace karto return nullptr; } + /** + * Get information matrix associated with the graph + */ + virtual Eigen::SparseMatrix GetInformationMatrix( + std::unordered_map * /* 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*); + kt_bool MarginalizeNodeFromGraph(Vertex*); void AddScanToLocalizationBuffer(LocalizedRangeScan* pScan, Vertex* scan_vertex); void ClearLocalizationBuffer(); @@ -2085,6 +2141,8 @@ namespace karto */ kt_bool HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const; + kt_bool RemoveEdgeFromGraph(Edge *); + public: ///////////////////////////////////////////// // fire information for listeners!! diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h new file mode 100644 index 000000000..4a8d9030f --- /dev/null +++ b/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 3 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 . + */ + +#ifndef KARTO_SDK__CHOW_LIU_TREE_APPROX_H_ +#define KARTO_SDK__CHOW_LIU_TREE_APPROX_H_ + +#include + +#include +#include + +#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 ComputeMarginalInformationMatrix( + const Eigen::SparseMatrix & 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 *> ComputeChowLiuTreeApproximation( + const std::vector *> & clique, + const Eigen::MatrixXd & covariance_matrix); + + } // namespace contrib +} // namespace karto + +#endif // KARTO_SDK__CHOW_LIU_TREE_APPROX_H_ diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h new file mode 100644 index 000000000..3414e70a4 --- /dev/null +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h @@ -0,0 +1,717 @@ +/* + * 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 3 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 . + */ + +#ifndef KARTO_SDK__EIGEN_EXTENSIONS_H_ +#define KARTO_SDK__EIGEN_EXTENSIONS_H_ + +#include +#include +#include +#include + +namespace Eigen { + +namespace internal { + +// Returns (a + b) or Dynamic if either operand is. +template +inline constexpr int size_sum_prefer_dynamic(A a, B b) { + static_assert( + std::is_enum::value || std::is_integral::value, + "Argument a must be an integer or enum"); + static_assert( + std::is_enum::value || std::is_integral::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 +inline constexpr int max_size_prefer_dynamic(A a, B b) { + static_assert( + std::is_enum::value || std::is_integral::value, + "Argument a must be an integer or enum"); + static_assert( + std::is_enum::value || std::is_integral::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 +struct constexpr_conditional_impl; + +template +struct constexpr_conditional_impl { + constexpr_conditional_impl(ThenT&& some_value, ElseT&&) + : value(some_value) + { + } + + ThenT value; +}; + +template +struct constexpr_conditional_impl { + 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 +inline constexpr auto +constexpr_conditional(ThenT&& some_value, ElseT&& other_value) +{ + return constexpr_conditional_impl{ + std::forward(some_value), + std::forward(other_value)}.value; +} + +} // namespace internal + +// Forward declaration. +template +class HorizontalStack; + +// Forward declaration. +template +class VerticalStack; + +// Forward declaration. +template +class View; + +namespace internal { + +template +struct promote_storage_kind; + +template +struct promote_storage_kind { using type = Sparse; }; + +template +struct promote_storage_kind { using type = Sparse; }; + +template <> +struct promote_storage_kind { using type = Sparse; }; + +template +struct promote_scalar { + static_assert( + std::is_convertible::value || + std::is_convertible::value, + "Scalar types are incommensurable"); + + using type = typename std::conditional< + std::is_convertible::value, B, A>::type; +}; + +template +struct traits> +{ + using XprKind = typename traits::XprKind; + using StorageKind = typename promote_storage_kind< + typename traits::StorageKind, + typename traits::StorageKind>::type; + using StorageIndex = typename promote_index_type< + typename traits::StorageIndex, + typename traits::StorageIndex>::type; + using Scalar = typename promote_scalar< + typename traits::Scalar, + typename traits::Scalar>::type; + enum { + RowsAtCompileTime = ( + traits::RowsAtCompileTime == Dynamic ? + traits::RowsAtCompileTime : + traits::RowsAtCompileTime), + ColsAtCompileTime = internal::size_sum_prefer_dynamic( + traits::ColsAtCompileTime, + traits::ColsAtCompileTime), + MaxRowsAtCompileTime = internal::max_size_prefer_dynamic( + traits::MaxRowsAtCompileTime, + traits::MaxRowsAtCompileTime), + MaxColsAtCompileTime = internal::size_sum_prefer_dynamic( + traits::MaxColsAtCompileTime, + traits::MaxColsAtCompileTime), + Flags = int(traits::Flags) & RowMajorBit + }; +}; + +template +struct traits> +{ + using XprKind = typename traits::XprKind; + using StorageKind = typename promote_storage_kind< + typename traits::StorageKind, + typename traits::StorageKind>::type; + using StorageIndex = typename promote_index_type< + typename traits::StorageIndex, + typename traits::StorageIndex>::type; + using Scalar = typename promote_scalar< + typename traits::Scalar, + typename traits::Scalar>::type; + enum { + RowsAtCompileTime = internal::size_sum_prefer_dynamic( + traits::RowsAtCompileTime, + traits::RowsAtCompileTime), + ColsAtCompileTime = ( + traits::ColsAtCompileTime == Dynamic ? + traits::ColsAtCompileTime : + traits::ColsAtCompileTime), + MaxRowsAtCompileTime = internal::size_sum_prefer_dynamic( + traits::MaxRowsAtCompileTime, + traits::MaxRowsAtCompileTime), + MaxColsAtCompileTime = internal::max_size_prefer_dynamic( + traits::MaxColsAtCompileTime, + traits::MaxColsAtCompileTime), + Flags = int(traits::Flags) & RowMajorBit + }; +}; + +template +struct traits> : traits +{ + enum { + RowsAtCompileTime = Dynamic, + ColsAtCompileTime = Dynamic, + MaxRowsAtCompileTime = RowsAtCompileTime, + MaxColsAtCompileTime = ColsAtCompileTime, + IsRowMajor = (int(traits::Flags) & RowMajorBit) != 0, + FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0, + Flags = int(traits::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 +class HorizontalStack : public internal::generic_xpr_base< + HorizontalStack>::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::Flags & RowMajorBit) == + (internal::traits::Flags & RowMajorBit))), + THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH) + + using Lhs = typename internal::remove_all::type; + using Rhs = typename internal::remove_all::type; + using Base = typename internal::generic_xpr_base< + HorizontalStack>::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::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::type; + using RhsTypeNested = typename internal::ref_selector::type; + using LhsTypeNestedNoRef = + typename internal::remove_reference::type; + using RhsTypeNestedNoRef = + typename internal::remove_reference::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 +class VerticalStack : public internal::generic_xpr_base< + VerticalStack>::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::Flags & RowMajorBit) == + (internal::traits::Flags & RowMajorBit))), + THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH) + + using Lhs = typename internal::remove_all::type; + using Rhs = typename internal::remove_all::type; + using Base = typename internal::generic_xpr_base< + VerticalStack>::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::ColsAtCompileTime; + return LhsColsAtCompileTime == Dynamic ? m_rhs.cols() : m_lhs.cols(); + } + + using LhsTypeNested = typename internal::ref_selector::type; + using RhsTypeNested = typename internal::ref_selector::type; + using LhsTypeNestedNoRef = + typename internal::remove_reference::type; + using RhsTypeNestedNoRef = + typename internal::remove_reference::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 +class View : public internal::generic_xpr_base< + View>::type, internal::no_assignment_operator +{ + public: + using Base = typename internal::generic_xpr_base< + View>::type; + using NestedExpression = typename internal::remove_all::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::type& + nestedExpression() const { return m_xpr; } + + typename internal::remove_reference::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::non_const_type; + XprTypeNested m_xpr; + RowIndices m_rowIndices; + ColIndices m_colIndices; +}; + +namespace internal { + +template +struct evaluator> + : public binary_evaluator> +{ + using XprType = HorizontalStack; + using Base = binary_evaluator>; + + explicit evaluator(const XprType& xpr) : Base(xpr) {} +}; + +template +struct binary_evaluator< + HorizontalStack, IteratorBased, IteratorBased +> : public evaluator_base> +{ + using XprType = HorizontalStack; + using LhsIteratorType = typename evaluator::InnerIterator; + using RhsIteratorType = typename evaluator::InnerIterator; + using StorageIndex = typename traits::StorageIndex; + using Scalar = typename traits::Scalar; + + public: + class InnerIterator + { + enum { + IsRowMajor = int(traits::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::CoeffReadCost) + + int(evaluator::CoeffReadCost)), + Flags = int(evaluator::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 m_lhsEval; + evaluator m_rhsEval; + StorageIndex m_lhsCols; +}; + +template +struct evaluator> + : public binary_evaluator> +{ + using XprType = VerticalStack; + using Base = binary_evaluator>; + + explicit evaluator(const XprType& xpr) : Base(xpr) {} +}; + +template +struct binary_evaluator< + VerticalStack, IteratorBased, IteratorBased +> : public evaluator_base> +{ + using XprType = VerticalStack; + using LhsIteratorType = typename evaluator::InnerIterator; + using RhsIteratorType = typename evaluator::InnerIterator; + using StorageIndex = typename traits::StorageIndex; + using Scalar = typename traits::Scalar; + + public: + class InnerIterator + { + enum { + IsRowMajor = int(traits::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::CoeffReadCost) + + int(evaluator::CoeffReadCost)), + Flags = int(evaluator::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 m_lhsEval; + evaluator m_rhsEval; + StorageIndex m_lhsRows; +}; + +template +struct unary_evaluator, IteratorBased> + : evaluator_base> +{ + using XprType = View; + + class InnerIterator + { + enum { + IsRowMajor = traits::IsRowMajor + }; + public: + using Scalar = typename traits::Scalar; + using StorageIndex = typename traits::StorageIndex; + + InnerIterator(const unary_evaluator& eval, const Index outer) + { + const auto & outerIndices = constexpr_conditional( + eval.m_xpr.rowIndices(), eval.m_xpr.colIndices()); + const auto & innerIndices = constexpr_conditional( + eval.m_xpr.colIndices(), eval.m_xpr.rowIndices()); + using ArgIteratorType = typename evaluator::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; + std::vector m_triplets; + typename std::vector::iterator m_tripletsIter; + }; + + enum { + CoeffReadCost = evaluator::CoeffReadCost, + + FlagsRowMajorBit = traits::FlagsRowMajorBit, + + Flags = evaluator::Flags & RowMajorBit, + }; + + explicit unary_evaluator(const XprType& xpr) + : m_argImpl(xpr.nestedExpression()), m_xpr(xpr) + { + EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); + } + +protected: + evaluator 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 +Eigen::HorizontalStack +StackHorizontally(const LhsType& lhs, const RhsType& rhs) +{ + return Eigen::HorizontalStack(lhs, rhs); +} + +/** + * Stacks two matrix expressions vertically + * i.e. from left to right, row by row. + */ +template +Eigen::VerticalStack +StackVertically(const LhsType& lhs, const RhsType& rhs) +{ + return Eigen::VerticalStack(lhs, rhs); +} + +/** + * Arranges a view of a matrix expression defined by + * arbitrary sequences of row and column indices. + */ +template< + typename XprType, + typename RowIndices = std::vector< + typename Eigen::internal::traits::StorageIndex>, + typename ColIndices = std::vector< + typename Eigen::internal::traits::StorageIndex>> +Eigen::View +ArrangeView(XprType & xpr, const RowIndices & rowIndices, const ColIndices & colIndices) +{ + return Eigen::View(xpr, rowIndices, colIndices); +} + +/** Computes the Moore-Penrose pseudoinverse of a dense matrix. */ +template +auto ComputeGeneralizedInverse(const Eigen::MatrixBase& matrix) +{ + auto svd = matrix.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); + auto values = svd.singularValues(); + for (Eigen::Index i = 0; i < values.size(); ++i) { + if (values(i) > 0.) { values(i) = 1. / values(i); } + } + auto sigma_generalized_inverse = Eigen::DiagonalWrapper{values}; + auto matrix_generalized_inverse = + svd.matrixV() * sigma_generalized_inverse * svd.matrixU().transpose(); + return matrix_generalized_inverse.eval(); +} + +/** Computes the Moore-Penrose pseudoinverse of a sparse matrix. */ +template +auto ComputeGeneralizedInverse(const Eigen::SparseMatrixBase& matrix) +{ + return ComputeGeneralizedInverse(matrix.toDense()); +} + + +} // namespace contrib +} // namespace karto + +#endif // KARTO_SDK__EIGEN_EXTENSIONS_H_ diff --git a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp index 74662169a..84aaab301 100644 --- a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp +++ b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp @@ -27,7 +27,12 @@ #include #include +#include +#include + #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); @@ -1659,6 +1664,40 @@ namespace karto return pEdge; } + kt_bool MapperGraph::AddEdge(Edge * pEdge) + { + using IteratorT = std::map *>::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 * pOtherEdge : GetEdges()) { + if (pEdge->GetSource() == pOtherEdge->GetSource() && + pEdge->GetTarget() == pOtherEdge->GetTarget()) { + return false; + } + if (pEdge->GetTarget() == pOtherEdge->GetSource() && + pEdge->GetSource() == pOtherEdge->GetTarget()) { + return false; + } + } + + Graph::AddEdge(pEdge); + return true; + } + void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan, const Pose2& rMean, const Matrix3& rCovariance) { @@ -3017,38 +3056,112 @@ namespace karto return; } + kt_bool Mapper::MarginalizeNodeFromGraph( + Vertex * 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 ordering; + const Eigen::SparseMatrix information_matrix = + m_pScanOptimizer->GetInformationMatrix(&ordering); + // (2) Marginalize variable from information matrix. + constexpr Eigen::Index block_size = 3; + auto block_index_of = [&](Vertex * vertex) { + return ordering[vertex->GetObject()->GetUniqueId()]; + }; + const Eigen::Index marginalized_block_index = + block_index_of(vertex_to_marginalize); + const Eigen::SparseMatrix 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 *> elimination_clique = + vertex_to_marginalize->GetAdjacentVertices(); + std::vector elimination_clique_indices; // need all indices + elimination_clique_indices.reserve(elimination_clique.size() * block_size); + for (Vertex * vertex : elimination_clique) { + Eigen::Index block_index = block_index_of(vertex); + if (block_index > marginalized_block_index) { + block_index -= block_size; // adjust for block removed + } + for (Eigen::Index offset = 0; offset < block_size; ++offset) { + elimination_clique_indices.push_back(block_index + offset); + } + } + const Eigen::MatrixXd local_marginal_covariance_matrix = + contrib::ComputeGeneralizedInverse( + contrib::ArrangeView(marginal_information_matrix, + elimination_clique_indices, + elimination_clique_indices)); + // (4) Remove node for marginalized variable. + RemoveNodeFromGraph(vertex_to_marginalize); + // (5) Remove all edges in the subgraph induced by the elimination clique. + for (Vertex * vertex : elimination_clique) { + for (Edge * edge : vertex->GetEdges()) { + Vertex * 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 *> chow_liu_tree_approximation = + contrib::ComputeChowLiuTreeApproximation( + elimination_clique, local_marginal_covariance_matrix); + // (7) Push tree edges to graph and solver (as constraints). + for (Edge * edge : chow_liu_tree_approximation) { + bool edge_added = m_pGraph->AddEdge(edge); + assert(edge_added); // otherwise internal logic is broken + m_pScanOptimizer->AddConstraint(edge); + } + return true; + } + + kt_bool Mapper::RemoveEdgeFromGraph(Edge * edge_to_remove) + { + Vertex * source = edge_to_remove->GetSource(); + Vertex * 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* vertex_to_remove) { // 1) delete edges in adjacent vertices, graph, and optimizer - std::vector*> adjVerts = - vertex_to_remove->GetAdjacentVertices(); - for (int i = 0; i != adjVerts.size(); i++) - { - std::vector*> adjEdges = adjVerts[i]->GetEdges(); + std::vector *> vertices = + vertex_to_remove->GetAdjacentVertices(); + for (Vertex * 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 * 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*> edges = m_pGraph->GetEdges(); - std::vector*>::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/slam_toolbox/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp b/slam_toolbox/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp new file mode 100644 index 000000000..6248ee591 --- /dev/null +++ b/slam_toolbox/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp @@ -0,0 +1,249 @@ +/* + * 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 3 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 . + */ + +#include "karto_sdk/contrib/ChowLiuTreeApprox.h" +#include "karto_sdk/contrib/EigenExtensions.h" + +#include +#include + +#include +#include + +namespace karto +{ + namespace contrib + { + + Eigen::SparseMatrix ComputeMarginalInformationMatrix( + const Eigen::SparseMatrix & 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 and which is the variable discarded (vectors `a` and `b` resp.). + Eigen::SparseMatrix + 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 generalized Schur's complement over the variables + // that are kept. + return (information_submatrix_aa - information_submatrix_ab * + ComputeGeneralizedInverse(information_submatrix_bb) * + information_submatrix_ba); + } + + namespace { + + // An uncertain, gaussian-distributed 2D pose. + struct UncertainPose2 + { + Pose2 mean; + Matrix3 covariance; + }; + + // Returns the target 2D pose relative to the source 2D pose, + // accounting for their joint distribution covariance. + UncertainPose2 ComputeRelativePose2( + const Pose2 & source_pose, const Pose2 & target_pose, + const Eigen::Matrix & 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 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; + const Eigen::Matrix3d covariance = + transform_jacobian * joint_pose_covariance * transform_jacobian.transpose(); + assert(covariance.isApprox(covariance.transpose())); // must be symmetric + assert((covariance.array() > 0.).all()); // must be positive semidefinite + relative_pose.covariance = Matrix3(covariance); + return relative_pose; + } + + } // namespace + + std::vector *> ComputeChowLiuTreeApproximation( + const std::vector *> & clique, + const Eigen::MatrixXd & 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>; + using VertexDescriptorT = + boost::graph_traits::vertex_descriptor; + WeightedGraphT clique_subgraph(clique.size()); + for (VertexDescriptorT u = 0; u < clique.size() - 1; ++u) { + for (VertexDescriptorT v = u + 1; v < clique.size(); ++v) { + const Eigen::Index i = u * 3, j = v * 3; // need block indices + const auto covariance_submatrix_ii = + covariance_matrix.block(i, i, 3, 3); + const auto covariance_submatrix_ij = + covariance_matrix.block(i, j, 3, 3); + const auto covariance_submatrix_ji = + covariance_matrix.block(j, i, 3, 3); + const auto covariance_submatrix_jj = + 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 * + ComputeGeneralizedInverse(covariance_submatrix_jj) * + covariance_submatrix_ji).determinant()); + boost::add_edge(u, v, -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::edge_descriptor; + std::vector minimum_spanning_tree_edges; + boost::kruskal_minimum_spanning_tree( + clique_subgraph, std::back_inserter(minimum_spanning_tree_edges)); + // (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 *> chow_liu_tree_approximation; + for (const EdgeDescriptorT & edge_descriptor : minimum_spanning_tree_edges) { + const VertexDescriptorT u = boost::source(edge_descriptor, clique_subgraph); + const VertexDescriptorT v = boost::target(edge_descriptor, clique_subgraph); + auto * edge = new Edge(clique[u], clique[v]); + const Eigen::Index i = u * 3, j = v * 3; // need block indices + Eigen::Matrix joint_pose_covariance_matrix; + joint_pose_covariance_matrix << // marginalized from the larger matrix + covariance_matrix.block(i, i, 3, 3), covariance_matrix.block(i, j, 3, 3), + covariance_matrix.block(j, i, 3, 3), 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); + // TODO(hidmic): figure out how to handle rank deficient constraints + assert(relative_pose.covariance.ToEigen().fullPivLu().rank() == 3); + 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/slam_toolbox/lib/karto_sdk/test/chow_liu_tree_approx_test.cpp b/slam_toolbox/lib/karto_sdk/test/chow_liu_tree_approx_test.cpp new file mode 100644 index 000000000..5a4852e68 --- /dev/null +++ b/slam_toolbox/lib/karto_sdk/test/chow_liu_tree_approx_test.cpp @@ -0,0 +1,86 @@ +/* + * 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 3 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 . + */ + +#include + +#include +#include + +#include "karto_sdk/contrib/ChowLiuTreeApprox.h" + +namespace +{ + +using namespace karto; +using namespace karto::contrib; + +TEST(ChowLiuTreeApproxTest, Marginalization) +{ + constexpr Eigen::Index block_size = 2; + Eigen::MatrixXd information_matrix(6, 6); + information_matrix << + 1.0, 0.0, 0., 0., 0.5, 0.0, + 0.0, 1.0, 0., 0., 0.0, 0.5, + 0.0, 0.0, 1., 0., 0.0, 0.0, + 0.0, 0.0, 0., 1., 0.0, 0.0, + 0.5, 0.0, 0., 0., 1.0, 0.0, + 0.0, 0.5, 0., 0., 0.0, 1.0; + + Eigen::MatrixXd left_marginal(4, 4); + left_marginal << + 0.75, 0.0, 0., 0., + 0.0, 0.75, 0., 0., + 0.0, 0.0, 1., 0., + 0.0, 0.0, 0., 1.; + constexpr Eigen::Index right_most_block_index = 4; + EXPECT_TRUE(left_marginal.sparseView().isApprox( + ComputeMarginalInformationMatrix( + information_matrix.sparseView(), + right_most_block_index, block_size))); + + Eigen::MatrixXd right_marginal(4, 4); + right_marginal << + 1., 0., 0., 0., + 0., 1., 0., 0., + 0., 0., 0.75, 0., + 0., 0., 0., 0.75; + constexpr Eigen::Index left_most_block_index = 0; + EXPECT_TRUE(right_marginal.sparseView().isApprox( + ComputeMarginalInformationMatrix( + information_matrix.sparseView(), + left_most_block_index, block_size))); + + Eigen::MatrixXd outer_marginal(4, 4); + outer_marginal << + 1.0, 0.0, 0.5, 0.0, + 0.0, 1.0, 0.0, 0.5, + 0.5, 0.0, 1.0, 0.0, + 0.0, 0.5, 0.0, 1.0; + constexpr Eigen::Index inner_block_index = 2; + EXPECT_TRUE(outer_marginal.sparseView().isApprox( + ComputeMarginalInformationMatrix( + information_matrix.sparseView(), + inner_block_index, block_size))); +} + +} // namespace + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/slam_toolbox/lib/karto_sdk/test/eigen_extensions_test.cpp b/slam_toolbox/lib/karto_sdk/test/eigen_extensions_test.cpp new file mode 100644 index 000000000..413d9f992 --- /dev/null +++ b/slam_toolbox/lib/karto_sdk/test/eigen_extensions_test.cpp @@ -0,0 +1,139 @@ +/* + * 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 3 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 . + */ + +#include + +#include +#include +#include + +#include "karto_sdk/contrib/EigenExtensions.h" + +namespace +{ + +using namespace karto::contrib; + +TEST(EigenSparseExtensionsTest, HorizontalStacking) +{ + Eigen::MatrixXd matrix(3, 4); + matrix << 1., 0., 3., 0., + 0., 0., 6., 2., + 7., 8., 0., 0.; + + Eigen::MatrixXd duplicated_matrix(3, 8); + duplicated_matrix << 1., 0., 3., 0., 1., 0., 3., 0., + 0., 0., 6., 2., 0., 0., 6., 2., + 7., 8., 0., 0., 7., 8., 0., 0.; + EXPECT_TRUE(duplicated_matrix.sparseView().isApprox( + StackHorizontally(matrix.sparseView(), + matrix.sparseView()))); + + Eigen::MatrixXd duplicated_row(1, 8); + duplicated_row << 1., 0., 3., 0., 1., 0., 3., 0.; + EXPECT_TRUE(duplicated_row.sparseView().isApprox( + StackHorizontally(matrix.sparseView().row(0), + matrix.sparseView().row(0)))); + + Eigen::MatrixXd duplicated_col(3, 2); + duplicated_col << 1., 1., + 0., 0., + 7., 7.; + EXPECT_TRUE(duplicated_col.sparseView().isApprox( + StackHorizontally(matrix.sparseView().col(0), + matrix.sparseView().col(0)))); +} + +TEST(EigenSparseExtensionsTest, VerticalStacking) +{ + Eigen::MatrixXd matrix(3, 4); + matrix << 1., 0., 3., 0., + 0., 0., 6., 2., + 7., 8., 0., 0.; + + Eigen::MatrixXd duplicated_matrix(6, 4); + duplicated_matrix << 1., 0., 3., 0., + 0., 0., 6., 2., + 7., 8., 0., 0., + 1., 0., 3., 0., + 0., 0., 6., 2., + 7., 8., 0., 0.; + EXPECT_TRUE(duplicated_matrix.sparseView().isApprox( + StackVertically(matrix.sparseView(), + matrix.sparseView()))); + + Eigen::MatrixXd duplicated_row(2, 4); + duplicated_row << 1., 0., 3., 0., + 1., 0., 3., 0.; + EXPECT_TRUE(duplicated_row.sparseView().isApprox( + StackVertically(matrix.sparseView().row(0), + matrix.sparseView().row(0)))); + + Eigen::MatrixXd duplicated_col(6, 1); + duplicated_col << 1., + 0., + 7., + 1., + 0., + 7.; + EXPECT_TRUE(duplicated_col.sparseView().isApprox( + StackVertically(matrix.sparseView().col(0), + matrix.sparseView().col(0)))); +} + +TEST(EigenSparseExtensionsTest, RearrangedView) +{ + Eigen::MatrixXd matrix(3, 4); + matrix << 1., 0., 3., 0., + 0., 0., 6., 2., + 7., 8., 0., 0.; + Eigen::MatrixXd rearranged_submatrix(2, 3); + rearranged_submatrix << 8., 0., 7., + 0., 0., 1.; + + EXPECT_TRUE(rearranged_submatrix.sparseView().isApprox( + ArrangeView(matrix.sparseView(), {2, 0}, {1, 3, 0}))); +} + +TEST(EigenSparseExtensionsTest, GeneralizedInverse) +{ + Eigen::MatrixXd invertible_matrix(3, 3); + invertible_matrix << 1., 2., -3., + 2., 1., 2., + -3., 2., 1.; + EXPECT_TRUE(invertible_matrix.inverse().isApprox( + ComputeGeneralizedInverse(invertible_matrix))); + + Eigen::MatrixXd singular_matrix(3, 3); + singular_matrix << 1., 2., 0., + 2., -1., 0., + 0., 0., 0.; + Eigen::MatrixXd singular_matrix_pseudoinverse(3, 3); + singular_matrix_pseudoinverse << 0.2, 0.4, 0., + 0.4, -0.2, 0., + 0.0, 0.0, 0.; + EXPECT_TRUE(singular_matrix_pseudoinverse.isApprox( + ComputeGeneralizedInverse(singular_matrix))); +} + +} // namespace + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/slam_toolbox/solvers/ceres_solver.cpp b/slam_toolbox/solvers/ceres_solver.cpp index 99a03a7b4..d2ad108ab 100644 --- a/slam_toolbox/solvers/ceres_solver.cpp +++ b/slam_toolbox/solvers/ceres_solver.cpp @@ -16,7 +16,8 @@ namespace solver_plugins /*****************************************************************************/ CeresSolver::CeresSolver() : - nodes_(new std::unordered_map()), + nodes_(new std::unordered_map()), + nodes_inverted_(new std::unordered_map()), blocks_(new std::unordered_map()), problem_(NULL), was_constant_set_(false) @@ -37,7 +38,9 @@ CeresSolver::CeresSolver() : 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,12 +185,10 @@ void CeresSolver::Compute() // populate contraint for static initial pose if (!was_constant_set_ && first_node_ != nodes_->end()) { - ROS_DEBUG("CeresSolver: Setting first node as a constant pose:" + ROS_DEBUG("CeresSolver: Setting 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_; } @@ -232,6 +233,31 @@ const karto::ScanSolver::IdPoseVector& CeresSolver::GetCorrections() const return corrections_; } +/*****************************************************************************/ +Eigen::SparseMatrix CeresSolver::GetInformationMatrix( + std::unordered_map * ordering) const +/****************************************************************************/ +{ + if (ordering) { + Eigen::Index index = 0u; + std::vector parameter_blocks; + problem_->GetParameterBlocks(¶meter_blocks); + for (auto * block : parameter_blocks) { + (*ordering)[(*nodes_inverted_)[block]] = index; + index += problem_->ParameterBlockSize(block); + } + } + ceres::CRSMatrix jacobian_data; + problem_->Evaluate(ceres::Problem::EvaluateOptions(), + nullptr, nullptr, nullptr, &jacobian_data); + Eigen::SparseMatrix jacobian( + problem_->NumResiduals(), problem_->NumParameters()); + jacobian.setFromTriplets( + CRSMatrixIterator::begin(jacobian_data), + CRSMatrixIterator::end(jacobian_data)); + return jacobian.transpose() * jacobian; +} + /*****************************************************************************/ void CeresSolver::Clear() /*****************************************************************************/ @@ -253,6 +279,11 @@ void CeresSolver::Reset() delete problem_; } + if (nodes_inverted_) + { + delete nodes_inverted_; + } + if (nodes_) { delete nodes_; @@ -264,11 +295,14 @@ void CeresSolver::Reset() } nodes_ = new std::unordered_map(); + nodes_inverted_ = new std::unordered_map(); blocks_ = new std::unordered_map(); 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()); } /*****************************************************************************/ @@ -288,6 +322,7 @@ void CeresSolver::AddNode(karto::Vertex* pVertex) boost::mutex::scoped_lock lock(nodes_mutex_); nodes_->insert(std::pair(id,pose2d)); + nodes_inverted_->insert(std::pair((*nodes_)[id].data(), id)); if (nodes_->size() == 1) { @@ -322,29 +357,19 @@ void CeresSolver::AddConstraint(karto::Edge* 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 information; - information(0, 0) = precisionMatrix(0, 0); - information(0, 1) = information(1, 0) = precisionMatrix(0, 1); - information(0, 2) = information(2, 0) = precisionMatrix(0, 2); - information(1, 1) = precisionMatrix(1, 1); - information(1, 2) = information(2, 1) = precisionMatrix(1, 2); - information(2, 2) = precisionMatrix(2, 2); - Eigen::Matrix3d sqrt_information = information.llt().matrixU(); + Eigen::Matrix3d sqrt_information = + pLinkInfo->GetCovariance().Inverse().ToEigen().llt().matrixU(); // populate residual and parameterization for heading normalization - ceres::CostFunction* cost_function = PoseGraph2dErrorTerm::Create(pose2d(0), - pose2d(1), pose2d(2), sqrt_information); + 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_); + cost_function, loss_function_, + node1it->second.data(), node2it->second.data()); + problem_->SetParameterization( + node1it->second.data(), local_parameterization_); + problem_->SetParameterization( + node2it->second.data(), local_parameterization_); blocks_->insert(std::pair( GetHash(node1, node2), block)); @@ -359,7 +384,14 @@ void CeresSolver::RemoveNode(kt_int32s id) GraphIterator nodeit = nodes_->find(id); if (nodeit != nodes_->end()) { - nodes_->erase(nodeit); + problem_->RemoveParameterBlock(nodeit->second.data()); + nodes_inverted_->erase(nodes_inverted_->find(nodeit->second.data())); + if (nodeit == first_node_) { + first_node_ = nodes_->erase(nodeit); + was_constant_set_ = false; + } else { + nodes_->erase(nodeit); + } } else { diff --git a/slam_toolbox/solvers/ceres_solver.hpp b/slam_toolbox/solvers/ceres_solver.hpp index 9c0f06083..cc29ede11 100644 --- a/slam_toolbox/solvers/ceres_solver.hpp +++ b/slam_toolbox/solvers/ceres_solver.hpp @@ -42,6 +42,8 @@ class CeresSolver : public karto::ScanSolver virtual void AddNode(karto::Vertex* pVertex); //Adds a node to the solver virtual void AddConstraint(karto::Edge* pEdge); //Adds a constraint to the solver virtual std::unordered_map* getGraph(); //Get graph stored + virtual Eigen::SparseMatrix GetInformationMatrix( + std::unordered_map * ordering) const; // Get information matrix associated with the graph virtual void RemoveNode(kt_int32s id); //Removes a node from the solver correction table virtual void RemoveConstraint(kt_int32s sourceId, kt_int32s targetId); // Removes constraints from the optimization problem @@ -57,11 +59,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* nodes_; + std::unordered_map* nodes_inverted_; std::unordered_map* blocks_; std::unordered_map::iterator first_node_; boost::mutex nodes_mutex_; diff --git a/slam_toolbox/solvers/ceres_utils.h b/slam_toolbox/solvers/ceres_utils.h index 08b5f62b2..ceef6cd97 100644 --- a/slam_toolbox/solvers/ceres_utils.h +++ b/slam_toolbox/solvers/ceres_utils.h @@ -6,8 +6,11 @@ #include #include #include +#include #include +#include + /*****************************************************************************/ /*****************************************************************************/ /*****************************************************************************/ @@ -20,6 +23,88 @@ inline std::size_t GetHash(const int& x, const int& y) /*****************************************************************************/ /*****************************************************************************/ +// Iterator for ceres::CRSMatrix elements that yields Eigen::Triplet instances. +// Helpful to populate Eigen::SparseMatrix instances from ceres::CRSMatrix ones. +class CRSMatrixIterator : public std::iterator< + std::input_iterator_tag, Eigen::Triplet> +{ +public: + static CRSMatrixIterator begin(const ceres::CRSMatrix & matrix) + { + return CRSMatrixIterator(matrix); + } + + static CRSMatrixIterator end(const ceres::CRSMatrix & matrix) + { + return CRSMatrixIterator(matrix, matrix.num_rows); + } + + 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 ¤t_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 MakeTriplet() const + { + return Eigen::Triplet( + row_index_, matrix_.cols[data_index_], + matrix_.values[data_index_]); + } + + const ceres::CRSMatrix & matrix_; + size_t row_index_; + size_t data_index_; + + Eigen::Triplet current_triplet_; +}; + +/*****************************************************************************/ +/*****************************************************************************/ +/*****************************************************************************/ + // Normalizes the angle in radians between [-pi and pi). template inline T NormalizeAngle(const T& angle_radians) { @@ -74,21 +159,26 @@ class PoseGraph2dErrorTerm } template - 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 p_a(*x_a, *y_a); - const Eigen::Matrix p_b(*x_b, *y_b); - Eigen::Map > residuals_map(residuals_ptr); - residuals_map.template head<2>() = RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast(); - residuals_map(2) = NormalizeAngle((*yaw_b - *yaw_a) - static_cast(yaw_ab_radians_)); + const Eigen::Matrix p_a(pose_a); + const Eigen::Matrix p_b(pose_b); + const T yaw_a = pose_a[2]; + const T yaw_b = pose_b[2]; + + Eigen::Map> residuals(residuals_ptr); + + residuals.template head<2>() = + RotationMatrix2D(yaw_a).transpose() * (p_b - p_a) - p_ab_.cast(); + residuals(2) = NormalizeAngle((yaw_b - yaw_a) - static_cast(yaw_ab_radians_)); // Scale the residuals by the square root information matrix to account for the measurement uncertainty. - residuals_map = sqrt_information_.template cast() * residuals_map; + residuals = sqrt_information_.template cast() * residuals; return true; } static ceres::CostFunction* Create(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d& sqrt_information) { - return (new ceres::AutoDiffCostFunction(new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab_radians, sqrt_information))); + return (new ceres::AutoDiffCostFunction(new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab_radians, sqrt_information))); } EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp b/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp index cf17a7214..d4c16ef33 100644 --- a/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp +++ b/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp @@ -46,6 +46,7 @@ LifelongSlamToolbox::LifelongSlamToolbox(ros::NodeHandle& nh) nh.param("lifelong_constraint_multiplier", constraint_scale_, 0.05); nh.param("lifelong_nearby_penalty", nearby_penalty_, 0.001); nh.param("lifelong_candidates_scale", candidates_scale_, 0.03); + nh.param("lifelong_node_marginalization", node_marginalization_, false); checkIsNotNormalized(iou_thresh_); checkIsNotNormalized(constraint_scale_); @@ -297,14 +298,20 @@ void LifelongSlamToolbox::removeFromSlamGraph( Vertex* vertex) /*****************************************************************************/ { - smapper_->getMapper()->RemoveNodeFromGraph(vertex); + if (node_marginalization_) + { + smapper_->getMapper()->MarginalizeNodeFromGraph(vertex); + } + else + { + smapper_->getMapper()->RemoveNodeFromGraph(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? } /*****************************************************************************/ From 4bc59034fa41e3eafaa28b5346f4d1e611e5267a Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Sat, 29 Jan 2022 23:21:06 -0300 Subject: [PATCH 2/6] Use LGPL 2.1 in lieu of LGPL 3 Signed-off-by: Michel Hidalgo --- .../lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h | 2 +- .../lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h | 2 +- slam_toolbox/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp | 2 +- slam_toolbox/lib/karto_sdk/test/chow_liu_tree_approx_test.cpp | 2 +- slam_toolbox/lib/karto_sdk/test/eigen_extensions_test.cpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h index 4a8d9030f..60a871b2e 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h @@ -3,7 +3,7 @@ * * 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 3 of the License, or + * 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, diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h index 3414e70a4..a8f9541f8 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h @@ -3,7 +3,7 @@ * * 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 3 of the License, or + * 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, diff --git a/slam_toolbox/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp b/slam_toolbox/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp index 6248ee591..845a3d5b4 100644 --- a/slam_toolbox/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp +++ b/slam_toolbox/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp @@ -3,7 +3,7 @@ * * 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 3 of the License, or + * 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, diff --git a/slam_toolbox/lib/karto_sdk/test/chow_liu_tree_approx_test.cpp b/slam_toolbox/lib/karto_sdk/test/chow_liu_tree_approx_test.cpp index 5a4852e68..279e88bfb 100644 --- a/slam_toolbox/lib/karto_sdk/test/chow_liu_tree_approx_test.cpp +++ b/slam_toolbox/lib/karto_sdk/test/chow_liu_tree_approx_test.cpp @@ -3,7 +3,7 @@ * * 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 3 of the License, or + * 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, diff --git a/slam_toolbox/lib/karto_sdk/test/eigen_extensions_test.cpp b/slam_toolbox/lib/karto_sdk/test/eigen_extensions_test.cpp index 413d9f992..2a4f41006 100644 --- a/slam_toolbox/lib/karto_sdk/test/eigen_extensions_test.cpp +++ b/slam_toolbox/lib/karto_sdk/test/eigen_extensions_test.cpp @@ -3,7 +3,7 @@ * * 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 3 of the License, or + * 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, From aa9ffa6f46a98b6972de9262a50cd45488973cfb Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 25 Mar 2022 18:25:51 -0300 Subject: [PATCH 3/6] Delete CeresSolver allocations upon destruction/reset Signed-off-by: Michel Hidalgo --- slam_toolbox/solvers/ceres_solver.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/slam_toolbox/solvers/ceres_solver.cpp b/slam_toolbox/solvers/ceres_solver.cpp index d2ad108ab..89e32f235 100644 --- a/slam_toolbox/solvers/ceres_solver.cpp +++ b/slam_toolbox/solvers/ceres_solver.cpp @@ -163,10 +163,18 @@ CeresSolver::~CeresSolver() { delete nodes_; } + if (nodes_inverted_ != NULL) + { + delete nodes_inverted_; + } if (problem_ != NULL) { delete problem_; } + if (local_parameterization_ != NULL) + { + delete local_parameterization_; + } } /*****************************************************************************/ @@ -279,6 +287,11 @@ void CeresSolver::Reset() delete problem_; } + if (local_parameterization_) + { + delete local_parameterization_; + } + if (nodes_inverted_) { delete nodes_inverted_; From c119840bef1eefd62e4735822723148913787f2e Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 25 Mar 2022 18:26:27 -0300 Subject: [PATCH 4/6] Drop duplicate edge checks from karto::Mapper Signed-off-by: Michel Hidalgo --- slam_toolbox/lib/karto_sdk/src/Mapper.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp index 84aaab301..8547853db 100644 --- a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp +++ b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp @@ -1682,18 +1682,6 @@ namespace karto return false; } - // see if edge already exists - for (Edge * pOtherEdge : GetEdges()) { - if (pEdge->GetSource() == pOtherEdge->GetSource() && - pEdge->GetTarget() == pOtherEdge->GetTarget()) { - return false; - } - if (pEdge->GetTarget() == pOtherEdge->GetSource() && - pEdge->GetSource() == pOtherEdge->GetTarget()) { - return false; - } - } - Graph::AddEdge(pEdge); return true; } From 1041a52eae3d3c75d1c48c87dedb3cae8bffb3d5 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Sun, 3 Apr 2022 23:17:39 -0300 Subject: [PATCH 5/6] Rework parameter/residual block handling in Ceres solver - Use a boost::bimap for node id <-> parameter block lookup - Use a boost::pool for parameter block allocation - Change Karto::Solver graph getter signature Signed-off-by: Michel Hidalgo --- .../lib/karto_sdk/include/karto_sdk/Mapper.h | 4 +- slam_toolbox/solvers/ceres_solver.cpp | 227 +++++++++--------- slam_toolbox/solvers/ceres_solver.hpp | 23 +- 3 files changed, 138 insertions(+), 116 deletions(-) diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h index 2e9a4e770..7c295fad2 100644 --- a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h +++ b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -1072,10 +1072,10 @@ namespace karto /** * Get graph stored */ - virtual std::unordered_map* getGraph() + virtual std::unordered_map> GetGraph() { std::cout << "getGraph method not implemented for this solver type. Graph visualization unavailable." << std::endl; - return nullptr; + return {}; } /** diff --git a/slam_toolbox/solvers/ceres_solver.cpp b/slam_toolbox/solvers/ceres_solver.cpp index 89e32f235..8156db0b4 100644 --- a/slam_toolbox/solvers/ceres_solver.cpp +++ b/slam_toolbox/solvers/ceres_solver.cpp @@ -16,11 +16,11 @@ namespace solver_plugins /*****************************************************************************/ CeresSolver::CeresSolver() : - nodes_(new std::unordered_map()), - nodes_inverted_(new std::unordered_map()), - blocks_(new std::unordered_map()), - problem_(NULL), was_constant_set_(false) + problem_(nullptr), was_constant_set_(false), + parameter_blocks_(new ParameterBlockMap()), + residual_blocks_(new ResidualBlockMap()), + parameter_block_pool_(sizeof(double) * 3u), + first_parameter_block_(nullptr) /*****************************************************************************/ { ros::NodeHandle nh("~"); @@ -34,9 +34,6 @@ CeresSolver::CeresSolver() : nh.getParam("mode", mode); nh.getParam("debug_logging", debug_logging_); - corrections_.clear(); - first_node_ = nodes_->end(); - // formulate problem local_parameterization_ = new ceres::ProductParameterization( new ceres::IdentityParameterization(2), @@ -147,25 +144,23 @@ CeresSolver::CeresSolver() : } problem_ = new ceres::Problem(options_problem_); - - return; } /*****************************************************************************/ CeresSolver::~CeresSolver() /*****************************************************************************/ { - if ( loss_function_ != NULL) + if (loss_function_ != NULL) { delete loss_function_; } - if (nodes_ != NULL) + if (residual_blocks_ != NULL) { - delete nodes_; + delete residual_blocks_; } - if (nodes_inverted_ != NULL) + if (parameter_blocks_ != NULL) { - delete nodes_inverted_; + delete parameter_blocks_; } if (problem_ != NULL) { @@ -181,9 +176,9 @@ CeresSolver::~CeresSolver() void CeresSolver::Compute() /*****************************************************************************/ { - boost::mutex::scoped_lock lock(nodes_mutex_); + boost::mutex::scoped_lock lock(mutex_); - if (nodes_->size() == 0) + if (parameter_blocks_->empty()) { ROS_ERROR("CeresSolver: Ceres was called when there are no nodes." " This shouldn't happen."); @@ -191,12 +186,12 @@ void CeresSolver::Compute() } // populate contraint for static initial pose - if (!was_constant_set_ && first_node_ != nodes_->end()) + if (!was_constant_set_ && first_parameter_block_ != nullptr) { ROS_DEBUG("CeresSolver: Setting 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.data()); + "%0.2f, %0.2f, %0.2f.", first_parameter_block_[0], + first_parameter_block_[1], first_parameter_block_[2]); + problem_->SetParameterBlockConstant(first_parameter_block_); was_constant_set_ = !was_constant_set_; } @@ -220,15 +215,16 @@ void CeresSolver::Compute() { corrections_.clear(); } - corrections_.reserve(nodes_->size()); + corrections_.reserve(parameter_blocks_->size()); + karto::Pose2 pose; - ConstGraphIterator iter = nodes_->begin(); - for ( iter; iter != nodes_->end(); ++iter ) + ParameterBlockMap::left_const_iterator it = parameter_blocks_->left.begin(); + for (; it != parameter_blocks_->left.end(); ++it) { - pose.SetX(iter->second(0)); - pose.SetY(iter->second(1)); - pose.SetHeading(iter->second(2)); - corrections_.push_back(std::make_pair(iter->first, pose)); + pose.SetX(it->second[0]); + pose.SetY(it->second[1]); + pose.SetHeading(it->second[2]); + corrections_.push_back(std::make_pair(it->first, pose)); } return; @@ -246,12 +242,14 @@ Eigen::SparseMatrix CeresSolver::GetInformationMatrix( std::unordered_map * ordering) const /****************************************************************************/ { - if (ordering) { + if (ordering) + { Eigen::Index index = 0u; std::vector parameter_blocks; problem_->GetParameterBlocks(¶meter_blocks); - for (auto * block : parameter_blocks) { - (*ordering)[(*nodes_inverted_)[block]] = index; + for (double * block : parameter_blocks) + { + (*ordering)[parameter_blocks_->right.at(block)] = index; index += problem_->ParameterBlockSize(block); } } @@ -277,7 +275,7 @@ void CeresSolver::Clear() void CeresSolver::Reset() /*****************************************************************************/ { - boost::mutex::scoped_lock lock(nodes_mutex_); + boost::mutex::scoped_lock lock(mutex_); corrections_.clear(); was_constant_set_ = false; @@ -292,27 +290,23 @@ void CeresSolver::Reset() delete local_parameterization_; } - if (nodes_inverted_) + if (parameter_blocks_) { - delete nodes_inverted_; + delete parameter_blocks_; } - if (nodes_) + if (residual_blocks_) { - delete nodes_; + delete residual_blocks_; } - if (blocks_) - { - delete blocks_; - } + parameter_block_pool_.purge_memory(); - nodes_ = new std::unordered_map(); - nodes_inverted_ = new std::unordered_map(); - blocks_ = new std::unordered_map(); - problem_ = new ceres::Problem(options_problem_); - first_node_ = nodes_->end(); + parameter_blocks_ = new ParameterBlockMap(); + residual_blocks_ = new ResidualBlockMap(); + first_parameter_block_ = nullptr; + problem_ = new ceres::Problem(options_problem_); local_parameterization_ = new ceres::ProductParameterization( new ceres::IdentityParameterization(2), AngleLocalParameterization::Create()); @@ -327,19 +321,18 @@ void CeresSolver::AddNode(karto::Vertex* pVertex) { return; } - - karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose(); - Eigen::Vector3d pose2d(pose.GetX(), pose.GetY(), pose.GetHeading()); + boost::mutex::scoped_lock lock(mutex_); const int id = pVertex->GetObject()->GetUniqueId(); - - boost::mutex::scoped_lock lock(nodes_mutex_); - nodes_->insert(std::pair(id,pose2d)); - nodes_inverted_->insert(std::pair((*nodes_)[id].data(), id)); - - if (nodes_->size() == 1) + const karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose(); + double* parameter_block = + static_cast(parameter_block_pool_.malloc()); + Eigen::Map parameter_block_as_vector(parameter_block); + parameter_block_as_vector << pose.GetX(), pose.GetY(), pose.GetHeading(); + parameter_blocks_->insert(ParameterBlockMap::value_type(id, parameter_block)); + if (parameter_blocks_->size() == 1) { - first_node_ = nodes_->find(id); + first_parameter_block_ = parameter_block; } } @@ -347,21 +340,24 @@ void CeresSolver::AddNode(karto::Vertex* pVertex) void CeresSolver::AddConstraint(karto::Edge* pEdge) /*****************************************************************************/ { - // get IDs in graph for this edge - boost::mutex::scoped_lock lock(nodes_mutex_); - if (!pEdge) { return; } - const int node1 = pEdge->GetSource()->GetObject()->GetUniqueId(); - GraphIterator node1it = nodes_->find(node1); - const int node2 = pEdge->GetTarget()->GetObject()->GetUniqueId(); - GraphIterator node2it = nodes_->find(node2); + // get IDs in graph for this edge + boost::mutex::scoped_lock lock(mutex_); + + ParameterBlockMap::left_const_iterator node1it = + parameter_blocks_->left.find( + pEdge->GetSource()->GetObject()->GetUniqueId()); + ParameterBlockMap::left_const_iterator node2it = + parameter_blocks_->left.find( + pEdge->GetTarget()->GetObject()->GetUniqueId()); - if (node1it == nodes_->end() || - node2it == nodes_->end() || node1it == node2it) + if (node1it == parameter_blocks_->left.end() || + node2it == parameter_blocks_->left.end() || + node1it == node2it) { ROS_WARN("CeresSolver: Failed to add constraint, could not find nodes."); return; @@ -376,34 +372,42 @@ void CeresSolver::AddConstraint(karto::Edge* pEdge) // populate residual and parameterization for heading normalization 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.data(), node2it->second.data()); - problem_->SetParameterization( - node1it->second.data(), local_parameterization_); - problem_->SetParameterization( - node2it->second.data(), local_parameterization_); - - blocks_->insert(std::pair( - GetHash(node1, node2), block)); - return; + ceres::ResidualBlockId residual_block = problem_->AddResidualBlock( + cost_function, loss_function_, node1it->second, node2it->second); + problem_->SetParameterization(node1it->second, local_parameterization_); + problem_->SetParameterization(node2it->second, local_parameterization_); + + residual_blocks_->emplace( + GetHash(node1it->first, node1it->first), residual_block); } /*****************************************************************************/ 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())); - if (nodeit == first_node_) { - first_node_ = nodes_->erase(nodeit); + boost::mutex::scoped_lock lock(mutex_); + ParameterBlockMap::left_iterator it = + parameter_blocks_->left.find(id); + if (it != parameter_blocks_->left.end()) + { + problem_->RemoveParameterBlock(it->second); + parameter_block_pool_.free(it->second); + if (it->second == first_parameter_block_) + { + it = parameter_blocks_->left.erase(it); + if (it != parameter_blocks_->left.end()) + { + first_parameter_block_ = it->second; + } + else + { + first_parameter_block_ = nullptr; + } was_constant_set_ = false; - } else { - nodes_->erase(nodeit); + } + else + { + parameter_blocks_->left.erase(it); } } else @@ -416,20 +420,20 @@ void CeresSolver::RemoveNode(kt_int32s id) void CeresSolver::RemoveConstraint(kt_int32s sourceId, kt_int32s targetId) /*****************************************************************************/ { - boost::mutex::scoped_lock lock(nodes_mutex_); - std::unordered_map::iterator it_a = - blocks_->find(GetHash(sourceId, targetId)); - std::unordered_map::iterator it_b = - blocks_->find(GetHash(targetId, sourceId)); - if (it_a != blocks_->end()) + boost::mutex::scoped_lock lock(mutex_); + ResidualBlockMap::iterator it_a = + residual_blocks_->find(GetHash(sourceId, targetId)); + ResidualBlockMap::iterator it_b = + residual_blocks_->find(GetHash(targetId, sourceId)); + if (it_a != residual_blocks_->end()) { problem_->RemoveResidualBlock(it_a->second); - blocks_->erase(it_a); + residual_blocks_->erase(it_a); } - else if (it_b != blocks_->end()) + else if (it_b != residual_blocks_->end()) { problem_->RemoveResidualBlock(it_b->second); - blocks_->erase(it_b); + residual_blocks_->erase(it_b); } else { @@ -442,13 +446,15 @@ void CeresSolver::RemoveConstraint(kt_int32s sourceId, kt_int32s targetId) void CeresSolver::ModifyNode(const int& unique_id, Eigen::Vector3d pose) /*****************************************************************************/ { - boost::mutex::scoped_lock lock(nodes_mutex_); - GraphIterator it = nodes_->find(unique_id); - if (it != nodes_->end()) + boost::mutex::scoped_lock lock(mutex_); + ParameterBlockMap::left_iterator it = + parameter_blocks_->left.find(unique_id); + if (it != parameter_blocks_->left.end()) { - double yaw_init = it->second(2); - it->second = pose; - it->second(2) += yaw_init; + Eigen::Map parameter_block_as_vector(it->second); + double yaw_init = parameter_block_as_vector(2); + parameter_block_as_vector = pose; + parameter_block_as_vector(2) += yaw_init; } } @@ -456,20 +462,27 @@ void CeresSolver::ModifyNode(const int& unique_id, Eigen::Vector3d pose) void CeresSolver::GetNodeOrientation(const int& unique_id, double& pose) /*****************************************************************************/ { - boost::mutex::scoped_lock lock(nodes_mutex_); - GraphIterator it = nodes_->find(unique_id); - if (it != nodes_->end()) + boost::mutex::scoped_lock lock(mutex_); + ParameterBlockMap::left_const_iterator it = + parameter_blocks_->left.find(unique_id); + if (it != parameter_blocks_->left.end()) { - pose = it->second(2); + pose = it->second[2]; } } /*****************************************************************************/ -std::unordered_map* CeresSolver::getGraph() +std::unordered_map> CeresSolver::GetGraph() /*****************************************************************************/ { - boost::mutex::scoped_lock lock(nodes_mutex_); - return nodes_; + boost::mutex::scoped_lock lock(mutex_); + std::unordered_map> graph; + ParameterBlockMap::left_const_iterator it = parameter_blocks_->left.begin(); + for (; it != parameter_blocks_->left.end(); ++it) + { + graph.emplace(it->first, Eigen::Map(it->second)); + } + return graph; } } // end namespace diff --git a/slam_toolbox/solvers/ceres_solver.hpp b/slam_toolbox/solvers/ceres_solver.hpp index cc29ede11..76616d38d 100644 --- a/slam_toolbox/solvers/ceres_solver.hpp +++ b/slam_toolbox/solvers/ceres_solver.hpp @@ -13,6 +13,10 @@ #include #include +#include +#include +#include + #include #include #include @@ -41,9 +45,9 @@ class CeresSolver : public karto::ScanSolver virtual void AddNode(karto::Vertex* pVertex); //Adds a node to the solver virtual void AddConstraint(karto::Edge* pEdge); //Adds a constraint to the solver - virtual std::unordered_map* getGraph(); //Get graph stored + virtual std::unordered_map> GetGraph(); //Get graph stored virtual Eigen::SparseMatrix GetInformationMatrix( - std::unordered_map * ordering) const; // Get information matrix associated with the graph + std::unordered_map * ordering) const; // Get information matrix associated with the graph virtual void RemoveNode(kt_int32s id); //Removes a node from the solver correction table virtual void RemoveConstraint(kt_int32s sourceId, kt_int32s targetId); // Removes constraints from the optimization problem @@ -63,11 +67,16 @@ class CeresSolver : public karto::ScanSolver bool was_constant_set_, debug_logging_; // graph - std::unordered_map* nodes_; - std::unordered_map* nodes_inverted_; - std::unordered_map* blocks_; - std::unordered_map::iterator first_node_; - boost::mutex nodes_mutex_; + using ParameterBlockMap = boost::bimap< + boost::bimaps::unordered_set_of, + boost::bimaps::unordered_set_of>; + using ResidualBlockMap = + std::unordered_map; + ResidualBlockMap* residual_blocks_; + ParameterBlockMap* parameter_blocks_; + boost::pool<> parameter_block_pool_; + double* first_parameter_block_; + boost::mutex mutex_; }; } From d65e03cb622e0e74c844be4adc55a0a5fc386a71 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Sun, 3 Apr 2022 23:22:04 -0300 Subject: [PATCH 6/6] Null edge pointer after delete Signed-off-by: Michel Hidalgo --- slam_toolbox/lib/karto_sdk/src/Mapper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp index 8547853db..bcc542eae 100644 --- a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp +++ b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp @@ -3150,6 +3150,7 @@ namespace karto edge->GetTarget()->GetObject()->GetUniqueId()); m_pGraph->RemoveEdge(edge); delete edge; + edge = nullptr; found = true; } }