Remove non raw memory access functions

gtsam/slam/RegularHessianFactor.h

@@ -51,16 +51,11 @@ public:
       HessianFactor(keys, augmentedInformation) {
   }
 
-  /** 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;
 
+  /** y += alpha * A'*A*x */
   void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
     // Create a vector of temporary y values, corresponding to rows i
     y.resize(size());
@@ -134,16 +129,12 @@ public:
           alpha * y[i];
   }
 
-  /** 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 {
+  virtual void hessianDiagonal(double* d) const {
 
     // Use eigen magic to access raw memory
-    typedef Eigen::Matrix<double, 9, 1> DVector;
+    //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
@@ -151,20 +142,18 @@ public:
       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();
+      //DMap(d + 9 * j) += B.diagonal();
+      DMap(d + D * j) += B.diagonal();
     }
   }
 
-  VectorValues gradientAtZero() const {
-    return HessianFactor::gradientAtZero();
-  }
-
   /* ************************************************************************* */
   // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
-  void gradientAtZero(double* d) const {
+  virtual void gradientAtZero(double* d) const {
 
     // Use eigen magic to access raw memory
-    typedef Eigen::Matrix<double, 9, 1> DVector;
+    //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
@@ -172,7 +161,8 @@ public:
       Key j = keys_[pos];
       // Get the diagonal block, and insert its diagonal
       VectorD dj =  -info_(pos,size()).knownOffDiagonal();
-      DMap(d + 9 * j) += dj;
+      //DMap(d + 9 * j) += dj;
+      DMap(d + D * j) += dj;
     }
   }
 

gtsam/slam/RegularImplicitSchurFactor.h

@@ -160,7 +160,7 @@ public:
    * @brief add the contribution of this factor to the diagonal of the hessian
    * d(output) = d(input) + deltaHessianFactor
    */
-  void hessianDiagonal(double* d) const {
+  virtual void hessianDiagonal(double* d) const {
     // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
     // Use eigen magic to access raw memory
     typedef Eigen::Matrix<double, D, 1> DVector;
@@ -435,7 +435,7 @@ public:
   /**
    * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
    */
-  void gradientAtZero(double* d) const {
+  virtual void gradientAtZero(double* d) const {
 
     // Use eigen magic to access raw memory
     typedef Eigen::Matrix<double, D, 1> DVector;

gtsam/slam/RegularJacobianFactor.h

@@ -57,13 +57,7 @@ public:
       JacobianFactor(keys, augmentedMatrix, sigmas) {
   }
 
-  /** y += alpha * A'*A*x */
+  /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
-  void multiplyHessianAdd(double alpha, const VectorValues& x,
-      VectorValues& y) const {
-    JacobianFactor::multiplyHessianAdd(alpha, x, y);
-  }
-
-  /** Raw memory access version of multiplyHessianAdd
    * 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,
@@ -126,16 +120,10 @@ 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 D (templated) and keys arranged from 0 to n
-   *
    */
-  void hessianDiagonal(double* d) const {
+  virtual 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
@@ -153,22 +141,18 @@ public:
     }
   }
 
-  /** // Gradient is really -A'*b / sigma^2 */
-  VectorValues gradientAtZero() const {
-    return JacobianFactor::gradientAtZero();
-  }
-
   /** Raw memory access version of gradientAtZero */
-  void gradientAtZero(double* d) const {
+  virtual void gradientAtZero(double* d) const {
     // Get vector b not weighted
     Vector b = getb();
     Vector b_sigma = model_ ? (model_->whiten(b)*model_->whiten(b)) : b;
 
-    // gradient -= A'*b/sigma^2
+    // Just iterate over all A matrices
-    // Loop over all variables in the factor
     for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
-      // Get the diagonal block, and insert its diagonal
+      DMap(d + D * j) = DVector::Zero();
       DVector dj;
+      // gradient -= A'*b/sigma^2
+      // Computing with each column
       for (size_t k = 0; k < D; ++k){
         Vector column_k = Ab_(j).col(k);
         dj(k) = -1.0*dot(b_sigma,column_k);

gtsam/slam/tests/testRegularHessianFactor.cpp

@@ -77,15 +77,6 @@ TEST(RegularHessianFactor, ConstructorNWay)
   expected.insert(1, Y.segment<2>(2));
   expected.insert(3, Y.segment<2>(4));
 
-  // VectorValues way
-  VectorValues actual;
-  factor.multiplyHessianAdd(1, x, actual);
-  EXPECT(assert_equal(expected, actual));
-
-  // now, do it with non-zero y
-  factor.multiplyHessianAdd(1, x, actual);
-  EXPECT(assert_equal(2*expected, actual));
-
   // RAW ACCESS
   Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
   Vector fast_y = gtsam::zero(8);

gtsam/slam/tests/testRegularJacobianFactor.cpp

@@ -98,12 +98,6 @@ TEST(RegularJacobianFactor, hessianDiagonal)
   // we compute hessian diagonal from the standard Jacobian
   VectorValues expectedHessianDiagonal = factor.hessianDiagonal();
 
-  // we compute hessian diagonal from the regular Jacobian, but still using the
-  // implementation of hessianDiagonal in the base class
-  //VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal();
-
-  //EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal));
-
   // we compare against the Raw memory access implementation of hessianDiagonal
   double actualValue[9];
   regularFactor.hessianDiagonal(actualValue);
@@ -122,18 +116,13 @@ TEST(RegularJacobian, gradientAtZero)
   // we compute gradient at zero from the standard Jacobian
   VectorValues expectedGradientAtZero = factor.gradientAtZero();
 
-  // we compute gradient at zero from the regular Jacobian, but still using the
+  //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero()));
-  // implementation of gradientAtZero in the base class
-  VectorValues actualGradientAtZero = regularFactor.gradientAtZero();
-
-  EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero()));
 
   // we compare against the Raw memory access implementation of gradientAtZero
   double actualValue[9];
   regularFactor.gradientAtZero(actualValue);
   VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
   EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
-
 }
 
 /* ************************************************************************* */
@@ -161,11 +150,6 @@ TEST(RegularJacobian, multiplyHessianAdd)
   VectorValues expectedMHA = Y;
   factor.multiplyHessianAdd(alpha, X, expectedMHA);
 
-  VectorValues actualMHA = Y;
-  regularFactor.multiplyHessianAdd(alpha, X, actualMHA);
-
-  EXPECT(assert_equal(expectedMHA, actualMHA));
-
   // create data for raw memory access
   double XRaw[9];
   vv2double(X, XRaw, nrKeys, fixedDim);