From ec3e89c8887a68e95e8f9e6f03b26952013c49c9 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Aug 2013 18:33:51 +0000 Subject: [PATCH] LinearContainerFactor works --- gtsam/nonlinear/CMakeLists.txt | 2 +- gtsam/nonlinear/LinearContainerFactor.cpp | 22 ++-- gtsam/nonlinear/LinearContainerFactor.h | 4 +- .../tests/testLinearContainerFactor.cpp | 105 +++++++----------- 4 files changed, 51 insertions(+), 82 deletions(-) diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt index 47f14c8d9..884346e2d 100644 --- a/gtsam/nonlinear/CMakeLists.txt +++ b/gtsam/nonlinear/CMakeLists.txt @@ -16,7 +16,7 @@ set(nonlinear_local_libs set(nonlinear_excluded_files # "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test #"" # 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" ) diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 176373ecb..9a38e0593 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -9,11 +9,10 @@ #include #include #include +#include #include -#if 0 - namespace gtsam { /* ************************************************************************* */ @@ -31,30 +30,27 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint) -: factor_(factor), linearizationPoint_(linearizationPoint) { - // Extract keys stashed in linear factor - BOOST_FOREACH(const Index& idx, factor_->keys()) - keys_.push_back(idx); +: NonlinearFactor(factor->keys()), factor_(factor), linearizationPoint_(linearizationPoint) { } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( const JacobianFactor& factor, const Values& linearizationPoint) -: factor_(factor.clone()) { +: NonlinearFactor(factor.keys()), factor_(factor.clone()) { initializeLinearizationPoint(linearizationPoint); } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( const HessianFactor& factor, const Values& linearizationPoint) -: factor_(factor.clone()) { +: NonlinearFactor(factor.keys()), factor_(factor.clone()) { initializeLinearizationPoint(linearizationPoint); } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor( const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint) -: factor_(factor->clone()) { +: NonlinearFactor(factor->keys()), factor_(factor->clone()) { initializeLinearizationPoint(linearizationPoint); } @@ -139,12 +135,12 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con /* ************************************************************************* */ bool LinearContainerFactor::isJacobian() const { - return boost::dynamic_pointer_cast(factor_); + return boost::dynamic_pointer_cast(factor_).get(); } /* ************************************************************************* */ bool LinearContainerFactor::isHessian() const { - return boost::dynamic_pointer_cast(factor_); + return boost::dynamic_pointer_cast(factor_).get(); } /* ************************************************************************* */ @@ -160,12 +156,13 @@ HessianFactor::shared_ptr LinearContainerFactor::toHessian() const { /* ************************************************************************* */ GaussianFactor::shared_ptr LinearContainerFactor::negateToGaussian() const { GaussianFactor::shared_ptr result = factor_->negate(); + return result; } /* ************************************************************************* */ NonlinearFactor::shared_ptr LinearContainerFactor::negateToNonlinear() const { GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place - return boost::make_shared(antifactor, linearizationPoint_); + return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor, linearizationPoint_)); } /* ************************************************************************* */ @@ -183,4 +180,3 @@ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( /* ************************************************************************* */ } // \namespace gtsam -#endif diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index f78edec1b..379821cfe 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -38,10 +38,10 @@ protected: 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()); - /** 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()); /** Constructor from shared_ptr */ diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 6c4617171..549fccd3d 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include #include #include #include @@ -28,8 +30,6 @@ Pose2 poseA1(0.0, 0.0, 0.0), poseA2(2.0, 0.0, 0.0); /* ************************************************************************* */ TEST( testLinearContainerFactor, generic_jacobian_factor ) { - Ordering initOrdering; initOrdering += x1, x2, l1, l2; - Matrix A1 = Matrix_(2,2, 2.74222, -0.0067457, 0.0, 2.63624); @@ -39,9 +39,9 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { Vector b = Vector_(2, 0.0277052, -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(actFactor.isJacobian()); EXPECT(!actFactor.isHessian()); @@ -56,22 +56,14 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { values.insert(x1, poseA1); values.insert(x2, poseA2); - // Check reconstruction from same ordering - GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values, initOrdering); + // Check reconstruction + GaussianFactor::shared_ptr actLinearizationA = actFactor.linearize(values); 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 ) { - Ordering ordering; ordering += x1, x2, l1, l2; - Matrix A1 = Matrix_(2,2, 2.74222, -0.0067457, 0.0, 2.63624); @@ -81,7 +73,7 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { Vector b = Vector_(2, 0.0277052, -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.insert(l1, landmark1); @@ -89,8 +81,8 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { values.insert(x1, poseA1); values.insert(x2, poseA2); - LinearContainerFactor actFactor(expLinFactor, ordering, values); - LinearContainerFactor actFactorNolin(expLinFactor, ordering); + LinearContainerFactor actFactor(expLinFactor, values); + LinearContainerFactor actFactorNolin(expLinFactor); EXPECT(assert_equal(actFactor, actFactor, 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_l2 = Vector_(2, 3.0, 4.0); - VectorValues delta = values.zeroVectors(ordering); - delta.at(ordering[l1]) = delta_l1; - delta.at(ordering[l2]) = delta_l2; - Values noisyValues = values.retract(delta, ordering); + VectorValues delta = values.zeroVectors(); + delta.at(l1) = delta_l1; + delta.at(l2) = delta_l2; + Values noisyValues = values.retract(delta); double expError = expLinFactor.error(delta); 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 - Ordering newOrdering; newOrdering += x1, l1, x2, l2; - GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues, newOrdering); + GaussianFactor::shared_ptr actLinearizationB = actFactor.linearize(noisyValues); 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)); } @@ -145,8 +136,7 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { double f = 10.0; - Ordering initOrdering; initOrdering += x1, x2, l1, l2; - HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1], + HessianFactor initFactor(x1, x2, l1, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); Values values; @@ -155,18 +145,12 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { values.insert(x1, poseA1); values.insert(x2, poseA2); - LinearContainerFactor actFactor(initFactor, initOrdering); + LinearContainerFactor actFactor(initFactor); EXPECT(!actFactor.isJacobian()); 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)); - - 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); G << G11, G12, Matrix::Zero(2,3), G22; - Ordering ordering; ordering += x1, x2, l1; - HessianFactor initFactor(ordering[x1], ordering[l1], G11, G12, g1, G22, g2, f); + HessianFactor initFactor(x1, l1, G11, G12, g1, G22, g2, f); Values linearizationPoint, expLinPoints; linearizationPoint.insert(l1, landmark1); @@ -205,7 +188,7 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { expLinPoints = linearizationPoint; linearizationPoint.insert(x2, poseA2); - LinearContainerFactor actFactor(initFactor, ordering, linearizationPoint); + LinearContainerFactor actFactor(initFactor, linearizationPoint); EXPECT(!actFactor.isJacobian()); EXPECT(actFactor.isHessian()); @@ -219,16 +202,16 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { Vector delta_x2 = Vector_(3, 6.0, 7.0, 0.3); // Check error calculation - VectorValues delta = linearizationPoint.zeroVectors(ordering); - delta.at(ordering[l1]) = delta_l1; - delta.at(ordering[x1]) = delta_x1; - delta.at(ordering[x2]) = delta_x2; + VectorValues delta = linearizationPoint.zeroVectors(); + delta.at(l1) = delta_l1; + delta.at(x1) = delta_x1; + delta.at(x2) = delta_x2; 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); 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 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 g2_prime = g_prime.tail(2); double f_prime = f + dv.transpose() * G.selfadjointView() * dv - 2.0 * dv.transpose() * g; - HessianFactor expNewFactor(ordering[x1], ordering[l1], G11, G12, g1_prime, G22, g2_prime, f_prime); - EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues, ordering), tol)); + HessianFactor expNewFactor(x1, l1, G11, G12, g1_prime, G22, g2_prime, f_prime); + EXPECT(assert_equal(*expNewFactor.clone(), *actFactor.linearize(noisyValues), tol)); } /* ************************************************************************* */ @@ -251,14 +234,10 @@ TEST( testLinearContainerFactor, creation ) { l5 = 15, l6 = 16, 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 SharedDiagonal model = noiseModel::Unit::Create(2); 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 gtsam::Values full_values, exp_values; @@ -267,7 +246,7 @@ TEST( testLinearContainerFactor, creation ) { exp_values = full_values; 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 std::vector expKeys; @@ -284,9 +263,6 @@ TEST( testLinearContainerFactor, jacobian_relinearize ) // Create a Between Factor from a Point3. This is actually a linear factor. gtsam::Key key1(1); gtsam::Key key2(2); - gtsam::Ordering ordering; - ordering.push_back(key1); - ordering.push_back(key2); gtsam::Values linpoint1; linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4)); linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0)); @@ -296,8 +272,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize ) gtsam::BetweenFactor betweenFactor(key1, key2, measured, model); // Create a jacobian container factor at linpoint 1 - gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1, ordering))); - gtsam::LinearContainerFactor jacobianContainer(jacobian, ordering, linpoint1); + gtsam::JacobianFactor::shared_ptr jacobian(new gtsam::JacobianFactor(*betweenFactor.linearize(linpoint1))); + gtsam::LinearContainerFactor jacobianContainer(jacobian, linpoint1); // Create a second linearization point gtsam::Values linpoint2; @@ -310,8 +286,8 @@ TEST( testLinearContainerFactor, jacobian_relinearize ) EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // Re-linearize around the new point and check the factors - gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2, ordering); - gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2, ordering); + gtsam::GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint2); + gtsam::GaussianFactor::shared_ptr actual_factor = jacobianContainer.linearize(linpoint2); 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. gtsam::Key key1(1); gtsam::Key key2(2); - gtsam::Ordering ordering; - ordering.push_back(key1); - ordering.push_back(key2); gtsam::Values linpoint1; linpoint1.insert(key1, gtsam::Point3(-22.4, +8.5, +2.4)); linpoint1.insert(key2, gtsam::Point3(-21.0, +5.0, +21.0)); @@ -333,8 +306,8 @@ TEST( testLinearContainerFactor, hessian_relinearize ) gtsam::BetweenFactor betweenFactor(key1, key2, measured, model); // Create a hessian container factor at linpoint 1 - gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1, ordering))); - gtsam::LinearContainerFactor hessianContainer(hessian, ordering, linpoint1); + gtsam::HessianFactor::shared_ptr hessian(new gtsam::HessianFactor(*betweenFactor.linearize(linpoint1))); + gtsam::LinearContainerFactor hessianContainer(hessian, linpoint1); // Create a second linearization point gtsam::Values linpoint2; @@ -347,8 +320,8 @@ TEST( testLinearContainerFactor, hessian_relinearize ) EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); // 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 actual_factor = hessianContainer.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); CHECK(gtsam::assert_equal(*expected_factor, *actual_factor)); }