diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index f128bee89..f19163301 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -9,68 +9,68 @@ using namespace std; namespace gtsam { - /* ************************************************************************* */ - void Pose2::print(const string& s) const { - cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; - } + /* ************************************************************************* */ + void Pose2::print(const string& s) const { + cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; + } - /* ************************************************************************* */ - bool Pose2::equals(const Pose2& q, double tol) const { - return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); - } + /* ************************************************************************* */ + bool Pose2::equals(const Pose2& q, double tol) const { + return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); + } - /* ************************************************************************* */ - Pose2 Pose2::exmap(const Vector& v) const { - return Pose2(t_+Point2(v(0),v(1)), r_.exmap(Vector_(1, v(2)))); - } + /* ************************************************************************* */ + Pose2 Pose2::exmap(const Vector& v) const { + return Pose2(t_+Point2(v(0),v(1)), r_.exmap(Vector_(1, v(2)))); + } - /* ************************************************************************* */ - Vector Pose2::vector() const { - return Vector_(3, t_.x(), t_.y(), r_.theta()); - } + /* ************************************************************************* */ + Vector Pose2::vector() const { + return Vector_(3, t_.x(), t_.y(), r_.theta()); + } - /* ************************************************************************* */ -// Pose2 Pose2::rotate(double theta) const { -// //return Pose2(t_, Rot2(theta)*r_); -// return Pose2(Point2(0.0,0.0),-theta)*(*this); -// } + /* ************************************************************************* */ + // Pose2 Pose2::rotate(double theta) const { + // //return Pose2(t_, Rot2(theta)*r_); + // return Pose2(Point2(0.0,0.0),-theta)*(*this); + // } - /* ************************************************************************* */ - Point2 transform_to(const Pose2& pose, const Point2& point) { - return pose*point; - } + /* ************************************************************************* */ + Point2 transform_to(const Pose2& pose, const Point2& point) { + return pose*point; + } - // TODO, have a combined function that returns both function value and derivative - Matrix Dtransform_to1(const Pose2& pose, const Point2& point) { - Matrix dx_dt = pose.r().negtranspose(); - Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t()); - return collect(2, &dx_dt, &dx_dr); - } + // TODO, have a combined function that returns both function value and derivative + Matrix Dtransform_to1(const Pose2& pose, const Point2& point) { + Matrix dx_dt = pose.r().negtranspose(); + Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t()); + return collect(2, &dx_dt, &dx_dr); + } - Matrix Dtransform_to2(const Pose2& pose, const Point2& point) { - return pose.r().transpose(); - } + Matrix Dtransform_to2(const Pose2& pose, const Point2& point) { + return pose.r().transpose(); + } - /* ************************************************************************* */ - Pose2 between(const Pose2& p1, const Pose2& p2) { - return Pose2( - p1.r().invcompose(p2.r()), - p1.r().unrotate(p2.t() - p1.t())); - } + /* ************************************************************************* */ + Pose2 between(const Pose2& p1, const Pose2& p2) { + return Pose2( + p1.r().invcompose(p2.r()), + p1.r().unrotate(p2.t() - p1.t())); + } - Matrix Dbetween1(const Pose2& p1, const Pose2& p2) { - Matrix dbt_dp = Dtransform_to1(p1, p2.t()); - Matrix dbr_dp = Matrix_(1,3, 0.0, 0.0, -1.0); - return stack(2, &dbt_dp, &dbr_dp); - } + Matrix Dbetween1(const Pose2& p1, const Pose2& p2) { + Matrix dbt_dp = Dtransform_to1(p1, p2.t()); + Matrix dbr_dp = Matrix_(1,3, 0.0, 0.0, -1.0); + return stack(2, &dbt_dp, &dbr_dp); + } - Matrix Dbetween2(const Pose2& p1, const Pose2& p2) { - Matrix db_dt2 = p1.r().transpose(); - return Matrix_(3,3, - db_dt2.data()[0], db_dt2.data()[1], 0.0, - db_dt2.data()[2], db_dt2.data()[3], 0.0, - 0.0, 0.0, 1.0); - } + Matrix Dbetween2(const Pose2& p1, const Pose2& p2) { + Matrix db_dt2 = p1.r().transpose(); + return Matrix_(3,3, + db_dt2.data()[0], db_dt2.data()[1], 0.0, + db_dt2.data()[2], db_dt2.data()[3], 0.0, + 0.0, 0.0, 1.0); + } - /* ************************************************************************* */ + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Pose2.h b/cpp/Pose2.h index b01618ea7..6a7142efe 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -16,111 +16,111 @@ namespace gtsam { -/** - * A 2D pose (Point2,Rot2) - */ -class Pose2: Testable { + /** + * A 2D pose (Point2,Rot2) + */ + class Pose2: Testable { -private: - Point2 t_; - Rot2 r_; + private: + Point2 t_; + Rot2 r_; -public: + public: - /** default constructor = origin */ - Pose2() : - t_(0.0, 0.0), r_(0) { } // default is origin + /** default constructor = origin */ + Pose2() : + t_(0.0, 0.0), r_(0) { } // default is origin - /** copy constructor */ - Pose2(const Pose2& pose) : - t_(pose.t_), r_(pose.r_) { } + /** copy constructor */ + Pose2(const Pose2& pose) : + t_(pose.t_), r_(pose.r_) { } + + /** + * construct from (x,y,theta) + * @param x x coordinate + * @param y y coordinate + * @param theta angle with positive X-axis + */ + Pose2(double x, double y, double theta) : + 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) { } + + /** print with optional string */ + void print(const std::string& s = "") const; + + /** assert equality up to a tolerance */ + bool equals(const Pose2& pose, double tol = 1e-9) const; + + /** get functions for x, y, theta */ + double x() const { return t_.x();} + double y() const { return t_.y();} + double theta() const { return r_.theta();} + Point2 t() const { return t_; } + Rot2 r() const { return r_; } + + /** return DOF, dimensionality of tangent space = 3 */ + size_t dim() const { return 3; } + + /* exponential map */ + Pose2 exmap(const Vector& v) const; + + /** return vectorized form (column-wise) */ + Vector vector() const; + + /** rotate pose by theta */ + // Pose2 rotate(double theta) const; + + /** inverse transformation */ + Pose2 inverse() const { + return Pose2(r_, r_.unrotate(t_)); + } + + /** compose this transformation onto another (pre-multiply this*p1) */ + Pose2 compose(const Pose2& p1) const { + return Pose2(p1.r_*r_, p1.r_*t_+p1.t_); + } + + /** same as compose (pre-multiply this*p1) */ + Pose2 operator*(const Pose2& p1) const { + return compose(p1); + } + + /** Return point coordinates in pose coordinate frame, same as transform_to */ + Point2 operator*(const Point2& point) const { + 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 /** - * construct from (x,y,theta) - * @param x x coordinate - * @param y y coordinate - * @param theta angle with positive X-axis + * Return point coordinates in pose coordinate frame */ - Pose2(double x, double y, double theta) : - t_(x,y), r_(theta) { } + Point2 transform_to(const Pose2& pose, const Point2& point); + Matrix Dtransform_to1(const Pose2& pose, const Point2& point); + Matrix Dtransform_to2(const Pose2& pose, const Point2& point); - /** 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) { } - - /** print with optional string */ - void print(const std::string& s = "") const; - - /** assert equality up to a tolerance */ - bool equals(const Pose2& pose, double tol = 1e-9) const; - - /** get functions for x, y, theta */ - double x() const { return t_.x();} - double y() const { return t_.y();} - double theta() const { return r_.theta();} - Point2 t() const { return t_; } - Rot2 r() const { return r_; } - - /** return DOF, dimensionality of tangent space = 3 */ - size_t dim() const { return 3; } - - /* exponential map */ - Pose2 exmap(const Vector& v) const; - - /** return vectorized form (column-wise) */ - Vector vector() const; - - /** rotate pose by theta */ -// Pose2 rotate(double theta) const; - - /** inverse transformation */ - Pose2 inverse() const { - return Pose2(r_, r_.unrotate(t_)); - } - - /** compose this transformation onto another (pre-multiply this*p1) */ - Pose2 compose(const Pose2& p1) const { - return Pose2(p1.r_*r_, p1.r_*t_+p1.t_); - } - - /** same as compose (pre-multiply this*p1) */ - Pose2 operator*(const Pose2& p1) const { - return compose(p1); - } - - /** Return point coordinates in pose coordinate frame, same as transform_to */ - Point2 operator*(const Point2& point) const { - 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 - -/** - * Return point coordinates in pose coordinate frame - */ -Point2 transform_to(const Pose2& pose, const Point2& point); -Matrix Dtransform_to1(const Pose2& pose, const Point2& point); -Matrix Dtransform_to2(const Pose2& pose, const Point2& point); - -/** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ -Pose2 between(const Pose2& p1, const Pose2& p2); -Matrix Dbetween1(const Pose2& p1, const Pose2& p2); -Matrix Dbetween2(const Pose2& p1, const Pose2& p2); + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose2 between(const Pose2& p1, const Pose2& p2); + Matrix Dbetween1(const Pose2& p1, const Pose2& p2); + Matrix Dbetween2(const Pose2& p1, const Pose2& p2); } // namespace gtsam diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp index 4bf531444..9ceffb828 100644 --- a/cpp/Rot2.cpp +++ b/cpp/Rot2.cpp @@ -11,109 +11,109 @@ using namespace std; namespace gtsam { - /* ************************************************************************* */ - void Rot2::print(const string& s) const { - cout << s << ":" << angle() << endl; - } + /* ************************************************************************* */ + void Rot2::print(const string& s) const { + cout << s << ":" << angle() << endl; + } - /* ************************************************************************* */ - bool Rot2::equals(const Rot2& R, double tol) const { - return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol; - } + /* ************************************************************************* */ + bool Rot2::equals(const Rot2& R, double tol) const { + return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol; + } - /* ************************************************************************* */ - Rot2 Rot2::exmap(const Vector& v) const { - if (zero(v)) return (*this); - return Rot2(v(0)) * (*this); // exponential map then compose - } + /* ************************************************************************* */ + Rot2 Rot2::exmap(const Vector& v) const { + if (zero(v)) return (*this); + return Rot2(v(0)) * (*this); // exponential map then compose + } - /* ************************************************************************* */ - Matrix Rot2::matrix() const { - return Matrix_(2, 2, c_, -s_, s_, c_); - } + /* ************************************************************************* */ + Matrix Rot2::matrix() const { + return Matrix_(2, 2, c_, -s_, s_, c_); + } - /* ************************************************************************* */ - Matrix Rot2::transpose() const { - return Matrix_(2, 2, c_, s_, -s_, c_); - } + /* ************************************************************************* */ + Matrix Rot2::transpose() const { + return Matrix_(2, 2, c_, s_, -s_, c_); + } - /* ************************************************************************* */ - Matrix Rot2::negtranspose() const { - return Matrix_(2, 2, -c_, -s_, s_, -c_); - } + /* ************************************************************************* */ + Matrix Rot2::negtranspose() const { + return Matrix_(2, 2, -c_, -s_, s_, -c_); + } - /* ************************************************************************* */ - Rot2 Rot2::inverse() const { return Rot2(c_, -s_);} + /* ************************************************************************* */ + Rot2 Rot2::inverse() const { return Rot2(c_, -s_);} - /* ************************************************************************* */ - Rot2 Rot2::invcompose(const Rot2& R) const { - return Rot2( - c_ * R.c_ + s_ * R.s_, - -s_ * R.c_ + c_ * R.s_); - } + /* ************************************************************************* */ + Rot2 Rot2::invcompose(const Rot2& R) const { + return Rot2( + c_ * R.c_ + s_ * R.s_, + -s_ * R.c_ + c_ * R.s_); + } - /* ************************************************************************* */ - Rot2 Rot2::operator*(const Rot2& R) const { - return Rot2( - c_ * R.c_ - s_ * R.s_, - s_ * R.c_ + c_ * R.s_ - ); - } + /* ************************************************************************* */ + Rot2 Rot2::operator*(const Rot2& R) const { + return Rot2( + c_ * R.c_ - s_ * R.s_, + s_ * R.c_ + c_ * R.s_ + ); + } - /* ************************************************************************* */ - Point2 Rot2::operator*(const Point2& p) const { - return Point2( - c_ * p.x() - s_ * p.y(), - s_ * p.x() + c_ * p.y() - ); - } + /* ************************************************************************* */ + Point2 Rot2::operator*(const Point2& p) const { + return Point2( + c_ * p.x() - s_ * p.y(), + s_ * p.x() + c_ * p.y() + ); + } - /* ************************************************************************* */ - Point2 Rot2::unrotate(const Point2& p) const { - return Point2( - c_ * p.x() + s_ * p.y(), - -s_ * p.x() + c_ * p.y() - ); - } + /* ************************************************************************* */ + Point2 Rot2::unrotate(const Point2& p) const { + return Point2( + c_ * p.x() + s_ * p.y(), + -s_ * p.x() + c_ * p.y() + ); + } - /* ************************************************************************* */ - Rot2 exmap(const Rot2& R, const Vector& v) { - return R.exmap(v); - } + /* ************************************************************************* */ + Rot2 exmap(const Rot2& R, const Vector& v) { + return R.exmap(v); + } - /* ************************************************************************* */ - Point2 rotate(const Rot2& R, const Point2& p) { - return R * p; - } + /* ************************************************************************* */ + Point2 rotate(const Rot2& R, const Point2& p) { + return R * p; + } - /* ************************************************************************* */ - // see libraries/caml/geometry/math.ml - Matrix Drotate1(const Rot2& R, const Point2& p) { - Point2 q = R*p; - return Matrix_(2, 1, -q.y(), q.x()); - } + /* ************************************************************************* */ + // see libraries/caml/geometry/math.ml + Matrix Drotate1(const Rot2& R, const Point2& p) { + Point2 q = R*p; + return Matrix_(2, 1, -q.y(), q.x()); + } - /* ************************************************************************* */ - Matrix Drotate2(const Rot2& R) { - return R.matrix(); - } + /* ************************************************************************* */ + Matrix Drotate2(const Rot2& R) { + return R.matrix(); + } - /* ************************************************************************* */ - Point2 unrotate(const Rot2& R, const Point2& p) { - return R.unrotate(p); - } + /* ************************************************************************* */ + Point2 unrotate(const Rot2& R, const Point2& p) { + return R.unrotate(p); + } - /* ************************************************************************* */ - /** see libraries/caml/geometry/math.lyx, derivative of unrotate */ - /* ************************************************************************* */ - Matrix Dunrotate1(const Rot2 & R, const Point2 & p) { - Point2 q = R.unrotate(p); - return Matrix_(2, 1, q.y(), -q.x()); - } + /* ************************************************************************* */ + /** see libraries/caml/geometry/math.lyx, derivative of unrotate */ + /* ************************************************************************* */ + Matrix Dunrotate1(const Rot2 & R, const Point2 & p) { + Point2 q = R.unrotate(p); + return Matrix_(2, 1, q.y(), -q.x()); + } - /* ************************************************************************* */ - Matrix Dunrotate2(const Rot2 & R) { - return R.transpose(); - } + /* ************************************************************************* */ + Matrix Dunrotate2(const Rot2 & R) { + return R.transpose(); + } } // gtsam diff --git a/cpp/Rot2.h b/cpp/Rot2.h index c0470eea3..86d12749c 100644 --- a/cpp/Rot2.h +++ b/cpp/Rot2.h @@ -14,110 +14,110 @@ namespace gtsam { - /* Rotation matrix */ - class Rot2: Testable { - private: - /** we store cos(theta) and sin(theta) */ - double c_, s_; + /* Rotation matrix */ + class Rot2: Testable { + private: + /** we store cos(theta) and sin(theta) */ + double c_, s_; - /** private constructor from cos/sin */ - Rot2(double c, double s) : - c_(c), s_(s) { - } + /** private constructor from cos/sin */ + Rot2(double c, double s) : + c_(c), s_(s) { + } - public: + public: - /** default constructor, zero rotation */ - Rot2() : c_(1.0), s_(0.0) {} + /** default constructor, zero rotation */ + Rot2() : c_(1.0), s_(0.0) {} - /** constructor from angle == exponential map at identity */ - Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {} + /** constructor from angle == exponential map at identity */ + Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {} - /** return angle */ - double theta() const { return atan2(s_,c_); } - double angle() const { return atan2(s_,c_); } + /** return angle */ + double theta() const { return atan2(s_,c_); } + double angle() const { return atan2(s_,c_); } - /** return cos */ - double c() const { return c_; } + /** return cos */ + double c() const { return c_; } - /** return sin */ - double s() const { return s_; } + /** return sin */ + double s() const { return s_; } - /** print */ - void print(const std::string& s = "theta") const; + /** print */ + void print(const std::string& s = "theta") const; - /** equals with an tolerance */ - bool equals(const Rot2& R, double tol = 1e-9) const; + /** equals with an tolerance */ + bool equals(const Rot2& R, double tol = 1e-9) const; - /** return DOF, dimensionality of tangent space */ - inline size_t dim() const { return 1;} + /** return DOF, dimensionality of tangent space */ + inline size_t dim() const { return 1;} - /** Given 1-dim tangent vector, create new rotation */ - Rot2 exmap(const Vector& d) const; + /** Given 1-dim tangent vector, create new rotation */ + Rot2 exmap(const Vector& d) const; - /** return vectorized form (column-wise)*/ - inline Vector vector() const { return Vector_(2,c_,s_);} + /** return vectorized form (column-wise)*/ + inline Vector vector() const { return Vector_(2,c_,s_);} - /** return 2*2 rotation matrix */ - Matrix matrix() const; + /** return 2*2 rotation matrix */ + Matrix matrix() const; - /** return 2*2 transpose (inverse) rotation matrix */ - Matrix transpose() const; + /** return 2*2 transpose (inverse) rotation matrix */ + Matrix transpose() const; - /** return 2*2 negative transpose */ - Matrix negtranspose() const; + /** return 2*2 negative transpose */ + Matrix negtranspose() const; - /** inverse transformation */ - Rot2 inverse() const; + /** inverse transformation */ + Rot2 inverse() const; - /** compose with the inverse of this rotation */ - Rot2 invcompose(const Rot2& R) const; + /** compose with the inverse of this rotation */ + Rot2 invcompose(const Rot2& R) const; - /** composition via sum and difference formulas */ - Rot2 operator*(const Rot2& R) const; + /** composition via sum and difference formulas */ + Rot2 operator*(const Rot2& R) const; - /** rotate from rotated to world, syntactic sugar = R*p */ - Point2 operator*(const Point2& p) const; + /** rotate from rotated to world, syntactic sugar = R*p */ + Point2 operator*(const Point2& p) const; - /** rotate from world to rotated = R'*p */ - Point2 unrotate(const Point2& p) const; + /** rotate from world to rotated = R'*p */ + Point2 unrotate(const Point2& p) const; - /** friends */ - friend Matrix Dunrotate1(const Rot2& R, const Point2& p); + /** friends */ + friend Matrix Dunrotate1(const Rot2& R, const Point2& p); - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(c_); - ar & BOOST_SERIALIZATION_NVP(s_); - } - }; + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(c_); + ar & BOOST_SERIALIZATION_NVP(s_); + } + }; - /** - * Update Rotation with incremental rotation - * @param v a vector of incremental angle - * @param R a 2D rotation - * @return incremental rotation matrix - */ - Rot2 exmap(const Rot2& R, const Vector& v); + /** + * Update Rotation with incremental rotation + * @param v a vector of incremental angle + * @param R a 2D rotation + * @return incremental rotation matrix + */ + Rot2 exmap(const Rot2& R, const Vector& v); - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - Point2 rotate(const Rot2& R, const Point2& p); - Matrix Drotate1(const Rot2& R, const Point2& p); - Matrix Drotate2(const Rot2& R); // does not depend on p ! + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + Point2 rotate(const Rot2& R, const Point2& p); + Matrix Drotate1(const Rot2& R, const Point2& p); + Matrix Drotate2(const Rot2& R); // does not depend on p ! - /** - * rotate point from world to rotated - * frame = R'*p - */ - Point2 unrotate(const Rot2& R, const Point2& p); - Matrix Dunrotate1(const Rot2& R, const Point2& p); - Matrix Dunrotate2(const Rot2& R); // does not depend on p ! + /** + * rotate point from world to rotated + * frame = R'*p + */ + Point2 unrotate(const Rot2& R, const Point2& p); + Matrix Dunrotate1(const Rot2& R, const Point2& p); + Matrix Dunrotate2(const Rot2& R); // does not depend on p ! } // gtsam diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index bbf372fe5..fcdaa815a 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -12,12 +12,18 @@ using namespace gtsam; +/* ************************************************************************* */ +//TEST(Pose2, print) { +// Pose2 pose(Point2(1.0,2.0), Rot2(3.0)); +// pose.print("thepose"); +//} + /* ************************************************************************* */ TEST(Pose2, constructors) { - Point2 p; - Pose2 pose(p,0); - Pose2 origin; - assert_equal(pose,origin); + Point2 p; + Pose2 pose(p,0); + Pose2 origin; + assert_equal(pose,origin); } /* ************************************************************************* */ @@ -39,35 +45,35 @@ TEST(Pose2, exmap) { /* ************************************************************************* */ TEST(Pose2, operators) { - CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1))); - CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1))); + CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1))); + CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1))); } /* ************************************************************************* */ TEST( Pose2, transform_to ) { - Pose2 pose(1,2,M_PI_2); // robot at (1,2) looking towards y - Point2 point(-1,4); // landmark at (-1,4) + Pose2 pose(1,2,M_PI_2); // robot at (1,2) looking towards y + Point2 point(-1,4); // landmark at (-1,4) - // expected - Point2 expected(2,2); - Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0); - Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0); + // expected + Point2 expected(2,2); + Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0); + Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0); - // actual - Point2 actual = transform_to(pose,point); - Matrix actualH1 = Dtransform_to1(pose,point); - Matrix actualH2 = Dtransform_to2(pose,point); + // actual + Point2 actual = transform_to(pose,point); + Matrix actualH1 = Dtransform_to1(pose,point); + Matrix actualH2 = Dtransform_to2(pose,point); - CHECK(assert_equal(expected,actual)); - CHECK(assert_equal(expectedH1,actualH1)); - CHECK(assert_equal(expectedH2,actualH2)); + CHECK(assert_equal(expected,actual)); + CHECK(assert_equal(expectedH1,actualH1)); + CHECK(assert_equal(expectedH2,actualH2)); Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5); - CHECK(assert_equal(numericalH1,actualH1)); + CHECK(assert_equal(numericalH1,actualH1)); Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5); - CHECK(assert_equal(numericalH2,actualH2)); + CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ @@ -104,42 +110,42 @@ TEST(Pose2, compose_b) /* ************************************************************************* */ TEST( Pose2, between ) { - Pose2 p1(1,2,M_PI_2); // robot at (1,2) looking towards y - Pose2 p2(-1,4,M_PI); // robot at (-1,4) loooking at negative x + Pose2 p1(1,2,M_PI_2); // robot at (1,2) looking towards y + Pose2 p2(-1,4,M_PI); // robot at (-1,4) loooking at negative x - // expected - Pose2 expected(2,2,M_PI_2); - Matrix expectedH1 = Matrix_(3,3, - 0.0,-1.0,2.0, - 1.0,0.0,-2.0, - 0.0,0.0,-1.0 - ); - Matrix expectedH2 = Matrix_(3,3, - 0.0,1.0,0.0, - -1.0,0.0,0.0, - 0.0,0.0,1.0 - ); + // expected + Pose2 expected(2,2,M_PI_2); + Matrix expectedH1 = Matrix_(3,3, + 0.0,-1.0,2.0, + 1.0,0.0,-2.0, + 0.0,0.0,-1.0 + ); + Matrix expectedH2 = Matrix_(3,3, + 0.0,1.0,0.0, + -1.0,0.0,0.0, + 0.0,0.0,1.0 + ); - // actual - Pose2 actual = between(p1,p2); - Matrix actualH1 = Dbetween1(p1,p2); - Matrix actualH2 = Dbetween2(p1,p2); + // actual + Pose2 actual = between(p1,p2); + Matrix actualH1 = Dbetween1(p1,p2); + Matrix actualH2 = Dbetween2(p1,p2); - CHECK(assert_equal(expected,actual)); - CHECK(assert_equal(expectedH1,actualH1)); - CHECK(assert_equal(expectedH2,actualH2)); + CHECK(assert_equal(expected,actual)); + CHECK(assert_equal(expectedH1,actualH1)); + CHECK(assert_equal(expectedH2,actualH2)); Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5); - CHECK(assert_equal(numericalH1,actualH1)); + CHECK(assert_equal(numericalH1,actualH1)); Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5); - CHECK(assert_equal(numericalH2,actualH2)); + CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */