From c26219e3d4ecff7d5574dd7d7f67d8107210c32c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 18 Feb 2010 16:27:01 +0000 Subject: [PATCH] Added Pose2::matrix() function. --- cpp/Pose2.cpp | 9 +++++++++ cpp/Pose2.h | 3 +++ 2 files changed, 12 insertions(+) diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index 96a29c364..472c93362 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -13,6 +13,15 @@ namespace gtsam { /** Explicit instantiation of base class to export members */ INSTANTIATE_LIE(Pose2); + /* ************************************************************************* */ + Matrix Pose2::matrix() const { + Matrix R = r_.matrix(); + Matrix Z = zeros(1,2); + R = stack(2, &R, &Z); + Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0); + return collect(2, &R, &T); + } + /* ************************************************************************* */ void Pose2::print(const string& s) const { cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; diff --git a/cpp/Pose2.h b/cpp/Pose2.h index b415b71fc..d6e634fb8 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -52,6 +52,9 @@ namespace gtsam { /** assert equality up to a tolerance */ bool equals(const Pose2& pose, double tol = 1e-9) const; + /** return transformation matrix */ + Matrix matrix() const; + /** get functions for x, y, theta */ double x() const { return t_.x(); } double y() const { return t_.y(); }