Document/refine

Author: dellaert

gtsam/linear/MultifrontalClique.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/linear/MultifrontalClique.h>
 
 #include <algorithm>
+#include <cassert>
 #include <iostream>
 #include <set>
 #include <stdexcept>
@@ -172,7 +173,6 @@ std::vector<size_t> MultifrontalClique::parentIndicesFor(
     throw std::runtime_error(
         "MultifrontalSolver: separator key not found in parent clique");
   }
-  indices.push_back(parent.frontals().size() + parent.separatorKeys_.size());
   return indices;
 }
 
@@ -184,7 +184,6 @@ void MultifrontalClique::initializeMatrices(
 }
 
 void MultifrontalClique::fillAb(const GaussianFactorGraph& graph) {
-  sbm_.blockStart() = 0;
   sbm_.setZero();
 
   // We only overwrite the fixed sparsity pattern, so Ab must be zeroed once in
@@ -222,30 +221,40 @@ void MultifrontalClique::fillAb(const GaussianFactorGraph& graph) {
   }
 }
 
-void MultifrontalClique::eliminateClique() {
+void MultifrontalClique::eliminate() {
+  // Update SBM with the local factors, Ab^T * Ab
   sbm_.selfadjointView().rankUpdate(Ab_.matrix().transpose());
+  
+  // Form normal equations and factor the frontal block (Schur complement step).
   sbm_.choleskyPartial(frontals().size());
 
   auto parent = parent_.lock();
   if (!parent) return;
-  const size_t nFrontals = frontals().size();
-  const size_t nSeparatorBlocks = separatorKeys_.size();
-  for (size_t i = 0; i <= nSeparatorBlocks; ++i) {
-    size_t p_i = parentIndices_[i];
-    parent->sbm_.updateDiagonalBlock(p_i, sbm_.diagonalBlock(nFrontals + i));
-    for (size_t j = i + 1; j <= nSeparatorBlocks; ++j) {
-      size_t p_j = parentIndices_[j];
-      parent->sbm_.updateOffDiagonalBlock(
-          p_i, p_j, sbm_.aboveDiagonalBlock(nFrontals + i, nFrontals + j));
+  // Expose only the separator+RHS view when contributing to the parent.
+  sbm_.blockStart() = frontals().size();
+  parent->updateWith(sbm_, parentIndices_);
+  sbm_.blockStart() = 0;
+}
+
+void MultifrontalClique::updateWith(const SymmetricBlockMatrix& separator,
+                                    const std::vector<size_t>& indices) {
+  const size_t numBlocks = indices.size();
+  assert(separator.nBlocks() == numBlocks + 1);
+  const size_t rhsBlock = sbm_.nBlocks() - 1;
+
+  for (size_t i = 0; i <= numBlocks; ++i) {
+    const size_t p_i = (i < numBlocks) ? indices[i] : rhsBlock;
+    sbm_.updateDiagonalBlock(p_i, separator.diagonalBlock(i));
+    for (size_t j = i + 1; j <= numBlocks; ++j) {
+      const size_t p_j = (j < numBlocks) ? indices[j] : rhsBlock;
+      sbm_.updateOffDiagonalBlock(p_i, p_j, separator.aboveDiagonalBlock(i, j));
     }
   }
 }
 
-void MultifrontalClique::solveClique() const {
+void MultifrontalClique::solve() const {
   // Solve with block back-substitution on the Cholesky-stored SBM, avoiding
   // materializing an explicit R matrix or split representation.
-  const auto oldStart = sbm_.blockStart();
-  sbm_.blockStart() = 0;
   const size_t nFrontals = frontalPtrs_.size();
   const size_t nSeparators = separatorPtrs_.size();
 
@@ -280,7 +289,6 @@ void MultifrontalClique::solveClique() const {
     values->noalias() = rhsScratch_.segment(offset, dim);
     offset += dim;
   }
-  sbm_.blockStart() = oldStart;
 }
 
 void MultifrontalClique::print(const std::string& s,

gtsam/linear/MultifrontalClique.h

@@ -72,6 +72,7 @@ class GTSAM_EXPORT MultifrontalClique {
 
   /// Compute parent indices for all children after separators are finalized.
   void assignParentIndicesForChildren();
+
   /// Cache pointers to frontal and separator update vectors.
   void cacheValuePointers(VectorValues* delta);
 
@@ -90,7 +91,7 @@ class GTSAM_EXPORT MultifrontalClique {
   void fillAb(const GaussianFactorGraph& graph);
   /// @}
 
-  /// @name Read-only accessors
+  /// @name Read-only methods
   /// @{
 
   /// Get the frontal keys for this clique.
@@ -120,41 +121,71 @@ class GTSAM_EXPORT MultifrontalClique {
   /// Get the symmetric block matrix (const).
   const SymmetricBlockMatrix& sbm() const { return sbm_; }
 
-  /// Get the parent indices for scatter operations.
-  const std::vector<size_t>& parentIndices() const { return parentIndices_; }
-  /// @}
-
-  /// @name Solve (non-const)
-  /// @{
-
-  /// Eliminate this clique and propagate to its parent.
-  void eliminateClique();
-
-  /// Solve for the variables in this clique and update the solution vector.
-  void solveClique() const;
-  /// @}
 
-  /// Compute block dimensions from variable dimensions (excluding RHS).
-  /// @param dims Variable dimensions.
-  /// @return Block dimensions for this clique.
+  /**
+   * Compute block dimensions from variable dimensions (excluding RHS).
+   * @param dims Variable dimensions.
+   * @return Block dimensions for this clique.
+   */
   std::vector<size_t> blockDims(const std::map<Key, size_t>& dims) const;
 
-  /// Count rows needed for the vertical block matrix.
-  /// @param graph The factor graph.
-  /// @return Total number of rows.
+  /**
+   * Count rows needed for the vertical block matrix.
+   * @param graph The factor graph.
+   * @return Total number of rows.
+   */
   size_t countRows(const GaussianFactorGraph& graph) const;
 
-  /// Compute parent scatter indices for this clique.
-  /// @param parent The parent clique.
-  /// @return Parent indices for scatter operations.
+  /**
+   * Compute parent scatter indices for this clique.
+   * @param parent The parent clique.
+   * @return Parent indices for separator blocks (excluding RHS).
+   */
   std::vector<size_t> parentIndicesFor(const MultifrontalClique& parent) const;
 
-  /// Print this clique.
-  /// @param s Optional string prefix.
-  /// @param keyFormatter Key formatter for printing.
+  /**
+   * Print this clique.
+   * @param s Optional string prefix.
+   * @param keyFormatter Key formatter for printing.
+   */
   void print(const std::string& s = "",
              const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
+  /// @}
+
+  /// @name Solve (non-const)
+  /// @{
+
+  /**
+   * Eliminate this clique and propagate its separator contribution upward.
+   *
+   * Computes the local normal equations (SBM) from the stacked Jacobian (Ab),
+   * performs partial Cholesky on the frontal blocks, and then updates the
+   * parent's SBM using only the separator view (plus RHS) of this clique.
+   * Requires parent indices to be precomputed.
+   */
+  void eliminate();
+
+  /**
+   * Update this clique using a child's contribution.
+   * @param separator Child clique's SBM restricted to its separator blocks,
+   * with the RHS block appended as the last block.
+   * @param indices Mapping from the child's separator blocks into this
+   * parent's SBM block indices (RHS is implicit).
+   */
+  void updateWith(const SymmetricBlockMatrix& separator,
+                  const std::vector<size_t>& indices);
+
+  /**
+   * Solve for this clique's frontal variables and write them back to the
+   * cached solution vectors.
+   *
+   * Uses block back-substitution using the upper triangular-part of the
+   * Cholesky-stored SBM, solving the triangular system for the frontal blocks.
+   */
+  void solve() const;
+  /// @}
+
  private:
   void setParentIndices(const std::vector<size_t>& indices) {
     parentIndices_ = indices;

gtsam/linear/MultifrontalSolver.cpp

@@ -142,14 +142,14 @@ void MultifrontalSolver::load(const GaussianFactorGraph& graph) {
 /* ************************************************************************* */
 void MultifrontalSolver::eliminate() {
   for (auto& clique : postOrderCliques_) {
-    clique->eliminateClique();
+    clique->eliminate();
   }
 }
 
 /* ************************************************************************* */
 const VectorValues& MultifrontalSolver::solve() const {
   for (const auto& clique : cliques_) {
-    clique->solveClique();
+    clique->solve();
   }
   return solution_;
 }

gtsam/linear/VectorValues.h

@@ -289,11 +289,11 @@ namespace gtsam {
     Vector vector(const CONTAINER& keys) const {
       DenseIndex totalDim = 0;
       FastVector<const Vector*> items;
-      items.reserve(keys.end() - keys.begin());
+      items.reserve(keys.size());
       for (Key key : keys) {
         const Vector* v = &at(key);
         totalDim += v->size();
-        items.push_back(v);
+        items.emplace_back(v);
       }
 
       Vector result(totalDim);

timing/timeMultifrontalSolver.cpp

@@ -50,26 +50,28 @@ static void runMultifrontalSolver(const GaussianFactorGraph& smoother,
 }
 
 int main() {
-  const size_t T = 500;
-  GaussianFactorGraph smoother = createSmoother(T);
-  const Ordering ordering = Ordering::Metis(smoother);
+  const std::vector<size_t> T_values = {10, 50, 100, 500, 1000, 5000};
   const size_t iterations = 1000;
 
-  auto start = std::chrono::high_resolution_clock::now();
-  runStandardSolver(smoother, ordering, iterations);
-  auto end = std::chrono::high_resolution_clock::now();
-  std::chrono::duration<double> t_standard = end - start;
+  for (size_t T : T_values) {
+    GaussianFactorGraph smoother = createSmoother(T);
+    const Ordering ordering = Ordering::Metis(smoother);
 
-  start = std::chrono::high_resolution_clock::now();
-  runMultifrontalSolver(smoother, ordering, iterations);
-  end = std::chrono::high_resolution_clock::now();
-  std::chrono::duration<double> t_imperative = end - start;
+    auto start = std::chrono::high_resolution_clock::now();
+    runStandardSolver(smoother, ordering, iterations);
+    auto end = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> t_standard = end - start;
 
-  cout << "\nBenchmark (T=" << T << ", iterations=" << iterations << "):\n";
-  cout << "  Standard GTSAM:     " << t_standard.count() << " s\n";
-  cout << "  MultifrontalSolver: " << t_imperative.count() << " s\n";
-  cout << "  Speedup:            " << t_standard.count() / t_imperative.count()
-       << "x\n";
+    start = std::chrono::high_resolution_clock::now();
+    runMultifrontalSolver(smoother, ordering, iterations);
+    end = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> t_imperative = end - start;
 
+    cout << "\nBenchmark (T=" << T << ", iterations=" << iterations << "):\n";
+    cout << "  Standard GTSAM:     " << t_standard.count() << " s\n";
+    cout << "  MultifrontalSolver: " << t_imperative.count() << " s\n";
+    cout << "  Speedup:            "
+         << t_standard.count() / t_imperative.count() << "x\n";
+  }
   return 0;
 }