From 3432b6c50fe875839f7f5a8b7b03a6cca127db85 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Aug 2009 01:24:26 +0000 Subject: [PATCH] new Pose2 class --- .cproject | 17 +++++++++----- cpp/Makefile.am | 6 +++-- cpp/Pose2.cpp | 39 ++++++++++++++++++++++++++++++++ cpp/Pose2.h | 57 +++++++++++++++++++++++++++++++++++++++++++++++ cpp/testPose2.cpp | 25 +++++++++++++++++++++ 5 files changed, 136 insertions(+), 8 deletions(-) create mode 100644 cpp/Pose2.cpp create mode 100644 cpp/Pose2.h create mode 100644 cpp/testPose2.cpp diff --git a/.cproject b/.cproject index 063d544b5..a071e0c00 100644 --- a/.cproject +++ b/.cproject @@ -300,7 +300,6 @@ make - check true true @@ -308,6 +307,7 @@ make + testSimpleCamera.run true true @@ -315,6 +315,7 @@ make + testCal3_S2.run true true @@ -322,7 +323,6 @@ make - testVSLAMFactor.run true true @@ -330,6 +330,7 @@ make + testCalibratedCamera.run true true @@ -337,15 +338,21 @@ make - testConditionalGaussian.run true true true + +make + +testPose2.run +true +true +true + make - install true true @@ -353,7 +360,6 @@ make - clean true true @@ -361,7 +367,6 @@ make - check true true diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 75bd46b33..fabc839e0 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -100,15 +100,17 @@ testConstrainedLinearFactorGraph_LDADD = libgtsam.la testConstrainedChordalBayesNet_LDADD = libgtsam.la # geometry -sources += Point2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp -check_PROGRAMS += testPoint2 testPoint3 testRot3 testPose3 testCal3_S2 +sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp +check_PROGRAMS += testPoint2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testPoint2_SOURCES = testPoint2.cpp +testPose2_SOURCES = testPose2.cpp testPoint3_SOURCES = testPoint3.cpp testRot3_SOURCES = testRot3.cpp testPose3_SOURCES = testPose3.cpp testCal3_S2_SOURCES = testCal3_S2.cpp testPoint2_LDADD = libgtsam.la +testPose2_LDADD = libgtsam.la testPoint3_LDADD = libgtsam.la testRot3_LDADD = libgtsam.la testPose3_LDADD = libgtsam.la diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp new file mode 100644 index 000000000..a66afad87 --- /dev/null +++ b/cpp/Pose2.cpp @@ -0,0 +1,39 @@ +/** + * @file Pose2.cpp + * @brief 2D Pose + */ + +#include "Pose2.h" + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + void Pose2::print(const string& s) const { + cout << s << "(" << x_ << ", " << y_ << ", " << theta_ << ")" << endl; + } + + /* ************************************************************************* */ + Pose2 Pose2::exmap(const Vector& v) const { + return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2)); + } + + /* ************************************************************************* */ + bool Pose2::equals(const Pose2& q, double tol) const { + return (fabs(x_ - q.x_) < tol && fabs(y_ - q.y_) < tol && fabs(theta_ + - q.theta_) < tol); + } + + /* ************************************************************************* */ + bool assert_equal(const Pose2& A, const Pose2& B, double tol) { + if (A.equals(B, tol)) return true; + printf("not equal:\n"); + A.print("A"); + B.print("B"); + return false; + } + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/cpp/Pose2.h b/cpp/Pose2.h new file mode 100644 index 000000000..3eec8d79a --- /dev/null +++ b/cpp/Pose2.h @@ -0,0 +1,57 @@ +/** + * @file Pose2.h + * @brief 3D Pose + * @author: Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include "Point2.h" + +namespace gtsam { + + /** + * A 2D pose (x,y,theta) + */ + class Pose2 { + + private: + double x_, y_, theta_; + + public: + + /** default constructor = origin */ + Pose2() : + x_(0), y_(0), theta_(0) { + } // default is origin + + /** copy constructor */ + Pose2(const Pose2& pose) : + x_(pose.x_), y_(pose.y_), theta_(pose.theta_) { + } + + /** construct from (x,y,theta) */ + Pose2(double x, double y, double theta) : + x_(x), y_(y), theta_(theta) { + } + + /** construct from rotation and translation */ + Pose2(const Point2& t, double theta) : + x_(t.x()), y_(t.y()), theta_(theta) { + } + + /** print with optional string */ + void print(const std::string& s = "") const; + + Pose2 exmap(const Vector& v) const; + + /** assert equality up to a tolerance */ + bool equals(const Pose2& pose, double tol = 1e-9) const; + }; + + /** assert equality up to a tolerance */ + bool assert_equal(const Pose2& A, const Pose2& B, double tol = 1e-9); + +} // namespace gtsam diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp new file mode 100644 index 000000000..e0bfc4a13 --- /dev/null +++ b/cpp/testPose2.cpp @@ -0,0 +1,25 @@ +/** + * @file testPose2.cpp + * @brief Unit tests for Pose2 class + */ + +#include +#include "Pose2.h" + +using namespace gtsam; + +/* ************************************************************************* */ +TEST(Pose2, constructors) { + Point2 p; + Pose2 pose(p,0); + Pose2 origin; + assert_equal(pose,origin); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +