Grabbed some methods from JacobianSchurFactor, added VectorValues versions

gtsam/slam/RegularJacobianFactor.h

@@ -21,25 +21,34 @@
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
 #include <boost/foreach.hpp>
-#include <vector>
 
 namespace gtsam {
 
+/**
+ * JacobianFactor with constant sized blocks
+ * Provides raw memory access versions of linear operator.
+ * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD
+ */
 template<size_t D>
 class RegularJacobianFactor: public JacobianFactor {
 
 private:
 
-  /** Use eigen magic to access raw memory */
+  // 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:
 
+  /// 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. */
+   *         collection of keys and matrices making up the factor.
+   * TODO Verify terms are regular
+   */
   template<typename TERMS>
   RegularJacobianFactor(const TERMS& terms, const Vector& b,
       const SharedDiagonal& model = SharedDiagonal()) :
@@ -49,7 +58,9 @@ public:
   /** Constructor with arbitrary number keys, and where the augmented matrix is given all together
    *  instead of in block terms.  Note that only the active view of the provided augmented matrix
    *  is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
-   *  factor. */
+   *  factor.
+   *  TODO Verify complies to regular
+   */
   template<typename KEYS>
   RegularJacobianFactor(const KEYS& keys,
       const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
@@ -63,52 +74,11 @@ public:
     JacobianFactor::multiplyHessianAdd(alpha, x, y);
   }
 
-  /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
+  /**
-   * Note: this is not assuming a fixed dimension for the variables,
+   * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x)
-   * but requires the vector accumulatedDims to tell the dimension of
+   * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
-   * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
+   */
-   * then accumulatedDims is [0 3 9 11 13]
-   * NOTE: size of accumulatedDims is size of keys + 1!! */
-  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());
-
-    /// Just iterate over all A matrices and multiply in correct config part (looping over keys)
-    /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
-    /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
-    for (size_t pos = 0; pos < size(); ++pos) {
-      Ax += Ab_(pos)
-          * ConstMapD(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_) {
-      model_->whitenInPlace(Ax);
-      model_->whitenInPlace(Ax);
-    }
-
-    /// multiply with alpha
-    Ax *= alpha;
-
-    /// 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_(
-          pos).transpose() * Ax;
-    }
-  }
-
-  /** Raw memory access version of multiplyHessianAdd */
   void multiplyHessianAdd(double alpha, const double* x, double* y) const {
-
     if (empty())
       return;
     Vector Ax = zero(Ab_.rows());
@@ -131,10 +101,13 @@ public:
       DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
   }
 
-  /** Raw memory access version of hessianDiagonal
+  /// Expose base class hessianDiagonal
-   * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
+  virtual VectorValues hessianDiagonal() const {
-   */
+    return JacobianFactor::hessianDiagonal();
-  virtual void hessianDiagonal(double* d) const {
+  }
+
+  /// Raw memory access version of hessianDiagonal
+  void hessianDiagonal(double* d) const {
     // Loop over all variables in the factor
     for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
       // Get the diagonal block, and insert its diagonal
@@ -152,10 +125,13 @@ public:
     }
   }
 
-  /** Raw memory access version of gradientAtZero
+  /// Expose base class gradientAtZero
-   * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
+  virtual VectorValues gradientAtZero() const {
-   */
+    return JacobianFactor::gradientAtZero();
-  virtual void gradientAtZero(double* d) const {
+  }
+
+  /// Raw memory access version of gradientAtZero
+  void gradientAtZero(double* d) const {
 
     // Get vector b not weighted
     Vector b = getb();
@@ -179,7 +155,78 @@ public:
     }
   }
 
+  /**
+   * @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;
+  }
+
+  /**
+   * @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;
+  }
+
+  /** 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
+   * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
+   * then accumulatedDims is [0 3 9 11 13]
+   * NOTE: size of accumulatedDims is size of keys + 1!!
+   * TODO Frank asks: why is this here if not regular ????
+   */
+  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::Map<Vector> VectorMap;
+    typedef Eigen::Map<const Vector> ConstVectorMap;
+
+    if (empty())
+      return;
+    Vector Ax = zero(Ab_.rows());
+
+    /// Just iterate over all A matrices and multiply in correct config part (looping over keys)
+    /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
+    /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
+    for (size_t pos = 0; pos < size(); ++pos) {
+      size_t offset = accumulatedDims[keys_[pos]];
+      size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
+      Ax += Ab_(pos) * ConstVectorMap(x + offset, dim);
+    }
+    /// 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^T into y
+    for (size_t pos = 0; pos < size(); ++pos) {
+      size_t offset = accumulatedDims[keys_[pos]];
+      size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
+      VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax;
+    }
+  }
+
 };
+// end class RegularJacobianFactor
 
-}
+}// end namespace gtsam
-