From 92b60a854390f8b9b6aa5ebfc77a38fda8e0064c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 21 Dec 2009 16:43:23 +0000 Subject: [PATCH] Made 'between' derivatives in the tangent space of the solution instead of tangent space of identity, this makes Pose2 an "origin-free" manifold. --- cpp/Pose2.cpp | 32 +++++--------- cpp/Pose2.h | 49 +++++++++------------ cpp/Pose2Config.h | 83 ++++++++++++++++++----------------- cpp/testPose2.cpp | 95 ++++++++++++++++++++++++++++++----------- cpp/testPose2Factor.cpp | 67 +++++++++++++++++++++-------- cpp/testPose2Graph.cpp | 16 +++---- 6 files changed, 204 insertions(+), 138 deletions(-) diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index 2a28f35e4..bfb26c3eb 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -19,20 +19,6 @@ namespace gtsam { return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); } - /* ************************************************************************* */ - Pose2 Pose2::exmap(const Vector& v) const { - return Pose2(r_.exmap(Vector_(1, v(2))), t_+Point2(v(0),v(1))); - } - - /* ************************************************************************* */ - Vector Pose2::log(const Pose2 &pose) const { - return Vector_(3, - pose.t().x() - t().x(), - pose.t().y() - t().y(), - pose.r().theta() - r().theta()); -// return between(*this, pose).vector(); - } - /* ************************************************************************* */ // Pose2 Pose2::rotate(double theta) const { // //return Pose2(t_, Rot2(theta)*r_); @@ -46,7 +32,7 @@ namespace gtsam { // TODO, have a combined function that returns both function value and derivative Matrix Dtransform_to1(const Pose2& pose, const Point2& point) { - Matrix dx_dt = pose.r().negtranspose(); + Matrix dx_dt = Matrix_(2,2, -1.0, 0.0, 0.0, -1.0); Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t()); return collect(2, &dx_dt, &dx_dr); } @@ -63,17 +49,21 @@ namespace gtsam { } Matrix Dbetween1(const Pose2& p1, const Pose2& p2) { - Matrix dbt_dp = Dtransform_to1(p1, p2.t()); - Matrix dbr_dp = Matrix_(1,3, 0.0, 0.0, -1.0); - return stack(2, &dbt_dp, &dbr_dp); + Matrix dt_dr = Dunrotate1(p1.r(), p2.t()-p1.t()); + Matrix dt_dt1 = -p2.r().invcompose(p1.r()).matrix(); + Matrix dt_dr1 = Dunrotate1(p2.r(), p2.t()-p1.t()); + return Matrix_(3,3, + dt_dt1(0,0), dt_dt1(0,1), dt_dr1(0,0), + dt_dt1(1,0), dt_dt1(1,1), dt_dr1(1,0), + 0.0, 0.0, -1.0); } Matrix Dbetween2(const Pose2& p1, const Pose2& p2) { Matrix db_dt2 = p1.r().transpose(); return Matrix_(3,3, - db_dt2.data()[0], db_dt2.data()[1], 0.0, - db_dt2.data()[2], db_dt2.data()[3], 0.0, - 0.0, 0.0, 1.0); + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0); } /* ************************************************************************* */ diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 0bfa68182..7e54b5c3c 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -16,6 +16,21 @@ namespace gtsam { + /** + * Return point coordinates in pose coordinate frame + */ + class Pose2; + Point2 transform_to(const Pose2& pose, const Point2& point); + Matrix Dtransform_to1(const Pose2& pose, const Point2& point); + Matrix Dtransform_to2(const Pose2& pose, const Point2& point); + + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose2 between(const Pose2& p1, const Pose2& p2); + Matrix Dbetween1(const Pose2& p1, const Pose2& p2); + Matrix Dbetween2(const Pose2& p1, const Pose2& p2); + /** * A 2D pose (Point2,Rot2) */ @@ -70,48 +85,26 @@ namespace gtsam { size_t dim() const { return 3; } /** exponential map */ - Pose2 exmap(const Vector& v) const; + Pose2 exmap(const Vector& v) const { return Pose2(v[0], v[1], v[2]) * (*this); } /** log map */ - Vector log(const Pose2 &pose) const; + Vector log(const Pose2 &pose) const { return between(*this, pose).vector(); } /** rotate pose by theta */ // Pose2 rotate(double theta) const; /** inverse transformation */ - Pose2 inverse() const { - return Pose2(r_, r_.unrotate(t_)); - } + Pose2 inverse() const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } /** compose this transformation onto another (pre-multiply this*p1) */ - Pose2 compose(const Pose2& p1) const { - return Pose2(p1.r_*r_, p1.r_*t_+p1.t_); - } + Pose2 compose(const Pose2& p1) const { return Pose2(p1.r_ * r_, p1.r_ * t_ + p1.t_); } /** same as compose (pre-multiply this*p1) */ - Pose2 operator*(const Pose2& p1) const { - return compose(p1); - } + Pose2 operator*(const Pose2& p1) const { return compose(p1); } /** Return point coordinates in pose coordinate frame, same as transform_to */ - Point2 operator*(const Point2& point) const { - return r_.unrotate(point-t_); - } + Point2 operator*(const Point2& point) const { return r_.unrotate(point-t_); } }; // Pose2 - /** - * Return point coordinates in pose coordinate frame - */ - Point2 transform_to(const Pose2& pose, const Point2& point); - Matrix Dtransform_to1(const Pose2& pose, const Point2& point); - Matrix Dtransform_to2(const Pose2& pose, const Point2& point); - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p1, const Pose2& p2); - Matrix Dbetween1(const Pose2& p1, const Pose2& p2); - Matrix Dbetween2(const Pose2& p1, const Pose2& p2); - } // namespace gtsam diff --git a/cpp/Pose2Config.h b/cpp/Pose2Config.h index d97467ada..899196bf3 100644 --- a/cpp/Pose2Config.h +++ b/cpp/Pose2Config.h @@ -18,44 +18,46 @@ namespace gtsam { -class Pose2Config: public Testable { + class Pose2Config: public Testable { -private: - std::map values_; + private: + std::map values_; -public: + public: + typedef std::map::const_iterator iterator; + typedef iterator const_iterator; - Pose2Config() {} - Pose2Config(const Pose2Config &config) : values_(config.values_) { } - virtual ~Pose2Config() { } + Pose2Config() {} + Pose2Config(const Pose2Config &config) : values_(config.values_) { } + virtual ~Pose2Config() { } - 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)); - } + 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)); + } - Pose2Config& operator=(Pose2Config& rhs) { - values_ = rhs.values_; - return (*this); - } + Pose2Config& operator=(Pose2Config& rhs) { + values_ = rhs.values_; + return (*this); + } - bool equals(const Pose2Config& expected, double tol) const { - std::cerr << "Pose2Config::equals not implemented!" << std::endl; - throw "Pose2Config::equals not implemented!"; - } + bool equals(const Pose2Config& expected, double tol) const { + std::cerr << "Pose2Config::equals not implemented!" << std::endl; + throw "Pose2Config::equals not implemented!"; + } - 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 + ": "); - } - } + 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 + ": "); + } + } /** * Add a delta config, needed for use in NonlinearOptimizer @@ -64,16 +66,19 @@ public: 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); - } + 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; } -}; + iterator begin() const { return values_.begin(); } + iterator end() const { return values_.end(); } + + }; } // namespace diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index 7a153499b..e5cfd0025 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -5,12 +5,33 @@ #include #include +#include #include "numericalDerivative.h" #include "Pose2.h" #include "Point2.h" #include "Rot2.h" using namespace gtsam; +using namespace std; + +Matrix toManifoldDerivative(Matrix vectorDerivative, Pose2 linearizationPoint) { + Matrix manifoldDerivative(vectorDerivative.size1(), vectorDerivative.size2()); + for(int j=0; j #include +#include +#include "NonlinearOptimizer-inl.h" +#include "NonlinearEquality.h" #include "Pose2Factor.h" #include "Pose2Graph.h" using namespace std; using namespace gtsam; - +using namespace boost; TEST( Pose2Factor, constructor ) { @@ -32,38 +35,68 @@ TEST( Pose2Factor, constructor ) Pose2Config config; config.insert("p1",p1); config.insert("p2",p2); - //Pose2 pose1; - //pose1=config.get("p1"); - //pose1.print("pose1"); // 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 - ); + 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 - ); + 1.0, 0.0, 0.0, + 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 - ); + -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); + Vector_(3, 0.1, -0.1, 0.0), 1.0); 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) { + 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-10, 1e-10); + 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(expected0, actual0)); +} + /* ************************************************************************* */ int main() { diff --git a/cpp/testPose2Graph.cpp b/cpp/testPose2Graph.cpp index f673db567..44b94b8d6 100644 --- a/cpp/testPose2Graph.cpp +++ b/cpp/testPose2Graph.cpp @@ -56,22 +56,22 @@ TEST( Pose2Graph, linerization ) // the expected linear factor GaussianFactorGraph lfg_expected; Matrix A1 = Matrix_(3,3, - 0.0, 2.0, -4.2, - -2.0, 0.0, 4.2, - 0.0, 0.0, 10.0); + 0.0, 2.0, 4.2, + -2.0, 0.0, 4.2, + 0.0, 0.0, 10.0); Matrix A2 = Matrix_(3,3, - 0.0, -2.0, 0.0, - 2.0, 0.0, 0.0, - 0.0, 0.0, -10.0); + -2.0, 0.0, 0.0, + 0.0,-2.0, 0.0, + 0.0, 0.0, -10.0); double sigma = 1; - Vector b = Vector_(3,0.1,0.1,0.0); + Vector b = Vector_(3,0.1,-0.1,0.0); lfg_expected.add("x1", A1, "x2", A2, b, sigma); - CHECK(lfg_expected.equals(lfg_linearized)); + CHECK(assert_equal(lfg_expected, lfg_linearized)); } /* ************************************************************************* */