diff --git a/gtsam.h b/gtsam.h index c0e90f4a7..7eb0a27b3 100644 --- a/gtsam.h +++ b/gtsam.h @@ -28,10 +28,10 @@ class Rot2 { class Pose2 { Pose2(); - Pose2(const Pose2& pose); Pose2(double x, double y, double theta); Pose2(double theta, const Point2& t); Pose2(const Rot2& r, const Point2& t); + Pose2(Vector v); void print(string s) const; bool equals(const Pose2& pose, double tol) const; double x() const; @@ -40,11 +40,11 @@ class Pose2 { int dim() const; Pose2* compose_(const Pose2& p2); Pose2* between_(const Pose2& p2); + Vector logmap(const Pose2& p); }; class SharedGaussian { SharedGaussian(Matrix covariance); - SharedGaussian(Vector sigmas); }; class SharedDiagonal { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index e45b9d72f..e1d40265a 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -70,7 +70,16 @@ namespace gtsam { /** Constructor from 3*3 matrix */ Pose2(const Matrix &T) : - r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { assert(T.rows()==3 && T.cols()==3); } + r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { + assert(T.rows() == 3 && T.cols() == 3); + } + + /** Construct from canonical coordinates (Lie algebra) */ + Pose2(const Vector& v) { + Pose2 pose = Expmap(v); + r_ = pose.r_; + t_ = pose.t_; + } /** print with optional string */ void print(const std::string& s = "") const; diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 95603fce1..104d47e31 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -103,7 +103,10 @@ TEST(Pose2, expmap2) { Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; Matrix expected = eye(3) + A + A2 + A3 + A4; - Pose2 pose = Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.99)); + Vector v = Vector_(3, 0.01, -0.015, 0.99); + Pose2 pose = Pose2::Expmap(v); + Pose2 pose2(v); + EXPECT(assert_equal(pose, pose2)); Matrix actual = pose.matrix(); //EXPECT(assert_equal(expected, actual)); }