Small changes: standardized constructors, added log() and unit tests, removed +,- ops, angle() changed to theta(), print functions

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 {
+  Vector Pose2::log(const Pose2 &pose) const {
-    return Vector_(3, t_.x(), t_.y(), r_.theta());
+    return Vector_(3,
+        pose.t().x() - t().x(),
+        pose.t().y() - t().y(),
+        pose.r().theta() - r().theta());
+//    return between(*this, pose).vector();
   }
 
   /* ************************************************************************* */

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) */
+    /** log map */
-    Vector vector() const;
+    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
 
   /**

cpp/Pose2Config.h

@@ -4,6 +4,7 @@
 #include <boost/exception.hpp>
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
+#include <boost/format.hpp>
 #include <iostream>
 #include <string>
 #include "Pose2.h"
@@ -20,26 +21,26 @@ namespace gtsam {
 class Pose2Config: public Testable<Pose2Config> {
 
 private:
-  std::map<std::string, Pose2> values;
+  std::map<std::string, Pose2> 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<std::string, Pose2>::const_iterator it = values.find(key);
+		std::map<std::string, Pose2>::const_iterator it = values_.find(key);
-		if (it == values.end())
+		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);

cpp/Pose2Graph.cpp

@@ -15,9 +15,6 @@ template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ;
 template class NonlinearFactorGraph<Pose2Config> ;
 //template class NonlinearOptimizer<Pose2Graph, Pose2Config> ;
 
-void Pose2Graph::print(const std::string& s) const {
-}
-
 bool Pose2Graph::equals(const Pose2Graph& p, double tol) const {
 	return false;
 }

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
    */

cpp/Rot2.cpp

@@ -13,7 +13,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void Rot2::print(const string& s) const {
-    cout << s << ":" << angle() << endl;
+    cout << s << ":" << theta() << endl;
   }
 
   /* ************************************************************************* */

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_);}
 

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";

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