From f523db2d9887d3b4a37537173e8cb4171b6b01fc Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 15 Jun 2015 00:52:43 -0700 Subject: [PATCH] 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 | 12 +-- gtsam/linear/tests/testHessianFactor.cpp | 111 +++++++++++----------- gtsam/linear/tests/testJacobianFactor.cpp | 6 +- 3 files changed, 65 insertions(+), 64 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index b38aa89e7..61986e71d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/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, jointFactor->Ab_.matrix().triangularView().setZero(); // Split elimination result into conditional and remaining factor - GaussianConditional::shared_ptr conditional = jointFactor->splitConditional( - keys.size()); + GaussianConditional::shared_ptr conditional = // + 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_ ? - std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) : + model_ ? std::min(model_->sigmas().size() - frontalDim, + maxRemainingRows) : maxRemainingRows; Ab_.rowStart() += frontalDim; Ab_.rowEnd() = Ab_.rowStart() + remainingRows; diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 13fb9bd0c..10fb34988 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/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, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + gfg.add(1, A01, b0); + gfg.add(1, A21, b2); Matrix63 A1; - A1 << A11, A21; - Vector6 b, sigmas; + A1 << A01, A21; + Vector6 b; b << b0, b2; - sigmas << s0, s2; // create a full, uneliminated version of the factor - JacobianFactor expectedFactor(1, A1, b, - noiseModel::Diagonal::Sigmas(sigmas, true)); + JacobianFactor jacobian(1, A1, b); // 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; - boost::tie(expectedConditional, expectedRemaining) = // - expectedFactor.eliminate(ordering); + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + 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; - vv.insert(1, Vector3(1, 2, 3)); - DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9); - EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 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) { - 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; - boost::tie(expectedConditional, expectedRemaining) = // - expectedFactor.eliminate(ordering); + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + 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; - vv.insert(1, Vector3(1, 2, 3)); - DOUBLES_EQUAL(expectedRemaining->error(vv), actualHessian->error(vv), 1e-9); - EXPECT(assert_equal(HessianFactor(*expectedRemaining), *actualHessian, 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)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 7b2d59171..17ceaf5f0 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/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, 8, 4, 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(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 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, 5)), actualJF.getb(), 0.001)); EXPECT(!actualJF.get_model()); }