fixed unit test with Luca

gtsam/slam/RegularJacobianFactor.h

@@ -56,8 +56,13 @@ public:
     JacobianFactor::multiplyHessianAdd(alpha, x, y);
   }
 
+  // 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,
+  // then accumulatedDims is [0 3 9 11 13]
+  // NOTE: size of accumulatedDims is size of keys + 1!!
   void multiplyHessianAdd(double alpha, const double* x, double* y,
-      std::vector<size_t> keys) const {
+      const std::vector<size_t>& accumulatedDims) const {
 
     // Use eigen magic to access raw memory
     typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
@@ -68,13 +73,13 @@ public:
       return;
     Vector Ax = zero(Ab_.rows());
 
-    // Just iterate over all A matrices and multiply in correct config part
+    // Just iterate over all A matrices and multiply in correct config part (looping over keys)
+    // E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
+    // Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
     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]],
+          * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[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_) {
@@ -85,11 +90,12 @@ public:
     // multiply with alpha
     Ax *= alpha;
 
-    // Again iterate over all A matrices and insert Ai^e into y
+    // Again iterate over all A matrices and insert Ai^T into y
-    for (size_t pos = 0; pos < size(); ++pos)
+    for (size_t pos = 0; pos < size(); ++pos){
-      DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_(
+      DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
           pos).transpose() * Ax;
     }
+  }
 
   void multiplyHessianAdd(double alpha, const double* x, double* y) const {
 
@@ -123,10 +129,9 @@ public:
 
   /// Raw memory access version of hessianDiagonal
   /* ************************************************************************* */
-  // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
+  // TODO: currently assumes all variables of the same size D (templated) 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;
 
@@ -134,11 +139,15 @@ public:
     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){
-      for (size_t k = 0; k < D; ++k)
+        if (model_){
+          Vector column_k = Ab_(j).col(k);
+          column_k = model_->whiten(column_k);
+          dj(k) = dot(column_k, column_k);
+        }else{
           dj(k) = Ab_(j).col(k).squaredNorm();
-
+        }
-      //DMap(d + 9 * j) += dj;
+      }
       DMap(d + D * j) += dj;
     }
   }

gtsam/slam/tests/testRegularJacobianFactor.cpp

@@ -22,13 +22,17 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
+static const size_t fixedDim = 3;
+static const size_t nrKeys = 3;
+
+// Keys are assumed to be from 0 to n
 namespace {
   namespace simple {
     // Terms we'll use
     const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
-      (make_pair(5, Matrix3::Identity()))
+      (make_pair(0, Matrix3::Identity()))
-      (make_pair(10, 2*Matrix3::Identity()))
+      (make_pair(1, 2*Matrix3::Identity()))
-      (make_pair(15, 3*Matrix3::Identity()));
+      (make_pair(2, 3*Matrix3::Identity()));
 
     // RHS and sigmas
     const Vector b = (Vector(3) << 1., 2., 3.);
@@ -36,89 +40,143 @@ namespace {
   }
 }
 
+/* ************************************************************************* */
+// Convert from double* to VectorValues
+VectorValues double2vv(const double* x,
+    const  size_t nrKeys, const size_t dim) {
+  // create map with dimensions
+  std::map<gtsam::Key, size_t> dims;
+  for (size_t i = 0; i < nrKeys; i++)
+    dims.insert(make_pair(i, dim));
+
+  size_t n = nrKeys*dim;
+  Vector xVec(n);
+  for (size_t i = 0; i < n; i++){
+    xVec(i) = x[i];
+  }
+  return VectorValues(xVec, dims);
+}
+
+/* ************************************************************************* */
+void vv2double(const VectorValues& vv, double* y,
+    const  size_t nrKeys, const size_t dim) {
+  // create map with dimensions
+  std::map<gtsam::Key, size_t> dims;
+  for (size_t i = 0; i < nrKeys; i++)
+    dims.insert(make_pair(i, dim));
+
+  Vector yvector = vv.vector(dims);
+  size_t n = nrKeys*dim;
+  for (size_t j = 0; j < n; j++)
+    y[j] = yvector[j];
+}
+
+
 /* ************************************************************************* */
 TEST(RegularJacobianFactor, constructorNway)
 {
   using namespace simple;
-  JacobianFactor expected(terms[0].first, terms[0].second,
+  JacobianFactor factor(terms[0].first, terms[0].second,
         terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
-  RegularJacobianFactor<3> actual(terms, b, noise);
+  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
 
-  LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
+  LONGS_EQUAL((long)terms[2].first, (long)regularFactor.keys().back());
-  EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
+  EXPECT(assert_equal(terms[2].second, regularFactor.getA(regularFactor.end() - 1)));
-  EXPECT(assert_equal(b, expected.getb()));
+  EXPECT(assert_equal(b, factor.getb()));
-  EXPECT(assert_equal(b, actual.getb()));
+  EXPECT(assert_equal(b, regularFactor.getb()));
-  EXPECT(noise == expected.get_model());
+  EXPECT(noise == factor.get_model());
-  EXPECT(noise == actual.get_model());
+  EXPECT(noise == regularFactor.get_model());
 }
 
 TEST(RegularJacobianFactor, hessianDiagonal)
 {
   using namespace simple;
-  JacobianFactor expected(terms[0].first, terms[0].second,
+  JacobianFactor factor(terms[0].first, terms[0].second,
           terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
-  RegularJacobianFactor<3> actual(terms, b, noise);
+  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
 
-  EXPECT(assert_equal(expected.hessianDiagonal(),actual.hessianDiagonal()));
+  // we compute hessian diagonal from the standard Jacobian
-  expected.hessianDiagonal().print();
+  VectorValues expectedHessianDiagonal = factor.hessianDiagonal();
-  actual.hessianDiagonal().print();
+
+  // 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];
-  actual.hessianDiagonal(actualValue);
+  regularFactor.hessianDiagonal(actualValue);
-  for(int i=0; i<9; ++i)
+  VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim);
-    std::cout << actualValue[i] << std::endl;
+  EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonalRaw));
-
-  // Why unwhitened?
 }
 
+/* ************************************************************************* */
 TEST(RegularJacobian, gradientAtZero)
 {
   using namespace simple;
-  JacobianFactor expected(terms[0].first, terms[0].second,
+  JacobianFactor factor(terms[0].first, terms[0].second,
           terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
-  RegularJacobianFactor<3> actual(terms, b, noise);
+  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
-  EXPECT(assert_equal(expected.gradientAtZero(),actual.gradientAtZero()));
+  EXPECT(assert_equal(factor.gradientAtZero(),regularFactor.gradientAtZero()));
 
+  // create and test raw memory access version
   // raw memory access is not available now
 }
 
+/* ************************************************************************* */
 TEST(RegularJacobian, multiplyHessianAdd)
 {
   using namespace simple;
-  RegularJacobianFactor<3> factor(terms, b, noise);
+  JacobianFactor factor(terms[0].first, terms[0].second,
+            terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
+  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
 
+  // arbitrary vector X
   VectorValues X;
-  X.insert(5, (Vector(3) << 10.,20.,30.));
+  X.insert(0, (Vector(3) << 10.,20.,30.));
-  X.insert(10, (Vector(3) << 10.,20.,30.));
+  X.insert(1, (Vector(3) << 10.,20.,30.));
-  X.insert(15, (Vector(3) << 10.,20.,30.));
+  X.insert(2, (Vector(3) << 10.,20.,30.));
 
+  // arbitrary vector Y
   VectorValues Y;
-  Y.insert(5, (Vector(3) << 10.,10.,10.));
+  Y.insert(0, (Vector(3) << 10.,10.,10.));
-  Y.insert(10, (Vector(3) << 20.,20.,20.));
+  Y.insert(1, (Vector(3) << 20.,20.,20.));
-  Y.insert(15, (Vector(3) << 30.,30.,30.));
+  Y.insert(2, (Vector(3) << 30.,30.,30.));
 
-  // muultiplyHessianAdd Y += alpha*A'A*X
+  // multiplyHessianAdd Y += alpha*A'A*X
-  factor.multiplyHessianAdd(2.0, X, Y);
+  double alpha = 2.0;
+  VectorValues expectedMHA = Y;
+  factor.multiplyHessianAdd(alpha, X, expectedMHA);
 
-  VectorValues expectedValues;
+  VectorValues actualMHA = Y;
-  expectedValues.insert(5, (Vector(3) << 490.,970.,1450.));
+  regularFactor.multiplyHessianAdd(alpha, X, actualMHA);
-  expectedValues.insert(10, (Vector(3) << 980.,1940.,2900.));
-  expectedValues.insert(15, (Vector(3) << 1470.,2910.,4350.));
 
-  EXPECT(assert_equal(expectedValues,Y));
+  EXPECT(assert_equal(expectedMHA, actualMHA));
 
-  //double dataX[9] = {10., 20., 30., 10., 20., 30., 10., 20., 30.};
+  // create data for raw memory access
-  //double dataY[9] = {10., 10., 10., 20., 20., 20., 30., 30., 30.};
+  double XRaw[9];
+  vv2double(X, XRaw, nrKeys, fixedDim);
 
-  std::cout << "size: " << factor.size() << std::endl;
+  // test 1st version: multiplyHessianAdd(double alpha, const double* x, double* y)
-  double dataX[9] = {0.,};
+  double actualMHARaw[9];
-  double dataY[9] = {0.,};
+  vv2double(Y, actualMHARaw, nrKeys, fixedDim);
-
+  regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw);
-  std::vector<Key> ks;
+  VectorValues actualMHARawVV = double2vv(actualMHARaw,nrKeys,fixedDim);
-  ks.push_back(5);ks.push_back(10);ks.push_back(15);
+  EXPECT(assert_equal(expectedMHA,actualMHARawVV));
-
-  // Raw memory access version
-  factor.multiplyHessianAdd(2.0, dataX, dataY, ks);
 
+  // test 2nd version: multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys)
+  double actualMHARaw2[9];
+  vv2double(Y, actualMHARaw2, nrKeys, fixedDim);
+  vector<size_t> dims;
+  size_t accumulatedDim = 0;
+  for (size_t i = 0; i < nrKeys+1; i++){
+    dims.push_back(accumulatedDim);
+    accumulatedDim += fixedDim;
+  }
+  regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw2, dims);
+  VectorValues actualMHARawVV2 = double2vv(actualMHARaw2,nrKeys,fixedDim);
+  EXPECT(assert_equal(expectedMHA,actualMHARawVV2));
 }
 
 /* ************************************************************************* */