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

LinearContainerFactor works

release/4.3a0
Alex Cunningham 2013-08-08 18:33:51 +00:00
parent cfd81093bd
commit ec3e89c888
4 changed files with 51 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/nonlinear/CMakeLists.txt
View File

@ -16,7 +16,7 @@ set(nonlinear_local_libs
set(nonlinear_excluded_files set(nonlinear_excluded_files
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test # "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
#"" # Add to this list, with full path, to exclude #"" # Add to this list, with full path, to exclude
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testLinearContainerFactor.cpp" #"${CMAKE_CURRENT_SOURCE_DIR}/tests/testLinearContainerFactor.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testWhiteNoiseFactor.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/tests/testWhiteNoiseFactor.cpp"
) )

22
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -9,11 +9,10 @@
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#if 0
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
@ -31,30 +30,27 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza
/* ************************************************************************* */ /* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor, LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
const boost::optional<Values>& linearizationPoint) const boost::optional<Values>& linearizationPoint)
: factor_(factor), linearizationPoint_(linearizationPoint) { : NonlinearFactor(factor->keys()), factor_(factor), linearizationPoint_(linearizationPoint) {
// Extract keys stashed in linear factor
BOOST_FOREACH(const Index& idx, factor_->keys())
keys_.push_back(idx);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor( LinearContainerFactor::LinearContainerFactor(
const JacobianFactor& factor, const Values& linearizationPoint) const JacobianFactor& factor, const Values& linearizationPoint)
: factor_(factor.clone()) { : NonlinearFactor(factor.keys()), factor_(factor.clone()) {
initializeLinearizationPoint(linearizationPoint); initializeLinearizationPoint(linearizationPoint);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor( LinearContainerFactor::LinearContainerFactor(
const HessianFactor& factor, const Values& linearizationPoint) const HessianFactor& factor, const Values& linearizationPoint)
: factor_(factor.clone()) { : NonlinearFactor(factor.keys()), factor_(factor.clone()) {
initializeLinearizationPoint(linearizationPoint); initializeLinearizationPoint(linearizationPoint);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearContainerFactor::LinearContainerFactor( LinearContainerFactor::LinearContainerFactor(
const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint) const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint)
: factor_(factor->clone()) { : NonlinearFactor(factor->keys()), factor_(factor->clone()) {
initializeLinearizationPoint(linearizationPoint); initializeLinearizationPoint(linearizationPoint);
} }
@ -139,12 +135,12 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con
/* ************************************************************************* */ /* ************************************************************************* */
bool LinearContainerFactor::isJacobian() const { bool LinearContainerFactor::isJacobian() const {
return boost::dynamic_pointer_cast<JacobianFactor>(factor_); return boost::dynamic_pointer_cast<JacobianFactor>(factor_).get();
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool LinearContainerFactor::isHessian() const { bool LinearContainerFactor::isHessian() const {
return boost::dynamic_pointer_cast<HessianFactor>(factor_); return boost::dynamic_pointer_cast<HessianFactor>(factor_).get();
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -160,12 +156,13 @@ HessianFactor::shared_ptr LinearContainerFactor::toHessian() const {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::shared_ptr LinearContainerFactor::negateToGaussian() const { GaussianFactor::shared_ptr LinearContainerFactor::negateToGaussian() const {
GaussianFactor::shared_ptr result = factor_->negate(); GaussianFactor::shared_ptr result = factor_->negate();
return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const { NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const {
GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place
return boost::make_shared<LinearContainerFactor>(antifactor, linearizationPoint_); return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor, linearizationPoint_));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -183,4 +180,3 @@ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace gtsam } // \namespace gtsam
#endif

4
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -38,10 +38,10 @@ protected:
public: public:
/** Primary constructor: store a linear factor and decode the ordering */ /** Primary constructor: store a linear factor with optional linearization point */
LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());
/** Primary constructor: store a linear factor and decode the ordering */ /** Primary constructor: store a linear factor with optional linearization point */
LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values()); LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values());
/** Constructor from shared_ptr */ /** Constructor from shared_ptr */

105
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -8,6 +8,8 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
@ -28,8 +30,6 @@ Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, generic_jacobian_factor ) { TEST( testLinearContainerFactor, generic_jacobian_factor ) {
Ordering initOrdering; initOrdering += x1, x2, l1, l2;
Matrix A1 = Matrix_(2,2, Matrix A1 = Matrix_(2,2,
2.74222, -0.0067457, 2.74222, -0.0067457,
0.0, 2.63624); 0.0, 2.63624);
@ -39,9 +39,9 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
Vector b = Vector_(2, 0.0277052, Vector b = Vector_(2, 0.0277052,
-0.0533393); -0.0533393);
JacobianFactor expLinFactor(initOrdering[l1], A1, initOrdering[l2], A2, b, diag_model2); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
LinearContainerFactor actFactor(expLinFactor, initOrdering); LinearContainerFactor actFactor(expLinFactor);
EXPECT_LONGS_EQUAL(2, actFactor.size()); EXPECT_LONGS_EQUAL(2, actFactor.size());
EXPECT(actFactor.isJacobian()); EXPECT(actFactor.isJacobian());
EXPECT(!actFactor.isHessian()); EXPECT(!actFactor.isHessian());
@ -56,22 +56,14 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
values.insert(x1, poseA1); values.insert(x1, poseA1);
values.insert(x2, poseA2); values.insert(x2, poseA2);
// Check reconstruction from same ordering // Check reconstruction
GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values, initOrdering); GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values);
EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol)); EXPECT(assert_equal(*expLinFactor.clone(), *actLinearizationA, tol));
// Check reconstruction from new ordering
Ordering newOrdering; newOrdering += x1, l1, x2, l2;
GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(values, newOrdering);
JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, b, diag_model2);
EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Ordering ordering; ordering += x1, x2, l1, l2;
Matrix A1 = Matrix_(2,2, Matrix A1 = Matrix_(2,2,
2.74222, -0.0067457, 2.74222, -0.0067457,
0.0, 2.63624); 0.0, 2.63624);
@ -81,7 +73,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Vector b = Vector_(2, 0.0277052, Vector b = Vector_(2, 0.0277052,
-0.0533393); -0.0533393);
JacobianFactor expLinFactor(ordering[l1], A1, ordering[l2], A2, b, diag_model2); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2);
Values values; Values values;
values.insert(l1, landmark1); values.insert(l1, landmark1);
@ -89,8 +81,8 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
values.insert(x1, poseA1); values.insert(x1, poseA1);
values.insert(x2, poseA2); values.insert(x2, poseA2);
LinearContainerFactor actFactor(expLinFactor, ordering, values); LinearContainerFactor actFactor(expLinFactor, values);
LinearContainerFactor actFactorNolin(expLinFactor, ordering); LinearContainerFactor actFactorNolin(expLinFactor);
EXPECT(assert_equal(actFactor, actFactor, tol)); EXPECT(assert_equal(actFactor, actFactor, tol));
EXPECT(assert_inequal(actFactor, actFactorNolin, tol)); EXPECT(assert_inequal(actFactor, actFactorNolin, tol));
@ -108,19 +100,18 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) {
Vector delta_l1 = Vector_(2, 1.0, 2.0); Vector delta_l1 = Vector_(2, 1.0, 2.0);
Vector delta_l2 = Vector_(2, 3.0, 4.0); Vector delta_l2 = Vector_(2, 3.0, 4.0);
VectorValues delta = values.zeroVectors(ordering); VectorValues delta = values.zeroVectors();
delta.at(ordering[l1]) = delta_l1; delta.at(l1) = delta_l1;
delta.at(ordering[l2]) = delta_l2; delta.at(l2) = delta_l2;
Values noisyValues = values.retract(delta, ordering); Values noisyValues = values.retract(delta);
double expError = expLinFactor.error(delta); double expError = expLinFactor.error(delta);
EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol); EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol);
EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors(ordering)), actFactor.error(values), tol); EXPECT_DOUBLES_EQUAL(expLinFactor.error(values.zeroVectors()), actFactor.error(values), tol);
// Check linearization with corrections for updated linearization point // Check linearization with corrections for updated linearization point
Ordering newOrdering; newOrdering += x1, l1, x2, l2; GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues);
GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues, newOrdering);
Vector bprime = b - A1 * delta_l1 - A2 * delta_l2; Vector bprime = b - A1 * delta_l1 - A2 * delta_l2;
JacobianFactor expLinFactor2(newOrdering[l1], A1, newOrdering[l2], A2, bprime, diag_model2); JacobianFactor expLinFactor2(l1, A1, l2, A2, bprime, diag_model2);
EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol));
} }
@ -145,8 +136,7 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) {
double f = 10.0; double f = 10.0;
Ordering initOrdering; initOrdering += x1, x2, l1, l2; HessianFactor initFactor(x1, x2, l1,
HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1],
G11, G12, G13, g1, G22, G23, g2, G33, g3, f); G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
Values values; Values values;
@ -155,18 +145,12 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) {
values.insert(x1, poseA1); values.insert(x1, poseA1);
values.insert(x2, poseA2); values.insert(x2, poseA2);
LinearContainerFactor actFactor(initFactor, initOrdering); LinearContainerFactor actFactor(initFactor);
EXPECT(!actFactor.isJacobian()); EXPECT(!actFactor.isJacobian());
EXPECT(actFactor.isHessian()); EXPECT(actFactor.isHessian());
GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering); GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values);
EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol)); EXPECT(assert_equal(*initFactor.clone(), *actLinearization1, tol));
Ordering newOrdering; newOrdering += l1, x1, x2, l2;
HessianFactor expLinFactor(newOrdering[x1], newOrdering[x2], newOrdering[l1],
G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
GaussianFactor::shared_ptr actLinearization2 = actFactor.linearize(values, newOrdering);
EXPECT(assert_equal(*expLinFactor.clone(), *actLinearization2, tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -196,8 +180,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Matrix G(5,5); Matrix G(5,5);
G << G11, G12, Matrix::Zero(2,3), G22; G << G11, G12, Matrix::Zero(2,3), G22;
Ordering ordering; ordering += x1, x2, l1; HessianFactor initFactor(x1, l1, G11, G12, g1, G22, g2, f);
HessianFactor initFactor(ordering[x1], ordering[l1], G11, G12, g1, G22, g2, f);
Values linearizationPoint, expLinPoints; Values linearizationPoint, expLinPoints;
linearizationPoint.insert(l1, landmark1); linearizationPoint.insert(l1, landmark1);
@ -205,7 +188,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
expLinPoints = linearizationPoint; expLinPoints = linearizationPoint;
linearizationPoint.insert(x2, poseA2); linearizationPoint.insert(x2, poseA2);
LinearContainerFactor actFactor(initFactor, ordering, linearizationPoint); LinearContainerFactor actFactor(initFactor, linearizationPoint);
EXPECT(!actFactor.isJacobian()); EXPECT(!actFactor.isJacobian());
EXPECT(actFactor.isHessian()); EXPECT(actFactor.isHessian());
@ -219,16 +202,16 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Vector delta_x2 = Vector_(3, 6.0, 7.0, 0.3); Vector delta_x2 = Vector_(3, 6.0, 7.0, 0.3);
// Check error calculation // Check error calculation
VectorValues delta = linearizationPoint.zeroVectors(ordering); VectorValues delta = linearizationPoint.zeroVectors();
delta.at(ordering[l1]) = delta_l1; delta.at(l1) = delta_l1;
delta.at(ordering[x1]) = delta_x1; delta.at(x1) = delta_x1;
delta.at(ordering[x2]) = delta_x2; delta.at(x2) = delta_x2;
EXPECT(assert_equal(Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys()))); EXPECT(assert_equal(Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0), delta.vector(initFactor.keys())));
Values noisyValues = linearizationPoint.retract(delta, ordering); Values noisyValues = linearizationPoint.retract(delta);
double expError = initFactor.error(delta); double expError = initFactor.error(delta);
EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol); EXPECT_DOUBLES_EQUAL(expError, actFactor.error(noisyValues), tol);
EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors(ordering)), actFactor.error(linearizationPoint), tol); EXPECT_DOUBLES_EQUAL(initFactor.error(linearizationPoint.zeroVectors()), actFactor.error(linearizationPoint), tol);
// Compute updated versions // Compute updated versions
Vector dv = Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0); Vector dv = Vector_(5, 3.0, 4.0, 0.5, 1.0, 2.0);
@ -239,8 +222,8 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) {
Vector g1_prime = g_prime.head(3); Vector g1_prime = g_prime.head(3);
Vector g2_prime = g_prime.tail(2); Vector g2_prime = g_prime.tail(2);
double f_prime = f + dv.transpose() * G.selfadjointView<Eigen::Upper>() * dv - 2.0 * dv.transpose() * g; double f_prime = f + dv.transpose() * G.selfadjointView<Eigen::Upper>() * dv - 2.0 * dv.transpose() * g;
HessianFactor expNewFactor(ordering[x1], ordering[l1], G11, G12, g1_prime, G22, g2_prime, f_prime); HessianFactor expNewFactor(x1, l1, G11, G12, g1_prime, G22, g2_prime, f_prime);
EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues, ordering), tol)); EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues), tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -251,14 +234,10 @@ TEST( testLinearContainerFactor, creation ) {
l5 = 15, l6 = 16, l5 = 15, l6 = 16,
l7 = 17, l8 = 18; l7 = 17, l8 = 18;
// creating an ordering to decode the linearized factor
Ordering ordering;
ordering += l1,l2,l3,l4,l5,l6,l7,l8;
// create a linear factor // create a linear factor
SharedDiagonal model = noiseModel::Unit::Create(2); SharedDiagonal model = noiseModel::Unit::Create(2);
JacobianFactor::shared_ptr linear_factor(new JacobianFactor( JacobianFactor::shared_ptr linear_factor(new JacobianFactor(
ordering[l3], eye(2,2), ordering[l5], 2.0 * eye(2,2), zero(2), model)); l3, eye(2,2), l5, 2.0 * eye(2,2), zero(2), model));
// create a set of values - build with full set of values // create a set of values - build with full set of values
gtsam::Values full_values, exp_values; gtsam::Values full_values, exp_values;
@ -267,7 +246,7 @@ TEST( testLinearContainerFactor, creation ) {
exp_values = full_values; exp_values = full_values;
full_values.insert(l1, Point2(3.0, 7.0)); full_values.insert(l1, Point2(3.0, 7.0));
LinearContainerFactor actual(linear_factor, ordering, full_values); LinearContainerFactor actual(linear_factor, full_values);
// Verify the keys // Verify the keys
std::vector<gtsam::Key> expKeys; std::vector<gtsam::Key> expKeys;
@ -284,9 +263,6 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
// Create a Between Factor from a Point3. This is actually a linear factor. // Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1); gtsam::Key key1(1);
gtsam::Key key2(2); gtsam::Key key2(2);
gtsam::Ordering ordering;
ordering.push_back(key1);
ordering.push_back(key2);
gtsam::Values linpoint1; gtsam::Values linpoint1;
linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4)); linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4));
linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0)); linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0));
@ -296,8 +272,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model); gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model);
// Create a jacobian container factor at linpoint 1 // Create a jacobian container factor at linpoint 1
gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1, ordering))); gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1)));
gtsam::LinearContainerFactor jacobianContainer(jacobian, ordering, linpoint1); gtsam::LinearContainerFactor jacobianContainer(jacobian, linpoint1);
// Create a second linearization point // Create a second linearization point
gtsam::Values linpoint2; gtsam::Values linpoint2;
@ -310,8 +286,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize )
EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
// Re-linearize around the new point and check the factors // Re-linearize around the new point and check the factors
gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2, ordering); gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2);
gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2, ordering); gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2);
CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
} }
@ -321,9 +297,6 @@ TEST( testLinearContainerFactor, hessian_relinearize )
// Create a Between Factor from a Point3. This is actually a linear factor. // Create a Between Factor from a Point3. This is actually a linear factor.
gtsam::Key key1(1); gtsam::Key key1(1);
gtsam::Key key2(2); gtsam::Key key2(2);
gtsam::Ordering ordering;
ordering.push_back(key1);
ordering.push_back(key2);
gtsam::Values linpoint1; gtsam::Values linpoint1;
linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4)); linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4));
linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0)); linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0));
@ -333,8 +306,8 @@ TEST( testLinearContainerFactor, hessian_relinearize )
gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model); gtsam::BetweenFactor<gtsam::Point3> betweenFactor(key1, key2, measured, model);
// Create a hessian container factor at linpoint 1 // Create a hessian container factor at linpoint 1
gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1, ordering))); gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1)));
gtsam::LinearContainerFactor hessianContainer(hessian, ordering, linpoint1); gtsam::LinearContainerFactor hessianContainer(hessian, linpoint1);
// Create a second linearization point // Create a second linearization point
gtsam::Values linpoint2; gtsam::Values linpoint2;
@ -347,8 +320,8 @@ TEST( testLinearContainerFactor, hessian_relinearize )
EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
// Re-linearize around the new point and check the factors // Re-linearize around the new point and check the factors
gtsam::GaussianFactor::shared_ptr expected_factor = gtsam::HessianFactor::shared_ptr(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint2, ordering))); gtsam::GaussianFactor::shared_ptr expected_factor = gtsam::HessianFactor::shared_ptr(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint2)));
gtsam::GaussianFactor::shared_ptr actual_factor = hessianContainer.linearize(linpoint2, ordering); gtsam::GaussianFactor::shared_ptr actual_factor = hessianContainer.linearize(linpoint2);
CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor));
} }