diff --git a/.cproject b/.cproject index 2dcea124a..bf7974660 100644 --- a/.cproject +++ b/.cproject @@ -300,7 +300,6 @@ make - install true true @@ -308,7 +307,6 @@ make - check true true @@ -324,7 +322,6 @@ make - testSimpleCamera.run true true @@ -340,6 +337,7 @@ make + testVSLAMFactor.run true true @@ -347,7 +345,6 @@ make - testCalibratedCamera.run true true @@ -355,6 +352,7 @@ make + testGaussianConditional.run true true @@ -362,7 +360,6 @@ make - testPose2.run true true @@ -370,7 +367,6 @@ make - testRot3.run true true @@ -378,6 +374,7 @@ make + testNonlinearOptimizer.run true true @@ -385,7 +382,6 @@ make - testGaussianFactor.run true true @@ -393,7 +389,6 @@ make - testGaussianFactorGraph.run true true @@ -401,6 +396,7 @@ make + testNonlinearFactorGraph.run true true @@ -408,7 +404,6 @@ make - testPose3.run true true @@ -416,6 +411,7 @@ make + testVectorConfig.run true true @@ -423,6 +419,7 @@ make + testPoint2.run true true @@ -430,7 +427,6 @@ make - testNonlinearFactor.run true true @@ -438,7 +434,6 @@ make - timeGaussianFactor.run true true @@ -446,7 +441,6 @@ make - timeGaussianFactorGraph.run true true @@ -454,7 +448,6 @@ make - testGaussianBayesNet.run true true @@ -462,6 +455,7 @@ make + testBayesTree.run true false @@ -469,7 +463,6 @@ make - testSymbolicBayesNet.run true false @@ -477,6 +470,7 @@ make + testSymbolicFactorGraph.run true false @@ -484,7 +478,6 @@ make - testVector.run true true @@ -492,7 +485,6 @@ make - testMatrix.run true true @@ -500,6 +492,7 @@ make + testVSLAMGraph.run true true @@ -507,6 +500,7 @@ make + testNonlinearEquality.run true true @@ -514,7 +508,6 @@ make - testSQP.run true true @@ -522,6 +515,7 @@ make + testNonlinearConstraint.run true true @@ -529,7 +523,6 @@ make - testSQPOptimizer.run true true @@ -537,7 +530,6 @@ make - testVSLAMConfig.run true true @@ -545,7 +537,6 @@ make - testControlConfig.run true true @@ -553,7 +544,6 @@ make - testControlPoint.run true true @@ -561,6 +551,7 @@ make + testControlGraph.run true true @@ -568,7 +559,6 @@ make - testOrdering.run true true @@ -576,6 +566,7 @@ make + testPose2Constraint.run true true @@ -583,7 +574,6 @@ make - testRot2.run true true @@ -591,7 +581,6 @@ make - testGaussianBayesTree.run true true @@ -599,6 +588,7 @@ make + testPose3Factor.run true true @@ -606,6 +596,7 @@ make + testUrbanGraph.run true true @@ -613,6 +604,7 @@ make + testUrbanConfig.run true true @@ -620,6 +612,7 @@ make + testUrbanMeasurement.run true true @@ -627,7 +620,6 @@ make - testUrbanOdometry.run true true @@ -635,6 +627,7 @@ make + testIterative.run true true @@ -642,6 +635,7 @@ make + testGaussianISAM2.run true true @@ -649,7 +643,6 @@ make - testSubgraphPreconditioner.run true true @@ -657,14 +650,22 @@ make - testBayesNetPreconditioner.run true true true + +make + +testPose2Factor.run +true +true +true + make + install true true @@ -672,6 +673,7 @@ make + clean true true @@ -679,6 +681,7 @@ make + check true true diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 933d92e19..eb6966d14 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -189,8 +189,8 @@ testSimulated3D_LDADD = libgtsam.la check_PROGRAMS += testSimulated3D # Pose constraints -headers += Pose2Config.h Pose2Factor.h Pose2Prior.h -sources += Pose3Factor.cpp Pose2Graph.cpp +headers += Pose2Factor.h Pose2Prior.h +sources += Pose2Config.cpp Pose2Graph.cpp Pose3Factor.cpp check_PROGRAMS += testPose2Factor testPose2Graph testPose3Factor testPose2Factor_SOURCES = $(example) testPose2Factor.cpp testPose2Factor_LDADD = libgtsam.la diff --git a/cpp/Pose2Config.cpp b/cpp/Pose2Config.cpp new file mode 100644 index 000000000..2148ba6bf --- /dev/null +++ b/cpp/Pose2Config.cpp @@ -0,0 +1,67 @@ +/** + * @file VectorConfig.cpp + * @brief Pose2Graph Configuration + * @author Frank Dellaert + */ + +#include +#include "VectorConfig.h" +#include "Pose2Config.h" + +// trick from some reading group +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + const Pose2& Pose2Config::get(std::string key) const { + std::map::const_iterator it = values_.find(key); + if (it == values_.end()) throw std::invalid_argument("invalid key"); + return it->second; + } + + /* ************************************************************************* */ + void Pose2Config::insert(const std::string& name, const Pose2& val) { + values_.insert(make_pair(name, val)); + } + + /* ************************************************************************* */ + bool Pose2Config::equals(const Pose2Config& expected, double tol) const { + if (values_.size() != expected.values_.size()) return false; + + // iterate over all nodes + string j; + Pose2 pj; + FOREACH_PAIR(j, pj, values_) + if(!pj.equals(expected.get(j),tol)) + return false; + return true; + } + + /* ************************************************************************* */ + void Pose2Config::print(const std::string &s) const { + std::cout << "Pose2Config " << s << ", size " << values_.size() << "\n"; + std::string j; Pose2 pj; + FOREACH_PAIR(j, pj, values_) + pj.print(j + ": "); + } + + /* ************************************************************************* */ + Pose2Config Pose2Config::exmap(const VectorConfig& delta) const { + Pose2Config newConfig; + std::string j; Pose2 pj; + FOREACH_PAIR(j, pj, values_) { + if (delta.contains(j)) { + const Vector& dj = delta[j]; + //check_size(j,vj,dj); + newConfig.insert(j, pj.exmap(dj)); + } else + newConfig.insert(j, pj); + } + return newConfig; + } + + /* ************************************************************************* */ +} // namespace diff --git a/cpp/Pose2Config.h b/cpp/Pose2Config.h index 899196bf3..9368a7852 100644 --- a/cpp/Pose2Config.h +++ b/cpp/Pose2Config.h @@ -1,84 +1,57 @@ +/** + * @file VectorConfig.cpp + * @brief Pose2Graph Configuration + * @author Frank Dellaert + */ + #pragma once #include -#include -#include -#include -#include -#include #include #include "Pose2.h" #include "Testable.h" -#include "VectorConfig.h" -#include "Vector.h" - -// trick from some reading group -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) - namespace gtsam { - class Pose2Config: public Testable { + class VectorConfig; - private: - std::map values_; + class Pose2Config: public Testable { - public: - typedef std::map::const_iterator iterator; - typedef iterator const_iterator; + private: + std::map values_; - Pose2Config() {} - Pose2Config(const Pose2Config &config) : values_(config.values_) { } - virtual ~Pose2Config() { } + public: - Pose2 get(std::string key) const { - std::map::const_iterator it = values_.find(key); - if (it == values_.end()) - throw std::invalid_argument("invalid key"); - return it->second; - } - void insert(const std::string& name, const Pose2& val){ - values_.insert(make_pair(name, val)); - } + typedef std::map::const_iterator iterator; + typedef iterator const_iterator; - Pose2Config& operator=(Pose2Config& rhs) { - values_ = rhs.values_; - return (*this); - } + Pose2Config() {} - bool equals(const Pose2Config& expected, double tol) const { - std::cerr << "Pose2Config::equals not implemented!" << std::endl; - throw "Pose2Config::equals not implemented!"; - } + Pose2Config(const Pose2Config &config) :values_(config.values_) {} - void print(const std::string &s) const { - std::cout << "Pose2Config " << s << ", size " << values_.size() << "\n"; - std::string j; Pose2 v; - FOREACH_PAIR(j, v, values_) { - v.print(j + ": "); - } - } + virtual ~Pose2Config() {} - /** - * Add a delta config, needed for use in NonlinearOptimizer - */ - Pose2Config exmap(const VectorConfig& delta) const { - Pose2Config newConfig; - std::string j; Pose2 vj; - FOREACH_PAIR(j, vj, values_) { - if (delta.contains(j)) { - const Vector& dj = delta[j]; - //check_size(j,vj,dj); - newConfig.insert(j, vj.exmap(dj)); - } else { - newConfig.insert(j, vj); - } - } - return newConfig; - } + const Pose2& get(std::string key) const; - iterator begin() const { return values_.begin(); } - iterator end() const { return values_.end(); } + void insert(const std::string& name, const Pose2& val); - }; + inline Pose2Config& operator=(const Pose2Config& rhs) { + values_ = rhs.values_; + return (*this); + } + + bool equals(const Pose2Config& expected, double tol) const; + + void print(const std::string &s) const; + + /** + * Add a delta config, needed for use in NonlinearOptimizer + */ + Pose2Config exmap(const VectorConfig& delta) const; + + inline const_iterator begin() const {return values_.begin();} + inline const_iterator end() const {return values_.end();} + inline iterator begin() {return values_.begin();} + inline iterator end() {return values_.end();} +}; } // namespace diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index 0205f5c3a..fbc5aadbd 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -6,9 +6,12 @@ /*STL/C++*/ #include +#include +#include +using namespace boost::assign; #include -#include + #include "NonlinearOptimizer-inl.h" #include "NonlinearEquality.h" #include "Pose2Factor.h" @@ -16,9 +19,9 @@ using namespace std; using namespace gtsam; -using namespace boost; -TEST( Pose2Factor, constructor ) +/* ************************************************************************* */ +TEST( Pose2Factor, linearize ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); @@ -36,10 +39,14 @@ TEST( Pose2Factor, constructor ) config.insert("p1",p1); config.insert("p2",p2); - // Linearize - boost::shared_ptr actual = constraint.linearize(config); - - // expected + // expected linearization + // we need the minus signs below as "inverse_square_root" + // uses SVD and the sign is simply arbitrary (but still a square root!) + Matrix square_root_inverse_covariance = Matrix_(3,3, + -2.0, 0.0, 0.0, + 0.0, -2.0, 0.0, + 0.0, 0.0, -10.0 + ); Matrix expectedH1 = Matrix_(3,3, 0.0,-1.0,-2.1, 1.0, 0.0,-2.1, @@ -50,53 +57,57 @@ TEST( Pose2Factor, constructor ) 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ); - // we need the minus signs as inverse_square_root uses SVD and sign is simply arbitrary (still a ssquare root!) - Matrix square_root_inverse_covariance = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -10.0 - ); GaussianFactor expected( "p1", square_root_inverse_covariance*expectedH1, "p2", square_root_inverse_covariance*expectedH2, Vector_(3, 0.1, -0.1, 0.0), 1.0); + // Actual linearization + boost::shared_ptr actual = constraint.linearize(config); CHECK(assert_equal(expected,*actual)); } +/* ************************************************************************* */ bool poseCompare(const std::string& key, const gtsam::Pose2Config& feasible, const gtsam::Pose2Config& input) { return feasible.get(key).equals(input.get(key)); } +/* ************************************************************************* */ TEST(Pose2Factor, optimize) { + + // create a Pose graph with one equality constraint and one measurement Pose2Graph fg; - shared_ptr config = shared_ptr(new Pose2Config()); Pose2Config feasible; feasible.insert("p0", Pose2(0,0,0)); fg.push_back(Pose2Graph::sharedFactor( new NonlinearEquality("p0", feasible, Pose2().dim(), poseCompare))); - config->insert("p0", Pose2(0,0,0)); fg.push_back(Pose2Graph::sharedFactor( new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), Matrix_(3,3, 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5)))); - config->insert("p1", Pose2(0,0,0)); - Ordering ordering; - ordering.push_back("p0"); - ordering.push_back("p1"); - NonlinearOptimizer optimizer(fg, ordering, config); - optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15); - Pose2 actual0 = optimizer.config()->get("p0"); - Pose2 actual1 = optimizer.config()->get("p1"); - Pose2 expected0 = Pose2(0,0,0); - Pose2 expected1 = Pose2(1,2,M_PI_2); - CHECK(assert_equal(expected0, actual0)); - CHECK(assert_equal(expected1, actual1)); -} + // Create initial config + boost::shared_ptr initial = + boost::shared_ptr(new Pose2Config()); + initial->insert("p0", Pose2(0,0,0)); + initial->insert("p1", Pose2(0,0,0)); + + // Choose an ordering and optimize + Ordering ordering; + ordering += "p0","p1"; + NonlinearOptimizer optimizer(fg, ordering, initial); + optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15); + + // Check with expected config + Pose2Config expected; + expected.insert("p0", Pose2(0,0,0)); + expected.insert("p1", Pose2(1,2,M_PI_2)); + CHECK(assert_equal(expected, *optimizer.config())); + +} /* ************************************************************************* */ int main() {