diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index e8c7510f4..befeef3ec 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -14,8 +14,8 @@ check_PROGRAMS = headers += concepts.h # Points and poses -sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp -check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3 tests/testPose3 +sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3M.cpp Rot3Q.cpp Pose3.cpp +check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3M tests/testRot3Q tests/testPose3 # Cameras headers += GeneralCameraT.h Cal3_S2Stereo.h diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 422cf4357..2fd5d7091 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -23,181 +23,178 @@ namespace gtsam { -/** - * A 3D pose (R,t) : (Rot3,Point3) - * @ingroup geometry - */ -class Pose3 { -public: - static const size_t dimension = 6; + /** + * A 3D pose (R,t) : (Rot3,Point3) + * @ingroup geometry + */ + class Pose3 { + public: + static const size_t dimension = 6; - /** Pose Concept requirements */ - typedef Rot3 Rotation; - typedef Point3 Translation; + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; -private: - Rot3 R_; - Point3 t_; + private: + Rot3 R_; + Point3 t_; -public: + public: - /** Default constructor is origin */ - Pose3() {} + /** Default constructor is origin */ + Pose3() {} - /** Copy constructor */ - Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} + /** Copy constructor */ + Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} - /** Construct from R,t */ - Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} + /** Construct from R,t */ + Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} - /** Constructor from 4*4 matrix */ - Pose3(const Matrix &T) : - R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), - T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} + /** Constructor from 4*4 matrix */ + Pose3(const Matrix &T) : + R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), + T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} - /** Constructor from 12D vector */ - Pose3(const Vector &V) : - R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), - t_(V(9), V(10),V(11)) {} + const Rot3& rotation() const { return R_; } + const Point3& translation() const { return t_; } - inline const Rot3& rotation() const { return R_; } - inline const Point3& translation() const { return t_; } + double x() const { return t_.x(); } + double y() const { return t_.y(); } + double z() const { return t_.z(); } - inline double x() const { return t_.x(); } - inline double y() const { return t_.y(); } - inline double z() const { return t_.z(); } + /** convert to 4*4 matrix */ + Matrix matrix() const; - /** convert to 4*4 matrix */ - Matrix matrix() const; + /** print with optional string */ + void print(const std::string& s = "") const; - /** print with optional string */ - void print(const std::string& s = "") const; + /** assert equality up to a tolerance */ + bool equals(const Pose3& pose, double tol = 1e-9) const; - /** assert equality up to a tolerance */ - bool equals(const Pose3& pose, double tol = 1e-9) const; + /** Compose two poses */ + Pose3 operator*(const Pose3& T) const { + return Pose3(R_*T.R_, t_ + R_*T.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; - Pose3 transform_to(const Pose3& pose) const; + /** dimension of the variable - used to autodetect sizes */ + static size_t Dim() { return dimension; } - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /** Lie requirements */ - /** Lie requirements */ + /** Dimensionality of the tangent space */ + size_t dim() const { return dimension; } - /** Dimensionality of the tangent space */ - inline size_t dim() const { return dimension; } + /** identity */ + static Pose3 identity() { + return Pose3(); + } - /** identity */ - inline static Pose3 identity() { - return Pose3(); - } + /** + * Derivative of inverse + */ + Pose3 inverse(boost::optional H1=boost::none) const; - /** - * Derivative of inverse - */ - Pose3 inverse(boost::optional H1=boost::none) const; + /** + * composes two poses (first (*this) then p2) + * with optional derivatives + */ + Pose3 compose(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** - * composes two poses (first (*this) then p2) - * with optional derivatives - */ - Pose3 compose(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /** receives the point in Pose coordinates and transforms it to world coordinates */ + Point3 transform_from(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** receives the point in Pose coordinates and transforms it to world coordinates */ - Point3 transform_from(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** syntactic sugar for transform */ + inline Point3 operator*(const Point3& p) const { return transform_from(p); } - /** syntactic sugar for transform */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } + /** receives the point in world coordinates and transforms it to Pose coordinates */ + Point3 transform_to(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** receives the point in world coordinates and transforms it to Pose coordinates */ - Point3 transform_to(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** Exponential map around another pose */ + Pose3 retract(const Vector& d) const; - /** Exponential map around another pose */ - Pose3 retract(const Vector& d) const; + /** Logarithm map around another pose T1 */ + Vector localCoordinates(const Pose3& T2) const; - /** Logarithm map around another pose T1 */ - Vector localCoordinates(const Pose3& T2) const; + /** non-approximated versions of Expmap/Logmap */ + static Pose3 Expmap(const Vector& xi); + static Vector Logmap(const Pose3& p); - /** non-approximated versions of Expmap/Logmap */ - static Pose3 Expmap(const Vector& xi); - static Vector Logmap(const Pose3& p); + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + * as well as optionally the derivatives + */ + Pose3 between(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - * as well as optionally the derivatives - */ - Pose3 between(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /** + * Calculate Adjoint map + * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) + */ + Matrix AdjointMap() const; + Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } - /** - * Calculate Adjoint map - * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) - */ - Matrix AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } + /** + * wedge for Pose3: + * @param xi 6-dim twist (omega,v) where + * omega = (wx,wy,wz) 3D angular velocity + * v (vx,vy,vz) = 3D velocity + * @return xihat, 4*4 element of Lie algebra that can be exponentiated + */ + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { + return Matrix_(4,4, + 0.,-wz, wy, vx, + wz, 0.,-wx, vy, + -wy, wx, 0., vz, + 0., 0., 0., 0.); + } - /** - * wedge for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = (wx,wy,wz) 3D angular velocity - * v (vx,vy,vz) = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ - static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { - return Matrix_(4,4, - 0.,-wz, wy, vx, - wz, 0.,-wx, vy, - -wy, wx, 0., vz, - 0., 0., 0., 0.); - } + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @return range (double) + */ + double range(const Point3& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @return range (double) - */ - double range(const Point3& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /** + * Calculate range to another pose + * @param point SO(3) pose of landmark + * @return range (double) + */ + double range(const Pose3& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; - /** - * Calculate range to another pose - * @param point SO(3) pose of landmark - * @return range (double) - */ - double range(const Pose3& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); - } -}; // Pose3 class + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + } + }; // Pose3 class -/** - * wedge for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = 3D angular velocity - * v = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ -template <> -inline Matrix wedge(const Vector& xi) { - return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); -} + /** + * wedge for Pose3: + * @param xi 6-dim twist (omega,v) where + * omega = 3D angular velocity + * v = 3D velocity + * @return xihat, 4*4 element of Lie algebra that can be exponentiated + */ + template <> + inline Matrix wedge(const Vector& xi) { + return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); + } } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 82a5ec39e..824e80abd 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -11,273 +11,50 @@ /** * @file Rot3.h - * @brief 3D Rotation represented as 3*3 matrix - * @author Alireza Fathi - * @author Christian Potthast - * @author Frank Dellaert + * @brief Contains a typedef to the default 3D rotation implementation determined at compile time + * @author Richard Roberts */ // \callgraph #pragma once -#include -#include + +// The following preprocessor blocks select the main 3D rotation implementation, +// creating a typedef from Rot3M (the rotation matrix implementation) or Rot3Q +// (the quaternion implementation) to Rot3. The type selected here will be +// used in all built-in gtsam geometry types that involve 3D rotations, such as +// Pose3, SimpleCamera, CalibratedCamera, StereoCamera, etc. +#ifdef GTSAM_DEFAULT_QUATERNIONS + +#include namespace gtsam { - -typedef Eigen::Quaterniond Quaternion; - -/** - * @brief 3D Rotations represented as rotation matrices - * @ingroup geometry - */ -class Rot3 { -public: - static const size_t dimension = 3; - -private: - /** we store columns ! */ - Point3 r1_, r2_, r3_; - -public: - - /** default constructor, unit rotation */ - 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)) {} - - /** constructor from columns */ - Rot3(const Point3& r1, const Point3& r2, const Point3& r3) : - r1_(r1), r2_(r2), r3_(r3) {} - - /** constructor from doubles in *row* order !!! */ - Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33) : - r1_(Point3(R11, R21, R31)), - r2_(Point3(R12, R22, R32)), - r3_(Point3(R13, R23, R33)) {} - - /** constructor from matrix */ - 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))) {} - - /** Constructor from a quaternion. This can also be called using a plain - * Vector, due to implicit conversion from Vector to Quaternion - * @param q The quaternion - */ - Rot3(const Quaternion& q) { - Eigen::Matrix3d R = q.toRotationMatrix(); - r1_ = Point3(R.col(0)); - r2_ = Point3(R.col(1)); - r3_ = Point3(R.col(2)); - } - - /** Static member function to generate some well known rotations */ - - /** - * Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix - * Counterclockwise when looking from unchanging axis. - */ - static Rot3 Rx(double t); - static Rot3 Ry(double t); - static Rot3 Rz(double t); - static Rot3 RzRyRx(double x, double y, double z); - inline static Rot3 RzRyRx(const Vector& xyz) { - assert(xyz.size() == 3); - return RzRyRx(xyz(0), xyz(1), xyz(2)); - } - - /** - * Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) - * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf - * Assumes vehicle coordinate frame X forward, Y right, Z down - */ - static Rot3 yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading) - static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude) - static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft) - static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} - - /** Create from Quaternion parameters */ - static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); } - - /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param w is the rotation axis, unit length - * @param theta rotation angle - * @return incremental rotation matrix - */ - static Rot3 rodriguez(const Vector& w, double theta); - - /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param v a vector of incremental roll,pitch,yaw - * @return incremental rotation matrix - */ - static Rot3 rodriguez(const Vector& v); - - /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param wx - * @param wy - * @param wz - * @return incremental rotation matrix - */ - static inline Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez(Vector_(3,wx,wy,wz));} - - /** print */ - void print(const std::string& s="R") const { gtsam::print(matrix(), s);} - - /** equals with an tolerance */ - bool equals(const Rot3& p, double tol = 1e-9) const; - - /** return 3*3 rotation matrix */ - Matrix matrix() const; - - /** return 3*3 transpose (inverse) rotation matrix */ - Matrix transpose() const; - - /** returns column vector specified by index */ - Point3 column(int index) const; - Point3 r1() const { return r1_; } - Point3 r2() const { return r2_; } - Point3 r3() const { return r3_; } - - /** - * Use RQ to calculate xyz angle representation - * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) - */ - Vector xyz() const; - - /** - * Use RQ to calculate yaw-pitch-roll angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) - */ - Vector ypr() const; - - /** - * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) - */ - Vector rpy() const; - - /** Compute the quaternion representation of this rotation. - * @return The quaternion - */ - Quaternion toQuaternion() const { - return Quaternion((Eigen::Matrix3d() << - r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z()).finished()); - } - - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } - - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - inline size_t dim() const { return dimension; } - - /** Compose two rotations i.e., R= (*this) * R2 - */ - Rot3 compose(const Rot3& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** Exponential map at identity - create a rotation from canonical coordinates - * using Rodriguez' formula - */ - static Rot3 Expmap(const Vector& v) { - if(zero(v)) return Rot3(); - else return rodriguez(v); - } - - /** identity */ - inline static Rot3 identity() { - return Rot3(); - } - - // Log map at identity - return the canonical coordinates of this rotation - static Vector Logmap(const Rot3& R); - - // Manifold requirements - - inline Rot3 retract(const Vector& v) const { return compose(Expmap(v)); } - - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const Rot3& t2) const { return Logmap(between(t2)); } - - - // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3() - inline Rot3 inverse(boost::optional H1=boost::none) const { - if (H1) *H1 = -matrix(); - return Rot3( - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()); - } - - /** - * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' - */ - Rot3 between(const Rot3& R2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - /** 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 - */ - inline Point3 operator*(const Point3& p) const { return rotate(p);} - - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - Point3 rotate(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** - * rotate point from world to rotated - * frame = R'*p - */ - Point3 unrotate(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(r1_); - ar & BOOST_SERIALIZATION_NVP(r2_); - ar & BOOST_SERIALIZATION_NVP(r3_); - } -}; - -/** - * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R - * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' - * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will - * be the identity and Q is a yaw-pitch-roll decomposition of A. - * The implementation uses Givens rotations and is based on Hartley-Zisserman. - * @param a 3 by 3 matrix A=RQ - * @return an upper triangular matrix R - * @return a vector [thetax, thetay, thetaz] in radians. - */ -std::pair RQ(const Matrix& A); - + /** + * 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. + */ + typedef Rot3Q Rot3; } + +#else + +#include + +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. + */ + typedef Rot3M Rot3; +} + +#endif diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3M.cpp similarity index 86% rename from gtsam/geometry/Rot3.cpp rename to gtsam/geometry/Rot3M.cpp index cb31a5c0a..86ed82d63 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file Rot3.cpp + * @file Rot3M.cpp * @brief Rotation (internal: 3*3 matrix representation*) * @author Alireza Fathi * @author Christian Potthast @@ -18,7 +18,7 @@ */ #include -#include +#include #include using namespace std; @@ -26,39 +26,39 @@ using namespace std; namespace gtsam { /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Rot3); + INSTANTIATE_LIE(Rot3M); static const Matrix I3 = eye(3); /* ************************************************************************* */ // static member functions to construct rotations - Rot3 Rot3::Rx(double t) { + Rot3M Rot3M::Rx(double t) { double st = sin(t), ct = cos(t); - return Rot3( + return Rot3M( 1, 0, 0, 0, ct,-st, 0, st, ct); } - Rot3 Rot3::Ry(double t) { + Rot3M Rot3M::Ry(double t) { double st = sin(t), ct = cos(t); - return Rot3( + return Rot3M( ct, 0, st, 0, 1, 0, -st, 0, ct); } - Rot3 Rot3::Rz(double t) { + Rot3M Rot3M::Rz(double t) { double st = sin(t), ct = cos(t); - return Rot3( + return Rot3M( ct,-st, 0, st, ct, 0, 0, 0, 1); } // Considerably faster than composing matrices above ! - Rot3 Rot3::RzRyRx(double x, double y, double z) { + Rot3M Rot3M::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); @@ -73,7 +73,7 @@ namespace gtsam { double s_c = sx * cz; double c_c = cx * cz; double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; - return Rot3( + return Rot3M( _cc,- c_s + ssc, s_s + csc, _cs, c_c + sss, -s_c + css, -sy, sc_, cc_ @@ -81,7 +81,7 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::rodriguez(const Vector& w, double theta) { + Rot3M Rot3M::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; @@ -97,25 +97,25 @@ namespace gtsam { double C11 = c_1*wwTyy, C12 = c_1*wy*wz; double C22 = c_1*wwTzz; - return Rot3( c + C00, -swz + C01, swy + C02, + return Rot3M( c + C00, -swz + C01, swy + C02, swz + C01, c + C11, -swx + C12, -swy + C02, swx + C12, c + C22); } /* ************************************************************************* */ - Rot3 Rot3::rodriguez(const Vector& w) { + Rot3M Rot3M::rodriguez(const Vector& w) { double t = w.norm(); - if (t < 1e-10) return Rot3(); + if (t < 1e-10) return Rot3M(); return rodriguez(w/t, t); } /* ************************************************************************* */ - bool Rot3::equals(const Rot3 & R, double tol) const { + bool Rot3M::equals(const Rot3M & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); } /* ************************************************************************* */ - Matrix Rot3::matrix() const { + Matrix Rot3M::matrix() const { double r[] = { r1_.x(), r2_.x(), r3_.x(), r1_.y(), r2_.y(), r3_.y(), r1_.z(), r2_.z(), r3_.z() }; @@ -123,7 +123,7 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix Rot3::transpose() const { + Matrix Rot3M::transpose() const { double r[] = { r1_.x(), r1_.y(), r1_.z(), r2_.x(), r2_.y(), r2_.z(), r3_.x(), r3_.y(), r3_.z()}; @@ -131,7 +131,7 @@ namespace gtsam { } /* ************************************************************************* */ - Point3 Rot3::column(int index) const{ + Point3 Rot3M::column(int index) const{ if(index == 3) return r3_; else if (index == 2) @@ -141,25 +141,25 @@ namespace gtsam { } /* ************************************************************************* */ - Vector Rot3::xyz() const { + Vector Rot3M::xyz() const { Matrix I;Vector q; boost::tie(I,q)=RQ(matrix()); return q; } - Vector Rot3::ypr() const { + Vector Rot3M::ypr() const { Vector q = xyz(); return Vector_(3,q(2),q(1),q(0)); } - Vector Rot3::rpy() const { + Vector Rot3M::rpy() const { Vector q = xyz(); return Vector_(3,q(0),q(1),q(2)); } /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation - Vector Rot3::Logmap(const Rot3& R) { + Vector Rot3M::Logmap(const Rot3M& R) { double tr = R.r1().x()+R.r2().y()+R.r3().z(); if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) return zero(3); @@ -170,7 +170,7 @@ namespace gtsam { R.r2().z()-R.r3().y(), R.r3().x()-R.r1().z(), R.r1().y()-R.r2().x()); - } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. + } else if (fabs(tr) - -1.0 < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. if(fabs(R.r3().z() - -1.0) > 1e-10) return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); @@ -190,7 +190,7 @@ namespace gtsam { } /* ************************************************************************* */ - Point3 Rot3::rotate(const Point3& p, + Point3 Rot3M::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(); @@ -199,7 +199,7 @@ namespace gtsam { /* ************************************************************************* */ // see doc/math.lyx, SO(3) section - Point3 Rot3::unrotate(const Point3& p, + Point3 Rot3M::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { const Matrix Rt(transpose()); Point3 q(Rt*p.vector()); // q = Rt*p @@ -209,7 +209,7 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::compose (const Rot3& R2, + Rot3M Rot3M::compose (const Rot3M& R2, boost::optional H1, boost::optional H2) const { if (H1) *H1 = R2.transpose(); if (H2) *H2 = I3; @@ -217,7 +217,7 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::between (const Rot3& R2, + Rot3M Rot3M::between (const Rot3M& R2, boost::optional H1, boost::optional H2) const { if (H1) *H1 = -(R2.transpose()*matrix()); if (H2) *H2 = I3; @@ -228,15 +228,15 @@ namespace gtsam { pair RQ(const Matrix& A) { double x = -atan2(-A(2, 1), A(2, 2)); - Rot3 Qx = Rot3::Rx(-x); + Rot3M Qx = Rot3M::Rx(-x); Matrix B = A * Qx.matrix(); double y = -atan2(B(2, 0), B(2, 2)); - Rot3 Qy = Rot3::Ry(-y); + Rot3M Qy = Rot3M::Ry(-y); Matrix C = B * Qy.matrix(); double z = -atan2(-C(1, 0), C(1, 1)); - Rot3 Qz = Rot3::Rz(-z); + Rot3M Qz = Rot3M::Rz(-z); Matrix R = C * Qz.matrix(); Vector xyz = Vector_(3, x, y, z); diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h new file mode 100644 index 000000000..0c17060b3 --- /dev/null +++ b/gtsam/geometry/Rot3M.h @@ -0,0 +1,284 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Rot3M.h + * @brief 3D Rotation represented as 3*3 matrix + * @author Alireza Fathi + * @author Christian Potthast + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include + +namespace gtsam { + + typedef Eigen::Quaternion Quaternion; ///< Typedef to an Eigen Quaternion + + /** + * @brief 3D Rotations represented as rotation matrices + * @ingroup geometry + */ + class Rot3M { + public: + static const size_t dimension = 3; + + private: + /** we store columns ! */ + Point3 r1_, r2_, r3_; + + public: + + /** default constructor, unit rotation */ + Rot3M() : + r1_(Point3(1.0,0.0,0.0)), + r2_(Point3(0.0,1.0,0.0)), + r3_(Point3(0.0,0.0,1.0)) {} + + /** constructor from columns */ + Rot3M(const Point3& r1, const Point3& r2, const Point3& r3) : + r1_(r1), r2_(r2), r3_(r3) {} + + /** constructor from doubles in *row* order !!! */ + Rot3M(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33) : + r1_(Point3(R11, R21, R31)), + r2_(Point3(R12, R22, R32)), + r3_(Point3(R13, R23, R33)) {} + + /** constructor from matrix */ + Rot3M(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))) {} + + /** Constructor from a quaternion. This can also be called using a plain + * Vector, due to implicit conversion from Vector to Quaternion + * @param q The quaternion + */ + Rot3M(const Quaternion& q) { + Eigen::Matrix3d R = q.toRotationMatrix(); + r1_ = Point3(R.col(0)); + r2_ = Point3(R.col(1)); + r3_ = Point3(R.col(2)); + } + + /** Static member function to generate some well known rotations */ + + /** + * Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix + * Counterclockwise when looking from unchanging axis. + */ + static Rot3M Rx(double t); + static Rot3M Ry(double t); + static Rot3M Rz(double t); + static Rot3M RzRyRx(double x, double y, double z); + static Rot3M RzRyRx(const Vector& xyz) { + assert(xyz.size() == 3); + return RzRyRx(xyz(0), xyz(1), xyz(2)); + } + + /** + * Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) + * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf + * Assumes vehicle coordinate frame X forward, Y right, Z down + */ + static Rot3M yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading) + static Rot3M pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude) + static Rot3M roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft) + static Rot3M ypr (double y, double p, double r) { return RzRyRx(r,p,y);} + + /** Create from Quaternion parameters */ + static Rot3M quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3M(q); } + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param w is the rotation axis, unit length + * @param theta rotation angle + * @return incremental rotation matrix + */ + static Rot3M rodriguez(const Vector& w, double theta); + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param v a vector of incremental roll,pitch,yaw + * @return incremental rotation matrix + */ + static Rot3M rodriguez(const Vector& v); + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param wx + * @param wy + * @param wz + * @return incremental rotation matrix + */ + static Rot3M rodriguez(double wx, double wy, double wz) + { return rodriguez(Vector_(3,wx,wy,wz));} + + /** print */ + void print(const std::string& s="R") const { gtsam::print(matrix(), s);} + + /** equals with an tolerance */ + bool equals(const Rot3M& p, double tol = 1e-9) const; + + /** return 3*3 rotation matrix */ + Matrix matrix() const; + + /** return 3*3 transpose (inverse) rotation matrix */ + Matrix transpose() const; + + /** returns column vector specified by index */ + Point3 column(int index) const; + Point3 r1() const { return r1_; } + Point3 r2() const { return r2_; } + Point3 r3() const { return r3_; } + + /** + * Use RQ to calculate xyz angle representation + * @return a vector containing x,y,z s.t. R = Rot3M::RzRyRx(x,y,z) + */ + Vector xyz() const; + + /** + * Use RQ to calculate yaw-pitch-roll angle representation + * @return a vector containing ypr s.t. R = Rot3M::ypr(y,p,r) + */ + Vector ypr() const; + + /** + * Use RQ to calculate roll-pitch-yaw angle representation + * @return a vector containing ypr s.t. R = Rot3M::ypr(y,p,r) + */ + Vector rpy() const; + + /** Compute the quaternion representation of this rotation. + * @return The quaternion + */ + Quaternion toQuaternion() const { + return Quaternion((Eigen::Matrix3d() << + r1_.x(), r2_.x(), r3_.x(), + r1_.y(), r2_.y(), r3_.y(), + r1_.z(), r2_.z(), r3_.z()).finished()); + } + + /** dimension of the variable - used to autodetect sizes */ + static size_t Dim() { return dimension; } + + /** Lie requirements */ + + /** return DOF, dimensionality of tangent space */ + size_t dim() const { return dimension; } + + /** Compose two rotations i.e., R= (*this) * R2 + */ + Rot3M compose(const Rot3M& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /** Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3M Expmap(const Vector& v) { + if(zero(v)) return Rot3M(); + else return rodriguez(v); + } + + /** identity */ + inline static Rot3M identity() { + return Rot3M(); + } + + // Log map at identity - return the canonical coordinates of this rotation + static Vector Logmap(const Rot3M& R); + + // Manifold requirements + + Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } + + /** + * Returns inverse retraction + */ + Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } + + + // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M() + Rot3M inverse(boost::optional H1=boost::none) const { + if (H1) *H1 = -matrix(); + return Rot3M( + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z()); + } + + /** + * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' + */ + Rot3M between(const Rot3M& R2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /** compose two rotations */ + Rot3M operator*(const Rot3M& R2) const { + return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); + } + + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + inline Point3 operator*(const Point3& p) const { return rotate(p);} + + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + Point3 rotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /** + * rotate point from world to rotated + * frame = R'*p + */ + Point3 unrotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(r1_); + ar & BOOST_SERIALIZATION_NVP(r2_); + ar & BOOST_SERIALIZATION_NVP(r3_); + } + }; + + /** + * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R + * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' + * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will + * be the identity and Q is a yaw-pitch-roll decomposition of A. + * The implementation uses Givens rotations and is based on Hartley-Zisserman. + * @param a 3 by 3 matrix A=RQ + * @return an upper triangular matrix R + * @return a vector [thetax, thetay, thetaz] in radians. + */ + std::pair RQ(const Matrix& A); + +} diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp new file mode 100644 index 000000000..108a0747b --- /dev/null +++ b/gtsam/geometry/Rot3Q.cpp @@ -0,0 +1,132 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Rot3Q.cpp + * @brief Rotation (internal: 3*3 matrix representation*) + * @author Alireza Fathi + * @author Christian Potthast + * @author Frank Dellaert + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + + /** Explicit instantiation of base class to export members */ + INSTANTIATE_LIE(Rot3Q); + + static const Matrix I3 = eye(3); + + /* ************************************************************************* */ + // static member functions to construct rotations + + // Considerably faster than composing matrices above ! + Rot3Q Rot3Q::RzRyRx(double x, double y, double z) { return Rot3Q( + 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) { + return Quaternion(Eigen::AngleAxisd(theta, w)); } + + /* ************************************************************************* */ + Rot3Q Rot3Q::rodriguez(const Vector& w) { + double t = w.norm(); + if (t < 1e-10) return Rot3Q(); + return rodriguez(w/t, t); + } + + /* ************************************************************************* */ + bool Rot3Q::equals(const Rot3Q & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); + } + + /* ************************************************************************* */ + Matrix Rot3Q::matrix() const { return quaternion_.toRotationMatrix(); } + + /* ************************************************************************* */ + Matrix Rot3Q::transpose() const { return quaternion_.toRotationMatrix().transpose(); } + + /* ************************************************************************* */ + Vector Rot3Q::xyz() const { + Matrix I;Vector q; + boost::tie(I,q)=RQ(matrix()); + return q; + } + + Vector Rot3Q::ypr() const { + Vector q = xyz(); + return Vector_(3,q(2),q(1),q(0)); + } + + Vector Rot3Q::rpy() const { + Vector q = xyz(); + return Vector_(3,q(0),q(1),q(2)); + } + + /* ************************************************************************* */ + // Log map at identity - return the canonical coordinates of this rotation + Vector Rot3Q::Logmap(const Rot3Q& 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 + if(angleAxis.angle() < -M_PI) // error continuous. + angleAxis.angle() += 2.0*M_PI; + return angleAxis.axis() * angleAxis.angle(); + } + + /* ************************************************************************* */ + Point3 Rot3Q::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()); + if (H2) *H2 = R; + Eigen::Vector3d r = R * p.vector(); + return Point3(r.x(), r.y(), r.z()); + } + + /* ************************************************************************* */ + // see doc/math.lyx, SO(3) section + Point3 Rot3Q::unrotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + const Matrix Rt(transpose()); + Point3 q(Rt*p.vector()); // q = Rt*p + if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); + if (H2) *H2 = Rt; + return q; + } + + /* ************************************************************************* */ + Rot3Q Rot3Q::compose(const Rot3Q& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return Rot3Q(quaternion_ * R2.quaternion_); + } + + /* ************************************************************************* */ + Rot3Q Rot3Q::between(const Rot3Q& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*matrix()); + if (H2) *H2 = I3; + return between_default(*this, R2); + } + + /* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/geometry/Rot3Q.h b/gtsam/geometry/Rot3Q.h new file mode 100644 index 000000000..d21f7e2fb --- /dev/null +++ b/gtsam/geometry/Rot3Q.h @@ -0,0 +1,272 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Rot3Q.h + * @brief 3D Rotation represented as 3*3 matrix + * @author Alireza Fathi + * @author Christian Potthast + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include +#include + +namespace gtsam { + + typedef Eigen::Quaternion Quaternion; ///< Typedef to an Eigen Quaternion + + /** + * @brief 3D Rotations represented as rotation matrices + * @ingroup geometry + */ + class Rot3Q { + public: + static const size_t dimension = 3; + + private: + /** Internal Eigen Quaternion */ + Quaternion quaternion_; + + public: + + /** default constructor, unit rotation */ + Rot3Q() : quaternion_(Quaternion::Identity()) {} + + /** constructor from columns */ + Rot3Q(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()) {} + + /** constructor from doubles in *row* order !!! */ + Rot3Q(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33) : + quaternion_((Eigen::Matrix3d() << + R11, R12, R13, + R21, R22, R23, + R31, R32, R33).finished()) {} + + /** constructor from matrix */ + Rot3Q(const Matrix& R) : quaternion_(Eigen::Matrix3d(R)) {} + + /** Constructor from a quaternion. This can also be called using a plain + * Vector, due to implicit conversion from Vector to Quaternion + * @param q The quaternion + */ + Rot3Q(const Quaternion& q) : quaternion_(q) {} + + /** Constructor from a rotation matrix rotation Rot3M */ + Rot3Q(const Rot3M& r) : quaternion_(Eigen::Matrix3d(r.matrix())) {} + + /* Static member function to generate some well known rotations */ + + /** + * Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix + * Counterclockwise when looking from unchanging axis. + */ + static Rot3Q Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } + static Rot3Q Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } + static Rot3Q Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } + static Rot3Q RzRyRx(double x, double y, double z); + inline static Rot3Q RzRyRx(const Vector& xyz) { + assert(xyz.size() == 3); + return RzRyRx(xyz(0), xyz(1), xyz(2)); + } + + /** + * Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) + * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf + * Assumes vehicle coordinate frame X forward, Y right, Z down + */ + static Rot3Q yaw (double t) { return Rz(t); } // positive yaw is to right (as in aircraft heading) + static Rot3Q pitch(double t) { return Ry(t); } // positive pitch is up (increasing aircraft altitude) + static Rot3Q roll (double t) { return Rx(t); } // positive roll is to right (increasing yaw in aircraft) + static Rot3Q ypr (double y, double p, double r) { return RzRyRx(r,p,y);} + + /** Create from Quaternion parameters */ + static Rot3Q quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3Q(q); } + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param w is the rotation axis, unit length + * @param theta rotation angle + * @return incremental rotation matrix + */ + static Rot3Q rodriguez(const Vector& w, double theta); + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param v a vector of incremental roll,pitch,yaw + * @return incremental rotation matrix + */ + static Rot3Q rodriguez(const Vector& v); + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param wx Incremental roll (about X) + * @param wy Incremental pitch (about Y) + * @param wz Incremental yaw (about Z) + * @return incremental rotation matrix + */ + static Rot3Q rodriguez(double wx, double wy, double wz) + { return rodriguez(Vector_(3,wx,wy,wz));} + + /** print */ + void print(const std::string& s="R") const { gtsam::print(matrix(), s);} + + /** equals with an tolerance */ + bool equals(const Rot3Q& p, double tol = 1e-9) const; + + /** return 3*3 rotation matrix */ + Matrix matrix() const; + + /** return 3*3 transpose (inverse) rotation matrix */ + Matrix transpose() const; + + /** returns column vector specified by index */ + Point3 r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } + Point3 r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); } + Point3 r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } + + /** + * Use RQ to calculate xyz angle representation + * @return a vector containing x,y,z s.t. R = Rot3Q::RzRyRx(x,y,z) + */ + Vector xyz() const; + + /** + * Use RQ to calculate yaw-pitch-roll angle representation + * @return a vector containing ypr s.t. R = Rot3Q::ypr(y,p,r) + */ + Vector ypr() const; + + /** + * Use RQ to calculate roll-pitch-yaw angle representation + * @return a vector containing ypr s.t. R = Rot3Q::ypr(y,p,r) + */ + Vector rpy() const; + + /** Compute the quaternion representation of this rotation. + * @return The quaternion + */ + Quaternion toQuaternion() const { return quaternion_; } + + /** dimension of the variable - used to autodetect sizes */ + static size_t Dim() { return dimension; } + + /** Lie requirements */ + + /** return DOF, dimensionality of tangent space */ + size_t dim() const { return dimension; } + + /** Compose two rotations i.e., R= (*this) * R2 + */ + Rot3Q compose(const Rot3Q& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /** Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3Q Expmap(const Vector& v) { + if(zero(v)) return Rot3Q(); + else return rodriguez(v); + } + + /** identity */ + inline static Rot3Q identity() { + return Rot3Q(); + } + + // Log map at identity - return the canonical coordinates of this rotation + static Vector Logmap(const Rot3Q& R); + + // Manifold requirements + + Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); } + + /** + * Returns inverse retraction + */ + Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); } + + + // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q() + Rot3Q inverse(boost::optional H1=boost::none) const { + if (H1) *H1 = -matrix(); + return Rot3Q(quaternion_.inverse()); + } + + /** + * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' + */ + Rot3Q between(const Rot3Q& R2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /** compose two rotations */ + Rot3Q operator*(const Rot3Q& R2) const { return Rot3Q(quaternion_ * R2.quaternion_); } + + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + inline Point3 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)); + } + + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + Point3 rotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /** + * rotate point from world to rotated + * frame = R'*p + */ + Point3 unrotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(quaternion_); + } + }; + + /** + * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R + * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' + * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will + * be the identity and Q is a yaw-pitch-roll decomposition of A. + * The implementation uses Givens rotations and is based on Hartley-Zisserman. + * @param a 3 by 3 matrix A=RQ + * @return an upper triangular matrix R + * @return a vector [thetax, thetay, thetaz] in radians. + */ + std::pair RQ(const Matrix& A); + +} diff --git a/gtsam/geometry/tests/testHomography2.cpp b/gtsam/geometry/tests/testHomography2.cpp index aa5aa4515..417d41977 100644 --- a/gtsam/geometry/tests/testHomography2.cpp +++ b/gtsam/geometry/tests/testHomography2.cpp @@ -96,7 +96,7 @@ TEST( Homography2, TestCase) Homography2 patchH(const Pose3& tEc) { Pose3 cEt = tEc.inverse(); Rot3 cRt = cEt.rotation(); - Point3 r1 = cRt.column(1), r2 = cRt.column(2), t = cEt.translation(); + Point3 r1 = cRt.r1(), r2 = cRt.r2(), t = cEt.translation(); // TODO cleanup !!!! // column 1 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index afaa25015..fd95f6973 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -278,7 +278,7 @@ TEST( Pose3, compose ) EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::compose, T2, T2); - EXPECT(assert_equal(numericalH2,actualDcompose2)); + EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4)); } /* ************************************************************************* */ @@ -296,7 +296,7 @@ TEST( Pose3, compose2 ) EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2); - EXPECT(assert_equal(numericalH2,actualDcompose2)); + EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5)); } /* ************************************************************************* */ @@ -567,7 +567,7 @@ TEST( Pose3, between ) EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3)); Matrix numericalH2 = numericalDerivative22(testing::between , T2, T3); - EXPECT(assert_equal(numericalH2,actualDBetween2)); + EXPECT(assert_equal(numericalH2,actualDBetween2,1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3M.cpp similarity index 66% rename from gtsam/geometry/tests/testRot3.cpp rename to gtsam/geometry/tests/testRot3M.cpp index ec001b465..5e8cb48ae 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -11,7 +11,7 @@ /** * @file testRot3.cpp - * @brief Unit tests for Rot3 class + * @brief Unit tests for Rot3M class * @author Alireza Fathi */ @@ -21,21 +21,18 @@ #include #include #include -#include +#include using namespace gtsam; -GTSAM_CONCEPT_TESTABLE_INST(Rot3) -GTSAM_CONCEPT_LIE_INST(Rot3) - -Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); +Rot3M R = Rot3M::rodriguez(0.1, 0.4, 0.2); Point3 P(0.2, 0.7, -2.0); double error = 1e-9, epsilon = 0.001; /* ************************************************************************* */ -TEST( Rot3, constructor) +TEST( Rot3M, constructor) { - Rot3 expected(eye(3, 3)); + Rot3M expected(eye(3, 3)); Vector r1(3), r2(3), r3(3); r1(0) = 1; r1(1) = 0; @@ -46,91 +43,91 @@ TEST( Rot3, constructor) r3(0) = 0; r3(1) = 0; r3(2) = 1; - Rot3 actual(r1, r2, r3); + Rot3M actual(r1, r2, r3); CHECK(assert_equal(actual,expected)); } /* ************************************************************************* */ -TEST( Rot3, constructor2) +TEST( Rot3M, constructor2) { Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.); - Rot3 actual(R); - Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33); + Rot3M actual(R); + Rot3M expected(11, 12, 13, 21, 22, 23, 31, 32, 33); CHECK(assert_equal(actual,expected)); } /* ************************************************************************* */ -TEST( Rot3, constructor3) +TEST( Rot3M, constructor3) { - Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9); + Rot3M 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(Rot3(r1,r2,r3),expected)); + CHECK(assert_equal(Rot3M(r1,r2,r3),expected)); } /* ************************************************************************* */ -TEST( Rot3, transpose) +TEST( Rot3M, transpose) { - Rot3 R(1, 2, 3, 4, 5, 6, 7, 8, 9); + Rot3M 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(),Rot3(r1,r2,r3))); + CHECK(assert_equal(R.inverse(),Rot3M(r1,r2,r3))); } /* ************************************************************************* */ -TEST( Rot3, equals) +TEST( Rot3M, equals) { CHECK(R.equals(R)); - Rot3 zero; + Rot3M zero; CHECK(!R.equals(zero)); } /* ************************************************************************* */ // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... -Rot3 slow_but_correct_rodriguez(const Vector& w) { +Rot3M slow_but_correct_rodriguez(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); - if (t < 1e-5) return Rot3(); + if (t < 1e-5) return Rot3M(); Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J); return R; } /* ************************************************************************* */ -TEST( Rot3, rodriguez) +TEST( Rot3M, rodriguez) { - Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); + Rot3M R1 = Rot3M::rodriguez(epsilon, 0, 0); Vector w = Vector_(3, epsilon, 0., 0.); - Rot3 R2 = slow_but_correct_rodriguez(w); + Rot3M R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3, rodriguez2) +TEST( Rot3M, rodriguez2) { Vector axis = Vector_(3,0.,1.,0.); // rotation around Y double angle = 3.14 / 4.0; - Rot3 actual = Rot3::rodriguez(axis, angle); - Rot3 expected(0.707388, 0, 0.706825, + Rot3M actual = Rot3M::rodriguez(axis, angle); + Rot3M expected(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388); CHECK(assert_equal(expected,actual,1e-5)); } /* ************************************************************************* */ -TEST( Rot3, rodriguez3) +TEST( Rot3M, rodriguez3) { Vector w = Vector_(3, 0.1, 0.2, 0.3); - Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); - Rot3 R2 = slow_but_correct_rodriguez(w); + Rot3M R1 = Rot3M::rodriguez(w / norm_2(w), norm_2(w)); + Rot3M R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3, rodriguez4) +TEST( Rot3M, rodriguez4) { Vector axis = Vector_(3,0.,0.,1.); // rotation around Z double angle = M_PI_2; - Rot3 actual = Rot3::rodriguez(axis, angle); + Rot3M actual = Rot3M::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); - Rot3 expected(c,-s, 0, + Rot3M expected(c,-s, 0, s, c, 0, 0, 0, 1); CHECK(assert_equal(expected,actual,1e-5)); @@ -138,62 +135,62 @@ TEST( Rot3, rodriguez4) } /* ************************************************************************* */ -TEST( Rot3, expmap) +TEST( Rot3M, expmap) { Vector v = zero(3); CHECK(assert_equal(R.retract(v), R)); } /* ************************************************************************* */ -TEST(Rot3, log) +TEST(Rot3M, log) { Vector w1 = Vector_(3, 0.1, 0.0, 0.0); - Rot3 R1 = Rot3::rodriguez(w1); - CHECK(assert_equal(w1, Rot3::Logmap(R1))); + Rot3M R1 = Rot3M::rodriguez(w1); + CHECK(assert_equal(w1, Rot3M::Logmap(R1))); Vector w2 = Vector_(3, 0.0, 0.1, 0.0); - Rot3 R2 = Rot3::rodriguez(w2); - CHECK(assert_equal(w2, Rot3::Logmap(R2))); + Rot3M R2 = Rot3M::rodriguez(w2); + CHECK(assert_equal(w2, Rot3M::Logmap(R2))); Vector w3 = Vector_(3, 0.0, 0.0, 0.1); - Rot3 R3 = Rot3::rodriguez(w3); - CHECK(assert_equal(w3, Rot3::Logmap(R3))); + Rot3M R3 = Rot3M::rodriguez(w3); + CHECK(assert_equal(w3, Rot3M::Logmap(R3))); Vector w = Vector_(3, 0.1, 0.4, 0.2); - Rot3 R = Rot3::rodriguez(w); - CHECK(assert_equal(w, Rot3::Logmap(R))); + Rot3M R = Rot3M::rodriguez(w); + CHECK(assert_equal(w, Rot3M::Logmap(R))); Vector w5 = Vector_(3, 0.0, 0.0, 0.0); - Rot3 R5 = Rot3::rodriguez(w5); - CHECK(assert_equal(w5, Rot3::Logmap(R5))); + Rot3M R5 = Rot3M::rodriguez(w5); + CHECK(assert_equal(w5, Rot3M::Logmap(R5))); Vector w6 = Vector_(3, boost::math::constants::pi(), 0.0, 0.0); - Rot3 R6 = Rot3::rodriguez(w6); - CHECK(assert_equal(w6, Rot3::Logmap(R6))); + Rot3M R6 = Rot3M::rodriguez(w6); + CHECK(assert_equal(w6, Rot3M::Logmap(R6))); Vector w7 = Vector_(3, 0.0, boost::math::constants::pi(), 0.0); - Rot3 R7 = Rot3::rodriguez(w7); - CHECK(assert_equal(w7, Rot3::Logmap(R7))); + Rot3M R7 = Rot3M::rodriguez(w7); + CHECK(assert_equal(w7, Rot3M::Logmap(R7))); Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi()); - Rot3 R8 = Rot3::rodriguez(w8); - CHECK(assert_equal(w8, Rot3::Logmap(R8))); + Rot3M R8 = Rot3M::rodriguez(w8); + CHECK(assert_equal(w8, Rot3M::Logmap(R8))); } /* ************************************************************************* */ -TEST(Rot3, manifold) +TEST(Rot3M, manifold) { - Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); - Rot3 origin; + Rot3M gR1 = Rot3M::rodriguez(0.1, 0.4, 0.2); + Rot3M gR2 = Rot3M::rodriguez(0.3, 0.1, 0.7); + Rot3M origin; // log behaves correctly Vector d12 = gR1.localCoordinates(gR2); CHECK(assert_equal(gR2, gR1.retract(d12))); - CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); + CHECK(assert_equal(gR2, gR1*Rot3M::Expmap(d12))); Vector d21 = gR2.localCoordinates(gR1); CHECK(assert_equal(gR1, gR2.retract(d21))); - CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); + CHECK(assert_equal(gR1, gR2*Rot3M::Expmap(d21))); // Check that log(t1,t2)=-log(t2,t1) CHECK(assert_equal(d12,-d21)); @@ -201,11 +198,11 @@ TEST(Rot3, 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(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); + CHECK(assert_equal(Rot3M::Expmap(-d),Rot3M::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Rot3 R2 = Rot3::Expmap (2 * d); - Rot3 R3 = Rot3::Expmap (3 * d); - Rot3 R5 = Rot3::Expmap (5 * d); + Rot3M R2 = Rot3M::Expmap (2 * d); + Rot3M R3 = Rot3M::Expmap (3 * d); + Rot3M R5 = Rot3M::Expmap (5 * d); CHECK(assert_equal(R5,R2*R3)); CHECK(assert_equal(R5,R3*R2)); } @@ -226,106 +223,106 @@ AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { } /* ************************************************************************* */ -TEST(Rot3, BCH) +TEST(Rot3M, BCH) { // Approximate exmap by BCH formula AngularVelocity w1(0.2, -0.1, 0.1); AngularVelocity w2(0.01, 0.02, -0.03); - Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector()); - Rot3 R3 = R1 * R2; - Vector expected = Rot3::Logmap(R3); + Rot3M R1 = Rot3M::Expmap (w1.vector()), R2 = Rot3M::Expmap (w2.vector()); + Rot3M R3 = R1 * R2; + Vector expected = Rot3M::Logmap(R3); Vector actual = BCH(w1, w2).vector(); CHECK(assert_equal(expected, actual,1e-5)); } /* ************************************************************************* */ -TEST( Rot3, rotate_derivatives) +TEST( Rot3M, 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( Rot3, unrotate) +TEST( Rot3M, 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( Rot3, compose ) +TEST( Rot3M, compose ) { - Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); + Rot3M R1 = Rot3M::rodriguez(0.1, 0.2, 0.3); + Rot3M R2 = Rot3M::rodriguez(0.2, 0.3, 0.5); - Rot3 expected = R1 * R2; + Rot3M expected = R1 * R2; Matrix actualH1, actualH2; - Rot3 actual = R1.compose(R2, actualH1, actualH2); + Rot3M 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( Rot3, inverse ) +TEST( Rot3M, inverse ) { - Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3M R = Rot3M::rodriguez(0.1, 0.2, 0.3); - Rot3 I; + Rot3M 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( Rot3, between ) +TEST( Rot3M, between ) { - Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 origin; + Rot3M R = Rot3M::rodriguez(0.1, 0.4, 0.2); + Rot3M origin; CHECK(assert_equal(R, origin.between(R))); CHECK(assert_equal(R.inverse(), R.between(origin))); - Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); + Rot3M R1 = Rot3M::rodriguez(0.1, 0.2, 0.3); + Rot3M R2 = Rot3M::rodriguez(0.2, 0.3, 0.5); - Rot3 expected = R1.inverse() * R2; + Rot3M expected = R1.inverse() * R2; Matrix actualH1, actualH2; - Rot3 actual = R1.between(R2, actualH1, actualH2); + Rot3M 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( Rot3, xyz ) +TEST( Rot3M, xyz ) { double t = 0.1, st = sin(t), ct = cos(t); @@ -335,47 +332,47 @@ TEST( Rot3, xyz ) // z // | * Y=(ct,st) // x----y - Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct); - CHECK(assert_equal(expected1,Rot3::Rx(t))); + Rot3M expected1(1, 0, 0, 0, ct, -st, 0, st, ct); + CHECK(assert_equal(expected1,Rot3M::Rx(t))); // x // | * Z=(ct,st) // y----z - Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); - CHECK(assert_equal(expected2,Rot3::Ry(t))); + Rot3M expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); + CHECK(assert_equal(expected2,Rot3M::Ry(t))); // y // | X=* (ct,st) // z----x - Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); - CHECK(assert_equal(expected3,Rot3::Rz(t))); + Rot3M expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); + CHECK(assert_equal(expected3,Rot3M::Rz(t))); // Check compound rotation - 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))); + 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))); } /* ************************************************************************* */ -TEST( Rot3, yaw_pitch_roll ) +TEST( Rot3M, yaw_pitch_roll ) { double t = 0.1; // yaw is around z axis - CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); + CHECK(assert_equal(Rot3M::Rz(t),Rot3M::yaw(t))); // pitch is around y axis - CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); + CHECK(assert_equal(Rot3M::Ry(t),Rot3M::pitch(t))); // roll is around x axis - CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); + CHECK(assert_equal(Rot3M::Rx(t),Rot3M::roll(t))); // Check compound rotation - 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))); + 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))); } /* ************************************************************************* */ -TEST( Rot3, RQ) +TEST( Rot3M, RQ) { // Try RQ on a pure rotation Matrix actualK; @@ -385,18 +382,18 @@ TEST( Rot3, RQ) CHECK(assert_equal(eye(3),actualK)); CHECK(assert_equal(expected,actual,1e-6)); - // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] + // Try using xyz call, asserting that Rot3M::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),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3M::RzRyRx(0.1,0.2,0.3).xyz())); - // 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 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 ypr for pure yaw-pitch-roll matrices - 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())); + 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())); // 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); @@ -407,60 +404,60 @@ TEST( Rot3, RQ) } /* ************************************************************************* */ -TEST( Rot3, expmapStability ) { +TEST( Rot3M, expmapStability ) { Vector w = Vector_(3, 78e-9, 5e-8, 97e-7); double theta = w.norm(); double theta2 = theta*theta; - Rot3 actualR = Rot3::Expmap(w); + Rot3M actualR = Rot3M::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 ; - Rot3 expectedR( Rmat ); + Rot3M expectedR( Rmat ); CHECK(assert_equal(expectedR, actualR, 1e-10)); } /* ************************************************************************* */ -TEST( Rot3, logmapStability ) { +TEST( Rot3M, logmapStability ) { Vector w = Vector_(3, 1e-8, 0.0, 0.0); - Rot3 R = Rot3::Expmap(w); + Rot3M R = Rot3M::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 = Rot3::Logmap(R); + Vector actualw = Rot3M::Logmap(R); CHECK(assert_equal(w, actualw, 1e-15)); } /* ************************************************************************* */ -TEST(Rot3, quaternion) { +TEST(Rot3M, quaternion) { // NOTE: This is also verifying the ability to convert Vector to Quaternion Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); - Rot3 R1 = Rot3(Matrix_(3,3, + Rot3M R1 = Rot3M(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); - Rot3 R2 = Rot3(Matrix_(3,3, + Rot3M R2 = Rot3M(Matrix_(3,3, -0.207341903877828, 0.250149415542075, 0.945745528564780, 0.881304914479026, -0.371869043667957, 0.291573424846290, 0.424630407073532, 0.893945571198514, -0.143353873763946)); - // 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 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 converting Rot3 to quaterion + // Check converting Rot3M 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 Rot3 represent the same rotation + // Check that quaternion and Rot3M represent the same rotation Point3 p1(1.0, 2.0, 3.0); Point3 p2(8.0, 7.0, 9.0); diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp new file mode 100644 index 000000000..d9fe425f3 --- /dev/null +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -0,0 +1,518 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testRot3.cpp + * @brief Unit tests for Rot3Q class + * @author Alireza Fathi + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +Rot3Q R = Rot3Q::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) +{ + Rot3Q expected(eye(3, 3)); + Vector r1(3), r2(3), r3(3); + r1(0) = 1; + r1(1) = 0; + r1(2) = 0; + r2(0) = 0; + r2(1) = 1; + r2(2) = 0; + r3(0) = 0; + r3(1) = 0; + r3(2) = 1; + Rot3Q actual(r1, r2, r3); + CHECK(assert_equal(actual,expected)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, 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); + CHECK(assert_equal(actual,expected)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, constructor3) +{ + Rot3Q 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)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, equals) +{ + CHECK(R.equals(R)); + Rot3Q 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) { + double t = norm_2(w); + Matrix J = skewSymmetric(w / t); + if (t < 1e-5) return Rot3Q(); + Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J); + return R; +} + +/* ************************************************************************* */ +TEST( Rot3Q, rodriguez) +{ + Rot3Q R1 = Rot3Q::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)); + CHECK(assert_equal(R2,R1)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, 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, + 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) +{ + 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)); + CHECK(assert_equal(R2,R1)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, rodriguez4) +{ + Vector axis = Vector_(3,0.,0.,1.); // rotation around Z + double angle = M_PI_2; + Rot3Q actual = Rot3Q::rodriguez(axis, angle); + double c=cos(angle),s=sin(angle); + Rot3Q expected1(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(slow_but_correct_rodriguez(axis*angle),actual,1e-5)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, expmap) +{ + Vector v = zero(3); + CHECK(assert_equal(R.retract(v), R)); +} + +/* ************************************************************************* */ +TEST(Rot3Q, 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)); + + 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)); + + 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)); + + 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)); + + 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)); + + 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)); + + 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)); + + 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)); +} + +/* ************************************************************************* */ +TEST(Rot3Q, 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())); + + // 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))); + 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 that log(t1,t2)=-log(t2,t1) + CHECK(assert_equal(d12,-d21)); + + // 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())); + // 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); + CHECK(assert_equal(R5,R2*R3)); + CHECK(assert_equal(R5,R3*R2)); +} + +/* ************************************************************************* */ +class AngularVelocity: public Point3 { +public: + AngularVelocity(const Point3& p) : + Point3(p) { + } + AngularVelocity(double wx, double wy, double wz) : + Point3(wx, wy, wz) { + } +}; + +AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { + return X.cross(Y); +} + +/* ************************************************************************* */ +TEST(Rot3Q, 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); + Vector actual = BCH(w1, w2).vector(); + CHECK(assert_equal(expected, actual,1e-5)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, 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); + EXPECT(assert_equal(numerical1,actualDrotate1a,error)); + EXPECT(assert_equal(numerical2,actualDrotate1b,error)); + EXPECT(assert_equal(numerical3,actualDrotate2, error)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, 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); + CHECK(assert_equal(numerical1,H1,error)); + + Matrix numerical2 = numericalDerivative22(testing::unrotate, R, w); + CHECK(assert_equal(numerical2,H2,error)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, compose ) +{ + Rot3Q R1 = Rot3Q::rodriguez(0.1, 0.2, 0.3); + Rot3Q R2 = Rot3Q::rodriguez(0.2, 0.3, 0.5); + + Rot3Q expected = R1 * R2; + Matrix actualH1, actualH2; + Rot3Q actual = R1.compose(R2, actualH1, actualH2); + CHECK(assert_equal(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(testing::compose, R1, + R2, 1e-2); + CHECK(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22(testing::compose, R1, + R2, 1e-2); + CHECK(assert_equal(numericalH2,actualH2)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, inverse ) +{ + Rot3Q R = Rot3Q::rodriguez(0.1, 0.2, 0.3); + + Rot3Q I; + Matrix actualH; + CHECK(assert_equal(I,R*R.inverse(actualH))); + CHECK(assert_equal(I,R.inverse()*R)); + + Matrix numericalH = numericalDerivative11(testing::inverse, R); + CHECK(assert_equal(numericalH,actualH, 1e-4)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, between ) +{ + Rot3Q r1 = Rot3Q::Rz(M_PI/3.0); + Rot3Q r2 = Rot3Q::Rz(2.0*M_PI/3.0); + + Matrix expectedr1 = Matrix_(3,3, + 0.5, -sqrt(3.0)/2.0, 0.0, + sqrt(3.0)/2.0, 0.5, 0.0, + 0.0, 0.0, 1.0); + EXPECT(assert_equal(expectedr1, r1.matrix())); + + Rot3Q R = Rot3Q::rodriguez(0.1, 0.4, 0.2); + Rot3Q 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); + + Rot3Q expected = R1.inverse() * R2; + Matrix actualH1, actualH2; + Rot3Q actual = R1.between(R2, actualH1, actualH2); + CHECK(assert_equal(expected,actual)); + + 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); + 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 ) +{ + double t = 0.1, st = sin(t), ct = cos(t); + + // Make sure all counterclockwise + // Diagrams below are all from from unchanging axis + + // z + // | * Y=(ct,st) + // x----y + Rot3Q expected1(1, 0, 0, 0, ct, -st, 0, st, ct); + CHECK(assert_equal(expected1,Rot3Q::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))); + + // y + // | X=* (ct,st) + // z----x + Rot3Q expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); + CHECK(assert_equal(expected3,Rot3Q::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))); +} + +/* ************************************************************************* */ +TEST( Rot3Q, yaw_pitch_roll ) +{ + double t = 0.1; + + // yaw is around z axis + CHECK(assert_equal(Rot3Q::Rz(t),Rot3Q::yaw(t))); + + // pitch is around y axis + CHECK(assert_equal(Rot3Q::Ry(t),Rot3Q::pitch(t))); + + // roll is around x axis + CHECK(assert_equal(Rot3Q::Rx(t),Rot3Q::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))); +} + +/* ************************************************************************* */ +TEST( Rot3Q, RQ) +{ + // Try RQ on a pure rotation + Matrix actualK; + Vector actual; + boost::tie(actualK, actual) = RQ(R.matrix()); + Vector expected = Vector_(3, 0.14715, 0.385821, 0.231671); + 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] + 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())); + + // 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 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())); + + // 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); + Matrix A = K * R.matrix(); + boost::tie(actualK, actual) = RQ(A); + CHECK(assert_equal(K,actualK)); + CHECK(assert_equal(expected,actual,1e-6)); +} + +/* ************************************************************************* */ +TEST( Rot3Q, expmapStability ) { + Vector w = Vector_(3, 78e-9, 5e-8, 97e-7); + double theta = w.norm(); + double theta2 = theta*theta; + Rot3Q actualR = Rot3Q::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 ); + CHECK(assert_equal(expectedR, actualR, 1e-10)); +} + +// Does not work with Quaternions +///* ************************************************************************* */ +//TEST( Rot3Q, logmapStability ) { +// Vector w = Vector_(3, 1e-8, 0.0, 0.0); +// Rot3Q R = Rot3Q::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); +// CHECK(assert_equal(w, actualw, 1e-15)); +//} + +/* ************************************************************************* */ +TEST(Rot3Q, 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, + 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, + -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 converting Rot3Q 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 + Point3 p1(1.0, 2.0, 3.0); + Point3 p2(8.0, 7.0, 9.0); + + Point3 expected1 = R1*p1; + Point3 expected2 = R2*p2; + + Point3 actual1 = Point3(q1*p1.vector()); + Point3 actual2 = Point3(q2*p2.vector()); + + EXPECT(assert_equal(expected1, actual1)); + EXPECT(assert_equal(expected2, actual2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/tests/Makefile.am b/tests/Makefile.am index 39f3bfa5d..fb3de57f7 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -23,6 +23,7 @@ check_PROGRAMS += testNonlinearEqualityConstraint check_PROGRAMS += testPose2SLAMwSPCG check_PROGRAMS += testGaussianISAM2 check_PROGRAMS += testExtendedKalmanFilter +check_PROGRAMS += testRot3QOptimization # only if serialization is available if ENABLE_SERIALIZATION diff --git a/tests/testRot3QOptimization.cpp b/tests/testRot3QOptimization.cpp new file mode 100644 index 000000000..b94cad3a5 --- /dev/null +++ b/tests/testRot3QOptimization.cpp @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testRot3.cpp + * @brief Unit tests for Rot3Q class + * @author Richard Roberts + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(Rot3Q, optimize1) { + 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)); +} + +/* ************************************************************************* */ +TEST(Rot3Q, optimize) { + + // Optimize a circle + ValuesQ truth; + ValuesQ initial; + GraphQ fg; + fg.add(PriorQ(0, Rot3Q(), 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))); + } + + NonlinearOptimizationParameters params; + //params.verbosity_ = NonlinearOptimizationParameters::TRYLAMBDA; + ValuesQ final = optimize(fg, initial, params); + + EXPECT(assert_equal(truth, final, 1e-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +