Restored VectorValues versions, they work fine

.cproject

@@ -1538,10 +1538,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testRegularJacobianFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testRegularHessianFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j4</buildArguments>
-				<buildTarget>testRegularJacobianFactor.run</buildTarget>
+				<buildTarget>testRegularHessianFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>

gtsam/slam/RegularHessianFactor.h

@@ -56,13 +56,14 @@ public:
   mutable std::vector<DVector> y;
 
   /** y += alpha * A'*A*x */
-  void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{
+  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
-    throw std::runtime_error(
+      VectorValues& y) const {
-          "RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead");
+    HessianFactor::multiplyHessianAdd(alpha, x, y);
   }
 
   /** y += alpha * A'*A*x */
-  void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
+  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)
@@ -95,6 +96,7 @@ public:
     }
   }
 
+  /// Raw memory version, with offsets TODO document reasoning
   void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
       std::vector<size_t> offsets) const {
 
@@ -131,43 +133,38 @@ public:
 
     // copy to yvalues
     for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
-      DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) +=
+      DMap(yvalues + offsets[keys_[i]],
-          alpha * y[i];
+          offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i];
   }
 
   /** Return the diagonal of the Hessian for this factor (raw memory version) */
   virtual 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 pos = 0; pos < (DenseIndex)size(); ++pos) {
+    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();
       DMap(d + D * j) += B.diagonal();
     }
   }
 
-  /* ************************************************************************* */
+  /// Add gradient at zero to d TODO: is it really the goal to add ??
-  // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
   virtual void gradientAtZero(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 pos = 0; pos < (DenseIndex)size(); ++pos) {
+    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();
+      VectorD dj = -info_(pos, size()).knownOffDiagonal();
-      //DMap(d + 9 * j) += dj;
       DMap(d + D * j) += dj;
     }
   }

gtsam/slam/RegularJacobianFactor.h

@@ -57,6 +57,12 @@ public:
       JacobianFactor(keys, augmentedMatrix, sigmas) {
   }
 
+  /** y += alpha * A'*A*x */
+  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
+      VectorValues& y) const {
+    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,
    * but requires the vector accumulatedDims to tell the dimension of

gtsam/slam/tests/testRegularHessianFactor.cpp

@@ -15,11 +15,10 @@
  * @date    March 4, 2014
  */
 
-#include <gtsam/linear/VectorValues.h>
-#include <CppUnitLite/TestHarness.h>
-
-//#include <gtsam_unstable/slam/RegularHessianFactor.h>
 #include <gtsam/slam/RegularHessianFactor.h>
+#include <gtsam/linear/VectorValues.h>
+
+#include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/std/map.hpp>
@@ -29,8 +28,6 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
-const double tol = 1e-5;
-
 /* ************************************************************************* */
 TEST(RegularHessianFactor, ConstructorNWay)
 {
@@ -77,15 +74,24 @@ TEST(RegularHessianFactor, ConstructorNWay)
   expected.insert(1, Y.segment<2>(2));
   expected.insert(3, Y.segment<2>(4));
 
+  // VectorValues version
+  double alpha = 1.0;
+  VectorValues actualVV;
+  actualVV.insert(0, zero(2));
+  actualVV.insert(1, zero(2));
+  actualVV.insert(3, zero(2));
+  factor.multiplyHessianAdd(alpha, x, actualVV);
+  EXPECT(assert_equal(expected, actualVV));
+
   // RAW ACCESS
   Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
   Vector fast_y = gtsam::zero(8);
   double xvalues[8] = {1,2,3,4,0,0,5,6};
-  factor.multiplyHessianAdd(1, xvalues, fast_y.data());
+  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
   EXPECT(assert_equal(expected_y, fast_y));
 
   // now, do it with non-zero y
-  factor.multiplyHessianAdd(1, xvalues, fast_y.data());
+  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
   EXPECT(assert_equal(2*expected_y, fast_y));
 
   // check some expressions