From e337ab8b1d6dc5262d04ae87f4cc468a4060ac5c Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 10 Jul 2012 18:36:21 +0000 Subject: [PATCH] Merge branch 'master' into wrap_mods_local --- gtsam/slam/visualSLAM.cpp | 4 +- .../nonlinear/LinearContainerFactor.cpp | 148 ++++++++++++++++++ .../nonlinear/LinearContainerFactor.h | 113 +++++++++++++ .../tests/testLinearContainerFactor.cpp | 113 +++++++++++++ tests/testGaussianISAM2.cpp | 2 +- 5 files changed, 377 insertions(+), 3 deletions(-) create mode 100644 gtsam_unstable/nonlinear/LinearContainerFactor.cpp create mode 100644 gtsam_unstable/nonlinear/LinearContainerFactor.h create mode 100644 gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 5b04b92c6..2133c0ee8 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -31,7 +31,7 @@ namespace visualSLAM { if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); if (Z.cols() != J.size()) throw std::invalid_argument( "insertBackProjections: J and Z must have same number of entries"); - for(size_t k=0;k + +#include + +namespace gtsam { + +/* ************************************************************************* */ +void LinearContainerFactor::rekeyFactor(const Ordering& ordering) { + Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert + rekeyFactor(invOrdering); +} + +/* ************************************************************************* */ +void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) { + BOOST_FOREACH(Index& idx, factor_->keys()) { + Key fullKey = invOrdering.find(idx)->second; + idx = fullKey; + keys_.push_back(fullKey); + } +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor( + const JacobianFactor& factor, const Ordering& ordering) +: factor_(factor.clone()) { + rekeyFactor(ordering); +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor( + const HessianFactor& factor, const Ordering& ordering) +: factor_(factor.clone()) { + rekeyFactor(ordering); +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor( + const GaussianFactor::shared_ptr& factor, const Ordering& ordering) +: factor_(factor->clone()) { + rekeyFactor(ordering); +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor( + const GaussianFactor::shared_ptr& factor) +: factor_(factor->clone()) +{ +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor, + const Ordering::InvertedMap& inverted_ordering) +: factor_(factor.clone()) { + rekeyFactor(inverted_ordering); +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor, + const Ordering::InvertedMap& inverted_ordering) +: factor_(factor.clone()) { + rekeyFactor(inverted_ordering); +} + +/* ************************************************************************* */ +LinearContainerFactor::LinearContainerFactor( + const GaussianFactor::shared_ptr& factor, + const Ordering::InvertedMap& ordering) +: factor_(factor->clone()) { + rekeyFactor(ordering); +} + +/* ************************************************************************* */ +void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { + Base::print(s+"LinearContainerFactor", keyFormatter); + if (factor_) + factor_->print(" Stored Factor", keyFormatter); +} + +/* ************************************************************************* */ +bool LinearContainerFactor::equals(const NonlinearFactor& f, double tol) const { + const LinearContainerFactor* jcf = dynamic_cast(&f); + return jcf && factor_->equals(*jcf->factor_, tol) && NonlinearFactor::equals(f); +} + +/* ************************************************************************* */ +double LinearContainerFactor::error(const Values& c) const { + // VectorValues vecvalues; + // // FIXME: add values correctly here + // return factor_.error(vecvalues); + return 0; // FIXME: placeholder +} + +/* ************************************************************************* */ +size_t LinearContainerFactor::dim() const { + if (isJacobian()) + return toJacobian()->get_model()->dim(); + else + return 1; // Hessians don't have true dimension +} + +/* ************************************************************************* */ +boost::shared_ptr +LinearContainerFactor::linearize(const Values& c, const Ordering& ordering) const { + // clone factor + boost::shared_ptr result = factor_->clone(); + + // rekey + BOOST_FOREACH(Index& key, result->keys()) + key = ordering[key]; + + return result; +} + +/* ************************************************************************* */ +bool LinearContainerFactor::isJacobian() const { + return boost::shared_dynamic_cast(factor_); +} + +/* ************************************************************************* */ +JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() const { + return boost::shared_dynamic_cast(factor_); +} + +/* ************************************************************************* */ +HessianFactor::shared_ptr LinearContainerFactor::toHessian() const { + return boost::shared_dynamic_cast(factor_); +} + +/* ************************************************************************* */ +GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& ordering) const { + GaussianFactor::shared_ptr result = factor_->negate(); + BOOST_FOREACH(Key& key, result->keys()) + key = ordering[key]; + return result; +} + +/* ************************************************************************* */ +} // \namespace gtsam + + + diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h new file mode 100644 index 000000000..23705f96e --- /dev/null +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h @@ -0,0 +1,113 @@ +/** + * @file LinearContainerFactor.h + * + * @brief Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph + * + * @date Jul 6, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Dummy version of a generic linear factor to be injected into a nonlinear factor graph + */ +class LinearContainerFactor : public NonlinearFactor { +protected: + + GaussianFactor::shared_ptr factor_; + +public: + + /** Primary constructor: store a linear factor and decode the ordering */ + LinearContainerFactor(const JacobianFactor& factor, const Ordering& ordering); + + /** Primary constructor: store a linear factor and decode the ordering */ + LinearContainerFactor(const HessianFactor& factor, const Ordering& ordering); + + /** Constructor from shared_ptr */ + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering); + + /** Constructor from re-keyed factor: all indices assumed replaced with Key */ + LinearContainerFactor(const GaussianFactor::shared_ptr& factor); + + /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/ + LinearContainerFactor(const JacobianFactor& factor, + const Ordering::InvertedMap& inverted_ordering); + + /** Alternate constructor: store a linear factor and decode keys with inverted ordering*/ + LinearContainerFactor(const HessianFactor& factor, + const Ordering::InvertedMap& inverted_ordering); + + /** Constructor from shared_ptr with inverted ordering*/ + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, + const Ordering::InvertedMap& ordering); + + // Access + + const GaussianFactor::shared_ptr& factor() const { return factor_; } + + // Testable + + /** print */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + + /** Check if two factors are equal */ + bool equals(const NonlinearFactor& f, double tol = 1e-9) const; + + // NonlinearFactor + + /** + * Calculate the error of the factor: uses the underlying linear factor to compute ordering + */ + double error(const Values& c) const; + + /** get the dimension of the factor: rows of linear factor */ + size_t dim() const; + + /** linearize to a GaussianFactor: values has no effect, just clones/rekeys underlying factor */ + boost::shared_ptr + linearize(const Values& c, const Ordering& ordering) const; + + /** + * Creates an anti-factor directly and performs rekeying due to ordering + */ + GaussianFactor::shared_ptr negate(const Ordering& ordering) const; + + /** + * Creates a shared_ptr clone of the factor - needs to be specialized to allow + * for subclasses + * + * Clones the underlying linear factor + */ + NonlinearFactor::shared_ptr clone() const { + return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_)); + } + + // casting syntactic sugar + + /** + * Simple check whether this is a Jacobian or Hessian factor + */ + bool isJacobian() const; + + /** Casts to JacobianFactor */ + JacobianFactor::shared_ptr toJacobian() const; + + /** Casts to HessianFactor */ + HessianFactor::shared_ptr toHessian() const; + +protected: + void rekeyFactor(const Ordering& ordering); + void rekeyFactor(const Ordering::InvertedMap& invOrdering); + +}; // \class LinearContainerFactor + +} // \namespace gtsam + + diff --git a/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp new file mode 100644 index 000000000..e85b880e8 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testLinearContainerFactor.cpp @@ -0,0 +1,113 @@ +/** + * @file testLinearContainerFactor.cpp + * + * @date Jul 6, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +#include + +using namespace gtsam; + +const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)); +const double tol = 1e-5; + +gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2; + +Point2 landmark1(5.0, 1.5), landmark2(7.0, 1.5); +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; + + JacobianFactor expLinFactor1( + initOrdering[l1], + Matrix_(2,2, + 2.74222, -0.0067457, + 0.0, 2.63624), + initOrdering[l2], + Matrix_(2,2, + -0.0455167, -0.0443573, + -0.0222154, -0.102489), + Vector_(2, 0.0277052, + -0.0533393), + diag_model2); + + LinearContainerFactor actFactor1(expLinFactor1, initOrdering); + Values values; + values.insert(l1, landmark1); + values.insert(l2, landmark2); + values.insert(x1, poseA1); + values.insert(x2, poseA2); + + // Check reconstruction from same ordering + GaussianFactor::shared_ptr actLinearizationA = actFactor1.linearize(values, initOrdering); + EXPECT(assert_equal(*expLinFactor1.clone(), *actLinearizationA, tol)); + + // Check reconstruction from new ordering + Ordering newOrdering; newOrdering += x1, l1, x2, l2; + GaussianFactor::shared_ptr actLinearizationB = actFactor1.linearize(values, newOrdering); + + JacobianFactor expLinFactor2( + newOrdering[l1], + Matrix_(2,2, + 2.74222, -0.0067457, + 0.0, 2.63624), + newOrdering[l2], + Matrix_(2,2, + -0.0455167, -0.0443573, + -0.0222154, -0.102489), + Vector_(2, 0.0277052, + -0.0533393), + diag_model2); + + EXPECT(assert_equal(*expLinFactor2.clone(), *actLinearizationB, tol)); +} + +/* ************************************************************************* */ +TEST( testLinearContainerFactor, generic_hessian_factor ) { + Matrix G11 = Matrix_(1,1, 1.0); + Matrix G12 = Matrix_(1,2, 2.0, 4.0); + Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0); + + Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); + Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0); + + Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); + + Vector g1 = Vector_(1, -7.0); + Vector g2 = Vector_(2, -8.0, -9.0); + Vector g3 = Vector_(3, 1.0, 2.0, 3.0); + + double f = 10.0; + + Ordering initOrdering; initOrdering += x1, x2, l1, l2; + HessianFactor initFactor(initOrdering[x1], initOrdering[x2], initOrdering[l1], + G11, G12, G13, g1, G22, G23, g2, G33, g3, f); + + Values values; + values.insert(l1, landmark1); + values.insert(l2, landmark2); + values.insert(x1, poseA1); + values.insert(x2, poseA2); + + LinearContainerFactor actFactor(initFactor, initOrdering); + GaussianFactor::shared_ptr actLinearization1 = actFactor.linearize(values, initOrdering); + 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)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f63d2add5..9fdd6512f 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -431,7 +431,7 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons bool totalGradOk = assert_equal(expectedGradient, actualGradient); EXPECT(totalGradOk); - return nodeGradientsOk && expectedGradOk && totalGradOk; + return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual; } /* ************************************************************************* */