12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Reenabled some code relating to Hessian factors that I had accidently left disabled

release/4.3a0
Richard Roberts 2013-08-18 17:17:09 +00:00
parent fe860be33f
commit f3fdf8abe9
4 changed files with 29 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -80,11 +80,10 @@ namespace gtsam {
JacobianFactor::shared_ptr jacobianFactor( JacobianFactor::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor)); boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (!jacobianFactor) { if (!jacobianFactor) {
//TODO : re-enable HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
//HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor)); if (hessian)
//if (hessian) jacobianFactor.reset(new JacobianFactor(*hessian));
// jacobianFactor.reset(new JacobianFactor(*hessian)); else
//else
throw invalid_argument( throw invalid_argument(
"GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
} }

5
gtsam/linear/JacobianFactor.h
View File

@ -28,7 +28,7 @@
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
//class HessianFactor; class HessianFactor;
class VariableSlots; class VariableSlots;
class GaussianFactorGraph; class GaussianFactorGraph;
class GaussianConditional; class GaussianConditional;
@ -139,9 +139,6 @@ namespace gtsam {
JacobianFactor( JacobianFactor(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); 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 * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots
* structure computed for \c graph is already available, providing it will reduce the amount of * structure computed for \c graph is already available, providing it will reduce the amount of

10
gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
View File

@ -143,16 +143,16 @@ TEST(GaussianFactorGraph, matrices) {
Vector expectedeta = expectedA.transpose() * expectedb; Vector expectedeta = expectedA.transpose() * expectedb;
Matrix actualJacobian = gfg.augmentedJacobian(); Matrix actualJacobian = gfg.augmentedJacobian();
//Matrix actualHessian = gfg.augmentedHessian(); Matrix actualHessian = gfg.augmentedHessian();
Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian(); 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(expectedJacobian, actualJacobian));
//EXPECT(assert_equal(expectedHessian, actualHessian)); EXPECT(assert_equal(expectedHessian, actualHessian));
EXPECT(assert_equal(expectedA, actualA)); EXPECT(assert_equal(expectedA, actualA));
EXPECT(assert_equal(expectedb, actualb)); EXPECT(assert_equal(expectedb, actualb));
//EXPECT(assert_equal(expectedL, actualL)); EXPECT(assert_equal(expectedL, actualL));
//EXPECT(assert_equal(expectedeta, actualeta)); EXPECT(assert_equal(expectedeta, actualeta));
} }
/* ************************************************************************* */ /* ************************************************************************* */

50
gtsam/linear/tests/testJacobianFactorUnordered.cpp
View File

@ -125,25 +125,25 @@ TEST(JacobianFactor, constructors_and_accessors)
} }
/* ************************************************************************* */ /* ************************************************************************* */
//TEST(JabobianFactor, Hessian_conversion) { TEST(JabobianFactor, Hessian_conversion) {
// HessianFactor hessian(0, (Matrix(4,4) << HessianFactor hessian(0, (Matrix(4,4) <<
// 1.57, 2.695, -1.1, -2.35, 1.57, 2.695, -1.1, -2.35,
// 2.695, 11.3125, -0.65, -10.225, 2.695, 11.3125, -0.65, -10.225,
// -1.1, -0.65, 1, 0.5, -1.1, -0.65, 1, 0.5,
// -2.35, -10.225, 0.5, 9.25).finished(), -2.35, -10.225, 0.5, 9.25).finished(),
// (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
// 73.1725); 73.1725);
//
// JacobianFactor expected(0, (Matrix(2,4) << JacobianFactor expected(0, (Matrix(2,4) <<
// 1.2530, 2.1508, -0.8779, -1.8755, 1.2530, 2.1508, -0.8779, -1.8755,
// 0, 2.5858, 0.4789, -2.3943).finished(), 0, 2.5858, 0.4789, -2.3943).finished(),
// (Vector(2) << -6.2929, -5.7941).finished(), (Vector(2) << -6.2929, -5.7941).finished(),
// noiseModel::Unit::Create(2)); noiseModel::Unit::Create(2));
//
// JacobianFactor actual(hessian); JacobianFactor actual(hessian);
//
// EXPECT(assert_equal(expected, actual, 1e-3)); EXPECT(assert_equal(expected, actual, 1e-3));
//} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( JacobianFactor, construct_from_graph) 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(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(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001));
EXPECT(!actualJF.get_model()); 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */