Sidestep Scatter altogether and just use HessianFactor keys_. In theory, n^3 lookup cost, but in practice (as keys is contiguous memory) just as fast as map.

gtsam/linear/GaussianFactor.h

@@ -126,7 +126,7 @@ namespace gtsam {
      * @param scatter A mapping from variable index to slot index in this HessianFactor
      * @param info The information matrix to be updated
      */
-    virtual void updateHessian(const Scatter& scatter,
+    virtual void updateHessian(const FastVector<Key>& keys,
                            SymmetricBlockMatrix* info) const = 0;
 
     /// y += alpha * A'*A*x
@@ -141,6 +141,12 @@ namespace gtsam {
     /// Gradient wrt a key at any values
     virtual Vector gradient(Key key, const VectorValues& x) const = 0;
 
+    // Determine position of a given key
+    template <typename CONTAINER>
+    static DenseIndex Slot(const CONTAINER& keys, Key key) {
+      return std::find(keys.begin(), keys.end(), key) - keys.begin();
+    }
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;
@@ -150,7 +156,7 @@ namespace gtsam {
     }
 
   }; // GaussianFactor
-  
+
 /// traits
 template<>
 struct traits<GaussianFactor> : public Testable<GaussianFactor> {

gtsam/linear/HessianFactor.cpp

@@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
   gttic(update);
   BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
     if(factor)
-      factor->updateHessian(*scatter, &info_);
+      factor->updateHessian(keys_, &info_);
   gttoc(update);
 }
 
@@ -346,15 +346,15 @@ double HessianFactor::error(const VectorValues& c) const {
 }
 
 /* ************************************************************************* */
-void HessianFactor::updateHessian(const Scatter& scatter,
+void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
                                   SymmetricBlockMatrix* info) const {
   gttic(updateHessian_HessianFactor);
   // Apply updates to the upper triangle
   DenseIndex n = size(), N = info->nBlocks()-1;
   for (DenseIndex j = 0; j <= n; ++j) {
-    const DenseIndex J = j==n ? N : scatter.slot(keys_[j]);
+    const DenseIndex J = (j==n) ? N :  Slot(infoKeys, keys_[j]);
     for (DenseIndex i = 0; i <= j; ++i) {
-      const DenseIndex I = i==n ? N : scatter.slot(keys_[i]);
+      const DenseIndex I = (i==n) ? N : Slot(infoKeys, keys_[i]);
       (*info)(I, J) += info_(i, j);
     }
   }

gtsam/linear/HessianFactor.h

@@ -344,7 +344,7 @@ namespace gtsam {
      * @param scatter A mapping from variable index to slot index in this HessianFactor
      * @param info The information matrix to be updated
      */
-    void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const;
+    void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
 
     /** y += alpha * A'*A*x */
     void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;

gtsam/linear/JacobianFactor.cpp

@@ -498,7 +498,7 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
 }
 
 /* ************************************************************************* */
-void JacobianFactor::updateHessian(const Scatter& scatter,
+void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
                                    SymmetricBlockMatrix* info) const {
   gttic(updateHessian_JacobianFactor);
 
@@ -512,7 +512,7 @@ void JacobianFactor::updateHessian(const Scatter& scatter,
           "JacobianFactor::updateHessian: cannot update information with "
           "constrained noise model");
     JacobianFactor whitenedFactor = whiten();
-    whitenedFactor.updateHessian(scatter, info);
+    whitenedFactor.updateHessian(infoKeys, info);
   } else {
     // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below
     DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1;
@@ -520,10 +520,10 @@ void JacobianFactor::updateHessian(const Scatter& scatter,
     // Apply updates to the upper triangle
     // Loop over blocks of A, including RHS with j==n
     for (DenseIndex j = 0; j <= n; ++j) {
-      const DenseIndex J = j==n ? N : scatter.slot(keys_[j]);
+      const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]);
       // Fill off-diagonal blocks with Ai'*Aj
       for (DenseIndex i = 0; i < j; ++i) {
-        const DenseIndex I = scatter.slot(keys_[i]);
+        const DenseIndex I = Slot(infoKeys, keys_[i]);
         (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j);
       }
       // Fill diagonal block with Aj'*Aj

gtsam/linear/JacobianFactor.h

@@ -278,7 +278,7 @@ namespace gtsam {
      * @param scatter A mapping from variable index to slot index in this HessianFactor
      * @param info The information matrix to be updated
      */
-    void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const;
+    void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
 
     /** Return A*x */
     Vector operator*(const VectorValues& x) const;

gtsam/linear/tests/testHessianFactor.cpp

@@ -38,6 +38,16 @@ using namespace gtsam;
 
 const double tol = 1e-5;
 
+/* ************************************************************************* */
+TEST(HessianFactor, Slot)
+{
+  FastVector<Key> keys = list_of(2)(4)(1);
+  EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
+  EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
+  EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
+  EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist
+}
+
 /* ************************************************************************* */
 TEST(HessianFactor, emptyConstructor)
 {
@@ -302,15 +312,17 @@ TEST(HessianFactor, CombineAndEliminate)
   gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
   gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
 
-  Matrix zero3x3 = zeros(3,3);
+  Matrix93 A0; A0 << A10, Z_3x3, Z_3x3;
-  Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
+  Matrix93 A1; A1 << A11, A01, A21;
-  Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
   Vector9 b; b << b1, b0, b2;
   Vector9 sigmas; sigmas << s1, s0, s2;
 
   // create a full, uneliminated version of the factor
   JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
 
+  // Make sure combining works
+  EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6));
+
   // perform elimination on jacobian
   GaussianConditional::shared_ptr expectedConditional;
   JacobianFactor::shared_ptr expectedRemainingFactor;

gtsam/slam/GeneralSFMFactor.h

@@ -149,7 +149,7 @@ namespace gtsam {
           : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {}
 
       // Fixed-size matrix update
-      void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const {
+      void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
         gttic(updateHessian_LinearizedFactor);
 
         // Whiten the factor if it has a noise model
@@ -160,15 +160,12 @@ namespace gtsam {
                 "JacobianFactor::updateHessian: cannot update information with "
                 "constrained noise model");
           JacobianFactor whitenedFactor = whiten();
-          whitenedFactor.updateHessian(scatter, info);
+          whitenedFactor.updateHessian(infoKeys, info);
         } else {
-          // N is number of variables in information matrix
-          DenseIndex N = info->nBlocks() - 1;
-
           // First build an array of slots
-          DenseIndex slotC = scatter.slot(keys_.front());
+          DenseIndex slotC = Slot(infoKeys, keys_.front());
-          DenseIndex slotL = scatter.slot(keys_.back());
+          DenseIndex slotL = Slot(infoKeys, keys_.back());
-          DenseIndex slotB = N;
+          DenseIndex slotB = info->nBlocks() - 1;
 
           // We perform I += A'*A to the upper triangle
           (*info)(slotC, slotC).knownOffDiagonal() += AC_.transpose() * AC_;

gtsam/slam/RegularImplicitSchurFactor.h

@@ -115,7 +115,7 @@ public:
     return D;
   }
 
-  virtual void updateHessian(const Scatter& scatter,
+  virtual void updateHessian(const FastVector<Key>& keys,
                          SymmetricBlockMatrix* info) const {
     throw std::runtime_error(
         "RegularImplicitSchurFactor::updateHessian non implemented");