From b87aa58c1fb21b36db3b61f1d3472634a0405a72 Mon Sep 17 00:00:00 2001 From: Viorela Ila Date: Wed, 9 Dec 2009 17:29:43 +0000 Subject: [PATCH] add Pose2Constraint class --- cpp/Makefile.am | 5 +- cpp/Pose2Constraint.h | 57 ++++++++++++++++++++++ cpp/gtsam.h | 13 +++++ cpp/testNonlinearFactor.cpp | 94 ------------------------------------- cpp/testPose2Constraint.cpp | 67 ++++++++++++++++++++++++++ 5 files changed, 141 insertions(+), 95 deletions(-) create mode 100644 cpp/Pose2Constraint.h create mode 100644 cpp/testPose2Constraint.cpp diff --git a/cpp/Makefile.am b/cpp/Makefile.am index b40233b6d..78d5eff29 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -121,9 +121,10 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h headers += SQPOptimizer.h SQPOptimizer-inl.h headers += NonlinearConstraint.h NonlinearConstraint-inl.h +headers += Pose2Constraint.h sources += NonlinearFactor.cpp sources += NonlinearEquality.h -check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer +check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Constraint testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_LDADD = libgtsam.la testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp @@ -138,6 +139,8 @@ testNonlinearEquality_SOURCES = testNonlinearEquality.cpp testNonlinearEquality_LDADD = libgtsam.la testSQP_SOURCES = $(example) testSQP.cpp testSQP_LDADD = libgtsam.la +testPose2Constraint_SOURCES = $(example) testPose2Constraint.cpp +testPose2Constraint_LDADD = libgtsam.la # geometry sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp diff --git a/cpp/Pose2Constraint.h b/cpp/Pose2Constraint.h new file mode 100644 index 000000000..414ea1b92 --- /dev/null +++ b/cpp/Pose2Constraint.h @@ -0,0 +1,57 @@ +/** + * @file Pose2Constraint.H + * @authors Frank Dellaert, Viorela Ila + **/ + +#include +#include "GaussianFactor.h" +#include "VectorConfig.h" +#include "Pose2.h" +#include "Matrix.h" + +namespace gtsam { + +class Pose2Config : public std::map { +public: + Pose2 get(std::string key) const { + std::map::const_iterator it = find(key); + if (it==end()) throw std::invalid_argument("invalid key"); + return it->second; + } +}; + +class Pose2Constraint : public Factor { +private: + std::string key1_, key2_; /** The keys of the two poses, order matters */ + Pose2 measured_; + Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ + +public: + Pose2Constraint(const std::string& key1, const std::string& key2, + const Pose2& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) { + square_root_inverse_covariance_ = inverse_square_root(measurement_covariance); + } + + /** implement functions needed for Testable */ + void print(const std::string& name) const {} + bool equals(const Factor& expected, double tol) const {return false;} + + /** implement functions needed to derive from Factor */ + double error(const Pose2Config& c) const {return 0;} + std::list keys() const { std::list l; return l; } + std::size_t size() const { return 2;} + + /** linearize */ + boost::shared_ptr linearize(const Pose2Config& config) const { + Pose2 p1 = config.get(key1_), p2 = config.get(key2_); + Vector b = (measured_ - between(p1,p2)).vector(); + Matrix H1 = Dbetween1(p1,p2); + Matrix H2 = Dbetween2(p1,p2); + boost::shared_ptr linearized(new GaussianFactor( + key1_, square_root_inverse_covariance_ * H1, + key2_, square_root_inverse_covariance_ * H2, + b, 1.0)); + return linearized; + } +}; +} /// namespace gtsam diff --git a/cpp/gtsam.h b/cpp/gtsam.h index ce188a592..0531a937a 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -158,3 +158,16 @@ class Simulated2DMeasurement { void print(string s) const; }; +class Pose2Config{ + Pose2 get(string key) const; +}; + +class Pose2Constraint { + Pose2Constraint(string key1, string key2, + const Pose2& measured, const Matrix& measurement_covariance); + void print(string name) const; + bool equals(const Pose2Constraint& expected, double tol) const; + double error(const Pose2Config& c) const; + size_t size() const; + GaussianFactor* linearize(const Pose2Config& config) const; +}; diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index 8b1b4c469..b021d4c49 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -172,100 +172,6 @@ TEST( NonlinearFactor, size ) CHECK(factor3->size() == 2); } -/* ************************************************************************* */ - -#include -#include "Pose2.h" -class Pose2Config : public map { -public: - Pose2 get(string key) const { - map::const_iterator it = find(key); - if (it==end()) throw std::invalid_argument("invalid key"); - return it->second; - } -}; - -class Pose2Constraint : public Factor { -private: - std::string key1_, key2_; /** The keys of the two poses, order matters */ - Pose2 measured_; - Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ - -public: - Pose2Constraint(const string& key1, const string& key2, - const Pose2& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) { - square_root_inverse_covariance_ = inverse_square_root(measurement_covariance); - } - - /** implement functions needed for Testable */ - void print(const std::string& name) const {} - bool equals(const Factor& expected, double tol) const {return false;} - - /** implement functions needed to derive from Factor */ - double error(const Pose2Config& c) const {return 0;} - std::list keys() const { list l; return l; } - std::size_t size() const { return 2;} - - /** linearize */ - boost::shared_ptr linearize(const Pose2Config& config) const { - Pose2 p1 = config.get(key1_), p2 = config.get(key2_); - Vector b = (measured_ - between(p1,p2)).vector(); - Matrix H1 = Dbetween1(p1,p2); - Matrix H2 = Dbetween2(p1,p2); - boost::shared_ptr linearized(new GaussianFactor( - key1_, square_root_inverse_covariance_ * H1, - key2_, square_root_inverse_covariance_ * H2, - b, 1.0)); - return linearized; - } -}; - -TEST( Pose2Constraint, constructor ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); - Matrix measurement_covariance = Matrix_(3,3, - 0.25, 0.0, 0.0, - 0.0, 0.25, 0.0, - 0.0, 0.0, 0.01 - ); - Pose2Constraint constraint("p1","p2",measured, measurement_covariance); - - // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) - Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) - Pose2Config config; - config.insert(make_pair("p1",p1)); - config.insert(make_pair("p2",p2)); - - // Linearize - boost::shared_ptr actual = constraint.linearize(config); - - // expected - Matrix expectedH1 = Matrix_(3,3, - 0.0,-1.0,2.1, - 1.0,0.0,-2.1, - 0.0,0.0,-1.0 - ); - Matrix expectedH2 = Matrix_(3,3, - 0.0,1.0,0.0, - -1.0,0.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); - - CHECK(assert_equal(expected,*actual)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/cpp/testPose2Constraint.cpp b/cpp/testPose2Constraint.cpp new file mode 100644 index 000000000..010a3fb53 --- /dev/null +++ b/cpp/testPose2Constraint.cpp @@ -0,0 +1,67 @@ +/** + * @file testPose2Constraint.cpp + * @brief Unit tests for Pose2Constraint Class + * @authors Frank Dellaert, Viorela Ila + **/ + +/*STL/C++*/ +#include + +#include +#include "Pose2Constraint.h" + +using namespace std; +using namespace gtsam; + + +TEST( Pose2Constraint, constructor ) +{ + // create a factor between unknown poses p1 and p2 + Pose2 measured(2,2,M_PI_2); + Matrix measurement_covariance = Matrix_(3,3, + 0.25, 0.0, 0.0, + 0.0, 0.25, 0.0, + 0.0, 0.0, 0.01 + ); + Pose2Constraint constraint("p1","p2",measured, measurement_covariance); + + // Choose a linearization point + Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) + Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) + Pose2Config config; + config.insert(make_pair("p1",p1)); + config.insert(make_pair("p2",p2)); + + // Linearize + boost::shared_ptr actual = constraint.linearize(config); + + // expected + Matrix expectedH1 = Matrix_(3,3, + 0.0,-1.0,2.1, + 1.0,0.0,-2.1, + 0.0,0.0,-1.0 + ); + Matrix expectedH2 = Matrix_(3,3, + 0.0,1.0,0.0, + -1.0,0.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); + + CHECK(assert_equal(expected,*actual)); +} +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */