From f3fdf8abe9e3e24456514e3c08e8b0b9f65a1215 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 18 Aug 2013 17:17:09 +0000 Subject: [PATCH] Reenabled some code relating to Hessian factors that I had accidently left disabled --- gtsam/linear/GaussianFactorGraph.cpp | 9 ++-- gtsam/linear/JacobianFactor.h | 5 +- .../testGaussianFactorGraphUnordered.cpp | 10 ++-- .../tests/testJacobianFactorUnordered.cpp | 50 +++++++------------ 4 files changed, 29 insertions(+), 45 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 7e5f0b16c..9e16e7e8c 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -80,11 +80,10 @@ namespace gtsam { JacobianFactor::shared_ptr jacobianFactor( boost::dynamic_pointer_cast(factor)); if (!jacobianFactor) { - //TODO : re-enable - //HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); - //if (hessian) - // jacobianFactor.reset(new JacobianFactor(*hessian)); - //else + HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + if (hessian) + jacobianFactor.reset(new JacobianFactor(*hessian)); + else throw invalid_argument( "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index e3412c79d..e39dc7050 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -28,7 +28,7 @@ namespace gtsam { // Forward declarations - //class HessianFactor; + class HessianFactor; class VariableSlots; class GaussianFactorGraph; class GaussianConditional; @@ -138,9 +138,6 @@ namespace gtsam { template JacobianFactor( const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); - - /** Convert from a HessianFactor (does Cholesky) */ - //JacobianFactor(const HessianFactor& factor); /** * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index bc0a0d1ba..e229e2245 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -143,16 +143,16 @@ TEST(GaussianFactorGraph, matrices) { Vector expectedeta = expectedA.transpose() * expectedb; Matrix actualJacobian = gfg.augmentedJacobian(); - //Matrix actualHessian = gfg.augmentedHessian(); + Matrix actualHessian = gfg.augmentedHessian(); Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian(); - //Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); + Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); EXPECT(assert_equal(expectedJacobian, actualJacobian)); - //EXPECT(assert_equal(expectedHessian, actualHessian)); + EXPECT(assert_equal(expectedHessian, actualHessian)); EXPECT(assert_equal(expectedA, actualA)); EXPECT(assert_equal(expectedb, actualb)); - //EXPECT(assert_equal(expectedL, actualL)); - //EXPECT(assert_equal(expectedeta, actualeta)); + EXPECT(assert_equal(expectedL, actualL)); + EXPECT(assert_equal(expectedeta, actualeta)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactorUnordered.cpp index b4d51794e..454e21298 100644 --- a/gtsam/linear/tests/testJacobianFactorUnordered.cpp +++ b/gtsam/linear/tests/testJacobianFactorUnordered.cpp @@ -125,25 +125,25 @@ TEST(JacobianFactor, constructors_and_accessors) } /* ************************************************************************* */ -//TEST(JabobianFactor, Hessian_conversion) { -// HessianFactor hessian(0, (Matrix(4,4) << -// 1.57, 2.695, -1.1, -2.35, -// 2.695, 11.3125, -0.65, -10.225, -// -1.1, -0.65, 1, 0.5, -// -2.35, -10.225, 0.5, 9.25).finished(), -// (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), -// 73.1725); -// -// JacobianFactor expected(0, (Matrix(2,4) << -// 1.2530, 2.1508, -0.8779, -1.8755, -// 0, 2.5858, 0.4789, -2.3943).finished(), -// (Vector(2) << -6.2929, -5.7941).finished(), -// noiseModel::Unit::Create(2)); -// -// JacobianFactor actual(hessian); -// -// EXPECT(assert_equal(expected, actual, 1e-3)); -//} +TEST(JabobianFactor, Hessian_conversion) { + HessianFactor hessian(0, (Matrix(4,4) << + 1.57, 2.695, -1.1, -2.35, + 2.695, 11.3125, -0.65, -10.225, + -1.1, -0.65, 1, 0.5, + -2.35, -10.225, 0.5, 9.25).finished(), + (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), + 73.1725); + + JacobianFactor expected(0, (Matrix(2,4) << + 1.2530, 2.1508, -0.8779, -1.8755, + 0, 2.5858, 0.4789, -2.3943).finished(), + (Vector(2) << -6.2929, -5.7941).finished(), + noiseModel::Unit::Create(2)); + + JacobianFactor actual(hessian); + + EXPECT(assert_equal(expected, actual, 1e-3)); +} /* ************************************************************************* */ TEST( JacobianFactor, construct_from_graph) @@ -458,18 +458,6 @@ TEST(JacobianFactor, EliminateQR) 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(!actualJF.get_model()); - - // Eliminate (3 frontal variables, 6 scalar columns) using Cholesky !!!! - // TODO: HessianFactor - //GaussianBayesNet actualFragment_Chol = *actualFactor_Chol.eliminate(3, JacobianFactor::SOLVE_CHOLESKY); - //EXPECT(assert_equal(expectedFragment, actualFragment_Chol, 0.001)); - //EXPECT(assert_equal(size_t(2), actualFactor_Chol.keys().size())); - //EXPECT(assert_equal(Index(9), actualFactor_Chol.keys()[0])); - //EXPECT(assert_equal(Index(11), actualFactor_Chol.keys()[1])); - //EXPECT(assert_equal(Ae1, actualFactor_Chol.getA(actualFactor_Chol.begin()), 0.001)); //// - //EXPECT(linear_dependent(Ae2, actualFactor_Chol.getA(actualFactor_Chol.begin()+1), 0.001)); - //EXPECT(assert_equal(be, actualFactor_Chol.getb(), 0.001)); //// - //EXPECT(assert_equal(ones(4), actualFactor_Chol.get_sigmas(), 0.001)); } /* ************************************************************************* */