Refactoring to get grouped factors to compile again

gtsam/slam/RegularHessianFactor.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/linear/HessianFactor.h>
+#include <gtsam/slam/RegularJacobianFactor.h>
 #include <boost/foreach.hpp>
 #include <vector>
 
@@ -27,15 +28,11 @@ namespace gtsam {
 template<size_t D>
 class RegularHessianFactor: public HessianFactor {
 
-private:
-
-  // Use Eigen magic to access raw memory
-  typedef Eigen::Matrix<double, D, 1> DVector;
-  typedef Eigen::Map<DVector> DMap;
-  typedef Eigen::Map<const DVector> ConstDMap;
-
 public:
 
+  typedef Eigen::Matrix<double, D, 1> VectorD;
+  typedef Eigen::Matrix<double, D, D> MatrixD;
+
   /** Construct an n-way factor.  Gs contains the upper-triangle blocks of the
    * quadratic term (the Hessian matrix) provided in row-order, gs the pieces
    * of the linear vector term, and f the constant term.
@@ -43,27 +40,68 @@ public:
   RegularHessianFactor(const std::vector<Key>& js,
       const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
       HessianFactor(js, Gs, gs, f) {
+    checkInvariants();
   }
 
   /** Construct a binary factor.  Gxx are the upper-triangle blocks of the
    * quadratic term (the Hessian matrix), gx the pieces of the linear vector
    * term, and f the constant term.
    */
-  RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12,
+  RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12,
-      const Vector& g1, const Matrix& G22, const Vector& g2, double f) :
+      const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) :
       HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
   }
 
+  /** Construct a ternary factor.  Gxx are the upper-triangle blocks of the
+   * quadratic term (the Hessian matrix), gx the pieces of the linear vector
+   * term, and f the constant term.
+   */
+  RegularHessianFactor(Key j1, Key j2, Key j3,
+      const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1,
+      const MatrixD& G22, const MatrixD& G23, const VectorD& g2,
+      const MatrixD& G33, const VectorD& g3, double f) :
+    HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
+  }
+
   /** Constructor with an arbitrary number of keys and with the augmented information matrix
    *   specified as a block matrix. */
   template<typename KEYS>
   RegularHessianFactor(const KEYS& keys,
       const SymmetricBlockMatrix& augmentedInformation) :
       HessianFactor(keys, augmentedInformation) {
+    checkInvariants();
   }
 
+  /// Construct from RegularJacobianFactor
+  RegularHessianFactor(const RegularJacobianFactor<D>& jf)
+      : HessianFactor(jf) {}
+
+  /// Construct from a GaussianFactorGraph
+  RegularHessianFactor(const GaussianFactorGraph& factors,
+                       boost::optional<const Scatter&> scatter = boost::none)
+      : HessianFactor(factors, scatter) {
+    checkInvariants();
+  }
+
+private:
+
+  /// Check invariants after construction
+  void checkInvariants() {
+    if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D)
+      throw std::invalid_argument(
+           "RegularHessianFactor constructor was given non-regular factors");
+  }
+
+  // Use Eigen magic to access raw memory
+  typedef Eigen::Map<VectorD> DMap;
+  typedef Eigen::Map<const VectorD> ConstDMap;
+
   // Scratch space for multiplyHessianAdd
-  mutable std::vector<DVector> y;
+  // According to link below this is thread-safe.
+  // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
+  mutable std::vector<VectorD> y_;
+
+public:
 
   /** y += alpha * A'*A*x */
   virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
@@ -74,32 +112,32 @@ public:
   /** y += alpha * A'*A*x */
   void multiplyHessianAdd(double alpha, const double* x,
       double* yvalues) const {
-    // Create a vector of temporary y values, corresponding to rows i
+    // Create a vector of temporary y_ values, corresponding to rows i
-    y.resize(size());
+    y_.resize(size());
-    BOOST_FOREACH(DVector & yi, y)
+    BOOST_FOREACH(VectorD & yi, y_)
       yi.setZero();
 
     // Accessing the VectorValues one by one is expensive
     // So we will loop over columns to access x only once per column
-    // And fill the above temporary y values, to be added into yvalues after
+    // And fill the above temporary y_ values, to be added into yvalues after
-    DVector xj(D);
+    VectorD xj(D);
     for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
       Key key = keys_[j];
       const double* xj = x + key * D;
       DenseIndex i = 0;
       for (; i < j; ++i)
-        y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
+        y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
       // blocks on the diagonal are only half
-      y[i] += info_(j, j).selfadjointView() * ConstDMap(xj);
+      y_[i] += info_(j, j).selfadjointView() * ConstDMap(xj);
       // for below diagonal, we take transpose block from upper triangular part
       for (i = j + 1; i < (DenseIndex) size(); ++i)
-        y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
+        y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
     }
 
     // copy to yvalues
     for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
       Key key = keys_[i];
-      DMap(yvalues + key * D) += alpha * y[i];
+      DMap(yvalues + key * D) += alpha * y_[i];
     }
   }
 
@@ -107,28 +145,27 @@ public:
   void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
       std::vector<size_t> offsets) const {
 
-    // Create a vector of temporary y values, corresponding to rows i
+    // Create a vector of temporary y_ values, corresponding to rows i
-    std::vector<Vector> y;
+    y_.resize(size());
-    y.reserve(size());
+    BOOST_FOREACH(VectorD & yi, y_)
-    for (const_iterator it = begin(); it != end(); it++)
+      yi.setZero();
-      y.push_back(zero(getDim(it)));
 
     // Accessing the VectorValues one by one is expensive
     // So we will loop over columns to access x only once per column
-    // And fill the above temporary y values, to be added into yvalues after
+    // And fill the above temporary y_ values, to be added into yvalues after
     for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
       DenseIndex i = 0;
       for (; i < j; ++i)
-        y[i] += info_(i, j).knownOffDiagonal()
+        y_[i] += info_(i, j).knownOffDiagonal()
             * ConstDMap(x + offsets[keys_[j]],
                 offsets[keys_[j] + 1] - offsets[keys_[j]]);
       // blocks on the diagonal are only half
-      y[i] += info_(j, j).selfadjointView()
+      y_[i] += info_(j, j).selfadjointView()
           * ConstDMap(x + offsets[keys_[j]],
               offsets[keys_[j] + 1] - offsets[keys_[j]]);
       // for below diagonal, we take transpose block from upper triangular part
       for (i = j + 1; i < (DenseIndex) size(); ++i)
-        y[i] += info_(i, j).knownOffDiagonal()
+        y_[i] += info_(i, j).knownOffDiagonal()
             * ConstDMap(x + offsets[keys_[j]],
                 offsets[keys_[j] + 1] - offsets[keys_[j]]);
     }
@@ -136,7 +173,7 @@ public:
     // copy to yvalues
     for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
       DMap(yvalues + offsets[keys_[i]],
-          offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i];
+          offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i];
   }
 
   /** Return the diagonal of the Hessian for this factor (raw memory version) */
@@ -158,7 +195,7 @@ public:
     for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
       Key j = keys_[pos];
       // Get the diagonal block, and insert its diagonal
-      DVector dj = -info_(pos, size()).knownOffDiagonal();
+      VectorD dj = -info_(pos, size()).knownOffDiagonal();
       DMap(d + D * j) += dj;
     }
   }

gtsam/slam/tests/testRegularHessianFactor.cpp

@@ -16,6 +16,7 @@
  */
 
 #include <gtsam/slam/RegularHessianFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 
 #include <CppUnitLite/TestHarness.h>
@@ -29,30 +30,58 @@ using namespace gtsam;
 using namespace boost::assign;
 
 /* ************************************************************************* */
-TEST(RegularHessianFactor, ConstructorNWay)
+TEST(RegularHessianFactor, Constructors)
 {
-  Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished();
+  // First construct a regular JacobianFactor
-  Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished();
+  Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2;
-  Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished();
+  Vector2 b(1,2);
+  vector<pair<Key, Matrix> > terms;
+  terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3);
+  RegularJacobianFactor<2> jf(terms, b);
 
-  Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished();
+  // Test conversion from JacobianFactor
-  Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished();
+  RegularHessianFactor<2> factor(jf);
 
-  Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished();
+  Matrix G11 = I_2x2;
+  Matrix G12 = I_2x2;
+  Matrix G13 = I_2x2;
 
-  Vector g1 = (Vector(2) << -7, -9).finished();
+  Matrix G22 = I_2x2;
-  Vector g2 = (Vector(2) << -9,  1).finished();
+  Matrix G23 = I_2x2;
-  Vector g3 = (Vector(2) <<  2,  3).finished();
+
+  Matrix G33 = I_2x2;
+
+  Vector2 g1 = b, g2 = b, g3 = b;
 
   double f = 10;
 
-  std::vector<Key> js;
+  // Test ternary constructor
-  js.push_back(0); js.push_back(1); js.push_back(3);
+  RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
-  std::vector<Matrix> Gs;
+  EXPECT(assert_equal(factor,factor2));
-  Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
+
-  std::vector<Vector> gs;
+  // Test n-way constructor
-  gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
+  vector<Key> keys; keys += 0, 1, 3;
-  RegularHessianFactor<2> factor(js, Gs, gs, f);
+  vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
+  vector<Vector> gs; gs += g1, g2, g3;
+  RegularHessianFactor<2> factor3(keys, Gs, gs, f);
+  EXPECT(assert_equal(factor, factor3));
+
+  // Test constructor from Gaussian Factor Graph
+  GaussianFactorGraph gfg;
+  gfg += jf;
+  RegularHessianFactor<2> factor4(gfg);
+  EXPECT(assert_equal(factor, factor4));
+  GaussianFactorGraph gfg2;
+  gfg2 += factor;
+  RegularHessianFactor<2> factor5(gfg);
+  EXPECT(assert_equal(factor, factor5));
+
+  // Test constructor from Information matrix
+  Matrix info = factor.augmentedInformation();
+  vector<size_t> dims; dims += 2, 2, 2;
+  SymmetricBlockMatrix sym(dims, info, true);
+  RegularHessianFactor<2> factor6(keys, sym);
+  EXPECT(assert_equal(factor, factor6));
 
   // multiplyHessianAdd:
   {
@@ -61,13 +90,13 @@ TEST(RegularHessianFactor, ConstructorNWay)
   HessianFactor::const_iterator i1 = factor.begin();
   HessianFactor::const_iterator i2 = i1 + 1;
   Vector X(6); X << 1,2,3,4,5,6;
-  Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696;
+  Vector Y(6); Y << 9, 12, 9, 12, 9, 12;
   EXPECT(assert_equal(Y,AtA*X));
 
   VectorValues x = map_list_of<Key, Vector>
-    (0, (Vector(2) << 1,2).finished())
+    (0, Vector2(1,2))
-    (1, (Vector(2) << 3,4).finished())
+    (1, Vector2(3,4))
-    (3, (Vector(2) << 5,6).finished());
+    (3, Vector2(5,6));
 
   VectorValues expected;
   expected.insert(0, Y.segment<2>(0));
@@ -77,15 +106,15 @@ TEST(RegularHessianFactor, ConstructorNWay)
   // VectorValues version
   double alpha = 1.0;
   VectorValues actualVV;
-  actualVV.insert(0, zero(2));
+  actualVV.insert(0, Vector2::Zero());
-  actualVV.insert(1, zero(2));
+  actualVV.insert(1, Vector2::Zero());
-  actualVV.insert(3, zero(2));
+  actualVV.insert(3, Vector2::Zero());
   factor.multiplyHessianAdd(alpha, x, actualVV);
   EXPECT(assert_equal(expected, actualVV));
 
   // RAW ACCESS
-  Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
+  Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12;
-  Vector fast_y = gtsam::zero(8);
+  Vector fast_y = Vector8::Zero();
   double xvalues[8] = {1,2,3,4,0,0,5,6};
   factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
   EXPECT(assert_equal(expected_y, fast_y));