From fa4af2e2116f848f2375508c1945265e473c145a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 2 Jan 2012 02:24:29 +0000 Subject: [PATCH] Merged Rot3M.h and Rot3Q.h into Rot3.h, which now defines both Rot3M and Rot3Q. --- Doxyfile | 8 +- gtsam/geometry/Makefile.am | 10 +- gtsam/geometry/Rot3.h | 405 ++++++++++++++++-- gtsam/geometry/Rot3M.cpp | 6 +- gtsam/geometry/Rot3M.h | 294 ------------- gtsam/geometry/Rot3Q.cpp | 6 +- gtsam/geometry/Rot3Q.h | 290 ------------- gtsam/geometry/tests/testRot3M.cpp | 2 +- gtsam/geometry/tests/testRot3Q.cpp | 2 +- tests/Makefile.am | 2 +- ...imization.cpp => testRot3Optimization.cpp} | 9 +- 11 files changed, 390 insertions(+), 644 deletions(-) delete mode 100644 gtsam/geometry/Rot3M.h delete mode 100644 gtsam/geometry/Rot3Q.h rename tests/{testRot3QOptimization.cpp => testRot3Optimization.cpp} (96%) diff --git a/Doxyfile b/Doxyfile index fafe6372f..341ae16dd 100644 --- a/Doxyfile +++ b/Doxyfile @@ -1485,13 +1485,13 @@ ENABLE_PREPROCESSING = YES # compilation will be performed. Macro expansion can be done in a controlled # way by setting EXPAND_ONLY_PREDEF to YES. -MACRO_EXPANSION = NO +MACRO_EXPANSION = YES # If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES # then the macro expansion is limited to the macros specified with the # PREDEFINED and EXPAND_AS_DEFINED tags. -EXPAND_ONLY_PREDEF = NO +EXPAND_ONLY_PREDEF = YES # If the SEARCH_INCLUDES tag is set to YES (the default) the includes files # pointed to by INCLUDE_PATH will be searched when a #include is found. @@ -1519,7 +1519,7 @@ INCLUDE_FILE_PATTERNS = # undefined via #undef or recursively expanded use the := operator # instead of the = operator. -PREDEFINED = +PREDEFINED = __DOXYGEN # If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then # this tag can be used to specify a list of macro names that should be expanded. @@ -1527,7 +1527,7 @@ PREDEFINED = # Use the PREDEFINED tag if you want to use a different macro definition that # overrules the definition found in the source code. -EXPAND_AS_DEFINED = +EXPAND_AS_DEFINED = Rot3 # If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then # doxygen's preprocessor will remove all references to function-like macros diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am index 42a325971..78bf06b9b 100644 --- a/gtsam/geometry/Makefile.am +++ b/gtsam/geometry/Makefile.am @@ -14,8 +14,7 @@ check_PROGRAMS = headers += concepts.h # Points and poses -sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3M.cpp Rot3Q.cpp Pose3.cpp -headers += Rot3.h +sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Pose3.cpp check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3M tests/testRot3Q tests/testPose3 # Cameras @@ -36,6 +35,11 @@ check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental # Timing tests noinst_PROGRAMS = tests/timeRot3 tests/timeCalibratedCamera +# Rot3M and Rot3Q both use Rot3.h, they do not have individual header files +allsources = $(sources) +allsources += Rot3M.cpp Rot3Q.cpp +headers += Rot3.h + #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed # It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am @@ -45,7 +49,7 @@ headers += $(sources:.cpp=.h) geometrydir = $(pkgincludedir)/geometry geometry_HEADERS = $(headers) noinst_LTLIBRARIES = libgeometry.la -libgeometry_la_SOURCES = $(sources) +libgeometry_la_SOURCES = $(allsources) AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_LDFLAGS = $(BOOST_LDFLAGS) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 1c34322fc..7f65cacee 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -11,50 +11,383 @@ /** * @file Rot3.h - * @brief Contains a typedef to the default 3D rotation implementation determined at compile time, see Rot3M and Rot3Q. + * @brief A common header file for rotation matrix and quaterion rotations, Rot3M and Rot3Q, as well as a typedef of Rot3 to the default implementation. * @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 +/* ************************************************************************* */ +// Below is the class definition of Rot3. By the macros at the end of this +// file, both Rot3M and Rot3Q are actually defined with this interface. +#if defined Rot3 || defined __DOXYGEN namespace gtsam { + + // Forward declarations; + class Rot3M; + class Rot3Q; + + /// Typedef to an Eigen Quaternion, we disable alignment because + /// geometry objects are stored in boost pool allocators, in Values + /// containers, and and these pool allocators do not support alignment. + typedef Eigen::Quaternion Quaternion; + /** - * 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. + * @brief A 3D rotation represented as a rotation matrix if the preprocessor + * symbol GTSAM_DEFAULT_QUATERNIONS is not defined, or as a quaternion if it + * is defined. + * @ingroup geometry + * \nosubgrouping */ - 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; -} + class Rot3 { + public: + static const size_t dimension = 3; + private: +#if defined ROT3_IS_MATRIX + /** We store columns ! */ + Point3 r1_, r2_, r3_; +#elif defined ROT3_IS_QUATERNION + /** Internal Eigen Quaternion */ + Quaternion quaternion_; #endif + + public: + + /// @name Constructors and named constructors + /// @{ + + /** default constructor, unit rotation */ + Rot3(); + + /** + * Constructor from columns + * @param r1 X-axis of rotated frame + * @param r2 Y-axis of rotated frame + * @param r3 Z-axis of rotated frame + */ + Rot3(const Point3& r1, const Point3& r2, const Point3& r3); + + /** constructor from a rotation matrix, as doubles in *row-major* order !!! */ + Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33); + + /** constructor from a rotation matrix */ + Rot3(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 + */ + Rot3(const Quaternion& q); + + /** Constructor from a rotation matrix in a Rot3M */ + Rot3(const Rot3M& r); + + /* Static member function to generate some well known rotations */ + + /// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. + static Rot3 Rx(double t); + + /// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. + static Rot3 Ry(double t); + + /// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. + static Rot3 Rz(double t); + + /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. + static Rot3 RzRyRx(double x, double y, double z); + + /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. + inline static Rot3 RzRyRx(const Vector& xyz) { + assert(xyz.size() == 3); + return RzRyRx(xyz(0), xyz(1), xyz(2)); + } + + /** + * Positive yaw is to right (as in aircraft heading). + * 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 pitch is up (increasing aircraft altitude). + * 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 pitch(double t) { return Ry(t); } + + /** + * Positive roll is to right (increasing yaw in aircraft). + * 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 roll (double t) { return Rx(t); } + + /** Returns rotation nRb from body to nav frame. + * Positive yaw is to right (as in aircraft heading). + * Positive pitch is up (increasing aircraft altitude). + * Positive roll is to right (increasing yaw in aircraft). + * 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 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} + + /** Create from Quaternion coefficients */ + 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 Incremental roll (about X) + * @param wy Incremental pitch (about Y) + * @param wz Incremental yaw (about Z) + * @return incremental rotation matrix + */ + static Rot3 rodriguez(double wx, double wy, double wz) + { return rodriguez(Vector_(3,wx,wy,wz));} + + /// @} + /// @name Testable + /// @{ + + /** 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; + + /// @} + /// @name Group + /// @{ + + /// identity rotation for group operation + inline static Rot3 identity() { + return Rot3(); + } + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3 compose(const Rot3& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /// rotate point from rotated coordinate frame to world = R*p + Point3 operator*(const Point3& p) const; + + /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity + Rot3 inverse(boost::optional H1=boost::none) const; + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + size_t dim() const { return dimension; } + + /// Retraction from R^3 to Pose2 manifold neighborhood around current pose + Rot3 retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + Vector localCoordinates(const Rot3& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /** + * 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); + } + + /** + * Log map at identity - return the canonical coordinates of this rotation + */ + static Vector Logmap(const Rot3& R); + + /// @} + + /** 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; + Point3 r2() const; + Point3 r3() const; + + /** + * 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; + + /** + * Accessor to get to component 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); } + + /** + * Accessor to get to component 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 pitch() const { return ypr()(1); } + + /** + * Accessor to get to component 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 yaw() const { return ypr()(0); } + + /** Compute the quaternion representation of this rotation. + * @return The quaternion + */ + Quaternion toQuaternion() const; + + /** + * 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; + + /** + * 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) + { +#if defined ROT3_IS_MATRIX + ar & BOOST_SERIALIZATION_NVP(r1_); + ar & BOOST_SERIALIZATION_NVP(r2_); + ar & BOOST_SERIALIZATION_NVP(r3_); +#elif defined ROT3_IS_QUATERNION + ar & BOOST_SERIALIZATION_NVP(quaternion_); +#endif + } + }; + + /** + * [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); +} + +#endif // if defined Rot3 || defined __DOXYGEN + + +/* ************************************************************************* */ +// This block of code defines both Rot3Q and Rot3M, by self-including Rot3.h +// twice and using preprocessor definitions of Rot3 to be Rot3M and Rot3Q. It +// then creates a typedef of Rot3 to either Rot3M or Rot3Q, depending on +// whether GTSAM_DEFAULT_QUATERNIONS is defined. +#if !defined __ROT3_H +#define __ROT3_H + +// Define Rot3M +#define Rot3 Rot3M +#define ROT3_IS_MATRIX +#include +#undef Rot3 +#undef ROT3_IS_MATRIX + +// Define Rot3Q +#define Rot3 Rot3Q +#define ROT3_IS_QUATERNION +#include +#undef Rot3 +#undef ROT3_IS_QUATERNION + +// Create Rot3 typedef +namespace gtsam { + /** + * Typedef to the main 3D rotation implementation, which is Rot3M by default, + * or Rot3Q if GTSAM_DEFAULT_QUATERNIONS is defined. Depending on whether + * GTSAM_DEFAULT_QUATERNIONS is defined, Rot3M (the rotation matrix + * implementation) or Rot3Q (the quaternion implementation) will used in all + * built-in gtsam geometry types that involve 3D rotations, such as Pose3, + * SimpleCamera, CalibratedCamera, StereoCamera, etc. + */ +#ifdef GTSAM_DEFAULT_QUATERNIONS + typedef Rot3Q Rot3; +#else + typedef Rot3M Rot3; +#endif +} + +#endif // if !defined Rot3 + diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 630c23755..f83764565 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -18,16 +18,12 @@ */ #include -#include -#include +#include using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Rot3M); - static const Matrix I3 = eye(3); /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h deleted file mode 100644 index 7fe6dc9f3..000000000 --- a/gtsam/geometry/Rot3M.h +++ /dev/null @@ -1,294 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 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: - static const size_t dimension = 3; - - private: - /** we store columns ! */ - Point3 r1_, r2_, r3_; - - public: - - /// @name Constructors and named constructors - /// @{ - - /** default constructor, unit rotation */ - Rot3M(); - - /** - * Constructor from columns - * @param r1 X-axis of rotated frame - * @param r2 Y-axis of rotated frame - * @param r3 Z-axis of rotated frame - */ - Rot3M(const Point3& r1, const Point3& r2, const Point3& r3); - - /** 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); - - /** 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); - - /** Constructor from a rotation matrix in a Rot3M */ - Rot3M(const Rot3M& r); - - /* 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) - - /// 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 coefficients */ - 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 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 - /// @{ - - /** 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; - - /// @} - /// @name Group - /// @{ - - /// identity for group operation - inline static Rot3M identity() { - return Rot3M(); - } - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3M compose(const Rot3M& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /// rotate point from rotated coordinate frame to world = R*p - Point3 operator*(const Point3& p) const; - - /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3M inverse(boost::optional H1=boost::none) const; - - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - static size_t Dim() { return dimension; } - - /// return dimensionality of tangent space, DOF = 3 - size_t dim() const { return dimension; } - - /// Retraction from R^3 to Pose2 manifold neighborhood around current pose - Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } - - /// Returns inverse retraction - Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } - - /// @} - /// @name Lie Group - /// @{ - - /** - * 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); - } - - /** - * Log map at identity - return the canonical coordinates of this rotation - */ - static Vector Logmap(const Rot3M& R); - - /// @} - - /** 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; - Point3 r2() const; - Point3 r3() const; - - /** - * 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 rpy s.t. R = Rot3M::ypr(y,p,r) - */ - Vector rpy() const; - - /** - * 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 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; - - /** - * 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 index 425aa54ca..232e896f3 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -18,16 +18,12 @@ */ #include -#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); /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.h b/gtsam/geometry/Rot3Q.h deleted file mode 100644 index 7cd9bac97..000000000 --- a/gtsam/geometry/Rot3Q.h +++ /dev/null @@ -1,290 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 a quaternion - * @author Richard Roberts - */ - -// \callgraph - -#pragma once - -#include -#include -#include - -namespace gtsam { - - /// 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 quaternions - * @ingroup geometry - * \nosubgrouping - */ - class Rot3Q { - public: - static const size_t dimension = 3; - - private: - /** Internal Eigen Quaternion */ - Quaternion quaternion_; - - public: - - /// @name Constructors and named constructors - /// @{ - - /** default constructor, unit rotation */ - Rot3Q(); - - /** - * 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); - - /** constructor from a rotation matrix */ - 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); - - /** Constructor from a rotation matrix in a Rot3M */ - Rot3Q(const Rot3M& r); - - /* 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); - 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); - 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) - - /// 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 */ - 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));} - - /// @} - /// @name Testable - /// @{ - - /** 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; - - /// @} - /// @name Group - /// @{ - - /// identity rotation for group operation - inline static Rot3Q identity() { - return Rot3Q(); - } - - /// 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 - 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 - /// @{ - - /// dimension of the variable - used to autodetect sizes - static size_t Dim() { return dimension; } - - /// return dimensionality of tangent space, DOF = 3 - size_t dim() const { return dimension; } - - /// Retraction from R^3 to Pose2 manifold neighborhood around current pose - Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); } - - /// Returns inverse retraction - Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); } - - /// @} - /// @name Lie Group - /// @{ - - /** - * 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); - } - - /** - * Log map at identity - return the canonical coordinates of this rotation - */ - static Vector Logmap(const Rot3Q& R); - - /// @} - - /** 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; - Point3 r2() const; - Point3 r3() const; - - /** - * 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; - - /** - * 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 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; - - /** - * 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(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/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 5e8cb48ae..0b1f1a31b 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include using namespace gtsam; diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index d9fe425f3..4619e5560 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include using namespace gtsam; diff --git a/tests/Makefile.am b/tests/Makefile.am index 2807e1fd8..2ed6139bb 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -22,7 +22,7 @@ check_PROGRAMS += testBoundingConstraint check_PROGRAMS += testPose2SLAMwSPCG check_PROGRAMS += testGaussianISAM2 check_PROGRAMS += testExtendedKalmanFilter -check_PROGRAMS += testRot3QOptimization +check_PROGRAMS += testRot3Optimization # only if serialization is available if ENABLE_SERIALIZATION diff --git a/tests/testRot3QOptimization.cpp b/tests/testRot3Optimization.cpp similarity index 96% rename from tests/testRot3QOptimization.cpp rename to tests/testRot3Optimization.cpp index 5995c5a98..7c575de0f 100644 --- a/tests/testRot3QOptimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -43,7 +43,9 @@ typedef BetweenFactor BetweenM; typedef NonlinearFactorGraph GraphM; /* ************************************************************************* */ -TEST(Rot3Q, optimize1) { +TEST(Rot3, optimize1) { + // Compare Rot3Q and Rot3M optimization + GraphQ fgQ; fgQ.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01))); fgQ.add(BetweenQ(0, 1, Rot3Q::Rz(M_PI/3.0), sharedSigma(3, 0.01))); @@ -101,7 +103,7 @@ TEST(Rot3Q, optimize1) { } /* ************************************************************************* */ -TEST(Rot3Q, optimize) { +TEST(Rot3, optimize) { // Optimize a circle ValuesQ truth; @@ -115,7 +117,6 @@ TEST(Rot3Q, optimize) { } NonlinearOptimizationParameters params; - //params.verbosity_ = NonlinearOptimizationParameters::TRYLAMBDA; ValuesQ final = optimize(fg, initial, params); EXPECT(assert_equal(truth, final, 1e-5));