diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 4e40b1923..e4b08be5c 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -5,6 +5,8 @@ #include "Pose3.h" +using namespace std; + namespace gtsam { /* ************************************************************************* */ @@ -12,6 +14,25 @@ Pose3 Pose3::exmap(const Vector& v) const { return Pose3(R_.exmap(sub(v,0,3)), t_.exmap(sub(v,3,6))); } +/* ************************************************************************* */ +Vector Pose3::vector() const { + Vector r = R_.vector(), t = t_.vector(); + return concatVectors(2, &r, &t); +} + +/* ************************************************************************* */ +Matrix Pose3::matrix() const { + const double row4[] = { 0, 0, 0, 1 }; + Matrix A34 = Matrix_(3, 4, vector()), A14 = Matrix_(1, 4, row4); + return stack(2, &A34, &A14); +} + +/* ************************************************************************* */ +void Pose3::print(const string& s) const { + R_.print(s + ".R"); + t_.print(s + ".t"); +} + /* ************************************************************************* */ Point3 transform_from(const Pose3& pose, const Point3& p) { return pose.R_ * p + pose.t_; diff --git a/cpp/Pose3.h b/cpp/Pose3.h index 75a2d813d..90d8041c8 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -65,23 +65,13 @@ namespace gtsam { } /** return 12D vectorized form (column-wise) */ - Vector vector() const { - Vector r = R_.vector(), t = t_.vector(); - return concatVectors(2, &r, &t); - } + Vector vector() const; /** convert to 4*4 matrix */ - Matrix matrix() const { - const double row4[] = { 0, 0, 0, 1 }; - Matrix A34 = Matrix_(3, 4, vector()), A14 = Matrix_(1, 4, row4); - return stack(2, &A34, &A14); - } + Matrix matrix() const; /** print with optional string */ - void print(const std::string& s = "") const { - R_.print(s + ".R"); - t_.print(s + ".t"); - } + void print(const std::string& s = "") const; /** transforms */ Pose3 transformPose_to(const Pose3& transform) const;