diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index a4816914e..630c23755 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -30,6 +30,41 @@ INSTANTIATE_LIE(Rot3M); static const Matrix I3 = eye(3); +/* ************************************************************************* */ +Rot3M::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)) {} + +/* ************************************************************************* */ +Rot3M::Rot3M(const Point3& r1, const Point3& r2, const Point3& r3) : + r1_(r1), r2_(r2), r3_(r3) {} + +/* ************************************************************************* */ +Rot3M::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)) {} + +/* ************************************************************************* */ +Rot3M::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))) {} + +/* ************************************************************************* */ +Rot3M::Rot3M(const Quaternion& q) { + Eigen::Matrix3d R = q.toRotationMatrix(); + r1_ = Point3(R.col(0)); + r2_ = Point3(R.col(1)); + r3_ = Point3(R.col(2)); +} + +/* ************************************************************************* */ +Rot3M::Rot3M(const Rot3M& r) : r1_(r.r1_), r2_(r.r2_), r3_(r.r3_) {} + /* ************************************************************************* */ Rot3M Rot3M::Rx(double t) { double st = sin(t), ct = cos(t); @@ -140,12 +175,23 @@ Matrix Rot3M::transpose() const { Point3 Rot3M::column(int index) const{ if(index == 3) return r3_; - else if (index == 2) + else if(index == 2) return r2_; - else + else if(index == 1) return r1_; // default returns r1 + else + throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); } +/* ************************************************************************* */ +Point3 Rot3M::r1() const { return r1_; } + +/* ************************************************************************* */ +Point3 Rot3M::r2() const { return r2_; } + +/* ************************************************************************* */ +Point3 Rot3M::r3() const { return r3_; } + /* ************************************************************************* */ Vector Rot3M::xyz() const { Matrix I;Vector q; @@ -226,6 +272,26 @@ Rot3M Rot3M::compose (const Rot3M& R2, return *this * R2; } +/* ************************************************************************* */ +Point3 Rot3M::operator*(const Point3& p) const { return rotate(p); } + +/* ************************************************************************* */ +Rot3M Rot3M::inverse(boost::optional H1) 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()); +} + +/* ************************************************************************* */ +Quaternion Rot3M::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()); +} + /* ************************************************************************* */ Rot3M Rot3M::between (const Rot3M& R2, boost::optional H1, boost::optional H2) const { @@ -234,6 +300,11 @@ Rot3M Rot3M::between (const Rot3M& R2, return between_default(*this, R2); } +/* ************************************************************************* */ +Rot3M Rot3M::operator*(const Rot3M& R2) const { + return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); +} + /* ************************************************************************* */ pair RQ(const Matrix& A) { diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index 532e06c83..7fe6dc9f3 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -26,11 +26,15 @@ namespace gtsam { - typedef Eigen::Quaternion Quaternion; ///< Typedef to an Eigen Quaternion + /// Typedef to an Eigen Quaternion, we disable alignment because + /// geometry objects are stored in boost pool allocators, Values containers, + /// and and these pool allocators do not support alignment. + typedef Eigen::Quaternion Quaternion; /** * @brief 3D Rotations represented as rotation matrices * @ingroup geometry + * \nosubgrouping */ class Rot3M { public: @@ -42,11 +46,11 @@ namespace gtsam { public: + /// @name Constructors and named constructors + /// @{ + /** 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)) {} + Rot3M(); /** * Constructor from columns @@ -54,35 +58,26 @@ namespace gtsam { * @param r2 Y-axis of rotated frame * @param r3 Z-axis of rotated frame */ - Rot3M(const Point3& r1, const Point3& r2, const Point3& r3) : - r1_(r1), r2_(r2), r3_(r3) {} + Rot3M(const Point3& r1, const Point3& r2, const Point3& r3); - /** constructor from doubles in *row* order !!! */ + /** constructor from a rotation matrix, as doubles in *row-major* 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)) {} + double R31, double R32, double 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 rotation matrix */ + Rot3M(const Matrix& 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 */ - Rot3M(const Quaternion& q) { - Eigen::Matrix3d R = q.toRotationMatrix(); - r1_ = Point3(R.col(0)); - r2_ = Point3(R.col(1)); - r3_ = Point3(R.col(2)); - } + Rot3M(const Quaternion& q); + + /** Constructor from a rotation matrix in a Rot3M */ + Rot3M(const Rot3M& r); - /** Static member function to generate some well known rotations */ + /* Static member function to generate some well known rotations */ /** * Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix @@ -102,14 +97,14 @@ namespace gtsam { * 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 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) /// Returns rotation matrix nRb from body to nav frame static Rot3M ypr (double y, double p, double r) { return RzRyRx(r,p,y);} - /** Create from Quaternion parameters */ + /** Create from Quaternion coefficients */ static Rot3M quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3M(q); } /** @@ -129,14 +124,15 @@ namespace gtsam { /** * Rodriguez' formula to compute an incremental rotation matrix - * @param wx - * @param wy - * @param wz + * @param wx Incremental roll (about X) + * @param wy Incremental pitch (about Y) + * @param wz Incremental yaw (about Z) * @return incremental rotation matrix */ static Rot3M rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));} + /// @} /// @name Testable /// @{ @@ -160,16 +156,10 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; /// rotate point from rotated coordinate frame to world = R*p - inline Point3 operator*(const Point3& p) const { return rotate(p);} + Point3 operator*(const Point3& p) const; - /// 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()); - } + /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity + Rot3M inverse(boost::optional H1=boost::none) const; /// @} /// @name Manifold @@ -181,7 +171,7 @@ namespace gtsam { /// return dimensionality of tangent space, DOF = 3 size_t dim() const { return dimension; } - /// Updates a with tangent space delta + /// Retraction from R^3 to Pose2 manifold neighborhood around current pose Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } /// Returns inverse retraction @@ -203,7 +193,7 @@ namespace gtsam { /** * Log map at identity - return the canonical coordinates of this rotation */ - static Vector Logmap(const Rot3M& R); + static Vector Logmap(const Rot3M& R); /// @} @@ -215,9 +205,9 @@ namespace gtsam { /** 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_; } + Point3 r1() const; + Point3 r2() const; + Point3 r3() const; /** * Use RQ to calculate xyz angle representation @@ -250,12 +240,7 @@ namespace gtsam { /** 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()); - } + Quaternion toQuaternion() const; /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' @@ -265,9 +250,7 @@ namespace gtsam { 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_)); - } + Rot3M operator*(const Rot3M& R2) const; /** * rotate point from rotated coordinate frame to diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 84aa4523a..425aa54ca 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -31,8 +31,44 @@ namespace gtsam { static const Matrix I3 = eye(3); /* ************************************************************************* */ - // static member functions to construct rotations + Rot3Q::Rot3Q() : quaternion_(Quaternion::Identity()) {} + /* ************************************************************************* */ + Rot3Q::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()) {} + + /* ************************************************************************* */ + Rot3Q::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()) {} + + /* ************************************************************************* */ + Rot3Q::Rot3Q(const Matrix& R) : + quaternion_(Eigen::Matrix3d(R)) {} + + /* ************************************************************************* */ + Rot3Q::Rot3Q(const Quaternion& q) : quaternion_(q) {} + + /* ************************************************************************* */ + Rot3Q::Rot3Q(const Rot3M& r) : quaternion_(Eigen::Matrix3d(r.matrix())) {} + + /* ************************************************************************* */ + Rot3Q Rot3Q::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } + + /* ************************************************************************* */ + Rot3Q Rot3Q::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } + + /* ************************************************************************* */ + Rot3Q Rot3Q::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } + + /* ************************************************************************* */ 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())) * @@ -61,6 +97,27 @@ namespace gtsam { /* ************************************************************************* */ Matrix Rot3Q::transpose() const { return quaternion_.toRotationMatrix().transpose(); } + /* ************************************************************************* */ + Point3 Rot3Q::column(int index) const{ + if(index == 3) + return r3(); + else if(index == 2) + return r2(); + else if(index == 1) + return r1(); // default returns r1 + else + throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); + } + + /* ************************************************************************* */ + Point3 Rot3Q::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } + + /* ************************************************************************* */ + Point3 Rot3Q::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); } + + /* ************************************************************************* */ + Point3 Rot3Q::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } + /* ************************************************************************* */ Vector Rot3Q::xyz() const { Matrix I;Vector q; @@ -68,11 +125,13 @@ namespace gtsam { 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)); @@ -118,6 +177,21 @@ namespace gtsam { return Rot3Q(quaternion_ * R2.quaternion_); } + /* ************************************************************************* */ + Point3 Rot3Q::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 { + if (H1) *H1 = -matrix(); + return Rot3Q(quaternion_.inverse()); + } + + /* ************************************************************************* */ + Quaternion Rot3Q::toQuaternion() const { return quaternion_; } + /* ************************************************************************* */ Rot3Q Rot3Q::between(const Rot3Q& R2, boost::optional H1, boost::optional H2) const { @@ -127,5 +201,10 @@ namespace gtsam { } /* ************************************************************************* */ + Rot3Q Rot3Q::operator*(const Rot3Q& R2) const { + return Rot3Q(quaternion_ * R2.quaternion_); + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3Q.h b/gtsam/geometry/Rot3Q.h index beed6ef41..7cd9bac97 100644 --- a/gtsam/geometry/Rot3Q.h +++ b/gtsam/geometry/Rot3Q.h @@ -49,35 +49,32 @@ namespace gtsam { /// @{ /** default constructor, unit rotation */ - Rot3Q() : quaternion_(Quaternion::Identity()) {} + Rot3Q(); - /** 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 columns + * @param r1 X-axis of rotated frame + * @param r2 Y-axis of rotated frame + * @param r3 Z-axis of rotated frame + */ + Rot3Q(const Point3& r1, const Point3& r2, const Point3& r3); /** constructor from a rotation matrix, as doubles in *row-major* 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()) {} + double R31, double R32, double R33); /** constructor from a rotation matrix */ - Rot3Q(const Matrix& R) : quaternion_(Eigen::Matrix3d(R)) {} + Rot3Q(const Matrix& 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) {} + Rot3Q(const Quaternion& q); /** Constructor from a rotation matrix in a Rot3M */ - Rot3Q(const Rot3M& r) : quaternion_(Eigen::Matrix3d(r.matrix())) {} + Rot3Q(const Rot3M& r); /* Static member function to generate some well known rotations */ @@ -85,9 +82,9 @@ namespace gtsam { * 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 Rx(double t); + static Rot3Q Ry(double t); + static Rot3Q Rz(double t); static Rot3Q RzRyRx(double x, double y, double z); inline static Rot3Q RzRyRx(const Vector& xyz) { assert(xyz.size() == 3); @@ -102,6 +99,8 @@ namespace gtsam { 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) + + /// Returns rotation matrix nRb from body to nav frame static Rot3Q ypr (double y, double p, double r) { return RzRyRx(r,p,y);} /** Create from Quaternion coefficients */ @@ -151,21 +150,15 @@ namespace gtsam { return Rot3Q(); } - /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3Q inverse(boost::optional H1=boost::none) const { - if (H1) *H1 = -matrix(); - return Rot3Q(quaternion_.inverse()); - } - /// Compose two rotations i.e., R= (*this) * R2 Rot3Q compose(const Rot3Q& R2, boost::optional H1=boost::none, boost::optional H2=boost::none) const; /// 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)); - } + Point3 operator*(const Point3& p) const; + + /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity + Rot3Q inverse(boost::optional H1=boost::none) const; /// @} /// @name Manifold @@ -210,9 +203,10 @@ namespace gtsam { 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)); } + Point3 column(int index) const; + Point3 r1() const; + Point3 r2() const; + Point3 r3() const; /** * Use RQ to calculate xyz angle representation @@ -232,12 +226,20 @@ namespace gtsam { */ Vector rpy() const; - /** Compute the quaternion representation of this rotation - this is to - * maintain a standard Rot3 API with Rot3M, but here just returns the - * internal quaternion. + /** + * Accessors to get to components of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient + */ + inline double roll() const { return ypr()(2); } + inline double pitch() const { return ypr()(1); } + inline double yaw() const { return ypr()(0); } + + /** Compute the quaternion representation of this rotation. * @return The quaternion */ - Quaternion toQuaternion() const { return quaternion_; } + Quaternion toQuaternion() const; /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' @@ -247,7 +249,7 @@ namespace gtsam { boost::optional H2=boost::none) const; /** compose two rotations */ - Rot3Q operator*(const Rot3Q& R2) const { return Rot3Q(quaternion_ * R2.quaternion_); } + Rot3Q operator*(const Rot3Q& R2) const; /** * rotate point from rotated coordinate frame to