Modify and add raw memory access for HessianFactor

gtsam/slam/RegularHessianFactor.h

@@ -31,6 +31,9 @@ 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:
 
@@ -51,30 +54,75 @@ 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 {
+  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
-    y.resize(size());
-    BOOST_FOREACH(DVector & yi, y)
-      yi.setZero();
-
-    typedef Eigen::Map<DVector> DMap;
-    typedef Eigen::Map<const DVector> ConstDMap;
+    std::vector<Vector> y;
+    y.reserve(size());
+    for (const_iterator it = begin(); it != end(); it++)
+      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
-    DVector xj(D);
+    for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
+      DenseIndex i = 0;
+      for (; i < j; ++i)
+        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()
+          * 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()
+            * ConstDMap(x + offsets[keys_[j]],
+                offsets[keys_[j] + 1] - offsets[keys_[j]]);
+    }
+
+    // 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];
+  }
+
+  // Scratch space for multiplyHessianAdd
+  mutable std::vector<VectorD> 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(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;
@@ -95,6 +143,22 @@ public:
     }
   }
 
+  /** eta for Hessian */
+  VectorValues gradientAtZero() const {
+    return HessianFactor::gradientAtZero();
+  }
+
+  /** eta for Hessian (raw memory version) */
+  void gradientAtZero(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
+      VectorD dj =  -info_(pos,size()).knownOffDiagonal();
+      DMap(d + D * j) += dj;
+    }
+  }
+
 };
 
 }