JacobianSchurFactor is merged into RegularJacobianFactor. Derived classes from JacobianSchurFactor are changed to be derived from RegularJacobianFactor.

gtsam/slam/JacobianFactorQ.h

@@ -6,16 +6,20 @@
 
 #pragma once
 
-#include "JacobianSchurFactor.h"
+#include <gtsam/slam/RegularJacobianFactor.h>
 
 namespace gtsam {
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
 template<size_t D>
-class JacobianFactorQ: public JacobianSchurFactor<D> {
+class JacobianFactorQ: public RegularJacobianFactor<D> {
 
-  typedef JacobianSchurFactor<D> Base;
+private:
+
+  typedef RegularJacobianFactor<D> Base;
+  typedef Eigen::Matrix<double, 2, D> Matrix2D;
+  typedef std::pair<Key, Matrix2D> KeyMatrix2D;
 
 public:
 
@@ -25,7 +29,7 @@ public:
 
   /// Empty constructor with keys
   JacobianFactorQ(const FastVector<Key>& keys,
-      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D>() {
+      const SharedDiagonal& model =  SharedDiagonal()) : RegularJacobianFactor<D>() {
     Matrix zeroMatrix = Matrix::Zero(0,D);
     Vector zeroVector = Vector::Zero(0);
     typedef std::pair<Key, Matrix> KeyMatrix;
@@ -37,10 +41,10 @@ public:
   }
 
   /// Constructor
-  JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  JacobianFactorQ(const std::vector<KeyMatrix2D>& Fblocks,
       const Matrix& E, const Matrix3& P, const Vector& b,
       const SharedDiagonal& model = SharedDiagonal()) :
-      JacobianSchurFactor<D>() {
+        RegularJacobianFactor<D>() {
     size_t j = 0, m2 = E.rows(), m = m2 / 2;
     // Calculate projector Q
     Matrix Q = eye(m2) - E * P * E.transpose();
@@ -50,7 +54,7 @@ public:
     std::vector < KeyMatrix > QF;
     QF.reserve(m);
     // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D)
-    BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks)
+    BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
       QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
     // Which is then passed to the normal JacobianFactor constructor
     JacobianFactor::fillTerms(QF, Q * b, model);

gtsam/slam/JacobianFactorQR.h

@@ -6,31 +6,35 @@
  */
 
 #pragma once
-#include <gtsam/slam/JacobianSchurFactor.h>
+#include <gtsam/slam/RegularJacobianFactor.h>
 
 namespace gtsam {
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
 template<size_t D>
-class JacobianFactorQR: public JacobianSchurFactor<D> {
+class JacobianFactorQR: public RegularJacobianFactor<D> {
 
-  typedef JacobianSchurFactor<D> Base;
+private:
+
+  typedef RegularJacobianFactor<D> Base;
+  typedef Eigen::Matrix<double, 2, D> Matrix2D;
+  typedef std::pair<Key, Matrix2D> KeyMatrix2D;
 
 public:
 
   /**
    * Constructor
    */
-  JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  JacobianFactorQR(const std::vector<KeyMatrix2D>& Fblocks,
       const Matrix& E, const Matrix3& P, const Vector& b,
       const SharedDiagonal& model = SharedDiagonal()) :
-      JacobianSchurFactor<D>() {
+        RegularJacobianFactor<D>() {
     // Create a number of Jacobian factors in a factor graph
     GaussianFactorGraph gfg;
     Symbol pointKey('p', 0);
     size_t i = 0;
-    BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) {
+    BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) {
       gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second,
           b.segment<2>(2 * i), model);
       i += 1;

gtsam/slam/JacobianFactorSVD.h

@@ -5,27 +5,29 @@
  */
 
 #pragma once
-#include "gtsam/slam/JacobianSchurFactor.h"
+#include <gtsam/slam/RegularJacobianFactor.h>
 
 namespace gtsam {
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
 template<size_t D>
-class JacobianFactorSVD: public JacobianSchurFactor<D> {
+class JacobianFactorSVD: public RegularJacobianFactor<D> {
 
-public:
+private:
 
   typedef Eigen::Matrix<double, 2, D> Matrix2D;
   typedef std::pair<Key, Matrix2D> KeyMatrix2D;
   typedef std::pair<Key, Matrix> KeyMatrix;
 
+public:
+
   /// Default constructor
   JacobianFactorSVD() {}
 
   /// Empty constructor with keys
   JacobianFactorSVD(const FastVector<Key>& keys,
-      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D>() {
+      const SharedDiagonal& model =  SharedDiagonal()) : RegularJacobianFactor<D>() {
     Matrix zeroMatrix = Matrix::Zero(0,D);
     Vector zeroVector = Vector::Zero(0);
     std::vector<KeyMatrix> QF;
@@ -37,7 +39,7 @@ public:
 
   /// Constructor
   JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
-      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D>() {
+      const SharedDiagonal& model =  SharedDiagonal()) : RegularJacobianFactor<D>() {
     size_t numKeys = Enull.rows() / 2;
     size_t j = 0, m2 = 2*numKeys-3;
     // PLAIN NULL SPACE TRICK

gtsam/slam/JacobianSchurFactor.h

@@ -1,93 +0,0 @@
-/*
- * @file  JacobianSchurFactor.h
- * @brief Jacobianfactor that combines and eliminates points
- * @date  Oct 27, 2013
- * @uthor Frank Dellaert
- */
-
-#pragma once
-
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/linear/VectorValues.h>
-#include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <boost/foreach.hpp>
-
-namespace gtsam {
-/**
- * JacobianFactor for Schur complement that uses Q noise model
- */
-template<size_t D>
-class JacobianSchurFactor: public JacobianFactor {
-
-public:
-
-  typedef Eigen::Matrix<double, 2, D> Matrix2D;
-  typedef std::pair<Key, Matrix2D> KeyMatrix2D;
-
-  // 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;
-
-  /**
-   * @brief double* Matrix-vector multiply, i.e. y = A*x
-   * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way
-   */
-  Vector operator*(const double* x) const {
-    Vector Ax = zero(Ab_.rows());
-    if (empty()) return Ax;
-
-    // Just iterate over all A matrices and multiply in correct config part
-    for(size_t pos=0; pos<size(); ++pos)
-      Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
-
-    return model_ ? model_->whiten(Ax) : Ax;
-  }
-
-  /**
-   * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e
-   * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way
-   */
-  void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const
-  {
-    Vector E = alpha * (model_ ? model_->whiten(e) : e);
-    // Just iterate over all A matrices and insert Ai^e into y
-    for(size_t pos=0; pos<size(); ++pos)
-      DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
-  }
-
-  /** y += alpha * A'*A*x */
-  void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const {
-    JacobianFactor::multiplyHessianAdd(alpha,x,y);
-  }
-
-  /**
-   * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x)
-   * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
-   */
-  void multiplyHessianAdd(double alpha, const double* x, double* y) const {
-//    Vector Ax = (*this)*x;
-//    this->transposeMultiplyAdd(alpha,Ax,y);
-    if (empty()) return;
-    Vector Ax = zero(Ab_.rows());
-
-    // Just iterate over all A matrices and multiply in correct config part
-    for(size_t pos=0; pos<size(); ++pos)
-      Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
-
-    // Deal with noise properly, need to Double* whiten as we are dividing by variance
-    if  (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); }
-
-    // multiply with alpha
-    Ax *= alpha;
-
-    // Again iterate over all A matrices and insert Ai^e into y
-    for(size_t pos=0; pos<size(); ++pos)
-      DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
-  }
-
-}; // class
-
-} // gtsam

gtsam/slam/RegularJacobianFactor.h

@@ -18,6 +18,8 @@
 
 #pragma once
 
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
 #include <boost/foreach.hpp>
@@ -30,6 +32,9 @@ class RegularJacobianFactor: public JacobianFactor {
 
 private:
 
+  typedef JacobianFactor Base;
+  typedef RegularJacobianFactor<D> This;
+
   /** Use eigen magic to access raw memory */
   typedef Eigen::Matrix<double, D, 1> DVector;
   typedef Eigen::Map<DVector> DMap;
@@ -37,6 +42,10 @@ private:
 
 public:
 
+  /// Default constructor
+  RegularJacobianFactor() {
+  }
+
   /** Construct an n-ary factor
    * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
    *         collection of keys and matrices making up the factor. */
@@ -57,6 +66,33 @@ public:
       JacobianFactor(keys, augmentedMatrix, sigmas) {
   }
 
+  /**
+   * @brief double* Matrix-vector multiply, i.e. y = A*x
+   * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way
+   */
+  Vector operator*(const double* x) const {
+    Vector Ax = zero(Ab_.rows());
+    if (empty()) return Ax;
+
+    // Just iterate over all A matrices and multiply in correct config part
+    for(size_t pos=0; pos<size(); ++pos)
+      Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
+
+    return model_ ? model_->whiten(Ax) : Ax;
+  }
+
+  /**
+   * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e
+   * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way
+   */
+  void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const
+  {
+    Vector E = alpha * (model_ ? model_->whiten(e) : e);
+    // Just iterate over all A matrices and insert Ai^e into y
+    for(size_t pos=0; pos<size(); ++pos)
+      DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
+  }
+
   /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
    * Note: this is not assuming a fixed dimension for the variables,
    * but requires the vector accumulatedDims to tell the dimension of
@@ -66,11 +102,6 @@ public:
   void multiplyHessianAdd(double alpha, const double* x, double* y,
       const std::vector<size_t>& accumulatedDims) const {
 
-    /// Use eigen magic to access raw memory
-    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorD;
-    typedef Eigen::Map<VectorD> MapD;
-    typedef Eigen::Map<const VectorD> ConstMapD;
-
     if (empty())
       return;
     Vector Ax = zero(Ab_.rows());
@@ -81,7 +112,7 @@ public:
     for (size_t pos = 0; pos < size(); ++pos)
     {
       Ax += Ab_(pos)
-          * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
+          * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
     }
     /// Deal with noise properly, need to Double* whiten as we are dividing by variance
     if (model_) {
@@ -94,12 +125,15 @@ public:
 
     /// Again iterate over all A matrices and insert Ai^T into y
     for (size_t pos = 0; pos < size(); ++pos){
-      MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
+      DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
           pos).transpose() * Ax;
     }
   }
 
-  /** Raw memory access version of multiplyHessianAdd */
+  /**
+   * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x)
+   * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
+   */
   void multiplyHessianAdd(double alpha, const double* x, double* y) const {
 
     if (empty()) return;
@@ -168,6 +202,13 @@ public:
     }
   }
 
+  /** Non-RAW memory access versions for testRegularImplicitSchurFactor
+   * TODO: They should be removed from Regular Factors
+   */
+  using Base::multiplyHessianAdd;
+  using Base::hessianDiagonal;
+  using Base::gradientAtZero;
+
 };
 
 }