From 57716646227b619272f7fa00f11a488489c0ba91 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 16:01:54 -0700 Subject: [PATCH 1/3] Starting to diagnose issue with lower-left entry of Hessian --- gtsam/linear/HessianFactor.cpp | 15 ++++----------- gtsam/linear/JacobianFactor.cpp | 8 ++++---- gtsam/linear/tests/testHessianFactor.cpp | 14 ++++++++------ 3 files changed, 16 insertions(+), 21 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index bbc684a10..21f4b117f 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/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(&lf)) + const HessianFactor* rhs = dynamic_cast(&lf); + if (!rhs || !Factor::equals(lf, tol)) return false; - else { - if (!Factor::equals(lf, tol)) - return false; - Matrix thisMatrix = info_.full().selfadjointView(); - thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0; - Matrix rhsMatrix = - static_cast(lf).info_.full().selfadjointView(); - rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0; - return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); - } + return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(), + tol); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 8214294b2..b38aa89e7 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/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(&gf)) - *this = JacobianFactor(*rhs); - else if (const HessianFactor* rhs = dynamic_cast(&gf)) - *this = JacobianFactor(*rhs); + if (const JacobianFactor* asJacobian = dynamic_cast(&gf)) + *this = JacobianFactor(*asJacobian); + else if (const HessianFactor* asHessian = dynamic_cast(&gf)) + *this = JacobianFactor(*asHessian); else throw std::invalid_argument( "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor"); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 96e61aa33..afd611112 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -286,8 +286,8 @@ TEST(HessianFactor, CombineAndEliminate) 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(); - Vector3 b0(1.5, 1.5, 1.5); - Vector3 s0(1.6, 1.6, 1.6); + 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, @@ -297,15 +297,15 @@ TEST(HessianFactor, CombineAndEliminate) -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); + 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(3.5, 3.5, 3.5); - Vector3 s2(3.6, 3.6, 3.6); + Vector3 b2 = Vector3::Zero();//(3.5, 3.5, 3.5); + Vector3 s2=Vector3::Ones();//(3.6, 3.6, 3.6); GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); @@ -334,6 +334,8 @@ TEST(HessianFactor, CombineAndEliminate) boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0))); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); + VectorValues vv; vv.insert(1, Vector3(1,2,3)); + EXPECT_DOUBLES_EQUAL(expectedRemainingFactor->error(vv), actualCholeskyFactor->error(vv), 1e-9); EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6)); } From 83171b60960d8df2c823f29aaa6986f1c9edbdcf Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 23:07:28 -0700 Subject: [PATCH 2/3] New example --- gtsam/linear/tests/testHessianFactor.cpp | 124 ++++++++++++++++------- 1 file changed, 87 insertions(+), 37 deletions(-) diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index afd611112..13fb9bd0c 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -280,63 +280,113 @@ TEST(HessianFactor, ConstructorNWay) } /* ************************************************************************* */ -TEST(HessianFactor, CombineAndEliminate) -{ - 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); +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 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); +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; GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - 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 A1; A1 << A11, A01, A21; - Vector9 b; b << b1, b0, b2; - Vector9 sigmas; sigmas << s1, s0, s2; + Matrix63 A1; + A1 << A11, A21; + Vector6 b, sigmas; + b << b0, b2; + sigmas << s0, s2; // create a full, uneliminated version of the factor - JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + JacobianFactor expectedFactor(1, A1, b, + noiseModel::Diagonal::Sigmas(sigmas, true)); // Make sure combining works EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6)); // perform elimination on jacobian + Ordering ordering = list_of(1); GaussianConditional::shared_ptr expectedConditional; - JacobianFactor::shared_ptr expectedRemainingFactor; - boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0))); + JacobianFactor::shared_ptr expectedRemaining; + boost::tie(expectedConditional, expectedRemaining) = // + expectedFactor.eliminate(ordering); // Eliminate GaussianConditional::shared_ptr actualConditional; - HessianFactor::shared_ptr actualCholeskyFactor; - boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0))); + HessianFactor::shared_ptr actualHessian; + boost::tie(actualConditional, actualHessian) = // + EliminateCholesky(gfg, ordering); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); - VectorValues vv; vv.insert(1, Vector3(1,2,3)); - EXPECT_DOUBLES_EQUAL(expectedRemainingFactor->error(vv), actualCholeskyFactor->error(vv), 1e-9); - EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 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)); +} + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate2) { + using namespace example; + GaussianFactorGraph gfg; + gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + + Matrix93 A0, A1; + A0 << A10, Z_3x3, Z_3x3; + A1 << A11, A01, A21; + 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)); + + // Make sure combining works + EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6)); + + // perform elimination on jacobian + Ordering ordering = list_of(0); + GaussianConditional::shared_ptr expectedConditional; + JacobianFactor::shared_ptr expectedRemaining; + boost::tie(expectedConditional, expectedRemaining) = // + expectedFactor.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)); + 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)); } /* ************************************************************************* */ From f523db2d9887d3b4a37537173e8cb4171b6b01fc Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 15 Jun 2015 00:52:43 -0700 Subject: [PATCH 3/3] 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()); }