Merged in fix/zeroRowInQR (pull request #155)

Change in QR factorization

gtsam/linear/HessianFactor.cpp

@@ -268,18 +268,11 @@ void HessianFactor::print(const std::string& s,
 
 /* ************************************************************************* */
 bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
-  if (!dynamic_cast<const HessianFactor*>(&lf))
+  const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&lf);
+  if (!rhs || !Factor::equals(lf, tol))
     return false;
-  else {
+  return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(),
-    if (!Factor::equals(lf, tol))
+      tol);
-      return false;
-    Matrix thisMatrix = info_.full().selfadjointView();
-    thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0;
-    Matrix rhsMatrix =
-        static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
-    rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0;
-    return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
-  }
 }
 
 /* ************************************************************************* */

gtsam/linear/JacobianFactor.cpp

@@ -66,10 +66,10 @@ JacobianFactor::JacobianFactor() :
 /* ************************************************************************* */
 JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
   // Copy the matrix data depending on what type of factor we're copying from
-  if (const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf))
+  if (const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(&gf))
-    *this = JacobianFactor(*rhs);
+    *this = JacobianFactor(*asJacobian);
-  else if (const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
+  else if (const HessianFactor* asHessian = dynamic_cast<const HessianFactor*>(&gf))
-    *this = JacobianFactor(*rhs);
+    *this = JacobianFactor(*asHessian);
   else
     throw std::invalid_argument(
         "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
@@ -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);
 }
@@ -729,8 +727,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);
 }
@@ -759,11 +757,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

@@ -280,30 +280,69 @@ TEST(HessianFactor, ConstructorNWay)
 }
 
 /* ************************************************************************* */
-TEST(HessianFactor, CombineAndEliminate)
+TEST(HessianFactor, CombineAndEliminate1) {
-{
+  Matrix3 A01 = 3.0 * I_3x3;
-  Matrix A01 = (Matrix(3,3) <<
+  Vector3 b0(1, 0, 0);
-      1.0, 0.0, 0.0,
+
-      0.0, 1.0, 0.0,
+  Matrix3 A21 = 4.0 * I_3x3;
-      0.0, 0.0, 1.0).finished();
+  Vector3 b2 = Vector3::Zero();
+
+  GaussianFactorGraph gfg;
+  gfg.add(1, A01, b0);
+  gfg.add(1, A21, b2);
+
+  Matrix63 A1;
+  A1 << A01, A21;
+  Vector6 b;
+  b << b0, b2;
+
+  // create a full, uneliminated version of the factor
+  JacobianFactor jacobian(1, A1, b);
+
+  // Make sure combining works
+  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 expectedFactor;
+  boost::tie(expectedConditional, expectedFactor) = //
+      jacobian.eliminate(ordering);
+
+  // Eliminate
+  GaussianConditional::shared_ptr actualConditional;
+  HessianFactor::shared_ptr actualHessian;
+  boost::tie(actualConditional, actualHessian) = //
+      EliminateCholesky(gfg, ordering);
+
+  EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
+  EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
+  EXPECT(
+      assert_equal(expectedFactor->augmentedInformation(),
+          actualHessian->augmentedInformation(), 1e-9));
+  EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(HessianFactor, CombineAndEliminate2) {
+  Matrix A01 = I_3x3;
   Vector3 b0(1.5, 1.5, 1.5);
   Vector3 s0(1.6, 1.6, 1.6);
 
-  Matrix A10 = (Matrix(3,3) <<
+  Matrix A10 = 2.0 * I_3x3;
-      2.0, 0.0, 0.0,
+  Matrix A11 = -2.0 * I_3x3;
-      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(2.5, 2.5, 2.5);
   Vector3 s1(2.6, 2.6, 2.6);
 
-  Matrix A21 = (Matrix(3,3) <<
+  Matrix A21 = 3.0 * I_3x3;
-      3.0, 0.0, 0.0,
-      0.0, 3.0, 0.0,
-      0.0, 0.0, 3.0).finished();
   Vector3 b2(3.5, 3.5, 3.5);
   Vector3 s2(3.6, 3.6, 3.6);
 
@@ -312,29 +351,45 @@ TEST(HessianFactor, CombineAndEliminate)
   gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
   gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
 
-  Matrix93 A0; A0 << A10, Z_3x3, Z_3x3;
+  Matrix93 A0, A1;
-  Matrix93 A1; A1 << A11, A01, A21;
+  A0 << A10, Z_3x3, Z_3x3;
-  Vector9 b; b << b1, b0, b2;
+  A1 << A11, A01, A21;
-  Vector9 sigmas; sigmas << s1, s0, s2;
+  Vector9 b, sigmas;
+  b << b1, b0, b2;
+  sigmas << s1, s0, s2;
 
   // create a full, uneliminated version of the factor
-  JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
+  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 expectedRemainingFactor;
+  JacobianFactor::shared_ptr expectedFactor;
-  boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0)));
+  boost::tie(expectedConditional, expectedFactor) = //
+      jacobian.eliminate(ordering);
 
   // Eliminate
   GaussianConditional::shared_ptr actualConditional;
-  HessianFactor::shared_ptr actualCholeskyFactor;
+  HessianFactor::shared_ptr actualHessian;
-  boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0)));
+  boost::tie(actualConditional, actualHessian) = //
+      EliminateCholesky(gfg, ordering);
 
   EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
-  EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6));
+  VectorValues v;
+  v.insert(1, Vector3(1, 2, 3));
+  EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
+  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());
 }