IMPORTANT CHANGE/BUGFIX: QR elimination now (I think properly) leaves a row of zeros if the RHS after QR contains a non-zero. A corresponding change in the error calculation makes that Jacobian and Hessian factors now agree on error. I think this was a bug, because it affected the error, but (I think) it only pertained to "empty" JacobianFactors which have no bearing on optimization/elimination.

gtsam/linear/JacobianFactor.cpp

@@ -432,8 +432,6 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const {
 
 /* ************************************************************************* */
 double JacobianFactor::error(const VectorValues& c) const {
-  if (empty())
-    return 0;
   Vector weighted = error_vector(c);
   return 0.5 * weighted.dot(weighted);
 }
@@ -684,8 +682,8 @@ std::pair<boost::shared_ptr<GaussianConditional>,
   jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
 
   // Split elimination result into conditional and remaining factor
-  GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(
+  GaussianConditional::shared_ptr conditional = //
-      keys.size());
+      jointFactor->splitConditional(keys.size());
 
   return make_pair(conditional, jointFactor);
 }
@@ -714,11 +712,11 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(
   }
   GaussianConditional::shared_ptr conditional = boost::make_shared<
       GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
-  const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd)
+  const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd)
       - Ab_.rowStart() - frontalDim;
   const DenseIndex remainingRows =
-      model_ ?
+      model_ ? std::min(model_->sigmas().size() - frontalDim,
-          std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) :
+          maxRemainingRows) :
           maxRemainingRows;
   Ab_.rowStart() += frontalDim;
   Ab_.rowEnd() = Ab_.rowStart() + remainingRows;

gtsam/linear/tests/testHessianFactor.cpp

@@ -279,60 +279,43 @@ TEST(HessianFactor, ConstructorNWay)
   EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2)));
 }
 
-/* ************************************************************************* */
-namespace example {
-Matrix A01 = (Matrix(3,3) <<
-    1.0, 0.0, 0.0,
-    0.0, 1.0, 0.0,
-    0.0, 0.0, 1.0).finished();
-Vector3 b0(1,0,0);//(1.5, 1.5, 1.5);
-Vector3 s0=Vector3::Ones();//(1.6, 1.6, 1.6);
-
-Matrix A10 = (Matrix(3,3) <<
-    2.0, 0.0, 0.0,
-    0.0, 2.0, 0.0,
-    0.0, 0.0, 2.0).finished();
-Matrix A11 = (Matrix(3,3) <<
-    -2.0, 0.0, 0.0,
-    0.0, -2.0, 0.0,
-    0.0, 0.0, -2.0).finished();
-Vector3 b1 = Vector3::Zero();//(2.5, 2.5, 2.5);
-Vector3 s1=Vector3::Ones();//(2.6, 2.6, 2.6);
-
-Matrix A21 = (Matrix(3,3) <<
-    3.0, 0.0, 0.0,
-    0.0, 3.0, 0.0,
-    0.0, 0.0, 3.0).finished();
-Vector3 b2 = Vector3::Zero();//(3.5, 3.5, 3.5);
-Vector3 s2=Vector3::Ones();//(3.6, 3.6, 3.6);
-}
-
 /* ************************************************************************* */
 TEST(HessianFactor, CombineAndEliminate1) {
-  using namespace example;
+  Matrix3 A01 = 3.0 * I_3x3;
+  Vector3 b0(1, 0, 0);
+
+  Matrix3 A21 = 4.0 * I_3x3;
+  Vector3 b2 = Vector3::Zero();
+
   GaussianFactorGraph gfg;
-  gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
+  gfg.add(1, A01, b0);
-  gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
+  gfg.add(1, A21, b2);
 
   Matrix63 A1;
-  A1 << A11, A21;
+  A1 << A01, A21;
-  Vector6 b, sigmas;
+  Vector6 b;
   b << b0, b2;
-  sigmas << s0, s2;
 
   // create a full, uneliminated version of the factor
-  JacobianFactor expectedFactor(1, A1, b,
+  JacobianFactor jacobian(1, A1, b);
-      noiseModel::Diagonal::Sigmas(sigmas, true));
 
   // Make sure combining works
-  EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6));
+  HessianFactor hessian(gfg);
+  VectorValues v;
+  v.insert(1, Vector3(1, 0, 0));
+  EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9);
+  EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
+  EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9));
+  EXPECT(
+      assert_equal(jacobian.augmentedInformation(),
+          hessian.augmentedInformation(), 1e-9));
 
   // perform elimination on jacobian
   Ordering ordering = list_of(1);
   GaussianConditional::shared_ptr expectedConditional;
-  JacobianFactor::shared_ptr expectedRemaining;
+  JacobianFactor::shared_ptr expectedFactor;
-  boost::tie(expectedConditional, expectedRemaining) = //
+  boost::tie(expectedConditional, expectedFactor) = //
-      expectedFactor.eliminate(ordering);
+      jacobian.eliminate(ordering);
 
   // Eliminate
   GaussianConditional::shared_ptr actualConditional;
@@ -341,15 +324,28 @@ TEST(HessianFactor, CombineAndEliminate1) {
       EliminateCholesky(gfg, ordering);
 
   EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
-  VectorValues vv;
+  EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
-  vv.insert(1, Vector3(1, 2, 3));
+  EXPECT(
-  DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9);
+      assert_equal(expectedFactor->augmentedInformation(),
-  EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 1e-6));
+          actualHessian->augmentedInformation(), 1e-9));
+  EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
 }
 
 /* ************************************************************************* */
 TEST(HessianFactor, CombineAndEliminate2) {
-  using namespace example;
+  Matrix A01 = I_3x3;
+  Vector3 b0(1.5, 1.5, 1.5);
+  Vector3 s0(1.6, 1.6, 1.6);
+
+  Matrix A10 = 2.0 * I_3x3;
+  Matrix A11 = -2.0 * I_3x3;
+  Vector3 b1(2.5, 2.5, 2.5);
+  Vector3 s1(2.6, 2.6, 2.6);
+
+  Matrix A21 = 3.0 * I_3x3;
+  Vector3 b2(3.5, 3.5, 3.5);
+  Vector3 s2(3.6, 3.6, 3.6);
+
   GaussianFactorGraph gfg;
   gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
   gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
@@ -363,18 +359,22 @@ TEST(HessianFactor, CombineAndEliminate2) {
   sigmas << s1, s0, s2;
 
   // create a full, uneliminated version of the factor
-  JacobianFactor expectedFactor(0, A0, 1, A1, b,
+  JacobianFactor jacobian(0, A0, 1, A1, b,
       noiseModel::Diagonal::Sigmas(sigmas, true));
 
   // Make sure combining works
-  EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6));
+  HessianFactor hessian(gfg);
+  EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
+  EXPECT(
+      assert_equal(jacobian.augmentedInformation(),
+          hessian.augmentedInformation(), 1e-9));
 
   // perform elimination on jacobian
   Ordering ordering = list_of(0);
   GaussianConditional::shared_ptr expectedConditional;
-  JacobianFactor::shared_ptr expectedRemaining;
+  JacobianFactor::shared_ptr expectedFactor;
-  boost::tie(expectedConditional, expectedRemaining) = //
+  boost::tie(expectedConditional, expectedFactor) = //
-      expectedFactor.eliminate(ordering);
+      jacobian.eliminate(ordering);
 
   // Eliminate
   GaussianConditional::shared_ptr actualConditional;
@@ -383,10 +383,13 @@ TEST(HessianFactor, CombineAndEliminate2) {
       EliminateCholesky(gfg, ordering);
 
   EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
-  VectorValues vv;
+  VectorValues v;
-  vv.insert(1, Vector3(1, 2, 3));
+  v.insert(1, Vector3(1, 2, 3));
-  DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9);
+  EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
-  EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 1e-6));
+  EXPECT(
+      assert_equal(expectedFactor->augmentedInformation(),
+          actualHessian->augmentedInformation(), 1e-9));
+  EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
 }
 
 /* ************************************************************************* */

gtsam/linear/tests/testJacobianFactor.cpp

@@ -540,9 +540,9 @@ TEST(JacobianFactor, EliminateQR)
   EXPECT(assert_equal(size_t(2), actualJF.keys().size()));
   EXPECT(assert_equal(Key(9), actualJF.keys()[0]));
   EXPECT(assert_equal(Key(11), actualJF.keys()[1]));
-  EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001));
+  EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001));
-  EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
+  EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
-  EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001));
+  EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001));
   EXPECT(!actualJF.get_model());
 }