12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged Rot3M.h and Rot3Q.h into Rot3.h, which now defines both Rot3M and Rot3Q.

release/4.3a0
Richard Roberts 2012-01-02 02:24:29 +00:00
parent c28bc7b06e
commit fa4af2e211
11 changed files with 390 additions and 644 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Doxyfile
View File

@ -1485,13 +1485,13 @@ ENABLE_PREPROCESSING = YES
# compilation will be performed. Macro expansion can be done in a controlled # compilation will be performed. Macro expansion can be done in a controlled
# way by setting EXPAND_ONLY_PREDEF to YES. # 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 # 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 # then the macro expansion is limited to the macros specified with the
# PREDEFINED and EXPAND_AS_DEFINED tags. # 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 # 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. # 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 # undefined via #undef or recursively expanded use the := operator
# instead of the = operator. # instead of the = operator.
PREDEFINED = PREDEFINED = __DOXYGEN
# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then # 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. # 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 # Use the PREDEFINED tag if you want to use a different macro definition that
# overrules the definition found in the source code. # 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 # If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then
# doxygen's preprocessor will remove all references to function-like macros # doxygen's preprocessor will remove all references to function-like macros

10
gtsam/geometry/Makefile.am
View File

@ -14,8 +14,7 @@ check_PROGRAMS =
headers += concepts.h headers += concepts.h
# Points and poses # Points and poses
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3M.cpp Rot3Q.cpp Pose3.cpp sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Pose3.cpp
headers += Rot3.h
check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3M tests/testRot3Q tests/testPose3 check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3M tests/testRot3Q tests/testPose3
# Cameras # Cameras
@ -36,6 +35,11 @@ check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental
# Timing tests # Timing tests
noinst_PROGRAMS = tests/timeRot3 tests/timeCalibratedCamera 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 # Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am # 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 geometrydir = $(pkgincludedir)/geometry
geometry_HEADERS = $(headers) geometry_HEADERS = $(headers)
noinst_LTLIBRARIES = libgeometry.la noinst_LTLIBRARIES = libgeometry.la
libgeometry_la_SOURCES = $(sources) libgeometry_la_SOURCES = $(allsources)
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS) AM_LDFLAGS = $(BOOST_LDFLAGS)

405
gtsam/geometry/Rot3.h
View File

@ -11,50 +11,383 @@
/** /**
* @file Rot3.h * @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 * @author Richard Roberts
*/ */
// \callgraph // \callgraph
#pragma once #include <gtsam/geometry/Point3.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
/* ************************************************************************* */
// The following preprocessor blocks select the main 3D rotation implementation, // Below is the class definition of Rot3. By the macros at the end of this
// creating a typedef from Rot3M (the rotation matrix implementation) or Rot3Q // file, both Rot3M and Rot3Q are actually defined with this interface.
// (the quaternion implementation) to Rot3. The type selected here will be #if defined Rot3 || defined __DOXYGEN
// used in all built-in gtsam geometry types that involve 3D rotations, such as
// Pose3, SimpleCamera, CalibratedCamera, StereoCamera, etc.
#ifdef GTSAM_DEFAULT_QUATERNIONS
#include <gtsam/geometry/Rot3Q.h>
namespace gtsam { namespace gtsam {
// Forward declarations;
class Rot3M;
class Rot3Q;
/// Typedef to an Eigen Quaternion<double>, 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<double, Eigen::DontAlign> Quaternion;
/** /**
* Typedef to the main 3D rotation implementation, which is Rot3M by default, * @brief A 3D rotation represented as a rotation matrix if the preprocessor
* or Rot3Q if GTSAM_DEFAULT_QUATERNIONS is defined. Depending on whether * symbol GTSAM_DEFAULT_QUATERNIONS is not defined, or as a quaternion if it
* GTSAM_DEFAULT_QUATERNIONS is defined, Rot3M (the rotation matrix * is defined.
* implementation) or Rot3Q (the quaternion implementation) will used in all * @ingroup geometry
* built-in gtsam geometry types that involve 3D rotations, such as Pose3, * \nosubgrouping
* SimpleCamera, CalibratedCamera, StereoCamera, etc.
*/ */
typedef Rot3Q Rot3; class Rot3 {
} public:
static const size_t dimension = 3;
#else
#include <gtsam/geometry/Rot3M.h>
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;
}
private:
#if defined ROT3_IS_MATRIX
/** We store columns ! */
Point3 r1_, r2_, r3_;
#elif defined ROT3_IS_QUATERNION
/** Internal Eigen Quaternion */
Quaternion quaternion_;
#endif #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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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<Matrix,Vector> 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 <gtsam/geometry/Rot3.h>
#undef Rot3
#undef ROT3_IS_MATRIX
// Define Rot3Q
#define Rot3 Rot3Q
#define ROT3_IS_QUATERNION
#include <gtsam/geometry/Rot3.h>
#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

6
gtsam/geometry/Rot3M.cpp
View File

@ -18,16 +18,12 @@
*/ */
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3M.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Lie-inl.h>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Rot3M);
static const Matrix I3 = eye(3); static const Matrix I3 = eye(3);
/* ************************************************************************* */ /* ************************************************************************* */

294
gtsam/geometry/Rot3M.h
View File

@ -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 <gtsam/geometry/Point3.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
namespace gtsam {
/// Typedef to an Eigen Quaternion<double>, 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<double, Eigen::DontAlign> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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<Matrix,Vector> RQ(const Matrix& A);
}

6
gtsam/geometry/Rot3Q.cpp
View File

@ -18,16 +18,12 @@
*/ */
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3Q.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Lie-inl.h>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Rot3Q);
static const Matrix I3 = eye(3); static const Matrix I3 = eye(3);
/* ************************************************************************* */ /* ************************************************************************* */

290
gtsam/geometry/Rot3Q.h
View File

@ -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 <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3M.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
namespace gtsam {
/// Typedef to an Eigen Quaternion<double>, 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<double, Eigen::DontAlign> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/**
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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<Matrix,Vector> RQ(const Matrix& A);
}

2
gtsam/geometry/tests/testRot3M.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3M.h> #include <gtsam/geometry/Rot3.h>
using namespace gtsam; using namespace gtsam;

2
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3Q.h> #include <gtsam/geometry/Rot3.h>
using namespace gtsam; using namespace gtsam;

2
tests/Makefile.am
View File

@ -22,7 +22,7 @@ check_PROGRAMS += testBoundingConstraint
check_PROGRAMS += testPose2SLAMwSPCG check_PROGRAMS += testPose2SLAMwSPCG
check_PROGRAMS += testGaussianISAM2 check_PROGRAMS += testGaussianISAM2
check_PROGRAMS += testExtendedKalmanFilter check_PROGRAMS += testExtendedKalmanFilter
check_PROGRAMS += testRot3QOptimization check_PROGRAMS += testRot3Optimization
# only if serialization is available # only if serialization is available
if ENABLE_SERIALIZATION if ENABLE_SERIALIZATION

9
tests/testRot3QOptimization.cpp → tests/testRot3Optimization.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3Q.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearOptimization.h> #include <gtsam/nonlinear/NonlinearOptimization.h>
@ -43,7 +43,9 @@ typedef BetweenFactor<ValuesM, KeyM> BetweenM;
typedef NonlinearFactorGraph<ValuesM> GraphM; typedef NonlinearFactorGraph<ValuesM> GraphM;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3Q, optimize1) { TEST(Rot3, optimize1) {
// Compare Rot3Q and Rot3M optimization
GraphQ fgQ; GraphQ fgQ;
fgQ.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01))); 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(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 // Optimize a circle
ValuesQ truth; ValuesQ truth;
@ -115,7 +117,6 @@ TEST(Rot3Q, optimize) {
} }
NonlinearOptimizationParameters params; NonlinearOptimizationParameters params;
//params.verbosity_ = NonlinearOptimizationParameters::TRYLAMBDA;
ValuesQ final = optimize(fg, initial, params); ValuesQ final = optimize(fg, initial, params);
EXPECT(assert_equal(truth, final, 1e-5)); EXPECT(assert_equal(truth, final, 1e-5));