Added unit tests for NULL noise model and fixed bug in hessianDiagonalXX for that case

gtsam/linear/JacobianFactor.cpp

@@ -447,11 +447,8 @@ namespace gtsam {
       size_t nj = Ab_(pos).cols();
       Vector dj(nj);
       for (size_t k = 0; k < nj; ++k) {
-        Vector column_k;
+        Vector column_k = Ab_(pos).col(k);
-        if(model_)
+        if (model_) column_k = model_->whiten(column_k);
-          column_k = model_->whiten(Ab_(pos).col(k));
-        else
-          column_k = Ab_(pos).col(k);
         dj(k) = dot(column_k,column_k);
       }
       d.insert(j,dj);
@@ -465,7 +462,8 @@ namespace gtsam {
     for(size_t pos=0; pos<size(); ++pos)
     {
       Key j = keys_[pos];
-      Matrix Aj = model_->Whiten(Ab_(pos));
+      Matrix Aj = Ab_(pos);
+      if (model_) Aj = model_->Whiten(Aj);
       blocks.insert(make_pair(j,Aj.transpose()*Aj));
     }
     return blocks;

gtsam/linear/tests/testJacobianFactorUnordered.cpp

@@ -204,9 +204,54 @@ TEST(JacobianFactor, error)
   DOUBLES_EQUAL(expected_error, actual_error, 1e-10);
 }
 
+/* ************************************************************************* */
+TEST(JacobianFactor, matrices_NULL)
+{
+  // Make sure everything works with NULL noise model
+  JacobianFactor factor(simple::terms, simple::b);
+
+  Matrix jacobianExpected(3, 9);
+  jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
+  Vector rhsExpected = simple::b;
+  Matrix augmentedJacobianExpected(3, 10);
+  augmentedJacobianExpected << jacobianExpected, rhsExpected;
+
+  Matrix augmentedHessianExpected =
+    augmentedJacobianExpected.transpose() * augmentedJacobianExpected;
+
+  // Hessian
+  EXPECT(assert_equal(Matrix(augmentedHessianExpected.topLeftCorner(9,9)), factor.information()));
+  EXPECT(assert_equal(augmentedHessianExpected, factor.augmentedInformation()));
+
+  // Whitened Jacobian
+  EXPECT(assert_equal(jacobianExpected, factor.jacobian().first));
+  EXPECT(assert_equal(rhsExpected, factor.jacobian().second));
+  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian()));
+
+  // Unwhitened Jacobian
+  EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first));
+  EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
+  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));
+
+  // hessianDiagonal
+  VectorValues expectDiagonal;
+  expectDiagonal.insert(5, ones(3));
+  expectDiagonal.insert(10, 4*ones(3));
+  expectDiagonal.insert(15, 9*ones(3));
+  EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));
+
+  // hessianBlockDiagonal
+  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
+  LONGS_EQUAL(3,actualBD.size());
+  EXPECT(assert_equal(1*eye(3),actualBD[5]));
+  EXPECT(assert_equal(4*eye(3),actualBD[10]));
+  EXPECT(assert_equal(9*eye(3),actualBD[15]));
+}
+
 /* ************************************************************************* */
 TEST(JacobianFactor, matrices)
 {
+  // And now witgh a non-unit noise model
   JacobianFactor factor(simple::terms, simple::b, simple::noise);
 
   Matrix jacobianExpected(3, 9);