Add some changes

gtsam/slam/RegularHessianFactor.h

@@ -31,9 +31,6 @@ private:
 
   typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
   typedef Eigen::Matrix<double, D, 1> VectorD;
-  // Use eigen magic to access raw memory
-  typedef Eigen::Map<VectorD> DMap;
-  typedef Eigen::Map<const VectorD> ConstDMap;
 
 public:
 
@@ -54,30 +51,57 @@ public:
       HessianFactor(keys, augmentedInformation) {
   }
 
-  /** Return the diagonal of the Hessian for this factor */
-  VectorValues hessianDiagonal() const {
-    return HessianFactor::hessianDiagonal();
-  }
-
-  /** Return the diagonal of the Hessian for this factor (raw memory version) */
-  void hessianDiagonal(double* d) const {
-    // Loop over all variables in the factor
-    for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
-      Key j = keys_[pos];
-      // Get the diagonal block, and insert its diagonal
-      const Matrix& B = info_(pos, pos).selfadjointView();
-      DMap(d + D * j) += B.diagonal();
-    }
-  }
-
   /** y += alpha * A'*A*x */
   void multiplyHessianAdd(double alpha, const VectorValues& x,
       VectorValues& y) const {
     HessianFactor::multiplyHessianAdd(alpha, x, y);
   }
 
+  // Scratch space for multiplyHessianAdd
+  typedef Eigen::Matrix<double, D, 1> DVector;
+  mutable std::vector<DVector> y;
+
+  void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
+    // Create a vector of temporary y values, corresponding to rows i
+    y.resize(size());
+    BOOST_FOREACH(DVector & yi, y)
+      yi.setZero();
+
+    typedef Eigen::Map<DVector> DMap;
+    typedef Eigen::Map<const DVector> ConstDMap;
+
+    // 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
+    DVector 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);
+      // blocks on the diagonal are only half
+      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);
+    }
+
+    // copy to yvalues
+    for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
+      Key key = keys_[i];
+      DMap(yvalues + key * D) += alpha * y[i];
+    }
+  }
+
   void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
       std::vector<size_t> offsets) const {
+
+    // Use eigen magic to access raw memory
+    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
+    typedef Eigen::Map<DVector> DMap;
+    typedef Eigen::Map<const DVector> ConstDMap;
+
     // Create a vector of temporary y values, corresponding to rows i
     std::vector<Vector> y;
     y.reserve(size());
@@ -110,52 +134,45 @@ public:
           alpha * y[i];
   }
 
-  // Scratch space for multiplyHessianAdd
+  /** Return the diagonal of the Hessian for this factor */
-  mutable std::vector<VectorD> y;
+  VectorValues hessianDiagonal() const {
-
+    return HessianFactor::hessianDiagonal();
-  void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
-    // Create a vector of temporary y values, corresponding to rows i
-    y.resize(size());
-    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
-    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);
-      // blocks on the diagonal are only half
-      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);
   }
 
-    // copy to yvalues
+  /** Return the diagonal of the Hessian for this factor (raw memory version) */
-    for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
+  void hessianDiagonal(double* d) const {
-      Key key = keys_[i];
+
-      DMap(yvalues + key * D) += alpha * y[i];
+    // Use eigen magic to access raw memory
+    typedef Eigen::Matrix<double, 9, 1> DVector;
+    typedef Eigen::Map<DVector> DMap;
+
+    // Loop over all variables in the factor
+    for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
+      Key j = keys_[pos];
+      // Get the diagonal block, and insert its diagonal
+      const Matrix& B = info_(pos, pos).selfadjointView();
+      DMap(d + 9 * j) += B.diagonal();
     }
   }
 
-  /** eta for Hessian */
   VectorValues gradientAtZero() const {
     return HessianFactor::gradientAtZero();
   }
 
-  /** eta for Hessian (raw memory version) */
+  /* ************************************************************************* */
+  // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
   void gradientAtZero(double* d) const {
+
+    // Use eigen magic to access raw memory
+    typedef Eigen::Matrix<double, 9, 1> DVector;
+    typedef Eigen::Map<DVector> DMap;
+
     // Loop over all variables in the factor
     for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
       Key j = keys_[pos];
       // Get the diagonal block, and insert its diagonal
       VectorD dj =  -info_(pos,size()).knownOffDiagonal();
-      DMap(d + D * j) += dj;
+      DMap(d + 9 * j) += dj;
     }
   }
 

gtsam/slam/RegularJacobianFactor.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/VectorValues.h>
 #include <boost/foreach.hpp>
 #include <vector>
 
@@ -27,14 +28,6 @@ namespace gtsam {
 template<size_t D>
 class RegularJacobianFactor: public JacobianFactor {
 
-private:
-
-  typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
-  typedef Eigen::Matrix<double, D, 1> VectorD;
-  // Use eigen magic to access raw memory
-  typedef Eigen::Map<VectorD> DMap;
-  typedef Eigen::Map<const VectorD> ConstDMap;
-
 public:
 
   /** Construct an n-ary factor
@@ -57,24 +50,6 @@ public:
       JacobianFactor(keys, augmentedMatrix, sigmas) {
   }
 
-  /// Return the diagonal of the Hessian for this factor
-  VectorValues hessianDiagonal() const {
-    return JacobianFactor::hessianDiagonal();
-  }
-
-  /// 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
-      DVector dj;
-      for (size_t k = 0; k < D; ++k)
-        dj(k) = Ab_(j).col(k).squaredNorm();
-
-      DMap(d + D * j) += dj;
-    }
-  }
-
   /// y += alpha * A'*A*x
   void multiplyHessianAdd(double alpha, const VectorValues& x,
       VectorValues& y) const {
@@ -83,16 +58,24 @@ public:
 
   void multiplyHessianAdd(double alpha, const double* x, double* y,
       std::vector<size_t> keys) const {
+
+    // Use eigen magic to access raw memory
+    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
+    typedef Eigen::Map<DVector> DMap;
+    typedef Eigen::Map<const DVector> ConstDMap;
+
     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)
+    {
+      std::cout << "pos: " << pos << " keys_[pos]: " << keys_[pos] << " keys[keys_[pos]]: " << keys[keys_[pos]] << std::endl;
       Ax += Ab_(pos)
           * ConstDMap(x + keys[keys_[pos]],
               keys[keys_[pos] + 1] - keys[keys_[pos]]);
-
+    }
     // Deal with noise properly, need to Double* whiten as we are dividing by variance
     if (model_) {
       model_->whitenInPlace(Ax);
@@ -109,6 +92,12 @@ public:
   }
 
   void multiplyHessianAdd(double alpha, const double* x, double* y) const {
+
+    // 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;
+
     if (empty()) return;
     Vector Ax = zero(Ab_.rows());
 
@@ -127,6 +116,33 @@ public:
       DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
   }
 
+  /// Return the diagonal of the Hessian for this factor
+  VectorValues hessianDiagonal() const {
+    return JacobianFactor::hessianDiagonal();
+  }
+
+  /// Raw memory access version of hessianDiagonal
+  /* ************************************************************************* */
+  // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
+  void hessianDiagonal(double* d) const {
+    // Use eigen magic to access raw memory
+    //typedef Eigen::Matrix<double, 9, 1> DVector;
+    typedef Eigen::Matrix<double, D, 1> DVector;
+    typedef Eigen::Map<DVector> DMap;
+
+    // Loop over all variables in the factor
+    for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
+      // Get the diagonal block, and insert its diagonal
+      DVector dj;
+      //for (size_t k = 0; k < 9; ++k)
+      for (size_t k = 0; k < D; ++k)
+        dj(k) = Ab_(j).col(k).squaredNorm();
+
+      //DMap(d + 9 * j) += dj;
+      DMap(d + D * j) += dj;
+    }
+  }
+
   VectorValues gradientAtZero() const {
     return JacobianFactor::gradientAtZero();
   }