Merge pull request #2332 from borglab/fix/constraints

Handle noise and NonlinearEquality constraints

Author: dellaert

gtsam/base/SymmetricBlockMatrix.cpp

@@ -107,6 +107,25 @@ VerticalBlockMatrix SymmetricBlockMatrix::split(DenseIndex nFrontals) {
 }
 
 /* ************************************************************************* */
+void SymmetricBlockMatrix::updateFromMappedBlocks(
+    const SymmetricBlockMatrix& other,
+    const std::vector<DenseIndex>& blockIndices) {
+  assert(static_cast<DenseIndex>(blockIndices.size()) == other.nBlocks());
+  const DenseIndex otherBlocks = other.nBlocks();
+  for (DenseIndex i = 0; i < otherBlocks; ++i) {
+    const DenseIndex I = blockIndices[i];
+    if (I < 0) continue;
+    assert(I < nBlocks());
+    updateDiagonalBlock(I, other.diagonalBlock(i));
+    for (DenseIndex j = i + 1; j < otherBlocks; ++j) {
+      const DenseIndex J = blockIndices[j];
+      if (J < 0) continue;
+      assert(J < nBlocks());
+      updateOffDiagonalBlock(I, J, other.aboveDiagonalBlock(i, j));
+    }
+  }
+}
 
-} //\ namespace gtsam
+/* ************************************************************************* */
 
+} //\ namespace gtsam

gtsam/base/SymmetricBlockMatrix.h

@@ -27,6 +27,7 @@
 #include <cassert>
 #include <stdexcept>
 #include <array>
+#include <vector>
 
 namespace boost {
 namespace serialization {
@@ -245,6 +246,11 @@ namespace gtsam {
       }
     }
 
+    /// Update this matrix with blocks from another block matrix using a mapping.
+    /// Entries with index -1 are skipped.
+    void updateFromMappedBlocks(const SymmetricBlockMatrix& other,
+                                const std::vector<DenseIndex>& blockIndices);
+
     /// @}
     /// @name Accessing the full matrix.
     /// @{

gtsam/base/tests/testSymmetricBlockMatrix.cpp

@@ -32,6 +32,7 @@ static SymmetricBlockMatrix testBlockMatrix(
   6, 12, 18, 24, 30, 36).finished());
 
 /* ************************************************************************* */
+// Read block accessors.
 TEST(SymmetricBlockMatrix, ReadBlocks)
 {
   // On the diagonal
@@ -51,6 +52,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks)
 }
 
 /* ************************************************************************* */
+// Write block setters.
 TEST(SymmetricBlockMatrix, WriteBlocks)
 {
   // On the diagonal
@@ -77,6 +79,7 @@ TEST(SymmetricBlockMatrix, WriteBlocks)
 }
 
 /* ************************************************************************* */
+// Verify block range access.
 TEST(SymmetricBlockMatrix, Ranges)
 {
   // On the diagonal
@@ -97,6 +100,7 @@ TEST(SymmetricBlockMatrix, Ranges)
 }
 
 /* ************************************************************************* */
+// Exercise block expression helpers.
 TEST(SymmetricBlockMatrix, expressions)
 {
   const std::vector<size_t> dimensions{2, 3, 1};
@@ -151,6 +155,40 @@ TEST(SymmetricBlockMatrix, expressions)
 }
 
 /* ************************************************************************* */
+// Update via block mapping.
+TEST(SymmetricBlockMatrix, UpdateFromMappedBlocks)
+{
+  const std::vector<size_t> destDims{1, 3, 2};
+  const std::vector<DenseIndex> mapping{1, 2, 0};
+
+  SymmetricBlockMatrix actual(destDims);
+  actual.setZero();
+  actual.updateFromMappedBlocks(testBlockMatrix, mapping);
+
+  SymmetricBlockMatrix expected(destDims);
+  expected.setZero();
+  for (DenseIndex i = 0; i < testBlockMatrix.nBlocks(); ++i) {
+    const DenseIndex I = static_cast<DenseIndex>(mapping[i]);
+    expected.updateDiagonalBlock(I, testBlockMatrix.diagonalBlock(i));
+    for (DenseIndex j = i + 1; j < testBlockMatrix.nBlocks(); ++j) {
+      const DenseIndex J = static_cast<DenseIndex>(mapping[j]);
+      expected.setOffDiagonalBlock(I, J,
+                                   testBlockMatrix.aboveDiagonalBlock(i, j));
+    }
+  }
+  EXPECT(assert_equal(Matrix(expected.selfadjointView()),
+                      actual.selfadjointView()));
+
+  SymmetricBlockMatrix doubled(destDims);
+  doubled.setZero();
+  doubled.updateFromMappedBlocks(testBlockMatrix, mapping);
+  doubled.updateFromMappedBlocks(testBlockMatrix, mapping);
+  EXPECT(assert_equal(2.0 * Matrix(expected.selfadjointView()),
+                      Matrix(doubled.selfadjointView())));
+}
+
+/* ************************************************************************* */
+// In-place inversion path.
 TEST(SymmetricBlockMatrix, inverseInPlace) {
   // generate an invertible matrix
   const Vector3 a(1.0, 0.2, 2.0), b(0.3, 0.8, -1.0), c(0.1, 0.2, 0.7);
@@ -171,4 +209,3 @@ TEST(SymmetricBlockMatrix, inverseInPlace) {
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
-

gtsam/inference/ClusterTree-inst.h

@@ -40,6 +40,34 @@ std::vector<size_t> ClusterTree<GRAPH>::Cluster::nrFrontalsOfChildren() const {
   return nrFrontals;
 }
 
+/* ************************************************************************* */
+template <class GRAPH>
+KeySet ClusterTree<GRAPH>::Cluster::separatorKeys(KeySetMap* cache) const {
+  if (cache) {
+    auto it = cache->find(this);
+    if (it != cache->end()) return it->second;
+  }
+
+  KeySet keys;
+  for (const auto& factor : factors) {
+    if (!factor) continue;
+    keys.insert(factor->begin(), factor->end());
+  }
+  for (const auto& child : children) {
+    KeySet childSeparators = child->separatorKeys(cache);
+    keys.insert(childSeparators.begin(), childSeparators.end());
+  }
+  for (Key key : orderedFrontalKeys) {
+    keys.erase(key);
+  }
+
+  if (cache) {
+    auto result = cache->emplace(this, std::move(keys));
+    return result.first->second;
+  }
+  return keys;
+}
+
 /* ************************************************************************* */
 template <class GRAPH>
 void ClusterTree<GRAPH>::Cluster::merge(const std::shared_ptr<Cluster>& cluster) {

gtsam/inference/ClusterTree.h

@@ -10,6 +10,7 @@
 #pragma once
 
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/FastMap.h>
 #include <gtsam/base/FastVector.h>
 #include <gtsam/inference/Ordering.h>
 
@@ -96,6 +97,11 @@ class ClusterTree {
     /// Return a vector with nrFrontal keys for each child
     std::vector<size_t> nrFrontalsOfChildren() const;
 
+    using KeySetMap = FastMap<const Cluster*, KeySet>;
+
+    /// Return the separator keys (subtree keys minus frontals), optionally cached.
+    KeySet separatorKeys(KeySetMap* cache = nullptr) const;
+
     /// Merge in given cluster
     void merge(const std::shared_ptr<Cluster>& cluster);
 

gtsam/linear/HessianFactor.cpp

@@ -357,19 +357,7 @@ void HessianFactor::updateHessian(const KeyVector& infoKeys,
     slots[j] = Slot(infoKeys, keys_[j]);
   slots[nrVariablesInThisFactor] = info->nBlocks() - 1;
 
-  // Apply updates to the upper triangle
-  // Loop over this factor's blocks with indices (i,j)
-  // For every block (i,j), we determine the block (I,J) in info.
-  for (DenseIndex j = 0; j <= nrVariablesInThisFactor; ++j) {
-    const DenseIndex J = slots[j];
-    info->updateDiagonalBlock(J, info_.diagonalBlock(j));
-    for (DenseIndex i = 0; i < j; ++i) {
-      const DenseIndex I = slots[i];
-      assert(i < j);
-      assert(I != J);
-      info->updateOffDiagonalBlock(I, J, info_.aboveDiagonalBlock(i, j));
-    }
-  }
+  info->updateFromMappedBlocks(info_, slots);
 }
 
 /* ************************************************************************* */