diff --git a/lib/karto_sdk/CMakeLists.txt b/lib/karto_sdk/CMakeLists.txt index be14006e4..ecbe7bea3 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/contrib/ChowLiuTreeApprox.h b/lib/karto_sdk/include/karto_sdk/contrib/ChowLiuTreeApprox.h new file mode 100644 index 000000000..7ddcd922d --- /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 + +#include +#include + +#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 joint_pose_covariance); + +/** 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 clique + * (i.e. a graphical representation of joint probability distribution). + */ +std::vector *> ComputeChowLiuTreeApproximation( + const std::vector *> & clique, + const Eigen::SparseMatrix & covariance_matrix); + +} // namespace contrib +} // namespace karto + +#endif // KARTO_SDK__CHOWLIUTREEAPPROX_H_ diff --git a/lib/karto_sdk/include/karto_sdk/Utils.h b/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h similarity index 91% rename from lib/karto_sdk/include/karto_sdk/Utils.h rename to lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h index a356483f2..d076b5f8a 100644 --- a/lib/karto_sdk/include/karto_sdk/Utils.h +++ b/lib/karto_sdk/include/karto_sdk/contrib/EigenExtensions.h @@ -1,5 +1,5 @@ -#ifndef KARTO_SDK__UTILS_H_ -#define KARTO_SDK__UTILS_H_ +#ifndef KARTO_SDK__EIGENEXTENSIONS_H_ +#define KARTO_SDK__EIGENEXTENSIONS_H_ #include #include @@ -9,6 +9,7 @@ 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( @@ -21,6 +22,7 @@ inline constexpr int size_sum_prefer_dynamic(A a, B b) { 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( @@ -38,41 +40,50 @@ struct constexpr_conditional_impl; template struct constexpr_conditional_impl { - constexpr_conditional_impl(ThenT& some_value, ElseT&) + constexpr_conditional_impl(ThenT&& some_value, ElseT&&) : value(some_value) { } - ThenT& value; + ThenT value; }; template struct constexpr_conditional_impl { - constexpr_conditional_impl(ThenT&, ElseT& other_value) + constexpr_conditional_impl(ThenT&&, ElseT&& other_value) : value(other_value) { } - ElseT& 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 auto constexpr_conditional(ThenT& some_value, ElseT& other_value) +inline constexpr auto +constexpr_conditional(ThenT&& some_value, ElseT&& other_value) { - using T = constexpr_conditional_impl; - return T{some_value, other_value}.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; // a *much* simplified equivalent to IndexedView in Eigen 3.4 +class View; namespace internal { @@ -177,6 +188,12 @@ struct traits> : traits } // 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 @@ -230,6 +247,13 @@ class HorizontalStack : public internal::generic_xpr_base< 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 @@ -283,6 +307,13 @@ class VerticalStack : public internal::generic_xpr_base< 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 @@ -596,6 +627,15 @@ struct unary_evaluator, IteratorBased> } // 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) @@ -603,6 +643,10 @@ 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) @@ -610,6 +654,10 @@ 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 Eigen::View ArrangeView(XprType & xpr, const RowIndices & rowIndices, const ColIndices & colIndices) @@ -617,6 +665,7 @@ ArrangeView(XprType & xpr, const RowIndices & rowIndices, const ColIndices & col return Eigen::View(xpr, rowIndices, colIndices); } +/** Computes the inverse of a sparse matrix. */ template >> @@ -634,4 +683,7 @@ ComputeSparseInverse(const Eigen::SparseMatrix& m return solver.solve(I); } -#endif // KARTO_SDK__UTILS_H_ +} // 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 ca22c08a5..9d0ef86ca 100644 --- a/lib/karto_sdk/src/Mapper.cpp +++ b/lib/karto_sdk/src/Mapper.cpp @@ -19,11 +19,8 @@ #include #include #include -#include -#include #include #include -#include #include #include #include @@ -32,13 +29,15 @@ #include #include #include +#include #include #include #include #include #include "karto_sdk/Mapper.h" -#include "karto_sdk/Utils.h" +#include "karto_sdk/contrib/ChowLiuTreeApprox.h" +#include "karto_sdk/contrib/EigenExtensions.h" BOOST_CLASS_EXPORT(karto::MapperGraph); BOOST_CLASS_EXPORT(karto::Graph); @@ -2990,205 +2989,21 @@ void Mapper::ClearLocalizationBuffer() return; } -namespace { - -Eigen::SparseMatrix ComputeMarginalInformationMatrix( - const Eigen::SparseMatrix & information_matrix, - const Eigen::Index discarded_variable_index, - const Eigen::Index discarded_variable_dimension) -{ - const Eigen::Index dimension = information_matrix.outerSize(); - assert(dimension == information_matrix.innerSize()); // must be square - const Eigen::Index marginal_dimension = dimension - discarded_variable_dimension; - const Eigen::Index last_variable_index = dimension - discarded_variable_dimension; - - 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, discarded_variable_dimension); - information_submatrix_ba = - information_matrix.topRightCorner( - discarded_variable_dimension, marginal_dimension); - information_submatrix_bb = - information_matrix.topLeftCorner( - discarded_variable_dimension, discarded_variable_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, discarded_variable_dimension); - information_submatrix_ba = - information_matrix.bottomLeftCorner( - discarded_variable_dimension, marginal_dimension); - information_submatrix_bb = - information_matrix.bottomRightCorner( - discarded_variable_dimension, discarded_variable_dimension); - } else { - const Eigen::Index next_variable_index = - discarded_variable_index + discarded_variable_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, - discarded_variable_dimension), - information_matrix.block( - next_variable_index, - discarded_variable_index, - dimension - next_variable_index, - discarded_variable_dimension)); - information_submatrix_ba = StackHorizontally( - information_matrix.block( - discarded_variable_index, - 0, - discarded_variable_dimension, - discarded_variable_index), - information_matrix.block( - discarded_variable_index, - next_variable_index, - discarded_variable_dimension, - dimension - next_variable_index)); - information_submatrix_bb = - information_matrix.block( - discarded_variable_index, - discarded_variable_index, - discarded_variable_dimension, - discarded_variable_dimension); - } - - return (information_submatrix_aa - information_submatrix_ba * - ComputeSparseInverse(information_submatrix_bb) * - information_submatrix_ab); -} - -struct UncertainPose2 { - Pose2 mean; - Matrix3 covariance; -}; - -UncertainPose2 ComputeRelativePose2( - Pose2 source_pose, Pose2 target_pose, - Eigen::Matrix joint_pose_covariance) -{ - UncertainPose2 relative_pose; - // Compute mean relative pose by transforming - // mean source and target poses - Transform source_transform(source_pose); - relative_pose.mean = - source_transform.InverseTransformPose(target_pose); - // Compute relative pose covariance by linearizing - // the transformation around mean source and target - // poses (aka SSC method). - 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; - relative_pose.covariance = Matrix3( - transform_jacobian * joint_pose_covariance * - transform_jacobian.transpose()); - return relative_pose; -} - -std::vector *> ComputeChowLiuTreeApproximation( - const std::vector *> & elimination_clique, - const Eigen::SparseMatrix & covariance_matrix) -{ - using WeightedGraphT = boost::adjacency_list< - boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, - boost::property>; - WeightedGraphT elimination_clique_subgraph(elimination_clique.size()); - for (size_t i = 0; i < elimination_clique.size() - 1; ++i) { - for (size_t j = i + 1; j < elimination_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, elimination_clique_subgraph); - } - } - using EdgeDescriptorT = - boost::graph_traits::edge_descriptor; - std::vector minimum_spanning_tree_edges; - boost::kruskal_minimum_spanning_tree( - elimination_clique_subgraph, - std::back_inserter(minimum_spanning_tree_edges)); - using VertexDescriptorT = - boost::graph_traits::vertex_descriptor; - std::vector *> chow_liu_tree_approximation; - for (const EdgeDescriptorT & edge_descriptor : minimum_spanning_tree_edges) { - const VertexDescriptorT i = boost::source( - edge_descriptor, elimination_clique_subgraph); - const VertexDescriptorT j = boost::target( - edge_descriptor, elimination_clique_subgraph); - auto * edge = new Edge(elimination_clique[i], - elimination_clique[j]); - Eigen::Matrix joint_pose_covariance_matrix; - joint_pose_covariance_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 - kt_bool Mapper::MarginalizeNodeFromGraph( Vertex * vertex_to_marginalize) { - // (1) Fetch information matrix from solver + // 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 vertex from information matrix + // (2) Marginalize variable from information matrix. constexpr Eigen::Index block_size = 3; auto block_index_of = [&](Vertex * vertex) { return ordering[vertex->GetObject()->GetUniqueId()]; @@ -3196,9 +3011,11 @@ kt_bool Mapper::MarginalizeNodeFromGraph( const Eigen::Index marginalized_block_index = block_index_of(vertex_to_marginalize); const Eigen::SparseMatrix marginal_information_matrix = - ComputeMarginalInformationMatrix( + contrib::ComputeMarginalInformationMatrix( information_matrix, marginalized_block_index, block_size); - // (3) Compute marginal covariance local to the elimination clique + // (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 @@ -3213,12 +3030,13 @@ kt_bool Mapper::MarginalizeNodeFromGraph( } } const Eigen::SparseMatrix local_marginal_covariance_matrix = - ComputeSparseInverse(ArrangeView(marginal_information_matrix, - elimination_clique_indices, - elimination_clique_indices).eval()); - // (4) Remove vertex being marginalized + 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 edges in the subgraph induced by the elimination clique + // (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 = @@ -3233,11 +3051,11 @@ kt_bool Mapper::MarginalizeNodeFromGraph( } } } - // (6) Compute Chow-Liu tree approximation to the elimination clique + // (6) Compute Chow-Liu tree approximation to the elimination clique. std::vector *> chow_liu_tree_approximation = - ComputeChowLiuTreeApproximation(elimination_clique, - local_marginal_covariance_matrix); - // (7) Push tree edges to graph and solver (as constraints) + 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) { assert(m_pGraph->AddEdge(edge)); m_pScanOptimizer->AddConstraint(edge); diff --git a/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp b/lib/karto_sdk/src/contrib/ChowLiuTreeApprox.cpp new file mode 100644 index 000000000..2924ee623 --- /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 +#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 (a) and which is the variable discarded (b). + 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 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 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; + relative_pose.covariance = Matrix3( + transform_jacobian * joint_pose_covariance * + transform_jacobian.transpose()); + return relative_pose; +} + +std::vector *> ComputeChowLiuTreeApproximation( + const std::vector *> & clique, + const Eigen::SparseMatrix & 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>; + 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::edge_descriptor; + std::vector minimum_spanning_tree_edges; + boost::kruskal_minimum_spanning_tree( + clique_subgraph, std::back_inserter(minimum_spanning_tree_edges)); + using VertexDescriptorT = + boost::graph_traits::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 *> 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(clique[i], clique[j]); + Eigen::Matrix 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