added raw memory access of hessianDiagonal

gtsam/linear/GaussianFactor.h

@@ -99,6 +99,9 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const = 0;
 
+    /// Return the diagonal of the Hessian for this factor (raw memory version)
+    virtual void hessianDiagonal(double* d) const = 0;
+
     /// Return the block diagonal of the Hessian for this factor
     virtual std::map<Key,Matrix> hessianBlockDiagonal() const = 0;
 

gtsam/linear/HessianFactor.cpp

@@ -358,6 +358,23 @@ VectorValues HessianFactor::hessianDiagonal() const {
   return d;
 }
 
+/* ************************************************************************* */
+// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
+void HessianFactor::hessianDiagonal(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 j = 0; j < (DenseIndex)size(); ++j) {
+      // Get the diagonal block, and insert its diagonal
+      Matrix B = info_(j, j).selfadjointView();
+      DVector dj =  B.diagonal();
+      DMap(d + 9 * j) += dj;
+    }
+}
+
 /* ************************************************************************* */
 map<Key,Matrix> HessianFactor::hessianBlockDiagonal() const {
   map<Key,Matrix> blocks;

gtsam/linear/HessianFactor.h

@@ -340,6 +340,9 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const;
 
+    /* ************************************************************************* */
+    virtual void hessianDiagonal(double* d) const;
+
     /// Return the block diagonal of the Hessian for this factor
     virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
 

gtsam/linear/JacobianFactor.cpp

@@ -456,6 +456,11 @@ namespace gtsam {
     return d;
   }
 
+  /* ************************************************************************* */
+  void JacobianFactor::hessianDiagonal(double* d) const {
+    throw std::runtime_error("JacobianFactor::hessianDiagonal non implemented (use VectorValues version)");
+  }
+
   /* ************************************************************************* */
   map<Key,Matrix> JacobianFactor::hessianBlockDiagonal() const {
     map<Key,Matrix> blocks;

gtsam/linear/JacobianFactor.h

@@ -185,6 +185,9 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const;
 
+    /* ************************************************************************* */
+    virtual void hessianDiagonal(double* d) const;
+
     /// Return the block diagonal of the Hessian for this factor
     virtual std::map<Key,Matrix> hessianBlockDiagonal() const;