Created BinaryJacobianFactor template

gtsam/linear/BinaryJacobianFactor.h

@@ -0,0 +1,91 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file BinaryJacobianFactor.h
+ *
+ * @brief A binary JacobianFactor specialization that uses fixed matrix math for speed
+ *
+ * @date June 2015
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/base/SymmetricBlockMatrix.h>
+#include <gtsam/base/timing.h>
+
+namespace gtsam {
+
+/**
+ * A binary JacobianFactor specialization that uses fixed matrix math for speed
+ */
+template<int M, int N1, int N2>
+struct BinaryJacobianFactor: JacobianFactor {
+
+  /// Constructor
+  BinaryJacobianFactor(Key key1, const Eigen::Matrix<double, M, N1>& A1,
+      Key key2, const Eigen::Matrix<double, M, N2>& A2,
+      const Eigen::Matrix<double, M, 1>& b, //
+      const SharedDiagonal& model = SharedDiagonal()) :
+      JacobianFactor(key1, A1, key2, A2, b, model) {
+  }
+
+  inline Key key1() const {
+    return keys_[0];
+  }
+  inline Key key2() const {
+    return keys_[1];
+  }
+
+  // Fixed-size matrix update
+  void updateHessian(const FastVector<Key>& infoKeys,
+      SymmetricBlockMatrix* info) const {
+    gttic(updateHessian_BinaryJacobianFactor);
+    // Whiten the factor if it has a noise model
+    const SharedDiagonal& model = get_model();
+    if (model && !model->isUnit()) {
+      if (model->isConstrained())
+        throw std::invalid_argument(
+            "BinaryJacobianFactor::updateHessian: cannot update information with "
+                "constrained noise model");
+      BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())),
+          key2(), model->Whiten(getA(end())), model->whiten(getb()));
+      whitenedFactor.updateHessian(infoKeys, info);
+    } else {
+      // First build an array of slots
+      DenseIndex slot1 = Slot(infoKeys, key1());
+      DenseIndex slot2 = Slot(infoKeys, key2());
+      DenseIndex slotB = info->nBlocks() - 1;
+
+      const Matrix& Ab = Ab_.matrix();
+      Eigen::Block<const Matrix, M, N1> A1(Ab, 0, 0);
+      Eigen::Block<const Matrix, M, N2> A2(Ab, 0, N1);
+      Eigen::Block<const Matrix, M, 1> b(Ab, 0, N1 + N2);
+
+      // We perform I += A'*A to the upper triangle
+      (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose());
+      (*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2;
+      (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b;
+      (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose());
+      (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b;
+      (*info)(slotB, slotB)(0, 0) += b.transpose() * b;
+    }
+  }
+};
+
+template<int M, int N1, int N2>
+struct traits<BinaryJacobianFactor<M, N1, N2> > : Testable<
+    BinaryJacobianFactor<M, N1, N2> > {
+};
+
+} //namespace gtsam

gtsam/slam/GeneralSFMFactor.h

@@ -25,7 +25,7 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/linear/HessianFactor.h>
+#include <gtsam/linear/BinaryJacobianFactor.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/base/concepts.h>
 #include <gtsam/base/Manifold.h>
@@ -133,56 +133,10 @@ public:
     }
   }
 
-  class BinaryJacobianFactor : public JacobianFactor {
-    // Fixed size matrices
-    // TODO(frank): implement generic BinaryJacobianFactor<N,M1,M2> next to
-    // JacobianFactor
-
-  public:
-    /// Constructor
-    BinaryJacobianFactor(Key key1, const JacobianC& A1, Key key2, const JacobianL& A2,
-        const Vector2& b,
-        const SharedDiagonal& model = SharedDiagonal())
-    : JacobianFactor(key1, A1, key2, A2, b, model) {}
-
-    // Fixed-size matrix update
-    void updateHessian(const FastVector<Key>& infoKeys, SymmetricBlockMatrix* info) const {
-      gttic(updateHessian_BinaryJacobianFactor);
-      // Whiten the factor if it has a noise model
-      const SharedDiagonal& model = get_model();
-      if (model && !model->isUnit()) {
-        if (model->isConstrained())
-        throw std::invalid_argument(
-            "BinaryJacobianFactor::updateHessian: cannot update information with "
-            "constrained noise model");
-        JacobianFactor whitenedFactor = whiten(); // TODO: make BinaryJacobianFactor
-        whitenedFactor.updateHessian(infoKeys, info);
-      } else {
-        // First build an array of slots
-        DenseIndex slot1 = Slot(infoKeys, keys_.front());
-        DenseIndex slot2 = Slot(infoKeys, keys_.back());
-        DenseIndex slotB = info->nBlocks() - 1;
-
-        const Matrix& Ab = Ab_.matrix();
-        Eigen::Block<const Matrix,2,DimC> A1(Ab, 0, 0);
-        Eigen::Block<const Matrix,2,DimL> A2(Ab, 0, DimC);
-        Eigen::Block<const Matrix,2,1> b(Ab, 0, DimC + DimL);
-
-        // We perform I += A'*A to the upper triangle
-        (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose());
-        (*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2;
-        (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b;
-        (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose());
-        (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b;
-        (*info)(slotB, slotB)(0,0) += b.transpose() * b;
-      }
-    }
-  };
-
   /// Linearize using fixed-size matrices
   boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
     // Only linearize if the factor is active
-    if (!this->active(values)) return boost::shared_ptr<BinaryJacobianFactor>();
+    if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
 
     const Key key1 = this->key1(), key2 = this->key2();
     JacobianC H1;
@@ -210,14 +164,13 @@ public:
       b = noiseModel->Whiten(b);
     }
 
+    // Create new (unit) noiseModel, preserving constraints if applicable
+    SharedDiagonal model;
     if (noiseModel && noiseModel->isConstrained()) {
-      using noiseModel::Constrained;
+      model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
-      return boost::make_shared<BinaryJacobianFactor>(
-          key1, H1, key2, H2, b,
-          boost::static_pointer_cast<Constrained>(noiseModel)->unit());
-    } else {
-      return boost::make_shared<BinaryJacobianFactor>(key1, H1, key2, H2, b);
     }
+
+    return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
   }
 
   /** return the measured */

gtsam/slam/tests/testGeneralSFMFactor.cpp

@@ -135,11 +135,11 @@ TEST( GeneralSFMFactor, equals ) {
   const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1);
   const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
   boost::shared_ptr<Projection> factor1(
-                                        new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
+      new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
-  
+
   boost::shared_ptr<Projection> factor2(
-                                        new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
+      new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
-  
+
   EXPECT(assert_equal(*factor1, *factor2));
 }
 
@@ -157,8 +157,8 @@ TEST( GeneralSFMFactor, error ) {
   Point3 l1;
   values.insert(L(1), l1);
   EXPECT(
-         assert_equal(((Vector ) Vector2(-3., 0.)),
+      assert_equal(((Vector ) Vector2(-3., 0.)),
-                      factor.unwhitenedError(values)));
+          factor.unwhitenedError(values)));
 }
 
 /* ************************************************************************* */