diff --git a/cpp/Lie-inl.h b/cpp/Lie-inl.h index b2ae16dec..1ebded962 100644 --- a/cpp/Lie-inl.h +++ b/cpp/Lie-inl.h @@ -8,7 +8,6 @@ #include "Lie.h" #define INSTANTIATE_LIE(T) \ - template T operator*(const T&, const T&); \ template T between(const T&, const T&); \ template Vector logmap(const T&, const T&); \ template T expmap(const T&, const Vector&); \ diff --git a/cpp/Lie.h b/cpp/Lie.h index 8a4eb7b02..61d369bf7 100644 --- a/cpp/Lie.h +++ b/cpp/Lie.h @@ -15,17 +15,12 @@ namespace gtsam { template T expmap(const Vector& v); /* Exponential map about identity */ - // Syntactic sugar - template - inline T operator*(const T& l1, const T& l0) { return compose(l1, l0); } - - // The following functions may be overridden in your own class file // with more efficient versions if possible. // Compute l1 s.t. l2=l1*l0 template - inline T between(const T& l1, const T& l2) { return inverse(l1)*l2; } + inline T between(const T& l1, const T& l2) { return compose(inverse(l1),l2); } // Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp template @@ -33,7 +28,7 @@ namespace gtsam { /* Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ template - inline T expmap(const T& t, const Vector& d) { return t * expmap(d); } + inline T expmap(const T& t, const Vector& d) { return compose(t,expmap(d)); } /** * Base class for Lie group type diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index 7fc5ffc2b..574fc0d20 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -14,7 +14,7 @@ namespace gtsam { INSTANTIATE_LIE(Pose2); static const Matrix I3 = eye(3), Z12 = zeros(1,2); - static const Rot2 R_PI_2(0., 1.); + static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); /* ************************************************************************* */ Matrix Pose2::matrix() const { @@ -35,6 +35,50 @@ namespace gtsam { } /* ************************************************************************* */ + +#ifdef SLOW_BUT_CORRECT_EXPMAP + + template<> Pose2 expmap(const Vector& xi) { + Point2 v(xi(0),xi(1)); + double w = xi(2); + if (fabs(w) < 1e-5) + return Pose2(xi[0], xi[1], xi[2]); + else { + Rot2 R(Rot2::fromAngle(w)); + Point2 v_ortho = R_PI_2 * v; // points towards rot center + Point2 t = (v_ortho - rotate(R,v_ortho)) / w; + return Pose2(R, t); + } + } + + Vector logmap(const Pose2& p) { + const Rot2& R = p.r(); + const Point2& t = p.t(); + double w = R.theta(); + if (fabs(w) < 1e-5) + return Vector_(3, t.x(), t.y(), w); + else { + double c_1 = R.c()-1.0, s = R.s(); + double det = c_1*c_1 + s*s; + Point2 p = R_PI_2 * (unrotate(R, t) - t); + Point2 v = (w/det) * p; + return Vector_(3, v.x(), v.y(), w); + } + } + +#else + + template<> Pose2 expmap(const Vector& v) { + return Pose2(v[0], v[1], v[2]); + } + + Vector logmap(const Pose2& p) { + return Vector_(3, p.x(), p.y(), p.theta()); + } + +#endif + + /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) Matrix AdjointMap(const Pose2& p) { @@ -109,7 +153,7 @@ namespace gtsam { // Calculate delta rotation = between(R1,R2) double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(c,s); + Rot2 R(Rot2::fromCosSin(c,s)); // Calculate delta translation = unrotate(R1, dt); Point2 dt = p2.t() - p1.t(); diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 63fb5d223..111a2db4c 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -29,7 +29,7 @@ namespace gtsam { public: /** default constructor = origin */ - Pose2() : t_(0.0, 0.0), r_(0) {} // default is origin + Pose2() {} // default is origin /** copy constructor */ Pose2(const Pose2& pose) : t_(pose.t_), r_(pose.r_) {} @@ -40,12 +40,20 @@ namespace gtsam { * @param y y coordinate * @param theta angle with positive X-axis */ - Pose2(double x, double y, double theta) : t_(x,y), r_(theta) {} + Pose2(double x, double y, double theta) : + t_(x, y), r_(Rot2::fromAngle(theta)) { + } /** construct from rotation and translation */ - Pose2(double theta, const Point2& t) : t_(t), r_(theta) {} + Pose2(double theta, const Point2& t) : + t_(t), r_(Rot2::fromAngle(theta)) { + } Pose2(const Rot2& r, const Point2& t) : t_(t), r_(r) {} + /** Constructor from 3*3 matrix */ + Pose2(const Matrix &T) : + r_(Rot2::fromCosSin(T(0, 0), T(1, 0))), t_(T(0, 2), T(1, 2)) {} + /** print with optional string */ void print(const std::string& s = "") const; @@ -67,63 +75,86 @@ namespace gtsam { }; // Pose2 + /** print using member print function, currently used by LieConfig */ + inline void print(const Pose2& obj, const std::string& str = "") { obj.print(str); } /** return DOF, dimensionality of tangent space = 3 */ inline size_t dim(const Pose2&) { return 3; } + /** + * Exponential map from se(2) to SE(2) + */ + template<> Pose2 expmap(const Vector& v); + + /** + * Inverse of exponential map, from SE(2) to se(2) + */ + Vector logmap(const Pose2& p); + /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) */ Matrix AdjointMap(const Pose2& p); - /** inverse transformation */ + /** + * wedge for SE(2): + * @param xi 3-dim twist (v,omega) where + * omega is angular velocity + * v (vx,vy) = 2D velocity + * @return xihat, 3*3 element of Lie algebra that can be exponentiated + */ + inline Matrix wedge(double vx, double vy, double w) { + return Matrix_(3,3, + 0.,-w, vx, + w, 0., vy, + 0., 0., 0.); + } + + template <> + inline Matrix wedge(const Vector& xi) { + return wedge(xi(0),xi(1),xi(2)); + } + + /** + * inverse transformation + */ Pose2 inverse(const Pose2& pose); Matrix Dinverse(const Pose2& pose); - /** compose this transformation onto another (first p1 and then p2) */ - inline Pose2 compose(const Pose2& p0, const Pose2& p1) { - return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); } + /** + * compose this transformation onto another (first p1 and then p2) + */ + inline Pose2 compose(const Pose2& p0, const Pose2& p1) + { return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t());} Matrix Dcompose1(const Pose2& p1, const Pose2& p2); Matrix Dcompose2(const Pose2& p1, const Pose2& p2); + inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0);} - /** exponential and log maps around identity */ - - /** Create an incremental pose from x,y,theta */ - template<> inline Pose2 expmap(const Vector& v) { return Pose2(v[0], v[1], v[2]); } - - /** Return the x,y,theta of this pose */ - inline Vector logmap(const Pose2& p) { return Vector_(3, p.x(), p.y(), p.theta()); } - - /** print using member print function, currently used by LieConfig */ - inline void print(const Pose2& obj, const std::string& str = "") { obj.print(str); } - - /** Return point coordinates in pose coordinate frame */ - inline Point2 transform_to(const Pose2& pose, const Point2& point) { - return unrotate(pose.r(), point - pose.t()); - } + /** + * Return point coordinates in pose coordinate frame + */ + inline Point2 transform_to(const Pose2& pose, const Point2& point) + { return unrotate(pose.r(), point - pose.t());} Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional H1, boost::optional H2); - /** Return point coordinates in global frame */ - inline Point2 transform_from(const Pose2& pose, const Point2& point) { - return rotate(pose.r(), point) + pose.t(); - } + /** + * Return point coordinates in global frame + */ + inline Point2 transform_from(const Pose2& pose, const Point2& point) + { return rotate(pose.r(), point) + pose.t();} Point2 transform_from(const Pose2& pose, const Point2& point, boost::optional H1, boost::optional H2); + inline Point2 operator*(const Pose2& pose, const Point2& point) + { return transform_from(pose, point);} - /** Return relative pose between p1 and p2, in p1 coordinate frame */ + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ Pose2 between(const Pose2& p1, const Pose2& p2, boost::optional H1, boost::optional H2); - /** same as compose (pre-multiply this*p1) */ - inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0); } - - /** Transform a point in this coordinate frame to global coordinates, - * same as transform_from */ - inline Point2 operator*(const Pose2& pose, const Point2& point) { - return transform_from(pose, point); } - /** * Calculate bearing to a landmark * @param pose 2D pose of robot @@ -131,10 +162,6 @@ namespace gtsam { * @return 2D rotation \in SO(2) */ Rot2 bearing(const Pose2& pose, const Point2& point); - - /** - * Calculate bearing and optional derivative(s) - */ Rot2 bearing(const Pose2& pose, const Point2& point, boost::optional H1, boost::optional H2); @@ -145,10 +172,6 @@ namespace gtsam { * @return range (double) */ double range(const Pose2& pose, const Point2& point); - - /** - * Calculate range and optional derivative(s) - */ double range(const Pose2& pose, const Point2& point, boost::optional H1, boost::optional H2); diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 337b09cc5..1144b93f7 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -108,7 +108,7 @@ namespace gtsam { /* ************************************************************************* */ Pose3 Pose3::transform_to(const Pose3& pose) const { - Rot3 cRv = R_ * Rot3(inverse(pose.R_)); + Rot3 cRv = R_ * Rot3(gtsam::inverse(pose.R_)); Point3 t = gtsam::transform_to(pose, t_); return Pose3(cRv, t); } diff --git a/cpp/Pose3.h b/cpp/Pose3.h index 85fe1261e..ae9bf518c 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -59,6 +59,17 @@ namespace gtsam { /** assert equality up to a tolerance */ bool equals(const Pose3& pose, double tol = 1e-9) const; + /** Find the inverse pose s.t. inverse(p)*p = Pose3() */ + inline Pose3 inverse() const { + const Rot3 Rt(R_.inverse()); + return Pose3(Rt, - (Rt*t_)); + } + + /** Compose two poses */ + inline Pose3 operator*(const Pose3& T) const { + return Pose3(R_*T.R_, t_ + R_*T.t_); + } + Pose3 transform_to(const Pose3& pose) const; /** get the dimension by the type */ @@ -81,10 +92,7 @@ namespace gtsam { inline size_t dim(const Pose3&) { return 6; } /** Compose two poses */ - inline Pose3 compose(const Pose3& p0, const Pose3& p1) { - return Pose3(p0.rotation()*p1.rotation(), - p0.translation() + p0.rotation()*p1.translation()); - } + inline Pose3 compose(const Pose3& p0, const Pose3& p1) { return p0*p1;} /** Find the inverse pose s.t. inverse(p)*p = Pose3() */ inline Pose3 inverse(const Pose3& p) { diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp index 6117b02df..1bd3e67fd 100644 --- a/cpp/Rot2.cpp +++ b/cpp/Rot2.cpp @@ -35,11 +35,6 @@ namespace gtsam { return Matrix_(2, 2, c_, s_, -s_, c_); } - /* ************************************************************************* */ - Matrix Rot2::negtranspose() const { - return Matrix_(2, 2, -c_, -s_, s_, -c_); - } - /* ************************************************************************* */ Point2 Rot2::rotate(const Point2& p) const { return Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); @@ -73,14 +68,14 @@ namespace gtsam { /* ************************************************************************* */ Rot2 relativeBearing(const Point2& d) { double n = d.norm(); - return Rot2(d.x() / n, d.y() / n); + return Rot2::fromCosSin(d.x() / n, d.y() / n); } /* ************************************************************************* */ Rot2 relativeBearing(const Point2& d, boost::optional H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if (H) *H = Matrix_(1, 2, -y / d2, x / d2); - return Rot2(x / n, y / n); + return Rot2::fromCosSin(x / n, y / n); } /* ************************************************************************* */ diff --git a/cpp/Rot2.h b/cpp/Rot2.h index 12de03caa..a17715b34 100644 --- a/cpp/Rot2.h +++ b/cpp/Rot2.h @@ -18,11 +18,22 @@ namespace gtsam { /* Rotation matrix */ class Rot2: Testable, public Lie { + private: + /** we store cos(theta) and sin(theta) */ double c_, s_; - public: + /** normalize to make sure cos and sin form unit vector */ + inline Rot2& normalize() { + double scale = c_*c_ + s_*s_; + if(scale != 1.0) { + scale = pow(scale, -0.5); + c_ *= scale; + s_ *= scale; + } + return *this; + } /** constructor from cos/sin */ inline Rot2(double c, double s) : @@ -30,23 +41,35 @@ namespace gtsam { #ifdef ROT2_RENORMALIZE // rtodo: Could do this scale correction only when creating from compose // Don't let scale drift - double scale = c*c + s*s; - if(scale != 1.0) { - scale = pow(scale, -0.5); - c_ *= scale; - s_ *= scale; - } + normalize(); #endif } + public: + /** 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)) {} + /** "named constructors" */ - /** return angle */ - double theta() const { return atan2(s_,c_); } + /** Named constructor from angle == exponential map at identity */ + static Rot2 fromAngle(double theta) { return Rot2(cos(theta),sin(theta));} + + /** Named constructor from cos(theta),sin(theta) pair, will *not* normalize! */ + static Rot2 fromCosSin(double c, double s) { + if (fabs(c*c + s*s - 1.0)>1e-9) + throw std::invalid_argument("Rot2::fromCosSin: needs cos/sin pair"); + return Rot2(c, s); + } + + /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ + static Rot2 atan2(double y, double x) { + Rot2 R(x,y); + return R.normalize(); + } + + /** return angle */ + double theta() const { return ::atan2(s_,c_); } /** return cos */ inline double c() const { return c_; } @@ -66,8 +89,13 @@ namespace gtsam { /** return 2*2 transpose (inverse) rotation matrix */ Matrix transpose() const; - /** return 2*2 negative transpose */ - Matrix negtranspose() const; + /** The inverse rotation - negative angle */ + Rot2 inverse() const { return Rot2(c_, -s_);} + + /** Compose - make a new rotation by adding angles */ + Rot2 operator*(const Rot2& R) const { + return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); + } /** rotate from world to rotated = R*p */ Point2 rotate(const Point2& p) const; @@ -86,8 +114,8 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(s_); } - }; + }; // Rot2 // Lie group functions @@ -100,7 +128,7 @@ namespace gtsam { /** Expmap around identity - create a rotation from an angle */ template<> inline Rot2 expmap(const Vector& v) { if (zero(v)) return (Rot2()); - else return Rot2(v(0)); + else return Rot2::fromAngle(v(0)); } /** Logmap around identity - return the angle of the rotation */ @@ -109,27 +137,10 @@ namespace gtsam { } /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& r0, const Rot2& r1) { - return Rot2( - r0.c() * r1.c() - r0.s() * r1.s(), - r0.s() * r1.c() + r0.c() * r1.s()); - } - - /** Syntactic sugar R1*R2 = compose(R1,R2) */ - inline Rot2 operator*(const Rot2& r0, const Rot2& r1) { - return compose(r0, r1); - } + inline Rot2 compose(const Rot2& R1, const Rot2& R2) { return R1*R2;} /** The inverse rotation - negative angle */ - inline Rot2 inverse(const Rot2& r) { return Rot2(r.c(), -r.s());} - - /** Shortcut to compose the inverse: invcompose(R0,R1) = inverse(R0)*R1 */ - inline Rot2 invcompose(const Rot2& r0, const Rot2& r1) { - return Rot2( - r0.c() * r1.c() + r0.s() * r1.s(), - -r0.s() * r1.c() + r0.c() * r1.s()); - } - + inline Rot2 inverse(const Rot2& R) { return R.inverse();} /** * rotate point from rotated coordinate frame to diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index 879b0ed02..12e9ff9ef 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -165,11 +165,6 @@ namespace gtsam { return rodriguez(w/t, t); } - /* ************************************************************************* */ - Point3 rotate(const Rot3& R, const Point3& p) { - return R.r1() * p.x() + R.r2() * p.y() + R.r3() * p.z(); - } - /* ************************************************************************* */ Matrix Drotate1(const Rot3& R, const Point3& p) { return R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); diff --git a/cpp/Rot3.h b/cpp/Rot3.h index 0c42dd09d..a53e955fa 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -109,6 +109,27 @@ namespace gtsam { /** get the dimension by the type */ static inline size_t dim() { return 3; }; + /* Find the inverse rotation R^T s.t. inverse(R)*R = I */ + inline Rot3 inverse() const { + return Rot3( + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z()); + } + + /** compose two rotations */ + Rot3 operator*(const Rot3& R2) const { + return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); + } + + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + Point3 rotate(const Point3& p) const + {return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();} + inline Point3 operator*(const Point3& p) const { return rotate(p);} + private: /** Serialization function */ friend class boost::serialization::access; @@ -147,7 +168,8 @@ namespace gtsam { * @param wz * @return incremental rotation matrix */ - inline Rot3 rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));} + inline Rot3 rodriguez(double wx, double wy, double wz) + { return rodriguez(Vector_(3,wx,wy,wz));} /** return DOF, dimensionality of tangent space */ inline size_t dim(const Rot3&) { return 3; } @@ -163,16 +185,10 @@ namespace gtsam { Vector logmap(const Rot3& R); // Compose two rotations - inline Rot3 compose(const Rot3& R1, const Rot3& R2) { - return Rot3(R1.matrix() * R2.matrix()); } + inline Rot3 compose(const Rot3& R1, const Rot3& R2) { return R1*R2;} // Find the inverse rotation R^T s.t. inverse(R)*R = Rot3() - inline Rot3 inverse(const Rot3& R) { - return Rot3( - R.r1().x(), R.r1().y(), R.r1().z(), - R.r2().x(), R.r2().y(), R.r2().z(), - R.r3().x(), R.r3().y(), R.r3().z()); - } + inline Rot3 inverse(const Rot3& R) { return R.inverse();} // and its derivative inline Matrix Dinverse(Rot3 R) { return -R.matrix();} @@ -181,8 +197,7 @@ namespace gtsam { * rotate point from rotated coordinate frame to * world = R*p */ - Point3 rotate(const Rot3& R, const Point3& p); - inline Point3 operator*(const Rot3& R, const Point3& p) { return rotate(R,p); } + inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;} Matrix Drotate1(const Rot3& R, const Point3& p); Matrix Drotate2(const Rot3& R); // does not depend on p ! diff --git a/cpp/testPlanarSLAM.cpp b/cpp/testPlanarSLAM.cpp index ee5f53f91..af7c7f537 100644 --- a/cpp/testPlanarSLAM.cpp +++ b/cpp/testPlanarSLAM.cpp @@ -23,7 +23,7 @@ SharedGaussian TEST( planarSLAM, BearingFactor ) { // Create factor - Rot2 z(M_PI_4 + 0.1); // h(x) - z = -0.1 + Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 planarSLAM::Bearing factor(2, 3, z, sigma); // create config @@ -72,7 +72,7 @@ TEST( planarSLAM, constructor ) G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), I3); // Create bearing factor - Rot2 z1(M_PI_4 + 0.1); // h(x) - z = -0.1 + Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 G.addBearing(2, 3, z1, sigma); // Create range factor diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index d574af26c..b5d2adc3e 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -14,6 +14,8 @@ using namespace gtsam; using namespace std; +// #define SLOW_BUT_CORRECT_EXPMAP + /* ************************************************************************* */ TEST(Pose2, constructors) { //cout << "constructors" << endl; @@ -21,6 +23,8 @@ TEST(Pose2, constructors) { Pose2 pose(0,p); Pose2 origin; assert_equal(pose,origin); + Pose2 t(M_PI_2+0.018, Point2(1.015, 2.01)); + CHECK(assert_equal(t,Pose2(t.matrix()))); } /* ************************************************************************* */ @@ -41,9 +45,13 @@ TEST(Pose2, manifold) { TEST(Pose2, expmap) { //cout << "expmap" << endl; Pose2 pose(M_PI_2, Point2(1, 2)); +#ifdef SLOW_BUT_CORRECT_EXPMAP + Pose2 expected(1.00811, 2.01528, 2.5608); +#else Pose2 expected(M_PI_2+0.99, Point2(1.015, 2.01)); +#endif Pose2 actual = expmap(pose, Vector_(3, 0.01, -0.015, 0.99)); - CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ @@ -66,19 +74,46 @@ TEST(Pose2, expmap2) { TEST(Pose2, expmap0) { //cout << "expmap0" << endl; Pose2 pose(M_PI_2, Point2(1, 2)); +#ifdef SLOW_BUT_CORRECT_EXPMAP + Pose2 expected(1.01491, 2.01013, 1.5888); +#else Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); +#endif Pose2 actual = pose * expmap(Vector_(3, 0.01, -0.015, 0.018)); - CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(expected, actual, 1e-5)); } +#ifdef SLOW_BUT_CORRECT_EXPMAP +/* ************************************************************************* */ +// test case for screw motion in the plane +namespace screw { + double w=0.3; + Vector xi = Vector_(3, 0.0, w, w); + Rot2 expectedR = Rot2::fromAngle(w); + Point2 expectedT(-0.0446635, 0.29552); + Pose2 expected(expectedR, expectedT); +} + +TEST(Pose3, expmap_c) +{ + CHECK(assert_equal(screw::expected, expm(screw::xi),1e-6)); + CHECK(assert_equal(screw::expected, expmap(screw::xi),1e-6)); + CHECK(assert_equal(screw::xi, logmap(screw::expected),1e-6)); +} +#endif + /* ************************************************************************* */ TEST(Pose2, logmap) { //cout << "logmap" << endl; Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01)); +#ifdef SLOW_BUT_CORRECT_EXPMAP + Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); +#else Vector expected = Vector_(3, 0.01, -0.015, 0.018); +#endif Vector actual = logmap(pose0,pose); - CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(expected, actual, 1e-5)); } /* ************************************************************************* */ @@ -172,10 +207,10 @@ TEST(Pose2, compose_a) TEST(Pose2, compose_b) { //cout << "compose_b" << endl; - Pose2 pose1(Rot2(M_PI/10.0), Point2(.75, .5)); - Pose2 pose2(Rot2(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585)); + Pose2 pose1(Rot2::fromAngle(M_PI/10.0), Point2(.75, .5)); + Pose2 pose2(Rot2::fromAngle(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585)); - Pose2 pose_expected(Rot2(M_PI/4.0), Point2(1.0, 2.0)); + Pose2 pose_expected(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 2.0)); Pose2 pose_actual_op = pose1 * pose2; Pose2 pose_actual_fcn = compose(pose1, pose2); @@ -184,7 +219,7 @@ TEST(Pose2, compose_b) Matrix numericalH1 = numericalDerivative21(compose, pose1, pose2, 1e-5); Matrix numericalH2 = numericalDerivative22(compose, pose1, pose2, 1e-5); - CHECK(assert_equal(numericalH1,actualDcompose1)); + CHECK(assert_equal(numericalH1,actualDcompose1,1e-5)); CHECK(assert_equal(numericalH2,actualDcompose2)); CHECK(assert_equal(pose_expected, pose_actual_op)); @@ -195,10 +230,10 @@ TEST(Pose2, compose_b) TEST(Pose2, compose_c) { //cout << "compose_c" << endl; - Pose2 pose1(Rot2(M_PI/4.0), Point2(1.0, 1.0)); - Pose2 pose2(Rot2(M_PI/4.0), Point2(sqrt(.5), sqrt(.5))); + Pose2 pose1(Rot2::fromAngle(M_PI/4.0), Point2(1.0, 1.0)); + Pose2 pose2(Rot2::fromAngle(M_PI/4.0), Point2(sqrt(.5), sqrt(.5))); - Pose2 pose_expected(Rot2(M_PI/2.0), Point2(1.0, 2.0)); + Pose2 pose_expected(Rot2::fromAngle(M_PI/2.0), Point2(1.0, 2.0)); Pose2 pose_actual_op = pose1 * pose2; Pose2 pose_actual_fcn = compose(pose1,pose2); @@ -207,7 +242,7 @@ TEST(Pose2, compose_c) Matrix numericalH1 = numericalDerivative21(compose, pose1, pose2, 1e-5); Matrix numericalH2 = numericalDerivative22(compose, pose1, pose2, 1e-5); - CHECK(assert_equal(numericalH1,actualDcompose1)); + CHECK(assert_equal(numericalH1,actualDcompose1,1e-5)); CHECK(assert_equal(numericalH2,actualDcompose2)); CHECK(assert_equal(pose_expected, pose_actual_op)); @@ -373,11 +408,11 @@ TEST( Pose2, bearing ) CHECK(assert_equal(Rot2(),bearing(x1,l1))); // establish bearing is indeed 45 degrees - CHECK(assert_equal(Rot2(M_PI_4),bearing(x1,l2))); + CHECK(assert_equal(Rot2::fromAngle(M_PI_4),bearing(x1,l2))); // establish bearing is indeed 45 degrees even if shifted Rot2 actual23 = bearing(x2, l3, actualH1, actualH2); - CHECK(assert_equal(Rot2(M_PI_4),actual23)); + CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual23)); // Check numerical derivatives expectedH1 = numericalDerivative21(bearing, x2, l3, 1e-5); @@ -387,7 +422,7 @@ TEST( Pose2, bearing ) // establish bearing is indeed 45 degrees even if rotated Rot2 actual34 = bearing(x3, l4, actualH1, actualH2); - CHECK(assert_equal(Rot2(M_PI_4),actual34)); + CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual34)); // Check numerical derivatives expectedH1 = numericalDerivative21(bearing, x3, l4, 1e-5); diff --git a/cpp/testRot2.cpp b/cpp/testRot2.cpp index 29fe15589..4304fb6b3 100644 --- a/cpp/testRot2.cpp +++ b/cpp/testRot2.cpp @@ -10,13 +10,16 @@ using namespace gtsam; -Rot2 R(0.1); +Rot2 R(Rot2::fromAngle(0.1)); Point2 P(0.2, 0.7); /* ************************************************************************* */ -TEST( Rot2, angle) +TEST( Rot2, constructors_and_angle) { + double c=cos(0.1), s=sin(0.1); DOUBLES_EQUAL(0.1,R.theta(),1e-9); + CHECK(assert_equal(R,Rot2::fromCosSin(c,s))); + CHECK(assert_equal(R,Rot2::atan2(s*5,c*5))); } /* ************************************************************************* */ @@ -25,23 +28,11 @@ TEST( Rot2, transpose) CHECK(assert_equal(inverse(R).matrix(),R.transpose())); } -/* ************************************************************************* */ -TEST( Rot2, negtranspose) -{ - CHECK(assert_equal(-inverse(R).matrix(),R.negtranspose())); -} - /* ************************************************************************* */ TEST( Rot2, compose) { - CHECK(assert_equal(Rot2(0.45), Rot2(0.2)*Rot2(0.25))); - CHECK(assert_equal(Rot2(0.45), Rot2(0.25)*Rot2(0.2))); -} - -/* ************************************************************************* */ -TEST( Rot2, invcompose) -{ - CHECK(assert_equal(Rot2(0.2), invcompose(Rot2(0.25),Rot2(0.45)))); + CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.2)*Rot2::fromAngle(0.25))); + CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.25)*Rot2::fromAngle(0.2))); } /* ************************************************************************* */ @@ -62,8 +53,8 @@ TEST( Rot2, expmap) /* ************************************************************************* */ TEST(Rot2, logmap) { - Rot2 rot0(M_PI_2); - Rot2 rot(M_PI); + Rot2 rot0(Rot2::fromAngle(M_PI_2)); + Rot2 rot(Rot2::fromAngle(M_PI)); Vector expected = Vector_(1, M_PI_2); Vector actual = logmap(rot0, rot); CHECK(assert_equal(expected, actual)); @@ -113,7 +104,7 @@ TEST( Rot2, relativeBearing ) // establish relativeBearing is indeed 45 degrees Rot2 actual2 = relativeBearing(l2, actualH); - CHECK(assert_equal(Rot2(M_PI_4),actual2)); + CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2)); // Check numerical derivative expectedH = numericalDerivative11(relativeBearing, l2, 1e-5);