diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 4aeadb0ed..a06b39905 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -16,20 +16,13 @@ */ // \callgraph +#pragma once + #include #include -/* ************************************************************************* */ -// Below is the class definition of Rot3. By the macros at the end of this -// file, both Rot3M and Rot3Q are actually defined with this interface. -#if defined Rot3 || defined __DOXYGEN - namespace gtsam { - // Forward declarations; - class Rot3M; - class Rot3Q; - /// Typedef to an Eigen Quaternion, we disable alignment because /// geometry objects are stored in boost pool allocators, in Values /// containers, and and these pool allocators do not support alignment. @@ -47,10 +40,10 @@ namespace gtsam { static const size_t dimension = 3; private: -#if defined ROT3_IS_MATRIX +#ifndef GTSAM_DEFAULT_QUATERNIONS /** We store columns ! */ Point3 r1_, r2_, r3_; -#elif defined ROT3_IS_QUATERNION +#else /** Internal Eigen Quaternion */ Quaternion quaternion_; #endif @@ -85,9 +78,6 @@ namespace gtsam { */ Rot3(const Quaternion& q); - /** Constructor from a rotation matrix in a Rot3M */ - Rot3(const Rot3M& r); - /* Static member function to generate some well known rotations */ /// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. @@ -324,11 +314,11 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { -#if defined ROT3_IS_MATRIX +#ifndef GTSAM_DEFAULT_QUATERNIONS ar & BOOST_SERIALIZATION_NVP(r1_); ar & BOOST_SERIALIZATION_NVP(r2_); ar & BOOST_SERIALIZATION_NVP(r3_); -#elif defined ROT3_IS_QUATERNION +#else ar & BOOST_SERIALIZATION_NVP(quaternion_); #endif } @@ -346,48 +336,3 @@ namespace gtsam { */ std::pair RQ(const Matrix& A); } - -#endif // if defined Rot3 || defined __DOXYGEN - - -/* ************************************************************************* */ -// This block of code defines both Rot3Q and Rot3M, by self-including Rot3.h -// twice and using preprocessor definitions of Rot3 to be Rot3M and Rot3Q. It -// then creates a typedef of Rot3 to either Rot3M or Rot3Q, depending on -// whether GTSAM_DEFAULT_QUATERNIONS is defined. -#if !defined __ROT3_H -#define __ROT3_H - -// Define Rot3M -#define Rot3 Rot3M -#define ROT3_IS_MATRIX -#include -#undef Rot3 -#undef ROT3_IS_MATRIX - -// Define Rot3Q -#define Rot3 Rot3Q -#define ROT3_IS_QUATERNION -#include -#undef Rot3 -#undef ROT3_IS_QUATERNION - -// Create Rot3 typedef -namespace gtsam { - /** - * Typedef to the main 3D rotation implementation, which is Rot3M by default, - * or Rot3Q if GTSAM_DEFAULT_QUATERNIONS is defined. Depending on whether - * GTSAM_DEFAULT_QUATERNIONS is defined, Rot3M (the rotation matrix - * implementation) or Rot3Q (the quaternion implementation) will used in all - * built-in gtsam geometry types that involve 3D rotations, such as Pose3, - * SimpleCamera, CalibratedCamera, StereoCamera, etc. - */ -#ifdef GTSAM_DEFAULT_QUATERNIONS - typedef Rot3Q Rot3; -#else - typedef Rot3M Rot3; -#endif -} - -#endif // if !defined Rot3 - diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 9b0d93f1c..0cb533360 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -10,13 +10,15 @@ * -------------------------------------------------------------------------- */ /** - * @file Rot3M.cpp + * @file Rot3.cpp * @brief Rotation (internal: 3*3 matrix representation*) * @author Alireza Fathi * @author Christian Potthast * @author Frank Dellaert */ +#ifndef GTSAM_DEFAULT_QUATERNIONS + #include #include @@ -27,17 +29,17 @@ namespace gtsam { static const Matrix I3 = eye(3); /* ************************************************************************* */ -Rot3M::Rot3M() : +Rot3::Rot3() : r1_(Point3(1.0,0.0,0.0)), r2_(Point3(0.0,1.0,0.0)), r3_(Point3(0.0,0.0,1.0)) {} /* ************************************************************************* */ -Rot3M::Rot3M(const Point3& r1, const Point3& r2, const Point3& r3) : +Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : r1_(r1), r2_(r2), r3_(r3) {} /* ************************************************************************* */ -Rot3M::Rot3M(double R11, double R12, double R13, +Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33) : r1_(Point3(R11, R21, R31)), @@ -45,13 +47,13 @@ Rot3M::Rot3M(double R11, double R12, double R13, r3_(Point3(R13, R23, R33)) {} /* ************************************************************************* */ -Rot3M::Rot3M(const Matrix& R): +Rot3::Rot3(const Matrix& R): r1_(Point3(R(0,0), R(1,0), R(2,0))), r2_(Point3(R(0,1), R(1,1), R(2,1))), r3_(Point3(R(0,2), R(1,2), R(2,2))) {} /* ************************************************************************* */ -Rot3M::Rot3M(const Quaternion& q) { +Rot3::Rot3(const Quaternion& q) { Eigen::Matrix3d R = q.toRotationMatrix(); r1_ = Point3(R.col(0)); r2_ = Point3(R.col(1)); @@ -59,30 +61,27 @@ Rot3M::Rot3M(const Quaternion& q) { } /* ************************************************************************* */ -Rot3M::Rot3M(const Rot3M& r) : r1_(r.r1_), r2_(r.r2_), r3_(r.r3_) {} - -/* ************************************************************************* */ -Rot3M Rot3M::Rx(double t) { +Rot3 Rot3::Rx(double t) { double st = sin(t), ct = cos(t); - return Rot3M( + return Rot3( 1, 0, 0, 0, ct,-st, 0, st, ct); } /* ************************************************************************* */ -Rot3M Rot3M::Ry(double t) { +Rot3 Rot3::Ry(double t) { double st = sin(t), ct = cos(t); - return Rot3M( + return Rot3( ct, 0, st, 0, 1, 0, -st, 0, ct); } /* ************************************************************************* */ -Rot3M Rot3M::Rz(double t) { +Rot3 Rot3::Rz(double t) { double st = sin(t), ct = cos(t); - return Rot3M( + return Rot3( ct,-st, 0, st, ct, 0, 0, 0, 1); @@ -90,7 +89,7 @@ Rot3M Rot3M::Rz(double t) { /* ************************************************************************* */ // Considerably faster than composing matrices above ! -Rot3M Rot3M::RzRyRx(double x, double y, double z) { +Rot3 Rot3::RzRyRx(double x, double y, double z) { double cx=cos(x),sx=sin(x); double cy=cos(y),sy=sin(y); double cz=cos(z),sz=sin(z); @@ -105,7 +104,7 @@ Rot3M Rot3M::RzRyRx(double x, double y, double z) { double s_c = sx * cz; double c_c = cx * cz; double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; - return Rot3M( + return Rot3( _cc,- c_s + ssc, s_s + csc, _cs, c_c + sss, -s_c + css, -sy, sc_, cc_ @@ -113,7 +112,7 @@ Rot3M Rot3M::RzRyRx(double x, double y, double z) { } /* ************************************************************************* */ -Rot3M Rot3M::rodriguez(const Vector& w, double theta) { +Rot3 Rot3::rodriguez(const Vector& w, double theta) { // get components of axis \omega double wx = w(0), wy=w(1), wz=w(2); double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; @@ -129,26 +128,26 @@ Rot3M Rot3M::rodriguez(const Vector& w, double theta) { double C11 = c_1*wwTyy, C12 = c_1*wy*wz; double C22 = c_1*wwTzz; - return Rot3M( + return Rot3( c + C00, -swz + C01, swy + C02, swz + C01, c + C11, -swx + C12, -swy + C02, swx + C12, c + C22); } /* ************************************************************************* */ -Rot3M Rot3M::rodriguez(const Vector& w) { +Rot3 Rot3::rodriguez(const Vector& w) { double t = w.norm(); - if (t < 1e-10) return Rot3M(); + if (t < 1e-10) return Rot3(); return rodriguez(w/t, t); } /* ************************************************************************* */ -bool Rot3M::equals(const Rot3M & R, double tol) const { +bool Rot3::equals(const Rot3 & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); } /* ************************************************************************* */ -Rot3M Rot3M::compose (const Rot3M& R2, +Rot3 Rot3::compose (const Rot3& R2, boost::optional H1, boost::optional H2) const { if (H1) *H1 = R2.transpose(); if (H2) *H2 = I3; @@ -156,19 +155,19 @@ Rot3M Rot3M::compose (const Rot3M& R2, } /* ************************************************************************* */ -Point3 Rot3M::operator*(const Point3& p) const { return rotate(p); } +Point3 Rot3::operator*(const Point3& p) const { return rotate(p); } /* ************************************************************************* */ -Rot3M Rot3M::inverse(boost::optional H1) const { +Rot3 Rot3::inverse(boost::optional H1) const { if (H1) *H1 = -matrix(); - return Rot3M( + return Rot3( r1_.x(), r1_.y(), r1_.z(), r2_.x(), r2_.y(), r2_.z(), r3_.x(), r3_.y(), r3_.z()); } /* ************************************************************************* */ -Rot3M Rot3M::between (const Rot3M& R2, +Rot3 Rot3::between (const Rot3& R2, boost::optional H1, boost::optional H2) const { if (H1) *H1 = -(R2.transpose()*matrix()); if (H2) *H2 = I3; @@ -176,12 +175,12 @@ Rot3M Rot3M::between (const Rot3M& R2, } /* ************************************************************************* */ -Rot3M Rot3M::operator*(const Rot3M& R2) const { - return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); +Rot3 Rot3::operator*(const Rot3& R2) const { + return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); } /* ************************************************************************* */ -Point3 Rot3M::rotate(const Point3& p, +Point3 Rot3::rotate(const Point3& p, boost::optional H1, boost::optional H2) const { if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = matrix(); @@ -190,7 +189,7 @@ Point3 Rot3M::rotate(const Point3& p, /* ************************************************************************* */ // see doc/math.lyx, SO(3) section -Point3 Rot3M::unrotate(const Point3& p, +Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { const Matrix Rt(transpose()); Point3 q(Rt*p.vector()); // q = Rt*p @@ -201,7 +200,7 @@ Point3 Rot3M::unrotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector Rot3M::Logmap(const Rot3M& R) { +Vector Rot3::Logmap(const Rot3& R) { double tr = R.r1().x()+R.r2().y()+R.r3().z(); // FIXME should tr in statement below be absolute value? if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) @@ -234,7 +233,7 @@ Vector Rot3M::Logmap(const Rot3M& R) { } /* ************************************************************************* */ -Matrix Rot3M::matrix() const { +Matrix Rot3::matrix() const { Matrix R(3,3); R << r1_.x(), r2_.x(), r3_.x(), @@ -244,7 +243,7 @@ Matrix Rot3M::matrix() const { } /* ************************************************************************* */ -Matrix Rot3M::transpose() const { +Matrix Rot3::transpose() const { Matrix Rt(3,3); Rt << r1_.x(), r1_.y(), r1_.z(), @@ -254,7 +253,7 @@ Matrix Rot3M::transpose() const { } /* ************************************************************************* */ -Point3 Rot3M::column(int index) const{ +Point3 Rot3::column(int index) const{ if(index == 3) return r3_; else if(index == 2) @@ -266,35 +265,35 @@ Point3 Rot3M::column(int index) const{ } /* ************************************************************************* */ -Point3 Rot3M::r1() const { return r1_; } +Point3 Rot3::r1() const { return r1_; } /* ************************************************************************* */ -Point3 Rot3M::r2() const { return r2_; } +Point3 Rot3::r2() const { return r2_; } /* ************************************************************************* */ -Point3 Rot3M::r3() const { return r3_; } +Point3 Rot3::r3() const { return r3_; } /* ************************************************************************* */ -Vector Rot3M::xyz() const { +Vector Rot3::xyz() const { Matrix I;Vector q; boost::tie(I,q)=RQ(matrix()); return q; } /* ************************************************************************* */ -Vector Rot3M::ypr() const { +Vector Rot3::ypr() const { Vector q = xyz(); return Vector_(3,q(2),q(1),q(0)); } /* ************************************************************************* */ -Vector Rot3M::rpy() const { +Vector Rot3::rpy() const { Vector q = xyz(); return Vector_(3,q(0),q(1),q(2)); } /* ************************************************************************* */ -Quaternion Rot3M::toQuaternion() const { +Quaternion Rot3::toQuaternion() const { return Quaternion((Eigen::Matrix3d() << r1_.x(), r2_.x(), r3_.x(), r1_.y(), r2_.y(), r3_.y(), @@ -305,15 +304,15 @@ Quaternion Rot3M::toQuaternion() const { pair RQ(const Matrix& A) { double x = -atan2(-A(2, 1), A(2, 2)); - Rot3M Qx = Rot3M::Rx(-x); + Rot3 Qx = Rot3::Rx(-x); Matrix B = A * Qx.matrix(); double y = -atan2(B(2, 0), B(2, 2)); - Rot3M Qy = Rot3M::Ry(-y); + Rot3 Qy = Rot3::Ry(-y); Matrix C = B * Qy.matrix(); double z = -atan2(-C(1, 0), C(1, 1)); - Rot3M Qz = Rot3M::Rz(-z); + Rot3 Qz = Rot3::Rz(-z); Matrix R = C * Qz.matrix(); Vector xyz = Vector_(3, x, y, z); @@ -323,3 +322,5 @@ pair RQ(const Matrix& A) { /* ************************************************************************* */ } // namespace gtsam + +#endif diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 082a8c403..6e45b191d 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -10,13 +10,15 @@ * -------------------------------------------------------------------------- */ /** - * @file Rot3Q.cpp + * @file Rot3.cpp * @brief Rotation (internal: 3*3 matrix representation*) * @author Alireza Fathi * @author Christian Potthast * @author Frank Dellaert */ +#ifdef GTSAM_DEFAULT_QUATERNIONS + #include #include @@ -27,17 +29,17 @@ namespace gtsam { static const Matrix I3 = eye(3); /* ************************************************************************* */ - Rot3Q::Rot3Q() : quaternion_(Quaternion::Identity()) {} + Rot3::Rot3() : quaternion_(Quaternion::Identity()) {} /* ************************************************************************* */ - Rot3Q::Rot3Q(const Point3& r1, const Point3& r2, const Point3& r3) : + Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : quaternion_((Eigen::Matrix3d() << r1.x(), r2.x(), r3.x(), r1.y(), r2.y(), r3.y(), r1.z(), r2.z(), r3.z()).finished()) {} /* ************************************************************************* */ - Rot3Q::Rot3Q(double R11, double R12, double R13, + Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33) : quaternion_((Eigen::Matrix3d() << @@ -46,69 +48,66 @@ namespace gtsam { R31, R32, R33).finished()) {} /* ************************************************************************* */ - Rot3Q::Rot3Q(const Matrix& R) : + Rot3::Rot3(const Matrix& R) : quaternion_(Eigen::Matrix3d(R)) {} /* ************************************************************************* */ - Rot3Q::Rot3Q(const Quaternion& q) : quaternion_(q) {} + Rot3::Rot3(const Quaternion& q) : quaternion_(q) {} /* ************************************************************************* */ - Rot3Q::Rot3Q(const Rot3M& r) : quaternion_(Eigen::Matrix3d(r.matrix())) {} + Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } /* ************************************************************************* */ - Rot3Q Rot3Q::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } + Rot3 Rot3::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } /* ************************************************************************* */ - Rot3Q Rot3Q::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } + Rot3 Rot3::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } /* ************************************************************************* */ - Rot3Q Rot3Q::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } - - /* ************************************************************************* */ - Rot3Q Rot3Q::RzRyRx(double x, double y, double z) { return Rot3Q( + Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } /* ************************************************************************* */ - Rot3Q Rot3Q::rodriguez(const Vector& w, double theta) { + Rot3 Rot3::rodriguez(const Vector& w, double theta) { return Quaternion(Eigen::AngleAxisd(theta, w)); } /* ************************************************************************* */ - Rot3Q Rot3Q::rodriguez(const Vector& w) { + Rot3 Rot3::rodriguez(const Vector& w) { double t = w.norm(); - if (t < 1e-10) return Rot3Q(); + if (t < 1e-10) return Rot3(); return rodriguez(w/t, t); } /* ************************************************************************* */ - bool Rot3Q::equals(const Rot3Q & R, double tol) const { + bool Rot3::equals(const Rot3 & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); } /* ************************************************************************* */ - Rot3Q Rot3Q::compose(const Rot3Q& R2, + Rot3 Rot3::compose(const Rot3& R2, boost::optional H1, boost::optional H2) const { if (H1) *H1 = R2.transpose(); if (H2) *H2 = I3; - return Rot3Q(quaternion_ * R2.quaternion_); + return Rot3(quaternion_ * R2.quaternion_); } /* ************************************************************************* */ - Point3 Rot3Q::operator*(const Point3& p) const { + Point3 Rot3::operator*(const Point3& p) const { Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z()); return Point3(r(0), r(1), r(2)); } /* ************************************************************************* */ - Rot3Q Rot3Q::inverse(boost::optional H1) const { + Rot3 Rot3::inverse(boost::optional H1) const { if (H1) *H1 = -matrix(); - return Rot3Q(quaternion_.inverse()); + return Rot3(quaternion_.inverse()); } /* ************************************************************************* */ - Rot3Q Rot3Q::between(const Rot3Q& R2, + Rot3 Rot3::between(const Rot3& R2, boost::optional H1, boost::optional H2) const { if (H1) *H1 = -(R2.transpose()*matrix()); if (H2) *H2 = I3; @@ -116,12 +115,12 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3Q Rot3Q::operator*(const Rot3Q& R2) const { - return Rot3Q(quaternion_ * R2.quaternion_); + Rot3 Rot3::operator*(const Rot3& R2) const { + return Rot3(quaternion_ * R2.quaternion_); } /* ************************************************************************* */ - Point3 Rot3Q::rotate(const Point3& p, + Point3 Rot3::rotate(const Point3& p, boost::optional H1, boost::optional H2) const { Matrix R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); @@ -132,7 +131,7 @@ namespace gtsam { /* ************************************************************************* */ // see doc/math.lyx, SO(3) section - Point3 Rot3Q::unrotate(const Point3& p, + Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { const Matrix Rt(transpose()); Point3 q(Rt*p.vector()); // q = Rt*p @@ -143,7 +142,7 @@ namespace gtsam { /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation - Vector Rot3Q::Logmap(const Rot3Q& R) { + Vector Rot3::Logmap(const Rot3& R) { Eigen::AngleAxisd angleAxis(R.quaternion_); if(angleAxis.angle() > M_PI) // Important: use the smallest possible angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep @@ -153,13 +152,13 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix Rot3Q::matrix() const { return quaternion_.toRotationMatrix(); } + Matrix Rot3::matrix() const { return quaternion_.toRotationMatrix(); } /* ************************************************************************* */ - Matrix Rot3Q::transpose() const { return quaternion_.toRotationMatrix().transpose(); } + Matrix Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); } /* ************************************************************************* */ - Point3 Rot3Q::column(int index) const{ + Point3 Rot3::column(int index) const{ if(index == 3) return r3(); else if(index == 2) @@ -171,36 +170,55 @@ namespace gtsam { } /* ************************************************************************* */ - Point3 Rot3Q::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } + Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } /* ************************************************************************* */ - Point3 Rot3Q::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); } + Point3 Rot3::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); } /* ************************************************************************* */ - Point3 Rot3Q::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } + Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } /* ************************************************************************* */ - Vector Rot3Q::xyz() const { + Vector Rot3::xyz() const { Matrix I;Vector q; boost::tie(I,q)=RQ(matrix()); return q; } /* ************************************************************************* */ - Vector Rot3Q::ypr() const { + Vector Rot3::ypr() const { Vector q = xyz(); return Vector_(3,q(2),q(1),q(0)); } /* ************************************************************************* */ - Vector Rot3Q::rpy() const { + Vector Rot3::rpy() const { Vector q = xyz(); return Vector_(3,q(0),q(1),q(2)); } /* ************************************************************************* */ - Quaternion Rot3Q::toQuaternion() const { return quaternion_; } + Quaternion Rot3::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ + pair RQ(const Matrix& A) { + + double x = -atan2(-A(2, 1), A(2, 2)); + Rot3 Qx = Rot3::Rx(-x); + Matrix B = A * Qx.matrix(); + + double y = -atan2(B(2, 0), B(2, 2)); + Rot3 Qy = Rot3::Ry(-y); + Matrix C = B * Qy.matrix(); + + double z = -atan2(-C(1, 0), C(1, 1)); + Rot3 Qz = Rot3::Rz(-z); + Matrix R = C * Qz.matrix(); + + Vector xyz = Vector_(3, x, y, z); + return make_pair(R, xyz); + } } // namespace gtsam + +#endif diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 0b1f1a31b..0e3358ce6 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -11,7 +11,7 @@ /** * @file testRot3.cpp - * @brief Unit tests for Rot3M class + * @brief Unit tests for Rot3 class * @author Alireza Fathi */ @@ -23,16 +23,18 @@ #include #include +#ifndef GTSAM_DEFAULT_QUATERNIONS + using namespace gtsam; -Rot3M R = Rot3M::rodriguez(0.1, 0.4, 0.2); +Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); Point3 P(0.2, 0.7, -2.0); double error = 1e-9, epsilon = 0.001; /* ************************************************************************* */ -TEST( Rot3M, constructor) +TEST( Rot3, constructor) { - Rot3M expected(eye(3, 3)); + Rot3 expected(eye(3, 3)); Vector r1(3), r2(3), r3(3); r1(0) = 1; r1(1) = 0; @@ -43,91 +45,91 @@ TEST( Rot3M, constructor) r3(0) = 0; r3(1) = 0; r3(2) = 1; - Rot3M actual(r1, r2, r3); + Rot3 actual(r1, r2, r3); CHECK(assert_equal(actual,expected)); } /* ************************************************************************* */ -TEST( Rot3M, constructor2) +TEST( Rot3, constructor2) { Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.); - Rot3M actual(R); - Rot3M expected(11, 12, 13, 21, 22, 23, 31, 32, 33); + Rot3 actual(R); + Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33); CHECK(assert_equal(actual,expected)); } /* ************************************************************************* */ -TEST( Rot3M, constructor3) +TEST( Rot3, constructor3) { - Rot3M expected(1, 2, 3, 4, 5, 6, 7, 8, 9); + Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9); Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9); - CHECK(assert_equal(Rot3M(r1,r2,r3),expected)); + CHECK(assert_equal(Rot3(r1,r2,r3),expected)); } /* ************************************************************************* */ -TEST( Rot3M, transpose) +TEST( Rot3, transpose) { - Rot3M R(1, 2, 3, 4, 5, 6, 7, 8, 9); + Rot3 R(1, 2, 3, 4, 5, 6, 7, 8, 9); Point3 r1(1, 2, 3), r2(4, 5, 6), r3(7, 8, 9); - CHECK(assert_equal(R.inverse(),Rot3M(r1,r2,r3))); + CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3))); } /* ************************************************************************* */ -TEST( Rot3M, equals) +TEST( Rot3, equals) { CHECK(R.equals(R)); - Rot3M zero; + Rot3 zero; CHECK(!R.equals(zero)); } /* ************************************************************************* */ // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... -Rot3M slow_but_correct_rodriguez(const Vector& w) { +Rot3 slow_but_correct_rodriguez(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); - if (t < 1e-5) return Rot3M(); + if (t < 1e-5) return Rot3(); Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J); return R; } /* ************************************************************************* */ -TEST( Rot3M, rodriguez) +TEST( Rot3, rodriguez) { - Rot3M R1 = Rot3M::rodriguez(epsilon, 0, 0); + Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); Vector w = Vector_(3, epsilon, 0., 0.); - Rot3M R2 = slow_but_correct_rodriguez(w); + Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3M, rodriguez2) +TEST( Rot3, rodriguez2) { Vector axis = Vector_(3,0.,1.,0.); // rotation around Y double angle = 3.14 / 4.0; - Rot3M actual = Rot3M::rodriguez(axis, angle); - Rot3M expected(0.707388, 0, 0.706825, + Rot3 actual = Rot3::rodriguez(axis, angle); + Rot3 expected(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388); CHECK(assert_equal(expected,actual,1e-5)); } /* ************************************************************************* */ -TEST( Rot3M, rodriguez3) +TEST( Rot3, rodriguez3) { Vector w = Vector_(3, 0.1, 0.2, 0.3); - Rot3M R1 = Rot3M::rodriguez(w / norm_2(w), norm_2(w)); - Rot3M R2 = slow_but_correct_rodriguez(w); + Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); + Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3M, rodriguez4) +TEST( Rot3, rodriguez4) { Vector axis = Vector_(3,0.,0.,1.); // rotation around Z double angle = M_PI_2; - Rot3M actual = Rot3M::rodriguez(axis, angle); + Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); - Rot3M expected(c,-s, 0, + Rot3 expected(c,-s, 0, s, c, 0, 0, 0, 1); CHECK(assert_equal(expected,actual,1e-5)); @@ -135,62 +137,62 @@ TEST( Rot3M, rodriguez4) } /* ************************************************************************* */ -TEST( Rot3M, expmap) +TEST( Rot3, expmap) { Vector v = zero(3); CHECK(assert_equal(R.retract(v), R)); } /* ************************************************************************* */ -TEST(Rot3M, log) +TEST(Rot3, log) { Vector w1 = Vector_(3, 0.1, 0.0, 0.0); - Rot3M R1 = Rot3M::rodriguez(w1); - CHECK(assert_equal(w1, Rot3M::Logmap(R1))); + Rot3 R1 = Rot3::rodriguez(w1); + CHECK(assert_equal(w1, Rot3::Logmap(R1))); Vector w2 = Vector_(3, 0.0, 0.1, 0.0); - Rot3M R2 = Rot3M::rodriguez(w2); - CHECK(assert_equal(w2, Rot3M::Logmap(R2))); + Rot3 R2 = Rot3::rodriguez(w2); + CHECK(assert_equal(w2, Rot3::Logmap(R2))); Vector w3 = Vector_(3, 0.0, 0.0, 0.1); - Rot3M R3 = Rot3M::rodriguez(w3); - CHECK(assert_equal(w3, Rot3M::Logmap(R3))); + Rot3 R3 = Rot3::rodriguez(w3); + CHECK(assert_equal(w3, Rot3::Logmap(R3))); Vector w = Vector_(3, 0.1, 0.4, 0.2); - Rot3M R = Rot3M::rodriguez(w); - CHECK(assert_equal(w, Rot3M::Logmap(R))); + Rot3 R = Rot3::rodriguez(w); + CHECK(assert_equal(w, Rot3::Logmap(R))); Vector w5 = Vector_(3, 0.0, 0.0, 0.0); - Rot3M R5 = Rot3M::rodriguez(w5); - CHECK(assert_equal(w5, Rot3M::Logmap(R5))); + Rot3 R5 = Rot3::rodriguez(w5); + CHECK(assert_equal(w5, Rot3::Logmap(R5))); Vector w6 = Vector_(3, boost::math::constants::pi(), 0.0, 0.0); - Rot3M R6 = Rot3M::rodriguez(w6); - CHECK(assert_equal(w6, Rot3M::Logmap(R6))); + Rot3 R6 = Rot3::rodriguez(w6); + CHECK(assert_equal(w6, Rot3::Logmap(R6))); Vector w7 = Vector_(3, 0.0, boost::math::constants::pi(), 0.0); - Rot3M R7 = Rot3M::rodriguez(w7); - CHECK(assert_equal(w7, Rot3M::Logmap(R7))); + Rot3 R7 = Rot3::rodriguez(w7); + CHECK(assert_equal(w7, Rot3::Logmap(R7))); Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi()); - Rot3M R8 = Rot3M::rodriguez(w8); - CHECK(assert_equal(w8, Rot3M::Logmap(R8))); + Rot3 R8 = Rot3::rodriguez(w8); + CHECK(assert_equal(w8, Rot3::Logmap(R8))); } /* ************************************************************************* */ -TEST(Rot3M, manifold) +TEST(Rot3, manifold) { - Rot3M gR1 = Rot3M::rodriguez(0.1, 0.4, 0.2); - Rot3M gR2 = Rot3M::rodriguez(0.3, 0.1, 0.7); - Rot3M origin; + Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); + Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); + Rot3 origin; // log behaves correctly Vector d12 = gR1.localCoordinates(gR2); CHECK(assert_equal(gR2, gR1.retract(d12))); - CHECK(assert_equal(gR2, gR1*Rot3M::Expmap(d12))); + CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); Vector d21 = gR2.localCoordinates(gR1); CHECK(assert_equal(gR1, gR2.retract(d21))); - CHECK(assert_equal(gR1, gR2*Rot3M::Expmap(d21))); + CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); // Check that log(t1,t2)=-log(t2,t1) CHECK(assert_equal(d12,-d21)); @@ -198,11 +200,11 @@ TEST(Rot3M, manifold) // lines in canonical coordinates correspond to Abelian subgroups in SO(3) Vector d = Vector_(3, 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) - CHECK(assert_equal(Rot3M::Expmap(-d),Rot3M::Expmap(d).inverse())); + CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Rot3M R2 = Rot3M::Expmap (2 * d); - Rot3M R3 = Rot3M::Expmap (3 * d); - Rot3M R5 = Rot3M::Expmap (5 * d); + Rot3 R2 = Rot3::Expmap (2 * d); + Rot3 R3 = Rot3::Expmap (3 * d); + Rot3 R5 = Rot3::Expmap (5 * d); CHECK(assert_equal(R5,R2*R3)); CHECK(assert_equal(R5,R3*R2)); } @@ -223,106 +225,106 @@ AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { } /* ************************************************************************* */ -TEST(Rot3M, BCH) +TEST(Rot3, BCH) { // Approximate exmap by BCH formula AngularVelocity w1(0.2, -0.1, 0.1); AngularVelocity w2(0.01, 0.02, -0.03); - Rot3M R1 = Rot3M::Expmap (w1.vector()), R2 = Rot3M::Expmap (w2.vector()); - Rot3M R3 = R1 * R2; - Vector expected = Rot3M::Logmap(R3); + Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector()); + Rot3 R3 = R1 * R2; + Vector expected = Rot3::Logmap(R3); Vector actual = BCH(w1, w2).vector(); CHECK(assert_equal(expected, actual,1e-5)); } /* ************************************************************************* */ -TEST( Rot3M, rotate_derivatives) +TEST( Rot3, rotate_derivatives) { Matrix actualDrotate1a, actualDrotate1b, actualDrotate2; R.rotate(P, actualDrotate1a, actualDrotate2); R.inverse().rotate(P, actualDrotate1b, boost::none); - Matrix numerical1 = numericalDerivative21(testing::rotate, R, P); - Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); - Matrix numerical3 = numericalDerivative22(testing::rotate, R, P); + Matrix numerical1 = numericalDerivative21(testing::rotate, R, P); + Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); + Matrix numerical3 = numericalDerivative22(testing::rotate, R, P); EXPECT(assert_equal(numerical1,actualDrotate1a,error)); EXPECT(assert_equal(numerical2,actualDrotate1b,error)); EXPECT(assert_equal(numerical3,actualDrotate2, error)); } /* ************************************************************************* */ -TEST( Rot3M, unrotate) +TEST( Rot3, unrotate) { Point3 w = R * P; Matrix H1,H2; Point3 actual = R.unrotate(w,H1,H2); CHECK(assert_equal(P,actual)); - Matrix numerical1 = numericalDerivative21(testing::unrotate, R, w); + Matrix numerical1 = numericalDerivative21(testing::unrotate, R, w); CHECK(assert_equal(numerical1,H1,error)); - Matrix numerical2 = numericalDerivative22(testing::unrotate, R, w); + Matrix numerical2 = numericalDerivative22(testing::unrotate, R, w); CHECK(assert_equal(numerical2,H2,error)); } /* ************************************************************************* */ -TEST( Rot3M, compose ) +TEST( Rot3, compose ) { - Rot3M R1 = Rot3M::rodriguez(0.1, 0.2, 0.3); - Rot3M R2 = Rot3M::rodriguez(0.2, 0.3, 0.5); + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); - Rot3M expected = R1 * R2; + Rot3 expected = R1 * R2; Matrix actualH1, actualH2; - Rot3M actual = R1.compose(R2, actualH1, actualH2); + Rot3 actual = R1.compose(R2, actualH1, actualH2); CHECK(assert_equal(expected,actual)); - Matrix numericalH1 = numericalDerivative21(testing::compose, R1, + Matrix numericalH1 = numericalDerivative21(testing::compose, R1, R2, 1e-2); CHECK(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(testing::compose, R1, + Matrix numericalH2 = numericalDerivative22(testing::compose, R1, R2, 1e-2); CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ -TEST( Rot3M, inverse ) +TEST( Rot3, inverse ) { - Rot3M R = Rot3M::rodriguez(0.1, 0.2, 0.3); + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3M I; + Rot3 I; Matrix actualH; CHECK(assert_equal(I,R*R.inverse(actualH))); CHECK(assert_equal(I,R.inverse()*R)); - Matrix numericalH = numericalDerivative11(testing::inverse, R); + Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); } /* ************************************************************************* */ -TEST( Rot3M, between ) +TEST( Rot3, between ) { - Rot3M R = Rot3M::rodriguez(0.1, 0.4, 0.2); - Rot3M origin; + Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); + Rot3 origin; CHECK(assert_equal(R, origin.between(R))); CHECK(assert_equal(R.inverse(), R.between(origin))); - Rot3M R1 = Rot3M::rodriguez(0.1, 0.2, 0.3); - Rot3M R2 = Rot3M::rodriguez(0.2, 0.3, 0.5); + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); - Rot3M expected = R1.inverse() * R2; + Rot3 expected = R1.inverse() * R2; Matrix actualH1, actualH2; - Rot3M actual = R1.between(R2, actualH1, actualH2); + Rot3 actual = R1.between(R2, actualH1, actualH2); CHECK(assert_equal(expected,actual)); - Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); + Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); CHECK(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); + Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ -TEST( Rot3M, xyz ) +TEST( Rot3, xyz ) { double t = 0.1, st = sin(t), ct = cos(t); @@ -332,47 +334,47 @@ TEST( Rot3M, xyz ) // z // | * Y=(ct,st) // x----y - Rot3M expected1(1, 0, 0, 0, ct, -st, 0, st, ct); - CHECK(assert_equal(expected1,Rot3M::Rx(t))); + Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct); + CHECK(assert_equal(expected1,Rot3::Rx(t))); // x // | * Z=(ct,st) // y----z - Rot3M expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); - CHECK(assert_equal(expected2,Rot3M::Ry(t))); + Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); + CHECK(assert_equal(expected2,Rot3::Ry(t))); // y // | X=* (ct,st) // z----x - Rot3M expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); - CHECK(assert_equal(expected3,Rot3M::Rz(t))); + Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); + CHECK(assert_equal(expected3,Rot3::Rz(t))); // Check compound rotation - Rot3M expected = Rot3M::Rz(0.3) * Rot3M::Ry(0.2) * Rot3M::Rx(0.1); - CHECK(assert_equal(expected,Rot3M::RzRyRx(0.1,0.2,0.3))); + Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1); + CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3))); } /* ************************************************************************* */ -TEST( Rot3M, yaw_pitch_roll ) +TEST( Rot3, yaw_pitch_roll ) { double t = 0.1; // yaw is around z axis - CHECK(assert_equal(Rot3M::Rz(t),Rot3M::yaw(t))); + CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); // pitch is around y axis - CHECK(assert_equal(Rot3M::Ry(t),Rot3M::pitch(t))); + CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); // roll is around x axis - CHECK(assert_equal(Rot3M::Rx(t),Rot3M::roll(t))); + CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); // Check compound rotation - Rot3M expected = Rot3M::yaw(0.1) * Rot3M::pitch(0.2) * Rot3M::roll(0.3); - CHECK(assert_equal(expected,Rot3M::ypr(0.1,0.2,0.3))); + Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); + CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); } /* ************************************************************************* */ -TEST( Rot3M, RQ) +TEST( Rot3, RQ) { // Try RQ on a pure rotation Matrix actualK; @@ -382,18 +384,18 @@ TEST( Rot3M, RQ) CHECK(assert_equal(eye(3),actualK)); CHECK(assert_equal(expected,actual,1e-6)); - // Try using xyz call, asserting that Rot3M::RzRyRx(x,y,z).xyz()==[x;y;z] + // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3M::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); - // Try using ypr call, asserting that Rot3M::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3M::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3M::ypr(0.1,0.2,0.3).rpy())); + // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] + CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3M::yaw (0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3M::pitch(0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3M::roll (0.1).ypr())); + CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); @@ -404,60 +406,60 @@ TEST( Rot3M, RQ) } /* ************************************************************************* */ -TEST( Rot3M, expmapStability ) { +TEST( Rot3, expmapStability ) { Vector w = Vector_(3, 78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; - Rot3M actualR = Rot3M::Expmap(w); + Rot3 actualR = Rot3::Expmap(w); Matrix W = Matrix_(3,3, 0.0, -w(2), w(1), w(2), 0.0, -w(0), -w(1), w(0), 0.0 ); Matrix W2 = W*W; Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0 - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; - Rot3M expectedR( Rmat ); + Rot3 expectedR( Rmat ); CHECK(assert_equal(expectedR, actualR, 1e-10)); } /* ************************************************************************* */ -TEST( Rot3M, logmapStability ) { +TEST( Rot3, logmapStability ) { Vector w = Vector_(3, 1e-8, 0.0, 0.0); - Rot3M R = Rot3M::Expmap(w); + Rot3 R = Rot3::Expmap(w); // double tr = R.r1().x()+R.r2().y()+R.r3().z(); // std::cout.precision(5000); // std::cout << "theta: " << w.norm() << std::endl; // std::cout << "trace: " << tr << std::endl; // R.print("R = "); - Vector actualw = Rot3M::Logmap(R); + Vector actualw = Rot3::Logmap(R); CHECK(assert_equal(w, actualw, 1e-15)); } /* ************************************************************************* */ -TEST(Rot3M, quaternion) { +TEST(Rot3, quaternion) { // NOTE: This is also verifying the ability to convert Vector to Quaternion Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); - Rot3M R1 = Rot3M(Matrix_(3,3, + Rot3 R1 = Rot3(Matrix_(3,3, 0.271018623057411, 0.278786459830371, 0.921318086098018, 0.578529366719085, 0.717799701969298, -0.387385285854279, -0.769319620053772, 0.637998195662053, 0.033250932803219)); Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); - Rot3M R2 = Rot3M(Matrix_(3,3, + Rot3 R2 = Rot3(Matrix_(3,3, -0.207341903877828, 0.250149415542075, 0.945745528564780, 0.881304914479026, -0.371869043667957, 0.291573424846290, 0.424630407073532, 0.893945571198514, -0.143353873763946)); - // Check creating Rot3M from quaternion - EXPECT(assert_equal(R1, Rot3M(q1))); - EXPECT(assert_equal(R1, Rot3M::quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); - EXPECT(assert_equal(R2, Rot3M(q2))); - EXPECT(assert_equal(R2, Rot3M::quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); + // Check creating Rot3 from quaternion + EXPECT(assert_equal(R1, Rot3(q1))); + EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); + EXPECT(assert_equal(R2, Rot3(q2))); + EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); - // Check converting Rot3M to quaterion + // Check converting Rot3 to quaterion EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs()))); EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs()))); - // Check that quaternion and Rot3M represent the same rotation + // Check that quaternion and Rot3 represent the same rotation Point3 p1(1.0, 2.0, 3.0); Point3 p2(8.0, 7.0, 9.0); @@ -471,6 +473,8 @@ TEST(Rot3M, quaternion) { EXPECT(assert_equal(expected2, actual2)); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index 4619e5560..02056eb2d 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -11,7 +11,7 @@ /** * @file testRot3.cpp - * @brief Unit tests for Rot3Q class + * @brief Unit tests for Rot3 class * @author Alireza Fathi */ @@ -23,16 +23,18 @@ #include #include +#ifdef GTSAM_DEFAULT_QUATERNIONS + using namespace gtsam; -Rot3Q R = Rot3Q::rodriguez(0.1, 0.4, 0.2); +Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); Point3 P(0.2, 0.7, -2.0); double error = 1e-9, epsilon = 0.001; /* ************************************************************************* */ -TEST( Rot3Q, constructor) +TEST( Rot3, constructor) { - Rot3Q expected(eye(3, 3)); + Rot3 expected(eye(3, 3)); Vector r1(3), r2(3), r3(3); r1(0) = 1; r1(1) = 0; @@ -43,178 +45,146 @@ TEST( Rot3Q, constructor) r3(0) = 0; r3(1) = 0; r3(2) = 1; - Rot3Q actual(r1, r2, r3); + Rot3 actual(r1, r2, r3); CHECK(assert_equal(actual,expected)); } /* ************************************************************************* */ -TEST( Rot3Q, constructor2) +TEST( Rot3, constructor2) { Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.); - Rot3Q actual(R); - Rot3Q expected(11, 12, 13, 21, 22, 23, 31, 32, 33); + Rot3 actual(R); + Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33); CHECK(assert_equal(actual,expected)); } /* ************************************************************************* */ -TEST( Rot3Q, constructor3) +TEST( Rot3, constructor3) { - Rot3Q expected(1, 2, 3, 4, 5, 6, 7, 8, 9); + Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9); Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9); - CHECK(assert_equal(Rot3Q(r1,r2,r3),expected)); + CHECK(assert_equal(Rot3(r1,r2,r3),expected)); } /* ************************************************************************* */ -TEST( Rot3Q, equals) +TEST( Rot3, equals) { CHECK(R.equals(R)); - Rot3Q zero; + Rot3 zero; CHECK(!R.equals(zero)); } /* ************************************************************************* */ // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... -Rot3Q slow_but_correct_rodriguez(const Vector& w) { +Rot3 slow_but_correct_rodriguez(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); - if (t < 1e-5) return Rot3Q(); + if (t < 1e-5) return Rot3(); Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J); return R; } /* ************************************************************************* */ -TEST( Rot3Q, rodriguez) +TEST( Rot3, rodriguez) { - Rot3Q R1 = Rot3Q::rodriguez(epsilon, 0, 0); + Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); Vector w = Vector_(3, epsilon, 0., 0.); - Rot3Q R2 = slow_but_correct_rodriguez(w); - Rot3Q expected2(Rot3M::rodriguez(epsilon, 0, 0)); - CHECK(assert_equal(expected2,R1,1e-5)); + Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3Q, rodriguez2) +TEST( Rot3, rodriguez2) { Vector axis = Vector_(3,0.,1.,0.); // rotation around Y double angle = 3.14 / 4.0; - Rot3Q actual = Rot3Q::rodriguez(axis, angle); - Rot3Q expected(0.707388, 0, 0.706825, + Rot3 actual = Rot3::rodriguez(axis, angle); + Rot3 expected(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388); - Rot3Q expected2(Rot3M::rodriguez(axis, angle)); CHECK(assert_equal(expected,actual,1e-5)); - CHECK(assert_equal(expected2,actual,1e-5)); } /* ************************************************************************* */ -TEST( Rot3Q, rodriguez3) +TEST( Rot3, rodriguez3) { Vector w = Vector_(3, 0.1, 0.2, 0.3); - Rot3Q R1 = Rot3Q::rodriguez(w / norm_2(w), norm_2(w)); - Rot3Q R2 = slow_but_correct_rodriguez(w); - Rot3Q expected2(Rot3M::rodriguez(w / norm_2(w), norm_2(w))); - CHECK(assert_equal(expected2,R1)); + Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); + Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3Q, rodriguez4) +TEST( Rot3, rodriguez4) { Vector axis = Vector_(3,0.,0.,1.); // rotation around Z double angle = M_PI_2; - Rot3Q actual = Rot3Q::rodriguez(axis, angle); + Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); - Rot3Q expected1(c,-s, 0, + Rot3 expected(c,-s, 0, s, c, 0, 0, 0, 1); - Rot3Q expected2(Rot3M::rodriguez(axis, angle)); - CHECK(assert_equal(expected1,actual,1e-5)); - CHECK(assert_equal(expected2,actual,1e-5)); + CHECK(assert_equal(expected,actual,1e-5)); CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5)); } /* ************************************************************************* */ -TEST( Rot3Q, expmap) +TEST( Rot3, expmap) { Vector v = zero(3); CHECK(assert_equal(R.retract(v), R)); } /* ************************************************************************* */ -TEST(Rot3Q, log) +TEST(Rot3, log) { Vector w1 = Vector_(3, 0.1, 0.0, 0.0); - Rot3Q R1 = Rot3Q::rodriguez(w1); - Rot3Q R1m = Rot3M::rodriguez(w1); - CHECK(assert_equal(w1, Rot3Q::Logmap(R1))); - CHECK(assert_equal(R1m, R1)); + Rot3 R1 = Rot3::rodriguez(w1); + CHECK(assert_equal(w1, Rot3::Logmap(R1))); Vector w2 = Vector_(3, 0.0, 0.1, 0.0); - Rot3Q R2 = Rot3Q::rodriguez(w2); - Rot3Q R2m = Rot3M::rodriguez(w2); - CHECK(assert_equal(w2, Rot3Q::Logmap(R2))); - CHECK(assert_equal(R2m, R2)); + Rot3 R2 = Rot3::rodriguez(w2); + CHECK(assert_equal(w2, Rot3::Logmap(R2))); Vector w3 = Vector_(3, 0.0, 0.0, 0.1); - Rot3Q R3 = Rot3Q::rodriguez(w3); - Rot3Q R3m = Rot3M::rodriguez(w3); - CHECK(assert_equal(w3, Rot3Q::Logmap(R3))); - CHECK(assert_equal(R3m, R3)); + Rot3 R3 = Rot3::rodriguez(w3); + CHECK(assert_equal(w3, Rot3::Logmap(R3))); Vector w = Vector_(3, 0.1, 0.4, 0.2); - Rot3Q R = Rot3Q::rodriguez(w); - Rot3Q Rm = Rot3M::rodriguez(w); - CHECK(assert_equal(w, Rot3Q::Logmap(R))); - CHECK(assert_equal(Rm, R)); + Rot3 R = Rot3::rodriguez(w); + CHECK(assert_equal(w, Rot3::Logmap(R))); Vector w5 = Vector_(3, 0.0, 0.0, 0.0); - Rot3Q R5 = Rot3Q::rodriguez(w5); - Rot3Q R5m = Rot3M::rodriguez(w5); - CHECK(assert_equal(w5, Rot3Q::Logmap(R5))); - CHECK(assert_equal(R5m, R5)); + Rot3 R5 = Rot3::rodriguez(w5); + CHECK(assert_equal(w5, Rot3::Logmap(R5))); Vector w6 = Vector_(3, boost::math::constants::pi(), 0.0, 0.0); - Rot3Q R6 = Rot3Q::rodriguez(w6); - Rot3Q R6m = Rot3M::rodriguez(w6); - CHECK(assert_equal(w6, Rot3Q::Logmap(R6))); - CHECK(assert_equal(R6m, R6)); + Rot3 R6 = Rot3::rodriguez(w6); + CHECK(assert_equal(w6, Rot3::Logmap(R6))); Vector w7 = Vector_(3, 0.0, boost::math::constants::pi(), 0.0); - Rot3Q R7 = Rot3Q::rodriguez(w7); - Rot3Q R7m = Rot3M::rodriguez(w7); - CHECK(assert_equal(w7, Rot3Q::Logmap(R7))); - CHECK(assert_equal(R7m, R7)); + Rot3 R7 = Rot3::rodriguez(w7); + CHECK(assert_equal(w7, Rot3::Logmap(R7))); Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi()); - Rot3Q R8 = Rot3Q::rodriguez(w8); - Rot3Q R8m = Rot3M::rodriguez(w8); - CHECK(assert_equal(w8, Rot3Q::Logmap(R8))); - CHECK(assert_equal(R8m, R8)); + Rot3 R8 = Rot3::rodriguez(w8); + CHECK(assert_equal(w8, Rot3::Logmap(R8))); } /* ************************************************************************* */ -TEST(Rot3Q, manifold) +TEST(Rot3, manifold) { - Rot3Q gR1 = Rot3Q::rodriguez(0.1, 0.4, 0.2); - Rot3Q gR2 = Rot3Q::rodriguez(0.3, 0.1, 0.7); - Rot3Q origin; - - Rot3M gR1m = Rot3M::rodriguez(0.1, 0.4, 0.2); - Rot3M gR2m = Rot3M::rodriguez(0.3, 0.1, 0.7); - - EXPECT(assert_equal(gR1m.matrix(), gR1.matrix())); - EXPECT(assert_equal(gR2m.matrix(), gR2.matrix())); + Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); + Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); + Rot3 origin; // log behaves correctly Vector d12 = gR1.localCoordinates(gR2); - EXPECT(assert_equal(gR1m.localCoordinates(gR2m), d12)); CHECK(assert_equal(gR2, gR1.retract(d12))); - CHECK(assert_equal(gR2, gR1*Rot3Q::Expmap(d12))); + CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); Vector d21 = gR2.localCoordinates(gR1); - EXPECT(assert_equal(gR2m.localCoordinates(gR1m), d21)); CHECK(assert_equal(gR1, gR2.retract(d21))); - CHECK(assert_equal(gR1, gR2*Rot3Q::Expmap(d21))); + CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); // Check that log(t1,t2)=-log(t2,t1) CHECK(assert_equal(d12,-d21)); @@ -222,11 +192,11 @@ TEST(Rot3Q, manifold) // lines in canonical coordinates correspond to Abelian subgroups in SO(3) Vector d = Vector_(3, 0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) - CHECK(assert_equal(Rot3Q::Expmap(-d),Rot3Q::Expmap(d).inverse())); + CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Rot3Q R2 = Rot3Q::Expmap (2 * d); - Rot3Q R3 = Rot3Q::Expmap (3 * d); - Rot3Q R5 = Rot3Q::Expmap (5 * d); + Rot3 R2 = Rot3::Expmap (2 * d); + Rot3 R3 = Rot3::Expmap (3 * d); + Rot3 R5 = Rot3::Expmap (5 * d); CHECK(assert_equal(R5,R2*R3)); CHECK(assert_equal(R5,R3*R2)); } @@ -247,86 +217,86 @@ AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { } /* ************************************************************************* */ -TEST(Rot3Q, BCH) +TEST(Rot3, BCH) { // Approximate exmap by BCH formula AngularVelocity w1(0.2, -0.1, 0.1); AngularVelocity w2(0.01, 0.02, -0.03); - Rot3Q R1 = Rot3Q::Expmap (w1.vector()), R2 = Rot3Q::Expmap (w2.vector()); - Rot3Q R3 = R1 * R2; - Vector expected = Rot3Q::Logmap(R3); + Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector()); + Rot3 R3 = R1 * R2; + Vector expected = Rot3::Logmap(R3); Vector actual = BCH(w1, w2).vector(); CHECK(assert_equal(expected, actual,1e-5)); } /* ************************************************************************* */ -TEST( Rot3Q, rotate_derivatives) +TEST( Rot3, rotate_derivatives) { Matrix actualDrotate1a, actualDrotate1b, actualDrotate2; R.rotate(P, actualDrotate1a, actualDrotate2); R.inverse().rotate(P, actualDrotate1b, boost::none); - Matrix numerical1 = numericalDerivative21(testing::rotate, R, P); - Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); - Matrix numerical3 = numericalDerivative22(testing::rotate, R, P); + Matrix numerical1 = numericalDerivative21(testing::rotate, R, P); + Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); + Matrix numerical3 = numericalDerivative22(testing::rotate, R, P); EXPECT(assert_equal(numerical1,actualDrotate1a,error)); EXPECT(assert_equal(numerical2,actualDrotate1b,error)); EXPECT(assert_equal(numerical3,actualDrotate2, error)); } /* ************************************************************************* */ -TEST( Rot3Q, unrotate) +TEST( Rot3, unrotate) { Point3 w = R * P; Matrix H1,H2; Point3 actual = R.unrotate(w,H1,H2); CHECK(assert_equal(P,actual)); - Matrix numerical1 = numericalDerivative21(testing::unrotate, R, w); + Matrix numerical1 = numericalDerivative21(testing::unrotate, R, w); CHECK(assert_equal(numerical1,H1,error)); - Matrix numerical2 = numericalDerivative22(testing::unrotate, R, w); + Matrix numerical2 = numericalDerivative22(testing::unrotate, R, w); CHECK(assert_equal(numerical2,H2,error)); } /* ************************************************************************* */ -TEST( Rot3Q, compose ) +TEST( Rot3, compose ) { - Rot3Q R1 = Rot3Q::rodriguez(0.1, 0.2, 0.3); - Rot3Q R2 = Rot3Q::rodriguez(0.2, 0.3, 0.5); + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); - Rot3Q expected = R1 * R2; + Rot3 expected = R1 * R2; Matrix actualH1, actualH2; - Rot3Q actual = R1.compose(R2, actualH1, actualH2); + Rot3 actual = R1.compose(R2, actualH1, actualH2); CHECK(assert_equal(expected,actual)); - Matrix numericalH1 = numericalDerivative21(testing::compose, R1, + Matrix numericalH1 = numericalDerivative21(testing::compose, R1, R2, 1e-2); CHECK(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(testing::compose, R1, + Matrix numericalH2 = numericalDerivative22(testing::compose, R1, R2, 1e-2); CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ -TEST( Rot3Q, inverse ) +TEST( Rot3, inverse ) { - Rot3Q R = Rot3Q::rodriguez(0.1, 0.2, 0.3); + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3Q I; + Rot3 I; Matrix actualH; CHECK(assert_equal(I,R*R.inverse(actualH))); CHECK(assert_equal(I,R.inverse()*R)); - Matrix numericalH = numericalDerivative11(testing::inverse, R); + Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH, 1e-4)); } /* ************************************************************************* */ -TEST( Rot3Q, between ) +TEST( Rot3, between ) { - Rot3Q r1 = Rot3Q::Rz(M_PI/3.0); - Rot3Q r2 = Rot3Q::Rz(2.0*M_PI/3.0); + Rot3 r1 = Rot3::Rz(M_PI/3.0); + Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0); Matrix expectedr1 = Matrix_(3,3, 0.5, -sqrt(3.0)/2.0, 0.0, @@ -334,32 +304,28 @@ TEST( Rot3Q, between ) 0.0, 0.0, 1.0); EXPECT(assert_equal(expectedr1, r1.matrix())); - Rot3Q R = Rot3Q::rodriguez(0.1, 0.4, 0.2); - Rot3Q origin; + Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); + Rot3 origin; CHECK(assert_equal(R, origin.between(R))); CHECK(assert_equal(R.inverse(), R.between(origin))); - Rot3Q R1 = Rot3Q::rodriguez(0.1, 0.2, 0.3); - Rot3Q R2 = Rot3Q::rodriguez(0.2, 0.3, 0.5); + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); - Rot3Q expected = R1.inverse() * R2; + Rot3 expected = R1.inverse() * R2; Matrix actualH1, actualH2; - Rot3Q actual = R1.between(R2, actualH1, actualH2); + Rot3 actual = R1.between(R2, actualH1, actualH2); CHECK(assert_equal(expected,actual)); - Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); + Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); CHECK(assert_equal(numericalH1,actualH1, 1e-4)); - Matrix numericalH1M = numericalDerivative21(testing::between , Rot3M(R1.matrix()), Rot3M(R2.matrix())); - CHECK(assert_equal(numericalH1M,actualH1, 1e-4)); - Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); + Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); CHECK(assert_equal(numericalH2,actualH2, 1e-4)); - Matrix numericalH2M = numericalDerivative22(testing::between , Rot3M(R1.matrix()), Rot3M(R2.matrix())); - CHECK(assert_equal(numericalH2M,actualH2, 1e-4)); } /* ************************************************************************* */ -TEST( Rot3Q, xyz ) +TEST( Rot3, xyz ) { double t = 0.1, st = sin(t), ct = cos(t); @@ -369,47 +335,47 @@ TEST( Rot3Q, xyz ) // z // | * Y=(ct,st) // x----y - Rot3Q expected1(1, 0, 0, 0, ct, -st, 0, st, ct); - CHECK(assert_equal(expected1,Rot3Q::Rx(t))); + Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct); + CHECK(assert_equal(expected1,Rot3::Rx(t))); // x // | * Z=(ct,st) // y----z - Rot3Q expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); - CHECK(assert_equal(expected2,Rot3Q::Ry(t))); + Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); + CHECK(assert_equal(expected2,Rot3::Ry(t))); // y // | X=* (ct,st) // z----x - Rot3Q expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); - CHECK(assert_equal(expected3,Rot3Q::Rz(t))); + Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); + CHECK(assert_equal(expected3,Rot3::Rz(t))); // Check compound rotation - Rot3Q expected = Rot3Q::Rz(0.3) * Rot3Q::Ry(0.2) * Rot3Q::Rx(0.1); - CHECK(assert_equal(expected,Rot3Q::RzRyRx(0.1,0.2,0.3))); + Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1); + CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3))); } /* ************************************************************************* */ -TEST( Rot3Q, yaw_pitch_roll ) +TEST( Rot3, yaw_pitch_roll ) { double t = 0.1; // yaw is around z axis - CHECK(assert_equal(Rot3Q::Rz(t),Rot3Q::yaw(t))); + CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); // pitch is around y axis - CHECK(assert_equal(Rot3Q::Ry(t),Rot3Q::pitch(t))); + CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); // roll is around x axis - CHECK(assert_equal(Rot3Q::Rx(t),Rot3Q::roll(t))); + CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); // Check compound rotation - Rot3Q expected = Rot3Q::yaw(0.1) * Rot3Q::pitch(0.2) * Rot3Q::roll(0.3); - CHECK(assert_equal(expected,Rot3Q::ypr(0.1,0.2,0.3))); + Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); + CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); } /* ************************************************************************* */ -TEST( Rot3Q, RQ) +TEST( Rot3, RQ) { // Try RQ on a pure rotation Matrix actualK; @@ -419,18 +385,18 @@ TEST( Rot3Q, RQ) CHECK(assert_equal(eye(3),actualK)); CHECK(assert_equal(expected,actual,1e-6)); - // Try using xyz call, asserting that Rot3Q::RzRyRx(x,y,z).xyz()==[x;y;z] + // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3Q::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); - // Try using ypr call, asserting that Rot3Q::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3Q::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3Q::ypr(0.1,0.2,0.3).rpy())); + // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] + CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3Q::yaw (0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3Q::pitch(0.1).ypr())); - CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3Q::roll (0.1).ypr())); + CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); @@ -441,61 +407,61 @@ TEST( Rot3Q, RQ) } /* ************************************************************************* */ -TEST( Rot3Q, expmapStability ) { +TEST( Rot3, expmapStability ) { Vector w = Vector_(3, 78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; - Rot3Q actualR = Rot3Q::Expmap(w); + Rot3 actualR = Rot3::Expmap(w); Matrix W = Matrix_(3,3, 0.0, -w(2), w(1), w(2), 0.0, -w(0), -w(1), w(0), 0.0 ); Matrix W2 = W*W; Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0 - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; - Rot3Q expectedR( Rmat ); + Rot3 expectedR( Rmat ); CHECK(assert_equal(expectedR, actualR, 1e-10)); } // Does not work with Quaternions ///* ************************************************************************* */ -//TEST( Rot3Q, logmapStability ) { +//TEST( Rot3, logmapStability ) { // Vector w = Vector_(3, 1e-8, 0.0, 0.0); -// Rot3Q R = Rot3Q::Expmap(w); +// Rot3 R = Rot3::Expmap(w); //// double tr = R.r1().x()+R.r2().y()+R.r3().z(); //// std::cout.precision(5000); //// std::cout << "theta: " << w.norm() << std::endl; //// std::cout << "trace: " << tr << std::endl; //// R.print("R = "); -// Vector actualw = Rot3Q::Logmap(R); +// Vector actualw = Rot3::Logmap(R); // CHECK(assert_equal(w, actualw, 1e-15)); //} /* ************************************************************************* */ -TEST(Rot3Q, quaternion) { +TEST(Rot3, quaternion) { // NOTE: This is also verifying the ability to convert Vector to Quaternion Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); - Rot3Q R1 = Rot3Q(Matrix_(3,3, + Rot3 R1 = Rot3(Matrix_(3,3, 0.271018623057411, 0.278786459830371, 0.921318086098018, 0.578529366719085, 0.717799701969298, -0.387385285854279, -0.769319620053772, 0.637998195662053, 0.033250932803219)); Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); - Rot3Q R2 = Rot3Q(Matrix_(3,3, + Rot3 R2 = Rot3(Matrix_(3,3, -0.207341903877828, 0.250149415542075, 0.945745528564780, 0.881304914479026, -0.371869043667957, 0.291573424846290, 0.424630407073532, 0.893945571198514, -0.143353873763946)); - // Check creating Rot3Q from quaternion - EXPECT(assert_equal(R1, Rot3Q(q1))); - EXPECT(assert_equal(R1, Rot3Q::quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); - EXPECT(assert_equal(R2, Rot3Q(q2))); - EXPECT(assert_equal(R2, Rot3Q::quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); + // Check creating Rot3 from quaternion + EXPECT(assert_equal(R1, Rot3(q1))); + EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); + EXPECT(assert_equal(R2, Rot3(q2))); + EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); - // Check converting Rot3Q to quaterion + // Check converting Rot3 to quaterion EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs()))); EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs()))); - // Check that quaternion and Rot3Q represent the same rotation + // Check that quaternion and Rot3 represent the same rotation Point3 p1(1.0, 2.0, 3.0); Point3 p2(8.0, 7.0, 9.0); @@ -509,6 +475,8 @@ TEST(Rot3Q, quaternion) { EXPECT(assert_equal(expected2, actual2)); } +#endif + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index 7c575de0f..a35165273 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -30,94 +30,28 @@ using namespace gtsam; -typedef TypedSymbol KeyQ; -typedef Values ValuesQ; -typedef PriorFactor PriorQ; -typedef BetweenFactor BetweenQ; -typedef NonlinearFactorGraph GraphQ; - -typedef TypedSymbol KeyM; -typedef Values ValuesM; -typedef PriorFactor PriorM; -typedef BetweenFactor BetweenM; -typedef NonlinearFactorGraph GraphM; - -/* ************************************************************************* */ -TEST(Rot3, optimize1) { - // Compare Rot3Q and Rot3M optimization - - GraphQ fgQ; - fgQ.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01))); - fgQ.add(BetweenQ(0, 1, Rot3Q::Rz(M_PI/3.0), sharedSigma(3, 0.01))); - fgQ.add(BetweenQ(1, 0, Rot3Q::Rz(5.0*M_PI/3.0), sharedSigma(3, 0.01))); - - GraphM fgM; - fgM.add(PriorM(0, Rot3M(), sharedSigma(3, 0.01))); - fgM.add(BetweenM(0, 1, Rot3M::Rz(M_PI/3.0), sharedSigma(3, 0.01))); - fgM.add(BetweenM(1, 0, Rot3M::Rz(5.0*M_PI/3.0), sharedSigma(3, 0.01))); - - ValuesQ initialQ; - initialQ.insert(0, Rot3Q::Rz(0.0)); - initialQ.insert(1, Rot3Q::Rz(M_PI/3.0 + 0.1)); - - ValuesM initialM; - initialM.insert(0, Rot3M::Rz(0.0)); - initialM.insert(1, Rot3M::Rz(M_PI/3.0 + 0.1)); - - ValuesQ truthQ; - truthQ.insert(0, Rot3Q::Rz(0.0)); - truthQ.insert(1, Rot3Q::Rz(M_PI/3.0)); - - ValuesM truthM; - truthM.insert(0, Rot3M::Rz(0.0)); - truthM.insert(1, Rot3M::Rz(M_PI/3.0)); - - // Compare Matrix and Quaternion between - Matrix H1M, H2M; - Rot3M betwM = initialM[1].between(initialM[0], H1M, H2M); - Matrix H1Q, H2Q; - Rot3Q betwQ = initialM[1].between(initialM[0], H1Q, H2Q); - EXPECT(assert_equal(betwM.matrix(), betwQ.matrix())); - EXPECT(assert_equal(H1M, H1Q)); - EXPECT(assert_equal(H2M, H2Q)); - Point3 x1(1.0,0.0,0.0), x2(0.0,1.0,0.0); - EXPECT(assert_equal(betwM*x1, betwQ*x1)); - EXPECT(assert_equal(betwM*x2, betwQ*x2)); - - // Compare Matrix and Quaternion logmap - Vector logM = initialM[1].localCoordinates(initialM[0]); - Vector logQ = initialQ[1].localCoordinates(initialQ[0]); - EXPECT(assert_equal(logM, logQ)); - - // Compare Matrix and Quaternion linear graph - Ordering ordering; ordering += KeyQ(0), KeyQ(1); - GaussianFactorGraph gfgQ(*fgQ.linearize(initialQ, ordering)); - GaussianFactorGraph gfgM(*fgM.linearize(initialM, ordering)); - EXPECT(assert_equal(gfgQ, gfgM, 1e-5)); - - NonlinearOptimizationParameters params; - //params.verbosity_ = NonlinearOptimizationParameters::TRYLAMBDA; - ValuesQ final = optimize(fgQ, initialQ, params); - - EXPECT(assert_equal(truthQ, final, 1e-5)); -} +typedef TypedSymbol Key; +typedef Values Rot3Values; +typedef PriorFactor Prior; +typedef BetweenFactor Between; +typedef NonlinearFactorGraph Graph; /* ************************************************************************* */ TEST(Rot3, optimize) { // Optimize a circle - ValuesQ truth; - ValuesQ initial; - GraphQ fg; - fg.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01))); + Rot3Values truth; + Rot3Values initial; + Graph fg; + fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01))); for(int j=0; j<6; ++j) { - truth.insert(j, Rot3Q::Rz(M_PI/3.0 * double(j))); - initial.insert(j, Rot3Q::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); - fg.add(BetweenQ(j, (j+1)%6, Rot3Q::Rz(M_PI/3.0), sharedSigma(3, 0.01))); + truth.insert(j, Rot3::Rz(M_PI/3.0 * double(j))); + initial.insert(j, Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); + fg.add(Between(j, (j+1)%6, Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); } NonlinearOptimizationParameters params; - ValuesQ final = optimize(fg, initial, params); + Rot3Values final = optimize(fg, initial, params); EXPECT(assert_equal(truth, final, 1e-5)); }