Merge pull request #2329 from borglab/feature/collect_and_gemv

Author: ProfFan

gtsam/base/ConcurrentMap.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/global_includes.h>
+#include <gtsam/base/FastMap.h>
 
 // Change class depending on whether we are using TBB
 #ifdef GTSAM_USE_TBB
@@ -41,8 +42,7 @@ using ConcurrentMapBase = tbb::concurrent_unordered_map<
 
 #else
 
-// If we're not using TBB, use a FastMap for ConcurrentMap
-#include <gtsam/base/FastMap.h>
+// If we're not using TBB, use a std::map
 template <typename KEY, typename VALUE>
 using ConcurrentMapBase = gtsam::FastMap<KEY, VALUE>;
 

gtsam/inference/BayesTree-inst.h

@@ -269,8 +269,35 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CLIQUE>
   bool BayesTree<CLIQUE>::equals(const BayesTree<CLIQUE>& other, double tol) const {
-    return size()==other.size() &&
-      std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>);
+    // Compare number of cliques first.
+    if (size() != other.size())
+      return false;
+
+    // Compare number of variables (nodes index size).
+    if (nodes_.size() != other.nodes_.size())
+      return false;
+
+    // Compare cliques by key so equality does not depend on the
+    // iteration order of the underlying ConcurrentMap.
+    for (const auto& kv : nodes_) {
+      const Key key = kv.first;
+      const sharedClique& clique = kv.second;
+
+      auto it = other.nodes_.find(key);
+      if (it == other.nodes_.end())
+        return false;
+
+      const sharedClique& otherClique = it->second;
+
+      if (!clique && !otherClique)
+        continue;
+      if (!clique || !otherClique)
+        return false;
+      if (!clique->equals(*otherClique, tol))
+        return false;
+    }
+
+    return true;
   }
 
   /* ************************************************************************* */

gtsam/linear/Errors.cpp

@@ -27,9 +27,9 @@ namespace gtsam {
 /* ************************************************************************* */
 Errors createErrors(const VectorValues& V) {
   Errors result;
-  for (const auto& [key, e] : V) {
-    result.push_back(e);
-  }
+  // Use a key-sorted view of VectorValues so the resulting Errors
+  // order is deterministic and independent of the underlying map.
+  for (const auto& [key, e] : V.sorted()) result.push_back(e);
   return result;
 }
 

gtsam/linear/GaussianConditional.cpp

@@ -305,23 +305,36 @@ namespace gtsam {
     const Vector x =
         frontalValues.vector(KeyVector(beginFrontals(), endFrontals()));
 
-    // Copy the augmented Jacobian matrix:
-    auto newAb = Ab_;
+    // Compute updated right-hand side: d - R * x
+    const auto RR = R().triangularView<Eigen::Upper>();
+    const Vector rhs = d() - RR * x;
 
-    // Restrict view to parent blocks
-    newAb.firstBlock() += nrFrontals_;
+    // Collect parent dimensions
+    FastVector<DenseIndex> parentDims;
+    parentDims.reserve(nrParents());
+    for (auto it = beginParents(); it != endParents(); ++it) {
+      parentDims.push_back(getDim(it));
+    }
 
-    // Update right-hand-side (last column)
-    auto last = newAb.matrix().cols() - 1;
-    const auto RR = R().triangularView<Eigen::Upper>();
-    newAb.matrix().col(last) -= RR * x;
+    // Build a VerticalBlockMatrix containing only parent blocks and RHS.
+    const DenseIndex m = rows();
+    VerticalBlockMatrix newAb(parentDims, m, true);
+
+    // Copy parent blocks (S matrices).
+    DenseIndex blockIndex = 0;
+    for (auto it = beginParents(); it != endParents(); ++it, ++blockIndex) {
+      newAb(blockIndex) = S(it);
+    }
+
+    // Set the RHS block.
+    const DenseIndex lastBlock = newAb.nBlocks() - 1;
+    newAb(lastBlock).col(0) = rhs;
 
     // The keys now do not include the frontal keys:
     KeyVector newKeys;
     newKeys.reserve(nrParents());
     for (auto&& key : parents()) newKeys.push_back(key);
 
-    // Hopefully second newAb copy below is optimized out...
     return std::make_shared<JacobianFactor>(newKeys, newAb, model_);
   }
 

gtsam/linear/JacobianFactor.cpp

@@ -491,10 +491,17 @@ bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const {
 
 /* ************************************************************************* */
 Vector JacobianFactor::unweighted_error(const VectorValues& c) const {
-  Vector e = -getb();
-  for (size_t pos = 0; pos < size(); ++pos)
-    e += Ab_(pos) * c[keys_[pos]];
-  return e;
+  const DenseIndex totalDim = c.totalDim(keys_);
+  Vector w(totalDim + 1);
+  c.fillVector(keys_, w);
+  w(totalDim) = -1.0;
+  // Fast path when the active view is the full matrix (no row/column offsets).
+  if (Ab_.firstBlock() == 0 && Ab_.rowStart() == 0 &&
+      Ab_.rowEnd() == Ab_.matrix().rows()) {
+    return Ab_.matrix() * w;
+  }
+  // Fallback that respects firstBlock/rowStart/rowEnd for subviews.
+  return Ab_.full() * w;
 }
 
 /* ************************************************************************* */

gtsam/linear/SubgraphPreconditioner.cpp

@@ -139,8 +139,9 @@ Errors SubgraphPreconditioner::operator*(const VectorValues &y) const {
 void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
 
   Errors::iterator ei = e.begin();
-  for(const auto& key_value: y) {
-    *ei = key_value.second;
+  // Fill the identity-part errors in key-sorted order, to match createErrors.
+  for (const auto& [key, value] : y.sorted()) {
+    *ei = value;
     ++ei;
   }
 
@@ -155,8 +156,9 @@ VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
 
   Errors::const_iterator it = e.begin();
   VectorValues y = zero();
-  for(auto& key_value: y) {
-    key_value.second = *it;
+  // Map the identity-part errors back into y using key-sorted order.
+  for (const auto& [key, value] : y.sorted()) {
+    y.at(key) = *it;
     ++it;
   }
   transposeMultiplyAdd2(1.0, it, e.end(), y);
@@ -169,9 +171,11 @@ void SubgraphPreconditioner::transposeMultiplyAdd
 (double alpha, const Errors& e, VectorValues& y) const {
 
   Errors::const_iterator it = e.begin();
-  for(auto& key_value: y) {
+  // Add the identity-part contribution in key-sorted order so it
+  // matches the layout produced by createErrors().
+  for (const auto& [key, value] : y.sorted()) {
     const Vector& ei = *it;
-    key_value.second += alpha * ei;
+    y.at(key) += alpha * ei;
     ++it;
   }
   transposeMultiplyAdd2(alpha, it, e.end(), y);