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 47ab3e9ca..6e14960fd 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"); @@ -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, 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); } @@ -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_ ? - 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 96e61aa33..10fb34988 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -280,30 +280,69 @@ 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(); +TEST(HessianFactor, CombineAndEliminate1) { + 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); + 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) << - 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(); + 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 = (Matrix(3,3) << - 3.0, 0.0, 0.0, - 0.0, 3.0, 0.0, - 0.0, 0.0, 3.0).finished(); + Matrix A21 = 3.0 * I_3x3; 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 A1; A1 << A11, A01, A21; - Vector9 b; b << b1, b0, b2; - Vector9 sigmas; sigmas << s1, s0, s2; + 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)); + 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; - boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0))); + JacobianFactor::shared_ptr expectedFactor; + boost::tie(expectedConditional, expectedFactor) = // + jacobian.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)); - 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)); } /* ************************************************************************* */ 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()); }