diff --git a/cpp/Makefile.am b/cpp/Makefile.am index c0497cfc7..38beeb0c1 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -147,9 +147,10 @@ testPose2Constraint_SOURCES = $(example) testPose2Constraint.cpp testPose2Constraint_LDADD = libgtsam.la # geometry -sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp -check_PROGRAMS += testPoint2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 +sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp +check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testPoint2_SOURCES = testPoint2.cpp +testRot2_SOURCES = testRot2.cpp testPose2_SOURCES = testPose2.cpp testPoint3_SOURCES = testPoint3.cpp testRot3_SOURCES = testRot3.cpp @@ -157,6 +158,7 @@ testPose3_SOURCES = testPose3.cpp testCal3_S2_SOURCES = testCal3_S2.cpp testPoint2_LDADD = libgtsam.la +testRot2_LDADD = libgtsam.la testPose2_LDADD = libgtsam.la testPoint3_LDADD = libgtsam.la testRot3_LDADD = libgtsam.la diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp new file mode 100644 index 000000000..c4a037639 --- /dev/null +++ b/cpp/Rot2.cpp @@ -0,0 +1,70 @@ +/* + * Rot2.cpp + * + * Created on: Dec 9, 2009 + * Author: Frank Dellaert + */ + +#include "Rot2.h" + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + 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; + } + + /* ************************************************************************* */ + Rot2 Rot2::exmap(const Vector& v) const { + if (zero(v)) return (*this); + return Rot2(v(0)) * (*this); + } + + /* ************************************************************************* */ + Rot2 exmap(const Rot2& R, const Vector& v) { + return R.exmap(v); + } + + /* ************************************************************************* */ + 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()); + } + + /* ************************************************************************* */ + Matrix Drotate2(const Rot2& R) { + return R.matrix(); + } + + /* ************************************************************************* */ + 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()); + } + + /* ************************************************************************* */ + Matrix Dunrotate2(const Rot2 & R) { + return R.transpose(); + } + +} // gtsam diff --git a/cpp/Rot2.h b/cpp/Rot2.h new file mode 100644 index 000000000..b29f02d20 --- /dev/null +++ b/cpp/Rot2.h @@ -0,0 +1,123 @@ +/* + * Rot2.h + * + * Created on: Dec 9, 2009 + * Author: Frank Dellaert + */ + +#ifndef ROT2_H_ +#define ROT2_H_ + +#include "Testable.h" +#include "Point2.h" +#include "Matrix.h" + +namespace gtsam { + + /* 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) { + } + + public: + + /** default constructor, zero rotation */ + Rot2() : c_(1.0), s_(0.0) {} + + /** constructor from angle */ + Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {} + + /** return angle */ + inline double angle() const { return atan2(s_, c_); } + + /** print */ + void print(const std::string& s = "theta") 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;} + + /** 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 2*2 rotation matrix */ + Matrix matrix() const { + double r[] = { c_, -s_, s_, c_ }; + return Matrix_(2, 2, r); + } + + /** return 3*3 transpose (inverse) rotation matrix */ + Matrix transpose() const { + double r[] = { c_, s_, -s_, c_ }; + return Matrix_(2, 2, r); + } + + /** inverse transformation */ + Rot2 inverse() const { return Rot2(c_, -s_);} + + /** composition via sum and difference formulas */ + inline Rot2 operator*(const Rot2& R) const { + return Rot2(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); + } + + /** rotate from rotated to world, syntactic sugar = R*p */ + inline Point2 operator*(const Point2& p) const { + return Point2(c_ * p.x() - s_ * p.y(), s_ * p.x() + c_ * p.y()); + } + + /** rotate from world to rotated = R'*p */ + Point2 unrotate(const Point2& p) const { + return Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); + } + + /** 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_); + } + }; + + /** + * 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 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 + +#endif /* ROT2_H_ */ diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index 3a365828c..b89820ef5 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -17,6 +17,12 @@ namespace gtsam { return equal_with_abs_tol(matrix(), R.matrix(), tol); } + /* ************************************************************************* */ + Rot3 Rot3::exmap(const Vector& v) const { + if (zero(v)) return (*this); + return rodriguez(v) * (*this); + } + /* ************************************************************************* */ /** faster than below ? */ /* ************************************************************************* */ @@ -52,13 +58,7 @@ namespace gtsam { /* ************************************************************************* */ Rot3 exmap(const Rot3& R, const Vector& v) { - return rodriguez(v) * R; - } - - /* ************************************************************************* */ - Rot3 Rot3::exmap(const Vector& v) const { - if (zero(v)) return (*this); - return rodriguez(v) * (*this); + return R.exmap(v); } /* ************************************************************************* */ diff --git a/cpp/testRot2.cpp b/cpp/testRot2.cpp new file mode 100644 index 000000000..8cd384409 --- /dev/null +++ b/cpp/testRot2.cpp @@ -0,0 +1,92 @@ +/** + * @file testRot2.cpp + * @brief Unit tests for Rot2 class + * @author Frank Dellaert + */ + +#include +#include "numericalDerivative.h" +#include "Rot2.h" + +using namespace gtsam; + +Rot2 R(0.1); +Point2 P(0.2, 0.7); + +/* ************************************************************************* */ +TEST( Rot2, angle) +{ + DOUBLES_EQUAL(0.1,R.angle(),1e-9); +} + +/* ************************************************************************* */ +TEST( Rot2, transpose) +{ + CHECK(assert_equal(R.inverse().matrix(),R.transpose())); +} + +/* ************************************************************************* */ +TEST( Rot2, equals) +{ + CHECK(R.equals(R)); + Rot2 zero; + CHECK(!R.equals(zero)); +} + +/* ************************************************************************* */ +TEST( Rot2, exmap) +{ + Vector v = zero(1); + CHECK(assert_equal(R.exmap(v), R)); +} + +/* ************************************************************************* */ +// rotate derivatives + +TEST( Rot2, Drotate1) +{ + Matrix computed = Drotate1(R, P); + Matrix numerical = numericalDerivative21(rotate, R, P); + CHECK(assert_equal(numerical,computed)); +} + +TEST( Rot2, Drotate2_DNrotate2) +{ + Matrix computed = Drotate2(R); + Matrix numerical = numericalDerivative22(rotate, R, P); + CHECK(assert_equal(numerical,computed)); +} + +/* ************************************************************************* */ +// unrotate + +TEST( Rot2, unrotate) +{ + Point2 w = R * P; + CHECK(assert_equal(unrotate(R,w),P)); +} + +/* ************************************************************************* */ +// unrotate derivatives + +TEST( Rot2, Dunrotate1) +{ + Matrix computed = Dunrotate1(R, P); + Matrix numerical = numericalDerivative21(unrotate, R, P); + CHECK(assert_equal(numerical,computed)); +} + +TEST( Rot2, Dunrotate2_DNunrotate2) +{ + Matrix computed = Dunrotate2(R); + Matrix numerical = numericalDerivative22(unrotate, R, P); + CHECK(assert_equal(numerical,computed)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +