diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index f19163301..2a28f35e4 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -21,12 +21,16 @@ namespace gtsam { /* ************************************************************************* */ Pose2 Pose2::exmap(const Vector& v) const { - return Pose2(t_+Point2(v(0),v(1)), r_.exmap(Vector_(1, v(2)))); + return Pose2(r_.exmap(Vector_(1, v(2))), t_+Point2(v(0),v(1))); } /* ************************************************************************* */ - Vector Pose2::vector() const { - return Vector_(3, t_.x(), t_.y(), r_.theta()); + 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(); } /* ************************************************************************* */ diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 6a7142efe..0bfa68182 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -45,12 +45,8 @@ namespace gtsam { t_(x,y), r_(theta) { } /** construct from rotation and translation */ - Pose2(const Point2& t, double theta) : - t_(t), r_(theta) { } Pose2(double theta, const Point2& t) : t_(t), r_(theta) { } - Pose2(const Point2& t, const Rot2& r) : - t_(t), r_(r) { } Pose2(const Rot2& r, const Point2& t) : t_(t), r_(r) { } @@ -67,14 +63,17 @@ namespace gtsam { Point2 t() const { return t_; } Rot2 r() const { return r_; } + /** return this pose2 as a vector (x,y,r) */ + Vector vector() const { return Vector_(3, t_.x(), t_.y(), r_.theta()); } + /** return DOF, dimensionality of tangent space = 3 */ size_t dim() const { return 3; } - /* exponential map */ + /** exponential map */ Pose2 exmap(const Vector& v) const; - /** return vectorized form (column-wise) */ - Vector vector() const; + /** log map */ + Vector log(const Pose2 &pose) const; /** rotate pose by theta */ // Pose2 rotate(double theta) const; @@ -99,14 +98,6 @@ namespace gtsam { return r_.unrotate(point-t_); } - // operators - Pose2 operator+(const Pose2& p2) const { - return Pose2(t_+p2.t_, r_*p2.r_); - } - - Pose2 operator-(const Pose2& p2) const { - return Pose2(t_-p2.t_, r_.invcompose(p2.r_)); - } }; // Pose2 /** diff --git a/cpp/Pose2Config.h b/cpp/Pose2Config.h index 399f77ba1..d97467ada 100644 --- a/cpp/Pose2Config.h +++ b/cpp/Pose2Config.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include "Pose2.h" @@ -20,26 +21,26 @@ namespace gtsam { class Pose2Config: public Testable { private: - std::map values; + std::map values_; public: Pose2Config() {} - Pose2Config(const Pose2Config &config) : values(config.values) { } + 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()) + 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)); + values_.insert(make_pair(name, val)); } Pose2Config& operator=(Pose2Config& rhs) { - values = rhs.values; + values_ = rhs.values_; return (*this); } @@ -49,7 +50,11 @@ public: } void print(const std::string &s) const { - std::cout << s << std::endl; + std::cout << "Pose2Config " << s << ", size " << values_.size() << "\n"; + std::string j; Pose2 v; + FOREACH_PAIR(j, v, values_) { + v.print(j + ": "); + } } /** @@ -58,7 +63,7 @@ public: Pose2Config exmap(const VectorConfig& delta) const { Pose2Config newConfig; std::string j; Pose2 vj; - FOREACH_PAIR(j, vj, values) { + FOREACH_PAIR(j, vj, values_) { if (delta.contains(j)) { const Vector& dj = delta[j]; //check_size(j,vj,dj); diff --git a/cpp/Pose2Graph.cpp b/cpp/Pose2Graph.cpp index c1ff3e4ef..c25b23d6b 100644 --- a/cpp/Pose2Graph.cpp +++ b/cpp/Pose2Graph.cpp @@ -15,9 +15,6 @@ template class FactorGraph > ; template class NonlinearFactorGraph ; //template class NonlinearOptimizer ; -void Pose2Graph::print(const std::string& s) const { -} - bool Pose2Graph::equals(const Pose2Graph& p, double tol) const { return false; } diff --git a/cpp/Pose2Graph.h b/cpp/Pose2Graph.h index 195a37355..335dc680a 100644 --- a/cpp/Pose2Graph.h +++ b/cpp/Pose2Graph.h @@ -27,11 +27,6 @@ public: /** default constructor is empty graph */ Pose2Graph() {} - /** - * print out graph - */ - void print(const std::string& s = "") const; - /** * equals */ diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp index 9ceffb828..3c2c2a54e 100644 --- a/cpp/Rot2.cpp +++ b/cpp/Rot2.cpp @@ -13,7 +13,7 @@ namespace gtsam { /* ************************************************************************* */ void Rot2::print(const string& s) const { - cout << s << ":" << angle() << endl; + cout << s << ":" << theta() << endl; } /* ************************************************************************* */ diff --git a/cpp/Rot2.h b/cpp/Rot2.h index 86d12749c..f9dad3c6d 100644 --- a/cpp/Rot2.h +++ b/cpp/Rot2.h @@ -35,7 +35,6 @@ namespace gtsam { /** return angle */ double theta() const { return atan2(s_,c_); } - double angle() const { return atan2(s_,c_); } /** return cos */ double c() const { return c_; } @@ -55,6 +54,9 @@ namespace gtsam { /** Given 1-dim tangent vector, create new rotation */ Rot2 exmap(const Vector& d) const; + /** Return the 1-dim tangent vector of R about this rotation */ + Vector log(const Rot2& R) const { return Vector_(1, R.theta() - theta()); } + /** return vectorized form (column-wise)*/ inline Vector vector() const { return Vector_(2,c_,s_);} diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index 77f8014b1..7a153499b 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -21,7 +21,7 @@ using namespace gtsam; /* ************************************************************************* */ TEST(Pose2, constructors) { Point2 p; - Pose2 pose(p,0); + Pose2 pose(0,p); Pose2 origin; assert_equal(pose,origin); } @@ -34,6 +34,15 @@ TEST(Pose2, exmap) { CHECK(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(Pose2, log) { + Pose2 pose0(M_PI_2, Point2(1, 2)); + Pose2 pose(M_PI, Point2(1.1, 2.1)); + Vector expected = Vector_(3, 0.1, 0.1, M_PI_2); + Vector actual = pose0.log(pose); + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ //TEST(Pose2, rotate) { // std::cout << "rotate\n"; diff --git a/cpp/testRot2.cpp b/cpp/testRot2.cpp index 31d93b8d6..1463a0d64 100644 --- a/cpp/testRot2.cpp +++ b/cpp/testRot2.cpp @@ -16,7 +16,7 @@ Point2 P(0.2, 0.7); /* ************************************************************************* */ TEST( Rot2, angle) { - DOUBLES_EQUAL(0.1,R.angle(),1e-9); + DOUBLES_EQUAL(0.1,R.theta(),1e-9); } /* ************************************************************************* */ @@ -59,6 +59,16 @@ TEST( Rot2, exmap) CHECK(assert_equal(R.exmap(v), R)); } +/* ************************************************************************* */ +TEST(Rot2, log) +{ + Rot2 rot0(M_PI_2); + Rot2 rot(M_PI); + Vector expected = Vector_(1, M_PI_2); + Vector actual = rot0.log(rot); + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ // rotate derivatives