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

Quaternion implementation of Rot3, made default with GTSAM_DEFAULT_QUATERNIONS

release/4.3a0
Richard Roberts 2011-11-09 01:40:20 +00:00
parent 3cd3a06eb7
commit 716c5175ab
13 changed files with 1681 additions and 573 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Makefile.am
View File

@ -14,8 +14,8 @@ check_PROGRAMS =
headers += concepts.h
# Points and poses
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp
check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3 tests/testPose3
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3M.cpp Rot3Q.cpp Pose3.cpp
check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3M tests/testRot3Q tests/testPose3
# Cameras
headers += GeneralCameraT.h Cal3_S2Stereo.h

281
gtsam/geometry/Pose3.h
View File

@ -23,181 +23,178 @@
namespace gtsam {
/**
* A 3D pose (R,t) : (Rot3,Point3)
* @ingroup geometry
*/
class Pose3 {
public:
static const size_t dimension = 6;
/**
* A 3D pose (R,t) : (Rot3,Point3)
* @ingroup geometry
*/
class Pose3 {
public:
static const size_t dimension = 6;
/** Pose Concept requirements */
typedef Rot3 Rotation;
typedef Point3 Translation;
/** Pose Concept requirements */
typedef Rot3 Rotation;
typedef Point3 Translation;
private:
Rot3 R_;
Point3 t_;
private:
Rot3 R_;
Point3 t_;
public:
public:
/** Default constructor is origin */
Pose3() {}
/** Default constructor is origin */
Pose3() {}
/** Copy constructor */
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {}
/** Copy constructor */
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {}
/** Construct from R,t */
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
/** Construct from R,t */
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
/** Constructor from 4*4 matrix */
Pose3(const Matrix &T) :
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {}
/** Constructor from 4*4 matrix */
Pose3(const Matrix &T) :
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {}
/** Constructor from 12D vector */
Pose3(const Vector &V) :
R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)),
t_(V(9), V(10),V(11)) {}
const Rot3& rotation() const { return R_; }
const Point3& translation() const { return t_; }
inline const Rot3& rotation() const { return R_; }
inline const Point3& translation() const { return t_; }
double x() const { return t_.x(); }
double y() const { return t_.y(); }
double z() const { return t_.z(); }
inline double x() const { return t_.x(); }
inline double y() const { return t_.y(); }
inline double z() const { return t_.z(); }
/** convert to 4*4 matrix */
Matrix matrix() const;
/** convert to 4*4 matrix */
Matrix matrix() const;
/** print with optional string */
void print(const std::string& s = "") const;
/** print with optional string */
void print(const std::string& s = "") const;
/** assert equality up to a tolerance */
bool equals(const Pose3& pose, double tol = 1e-9) const;
/** assert equality up to a tolerance */
bool equals(const Pose3& pose, double tol = 1e-9) const;
/** Compose two poses */
Pose3 operator*(const Pose3& T) const {
return Pose3(R_*T.R_, t_ + R_*T.t_);
}
/** Compose two poses */
inline Pose3 operator*(const Pose3& T) const {
return Pose3(R_*T.R_, t_ + R_*T.t_);
}
Pose3 transform_to(const Pose3& pose) const;
Pose3 transform_to(const Pose3& pose) const;
/** dimension of the variable - used to autodetect sizes */
static size_t Dim() { return dimension; }
/** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; }
/** Lie requirements */
/** Lie requirements */
/** Dimensionality of the tangent space */
size_t dim() const { return dimension; }
/** Dimensionality of the tangent space */
inline size_t dim() const { return dimension; }
/** identity */
static Pose3 identity() {
return Pose3();
}
/** identity */
inline static Pose3 identity() {
return Pose3();
}
/**
* Derivative of inverse
*/
Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/**
* Derivative of inverse
*/
Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/**
* composes two poses (first (*this) then p2)
* with optional derivatives
*/
Pose3 compose(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* composes two poses (first (*this) then p2)
* with optional derivatives
*/
Pose3 compose(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** syntactic sugar for transform */
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
/** syntactic sugar for transform */
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Exponential map around another pose */
Pose3 retract(const Vector& d) const;
/** Exponential map around another pose */
Pose3 retract(const Vector& d) const;
/** Logarithm map around another pose T1 */
Vector localCoordinates(const Pose3& T2) const;
/** Logarithm map around another pose T1 */
Vector localCoordinates(const Pose3& T2) const;
/** non-approximated versions of Expmap/Logmap */
static Pose3 Expmap(const Vector& xi);
static Vector Logmap(const Pose3& p);
/** non-approximated versions of Expmap/Logmap */
static Pose3 Expmap(const Vector& xi);
static Vector Logmap(const Pose3& p);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate Adjoint map
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
*/
Matrix AdjointMap() const;
Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
/**
* Calculate Adjoint map
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
*/
Matrix AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = (wx,wy,wz) 3D angular velocity
* v (vx,vy,vz) = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
return Matrix_(4,4,
0.,-wz, wy, vx,
wz, 0.,-wx, vy,
-wy, wx, 0., vz,
0., 0., 0., 0.);
}
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = (wx,wy,wz) 3D angular velocity
* v (vx,vy,vz) = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
static inline Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
return Matrix_(4,4,
0.,-wz, wy, vx,
wz, 0.,-wx, vy,
-wy, wx, 0., vz,
0., 0., 0., 0.);
}
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @return range (double)
*/
double range(const Point3& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @return range (double)
*/
double range(const Point3& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate range to another pose
* @param point SO(3) pose of landmark
* @return range (double)
*/
double range(const Pose3& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate range to another pose
* @param point SO(3) pose of landmark
* @return range (double)
*/
double range(const Pose3& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}
}; // Pose3 class
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}
}; // Pose3 class
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = 3D angular velocity
* v = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
template <>
inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
}
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = 3D angular velocity
* v = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
template <>
inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
}
} // namespace gtsam

299
gtsam/geometry/Rot3.h
View File

@ -11,273 +11,50 @@
/**
* @file Rot3.h
* @brief 3D Rotation represented as 3*3 matrix
* @author Alireza Fathi
* @author Christian Potthast
* @author Frank Dellaert
* @brief Contains a typedef to the default 3D rotation implementation determined at compile time
* @author Richard Roberts
*/
// \callgraph
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
// 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 <gtsam/geometry/Rot3Q.h>
namespace gtsam {
typedef Eigen::Quaterniond Quaternion;
/**
* @brief 3D Rotations represented as rotation matrices
* @ingroup geometry
*/
class Rot3 {
public:
static const size_t dimension = 3;
private:
/** we store columns ! */
Point3 r1_, r2_, r3_;
public:
/** default constructor, unit rotation */
Rot3() :
r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {}
/** constructor from columns */
Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {}
/** constructor from doubles in *row* order !!! */
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
/** constructor from matrix */
Rot3(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
* @param q The quaternion
*/
Rot3(const Quaternion& q) {
Eigen::Matrix3d R = q.toRotationMatrix();
r1_ = Point3(R.col(0));
r2_ = Point3(R.col(1));
r3_ = Point3(R.col(2));
}
/** Static member function to generate some well known rotations */
/**
* Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix
* Counterclockwise when looking from unchanging axis.
*/
static Rot3 Rx(double t);
static Rot3 Ry(double t);
static Rot3 Rz(double t);
static Rot3 RzRyRx(double x, double y, double z);
inline static Rot3 RzRyRx(const Vector& xyz) {
assert(xyz.size() == 3);
return RzRyRx(xyz(0), xyz(1), xyz(2));
}
/**
* Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw)
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf
* Assumes vehicle coordinate frame X forward, Y right, Z down
*/
static Rot3 yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading)
static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude)
static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft)
static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion parameters */
static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); }
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
* @param theta rotation angle
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Vector& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Vector& v);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param wx
* @param wy
* @param wz
* @return incremental rotation matrix
*/
static inline Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector_(3,wx,wy,wz));}
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const;
/** return 3*3 rotation matrix */
Matrix matrix() const;
/** return 3*3 transpose (inverse) rotation matrix */
Matrix transpose() const;
/** returns column vector specified by index */
Point3 column(int index) const;
Point3 r1() const { return r1_; }
Point3 r2() const { return r2_; }
Point3 r3() const { return r3_; }
/**
* Use RQ to calculate xyz angle representation
* @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
*/
Vector xyz() const;
/**
* Use RQ to calculate yaw-pitch-roll angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
*/
Vector ypr() const;
/**
* Use RQ to calculate roll-pitch-yaw angle representation
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
*/
Vector rpy() const;
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/
Quaternion toQuaternion() const {
return Quaternion((Eigen::Matrix3d() <<
r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z()).finished());
}
/** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; }
/** Lie requirements */
/** return DOF, dimensionality of tangent space */
inline size_t dim() const { return dimension; }
/** Compose two rotations i.e., R= (*this) * R2
*/
Rot3 compose(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Exponential map at identity - create a rotation from canonical coordinates
* using Rodriguez' formula
*/
static Rot3 Expmap(const Vector& v) {
if(zero(v)) return Rot3();
else return rodriguez(v);
}
/** identity */
inline static Rot3 identity() {
return Rot3();
}
// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Rot3& R);
// Manifold requirements
inline Rot3 retract(const Vector& v) const { return compose(Expmap(v)); }
/**
* Returns inverse retraction
*/
inline Vector localCoordinates(const Rot3& t2) const { return Logmap(between(t2)); }
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()
inline Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const {
if (H1) *H1 = -matrix();
return Rot3(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const {
return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
inline Point3 operator*(const Point3& p) const { return rotate(p);}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate(const Point3& p,
boost::optional<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);
/**
* Typedef to the main 3D rotation implementation, which is Rot3M by default,
* or Rot3Q if GTSAM_DEFAULT_QUATERNIONS is defined. Depending on whether
* GTSAM_DEFAULT_QUATERNIONS is defined, Rot3M (the rotation matrix
* implementation) or Rot3Q (the quaternion implementation) will used in all
* built-in gtsam geometry types that involve 3D rotations, such as Pose3,
* SimpleCamera, CalibratedCamera, StereoCamera, etc.
*/
typedef Rot3Q Rot3;
}
#else
#include <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;
}
#endif

62
gtsam/geometry/Rot3.cpp → gtsam/geometry/Rot3M.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file Rot3.cpp
* @file Rot3M.cpp
* @brief Rotation (internal: 3*3 matrix representation*)
* @author Alireza Fathi
* @author Christian Potthast
@ -18,7 +18,7 @@
*/
#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Rot3M.h>
#include <gtsam/base/Lie-inl.h>
using namespace std;
@ -26,39 +26,39 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Rot3);
INSTANTIATE_LIE(Rot3M);
static const Matrix I3 = eye(3);
/* ************************************************************************* */
// static member functions to construct rotations
Rot3 Rot3::Rx(double t) {
Rot3M Rot3M::Rx(double t) {
double st = sin(t), ct = cos(t);
return Rot3(
return Rot3M(
1, 0, 0,
0, ct,-st,
0, st, ct);
}
Rot3 Rot3::Ry(double t) {
Rot3M Rot3M::Ry(double t) {
double st = sin(t), ct = cos(t);
return Rot3(
return Rot3M(
ct, 0, st,
0, 1, 0,
-st, 0, ct);
}
Rot3 Rot3::Rz(double t) {
Rot3M Rot3M::Rz(double t) {
double st = sin(t), ct = cos(t);
return Rot3(
return Rot3M(
ct,-st, 0,
st, ct, 0,
0, 0, 1);
}
// Considerably faster than composing matrices above !
Rot3 Rot3::RzRyRx(double x, double y, double z) {
Rot3M Rot3M::RzRyRx(double x, double y, double z) {
double cx=cos(x),sx=sin(x);
double cy=cos(y),sy=sin(y);
double cz=cos(z),sz=sin(z);
@ -73,7 +73,7 @@ namespace gtsam {
double s_c = sx * cz;
double c_c = cx * cz;
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
return Rot3(
return Rot3M(
_cc,- c_s + ssc, s_s + csc,
_cs, c_c + sss, -s_c + css,
-sy, sc_, cc_
@ -81,7 +81,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
// get components of axis \omega
double wx = w(0), wy=w(1), wz=w(2);
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
@ -97,25 +97,25 @@ namespace gtsam {
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
double C22 = c_1*wwTzz;
return Rot3( c + C00, -swz + C01, swy + C02,
return Rot3M( c + C00, -swz + C01, swy + C02,
swz + C01, c + C11, -swx + C12,
-swy + C02, swx + C12, c + C22);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
Rot3M Rot3M::rodriguez(const Vector& w) {
double t = w.norm();
if (t < 1e-10) return Rot3();
if (t < 1e-10) return Rot3M();
return rodriguez(w/t, t);
}
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
bool Rot3M::equals(const Rot3M & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
Matrix Rot3::matrix() const {
Matrix Rot3M::matrix() const {
double r[] = { r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z() };
@ -123,7 +123,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Matrix Rot3::transpose() const {
Matrix Rot3M::transpose() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z()};
@ -131,7 +131,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Point3 Rot3::column(int index) const{
Point3 Rot3M::column(int index) const{
if(index == 3)
return r3_;
else if (index == 2)
@ -141,25 +141,25 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector Rot3::xyz() const {
Vector Rot3M::xyz() const {
Matrix I;Vector q;
boost::tie(I,q)=RQ(matrix());
return q;
}
Vector Rot3::ypr() const {
Vector Rot3M::ypr() const {
Vector q = xyz();
return Vector_(3,q(2),q(1),q(0));
}
Vector Rot3::rpy() const {
Vector Rot3M::rpy() const {
Vector q = xyz();
return Vector_(3,q(0),q(1),q(2));
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3::Logmap(const Rot3& R) {
Vector Rot3M::Logmap(const Rot3M& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
return zero(3);
@ -170,7 +170,7 @@ namespace gtsam {
R.r2().z()-R.r3().y(),
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
} else if (fabs(tr) - -1.0 < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
if(fabs(R.r3().z() - -1.0) > 1e-10)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
@ -190,7 +190,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
Point3 Rot3M::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = matrix();
@ -199,7 +199,7 @@ namespace gtsam {
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
Point3 Rot3M::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix Rt(transpose());
Point3 q(Rt*p.vector()); // q = Rt*p
@ -209,7 +209,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2,
Rot3M Rot3M::compose (const Rot3M& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
@ -217,7 +217,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
Rot3M Rot3M::between (const Rot3M& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
@ -228,15 +228,15 @@ namespace gtsam {
pair<Matrix, Vector> RQ(const Matrix& A) {
double x = -atan2(-A(2, 1), A(2, 2));
Rot3 Qx = Rot3::Rx(-x);
Rot3M Qx = Rot3M::Rx(-x);
Matrix B = A * Qx.matrix();
double y = -atan2(B(2, 0), B(2, 2));
Rot3 Qy = Rot3::Ry(-y);
Rot3M Qy = Rot3M::Ry(-y);
Matrix C = B * Qy.matrix();
double z = -atan2(-C(1, 0), C(1, 1));
Rot3 Qz = Rot3::Rz(-z);
Rot3M Qz = Rot3M::Rz(-z);
Matrix R = C * Qz.matrix();
Vector xyz = Vector_(3, x, y, z);

284
gtsam/geometry/Rot3M.h Normal file
View File

@ -0,0 +1,284 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Rot3M.h
* @brief 3D Rotation represented as 3*3 matrix
* @author Alireza Fathi
* @author Christian Potthast
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
namespace gtsam {
typedef Eigen::Quaternion<double,Eigen::DontAlign> Quaternion; ///< Typedef to an Eigen Quaternion<double>
/**
* @brief 3D Rotations represented as rotation matrices
* @ingroup geometry
*/
class Rot3M {
public:
static const size_t dimension = 3;
private:
/** we store columns ! */
Point3 r1_, r2_, r3_;
public:
/** default constructor, unit rotation */
Rot3M() :
r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {}
/** constructor from columns */
Rot3M(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {}
/** constructor from doubles in *row* order !!! */
Rot3M(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
/** constructor from matrix */
Rot3M(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
* @param q The quaternion
*/
Rot3M(const Quaternion& q) {
Eigen::Matrix3d R = q.toRotationMatrix();
r1_ = Point3(R.col(0));
r2_ = Point3(R.col(1));
r3_ = Point3(R.col(2));
}
/** Static member function to generate some well known rotations */
/**
* Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix
* Counterclockwise when looking from unchanging axis.
*/
static Rot3M Rx(double t);
static Rot3M Ry(double t);
static Rot3M Rz(double t);
static Rot3M RzRyRx(double x, double y, double z);
static Rot3M RzRyRx(const Vector& xyz) {
assert(xyz.size() == 3);
return RzRyRx(xyz(0), xyz(1), xyz(2));
}
/**
* Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw)
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf
* Assumes vehicle coordinate frame X forward, Y right, Z down
*/
static Rot3M yaw (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading)
static Rot3M pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude)
static Rot3M roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft)
static Rot3M ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion parameters */
static Rot3M quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3M(q); }
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
* @param theta rotation angle
* @return incremental rotation matrix
*/
static Rot3M rodriguez(const Vector& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix
*/
static Rot3M rodriguez(const Vector& v);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param wx
* @param wy
* @param wz
* @return incremental rotation matrix
*/
static Rot3M rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector_(3,wx,wy,wz));}
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** equals with an tolerance */
bool equals(const Rot3M& p, double tol = 1e-9) const;
/** return 3*3 rotation matrix */
Matrix matrix() const;
/** return 3*3 transpose (inverse) rotation matrix */
Matrix transpose() const;
/** returns column vector specified by index */
Point3 column(int index) const;
Point3 r1() const { return r1_; }
Point3 r2() const { return r2_; }
Point3 r3() const { return r3_; }
/**
* Use RQ to calculate xyz angle representation
* @return a vector containing x,y,z s.t. R = Rot3M::RzRyRx(x,y,z)
*/
Vector xyz() const;
/**
* Use RQ to calculate yaw-pitch-roll angle representation
* @return a vector containing ypr s.t. R = Rot3M::ypr(y,p,r)
*/
Vector ypr() const;
/**
* Use RQ to calculate roll-pitch-yaw angle representation
* @return a vector containing ypr s.t. R = Rot3M::ypr(y,p,r)
*/
Vector rpy() const;
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/
Quaternion toQuaternion() const {
return Quaternion((Eigen::Matrix3d() <<
r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z()).finished());
}
/** dimension of the variable - used to autodetect sizes */
static size_t Dim() { return dimension; }
/** Lie requirements */
/** return DOF, dimensionality of tangent space */
size_t dim() const { return dimension; }
/** Compose two rotations i.e., R= (*this) * R2
*/
Rot3M compose(const Rot3M& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Exponential map at identity - create a rotation from canonical coordinates
* using Rodriguez' formula
*/
static Rot3M Expmap(const Vector& v) {
if(zero(v)) return Rot3M();
else return rodriguez(v);
}
/** identity */
inline static Rot3M identity() {
return Rot3M();
}
// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Rot3M& R);
// Manifold requirements
Rot3M retract(const Vector& v) const { return compose(Expmap(v)); }
/**
* Returns inverse retraction
*/
Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); }
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M()
Rot3M inverse(boost::optional<Matrix&> H1=boost::none) const {
if (H1) *H1 = -matrix();
return Rot3M(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3M between(const Rot3M& R2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */
Rot3M operator*(const Rot3M& R2) const {
return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
inline Point3 operator*(const Point3& p) const { return rotate(p);}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate(const Point3& p,
boost::optional<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);
}

132
gtsam/geometry/Rot3Q.cpp Normal file
View File

@ -0,0 +1,132 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Rot3Q.cpp
* @brief Rotation (internal: 3*3 matrix representation*)
* @author Alireza Fathi
* @author Christian Potthast
* @author Frank Dellaert
*/
#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3Q.h>
#include <gtsam/base/Lie-inl.h>
using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Rot3Q);
static const Matrix I3 = eye(3);
/* ************************************************************************* */
// static member functions to construct rotations
// Considerably faster than composing matrices above !
Rot3Q Rot3Q::RzRyRx(double x, double y, double z) { return Rot3Q(
Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
}
/* ************************************************************************* */
Rot3Q Rot3Q::rodriguez(const Vector& w, double theta) {
return Quaternion(Eigen::AngleAxisd(theta, w)); }
/* ************************************************************************* */
Rot3Q Rot3Q::rodriguez(const Vector& w) {
double t = w.norm();
if (t < 1e-10) return Rot3Q();
return rodriguez(w/t, t);
}
/* ************************************************************************* */
bool Rot3Q::equals(const Rot3Q & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
Matrix Rot3Q::matrix() const { return quaternion_.toRotationMatrix(); }
/* ************************************************************************* */
Matrix Rot3Q::transpose() const { return quaternion_.toRotationMatrix().transpose(); }
/* ************************************************************************* */
Vector Rot3Q::xyz() const {
Matrix I;Vector q;
boost::tie(I,q)=RQ(matrix());
return q;
}
Vector Rot3Q::ypr() const {
Vector q = xyz();
return Vector_(3,q(2),q(1),q(0));
}
Vector Rot3Q::rpy() const {
Vector q = xyz();
return Vector_(3,q(0),q(1),q(2));
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector Rot3Q::Logmap(const Rot3Q& R) {
Eigen::AngleAxisd angleAxis(R.quaternion_);
if(angleAxis.angle() > M_PI) // Important: use the smallest possible
angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep
if(angleAxis.angle() < -M_PI) // error continuous.
angleAxis.angle() += 2.0*M_PI;
return angleAxis.axis() * angleAxis.angle();
}
/* ************************************************************************* */
Point3 Rot3Q::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Matrix R = matrix();
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R;
Eigen::Vector3d r = R * p.vector();
return Point3(r.x(), r.y(), r.z());
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3Q::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix Rt(transpose());
Point3 q(Rt*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = Rt;
return q;
}
/* ************************************************************************* */
Rot3Q Rot3Q::compose(const Rot3Q& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3Q(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Rot3Q Rot3Q::between(const Rot3Q& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return between_default(*this, R2);
}
/* ************************************************************************* */
} // namespace gtsam

272
gtsam/geometry/Rot3Q.h Normal file
View File

@ -0,0 +1,272 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Rot3Q.h
* @brief 3D Rotation represented as 3*3 matrix
* @author Alireza Fathi
* @author Christian Potthast
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3M.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
namespace gtsam {
typedef Eigen::Quaternion<double,Eigen::DontAlign> Quaternion; ///< Typedef to an Eigen Quaternion<double>
/**
* @brief 3D Rotations represented as rotation matrices
* @ingroup geometry
*/
class Rot3Q {
public:
static const size_t dimension = 3;
private:
/** Internal Eigen Quaternion */
Quaternion quaternion_;
public:
/** default constructor, unit rotation */
Rot3Q() : quaternion_(Quaternion::Identity()) {}
/** constructor from columns */
Rot3Q(const Point3& r1, const Point3& r2, const Point3& r3) :
quaternion_((Eigen::Matrix3d() <<
r1.x(), r2.x(), r3.x(),
r1.y(), r2.y(), r3.y(),
r1.z(), r2.z(), r3.z()).finished()) {}
/** constructor from doubles in *row* order !!! */
Rot3Q(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
quaternion_((Eigen::Matrix3d() <<
R11, R12, R13,
R21, R22, R23,
R31, R32, R33).finished()) {}
/** constructor from matrix */
Rot3Q(const Matrix& R) : quaternion_(Eigen::Matrix3d(R)) {}
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
* @param q The quaternion
*/
Rot3Q(const Quaternion& q) : quaternion_(q) {}
/** Constructor from a rotation matrix rotation Rot3M */
Rot3Q(const Rot3M& r) : quaternion_(Eigen::Matrix3d(r.matrix())) {}
/* Static member function to generate some well known rotations */
/**
* Rotations around axes as in http://en.wikipedia.org/wiki/Rotation_matrix
* Counterclockwise when looking from unchanging axis.
*/
static Rot3Q Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }
static Rot3Q Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); }
static Rot3Q Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }
static Rot3Q RzRyRx(double x, double y, double z);
inline static Rot3Q RzRyRx(const Vector& xyz) {
assert(xyz.size() == 3);
return RzRyRx(xyz(0), xyz(1), xyz(2));
}
/**
* Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw)
* as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf
* Assumes vehicle coordinate frame X forward, Y right, Z down
*/
static Rot3Q yaw (double t) { return Rz(t); } // positive yaw is to right (as in aircraft heading)
static Rot3Q pitch(double t) { return Ry(t); } // positive pitch is up (increasing aircraft altitude)
static Rot3Q roll (double t) { return Rx(t); } // positive roll is to right (increasing yaw in aircraft)
static Rot3Q ypr (double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion parameters */
static Rot3Q quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3Q(q); }
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
* @param theta rotation angle
* @return incremental rotation matrix
*/
static Rot3Q rodriguez(const Vector& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix
*/
static Rot3Q rodriguez(const Vector& v);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param wx Incremental roll (about X)
* @param wy Incremental pitch (about Y)
* @param wz Incremental yaw (about Z)
* @return incremental rotation matrix
*/
static Rot3Q rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector_(3,wx,wy,wz));}
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** equals with an tolerance */
bool equals(const Rot3Q& p, double tol = 1e-9) const;
/** return 3*3 rotation matrix */
Matrix matrix() const;
/** return 3*3 transpose (inverse) rotation matrix */
Matrix transpose() const;
/** returns column vector specified by index */
Point3 r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }
Point3 r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }
Point3 r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
/**
* Use RQ to calculate xyz angle representation
* @return a vector containing x,y,z s.t. R = Rot3Q::RzRyRx(x,y,z)
*/
Vector xyz() const;
/**
* Use RQ to calculate yaw-pitch-roll angle representation
* @return a vector containing ypr s.t. R = Rot3Q::ypr(y,p,r)
*/
Vector ypr() const;
/**
* Use RQ to calculate roll-pitch-yaw angle representation
* @return a vector containing ypr s.t. R = Rot3Q::ypr(y,p,r)
*/
Vector rpy() const;
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/
Quaternion toQuaternion() const { return quaternion_; }
/** dimension of the variable - used to autodetect sizes */
static size_t Dim() { return dimension; }
/** Lie requirements */
/** return DOF, dimensionality of tangent space */
size_t dim() const { return dimension; }
/** Compose two rotations i.e., R= (*this) * R2
*/
Rot3Q compose(const Rot3Q& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Exponential map at identity - create a rotation from canonical coordinates
* using Rodriguez' formula
*/
static Rot3Q Expmap(const Vector& v) {
if(zero(v)) return Rot3Q();
else return rodriguez(v);
}
/** identity */
inline static Rot3Q identity() {
return Rot3Q();
}
// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Rot3Q& R);
// Manifold requirements
Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); }
/**
* Returns inverse retraction
*/
Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); }
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q()
Rot3Q inverse(boost::optional<Matrix&> H1=boost::none) const {
if (H1) *H1 = -matrix();
return Rot3Q(quaternion_.inverse());
}
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3Q between(const Rot3Q& R2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */
Rot3Q operator*(const Rot3Q& R2) const { return Rot3Q(quaternion_ * R2.quaternion_); }
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
inline Point3 operator*(const Point3& p) const {
Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
return Point3(r(0), r(1), r(2));
}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate(const Point3& p,
boost::optional<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;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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/testHomography2.cpp
View File

@ -96,7 +96,7 @@ TEST( Homography2, TestCase)
Homography2 patchH(const Pose3& tEc) {
Pose3 cEt = tEc.inverse();
Rot3 cRt = cEt.rotation();
Point3 r1 = cRt.column(1), r2 = cRt.column(2), t = cEt.translation();
Point3 r1 = cRt.r1(), r2 = cRt.r2(), t = cEt.translation();
// TODO cleanup !!!!
// column 1

6
gtsam/geometry/tests/testPose3.cpp
View File

@ -278,7 +278,7 @@ TEST( Pose3, compose )
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2);
EXPECT(assert_equal(numericalH2,actualDcompose2));
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4));
}
/* ************************************************************************* */
@ -296,7 +296,7 @@ TEST( Pose3, compose2 )
EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2);
EXPECT(assert_equal(numericalH2,actualDcompose2));
EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5));
}
/* ************************************************************************* */
@ -567,7 +567,7 @@ TEST( Pose3, between )
EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3));
Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3);
EXPECT(assert_equal(numericalH2,actualDBetween2));
EXPECT(assert_equal(numericalH2,actualDBetween2,1e-5));
}
/* ************************************************************************* */

263
gtsam/geometry/tests/testRot3.cpp → gtsam/geometry/tests/testRot3M.cpp
View File

@ -11,7 +11,7 @@
/**
* @file testRot3.cpp
* @brief Unit tests for Rot3 class
* @brief Unit tests for Rot3M class
* @author Alireza Fathi
*/
@ -21,21 +21,18 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Rot3M.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot3)
GTSAM_CONCEPT_LIE_INST(Rot3)
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3M R = Rot3M::rodriguez(0.1, 0.4, 0.2);
Point3 P(0.2, 0.7, -2.0);
double error = 1e-9, epsilon = 0.001;
/* ************************************************************************* */
TEST( Rot3, constructor)
TEST( Rot3M, constructor)
{
Rot3 expected(eye(3, 3));
Rot3M expected(eye(3, 3));
Vector r1(3), r2(3), r3(3);
r1(0) = 1;
r1(1) = 0;
@ -46,91 +43,91 @@ TEST( Rot3, constructor)
r3(0) = 0;
r3(1) = 0;
r3(2) = 1;
Rot3 actual(r1, r2, r3);
Rot3M actual(r1, r2, r3);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3, constructor2)
TEST( Rot3M, constructor2)
{
Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Rot3 actual(R);
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
Rot3M actual(R);
Rot3M expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3, constructor3)
TEST( Rot3M, constructor3)
{
Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
Rot3M expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9);
CHECK(assert_equal(Rot3(r1,r2,r3),expected));
CHECK(assert_equal(Rot3M(r1,r2,r3),expected));
}
/* ************************************************************************* */
TEST( Rot3, transpose)
TEST( Rot3M, transpose)
{
Rot3 R(1, 2, 3, 4, 5, 6, 7, 8, 9);
Rot3M R(1, 2, 3, 4, 5, 6, 7, 8, 9);
Point3 r1(1, 2, 3), r2(4, 5, 6), r3(7, 8, 9);
CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3)));
CHECK(assert_equal(R.inverse(),Rot3M(r1,r2,r3)));
}
/* ************************************************************************* */
TEST( Rot3, equals)
TEST( Rot3M, equals)
{
CHECK(R.equals(R));
Rot3 zero;
Rot3M zero;
CHECK(!R.equals(zero));
}
/* ************************************************************************* */
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_rodriguez(const Vector& w) {
Rot3M slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
if (t < 1e-5) return Rot3M();
Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
}
/* ************************************************************************* */
TEST( Rot3, rodriguez)
TEST( Rot3M, rodriguez)
{
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
Rot3M R1 = Rot3M::rodriguez(epsilon, 0, 0);
Vector w = Vector_(3, epsilon, 0., 0.);
Rot3 R2 = slow_but_correct_rodriguez(w);
Rot3M R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez2)
TEST( Rot3M, rodriguez2)
{
Vector axis = Vector_(3,0.,1.,0.); // rotation around Y
double angle = 3.14 / 4.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825,
Rot3M actual = Rot3M::rodriguez(axis, angle);
Rot3M expected(0.707388, 0, 0.706825,
0, 1, 0,
-0.706825, 0, 0.707388);
CHECK(assert_equal(expected,actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez3)
TEST( Rot3M, rodriguez3)
{
Vector w = Vector_(3, 0.1, 0.2, 0.3);
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
Rot3M R1 = Rot3M::rodriguez(w / norm_2(w), norm_2(w));
Rot3M R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez4)
TEST( Rot3M, rodriguez4)
{
Vector axis = Vector_(3,0.,0.,1.); // rotation around Z
double angle = M_PI_2;
Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3M actual = Rot3M::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle);
Rot3 expected(c,-s, 0,
Rot3M expected(c,-s, 0,
s, c, 0,
0, 0, 1);
CHECK(assert_equal(expected,actual,1e-5));
@ -138,62 +135,62 @@ TEST( Rot3, rodriguez4)
}
/* ************************************************************************* */
TEST( Rot3, expmap)
TEST( Rot3M, expmap)
{
Vector v = zero(3);
CHECK(assert_equal(R.retract(v), R));
}
/* ************************************************************************* */
TEST(Rot3, log)
TEST(Rot3M, log)
{
Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
Rot3 R1 = Rot3::rodriguez(w1);
CHECK(assert_equal(w1, Rot3::Logmap(R1)));
Rot3M R1 = Rot3M::rodriguez(w1);
CHECK(assert_equal(w1, Rot3M::Logmap(R1)));
Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
Rot3 R2 = Rot3::rodriguez(w2);
CHECK(assert_equal(w2, Rot3::Logmap(R2)));
Rot3M R2 = Rot3M::rodriguez(w2);
CHECK(assert_equal(w2, Rot3M::Logmap(R2)));
Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
Rot3 R3 = Rot3::rodriguez(w3);
CHECK(assert_equal(w3, Rot3::Logmap(R3)));
Rot3M R3 = Rot3M::rodriguez(w3);
CHECK(assert_equal(w3, Rot3M::Logmap(R3)));
Vector w = Vector_(3, 0.1, 0.4, 0.2);
Rot3 R = Rot3::rodriguez(w);
CHECK(assert_equal(w, Rot3::Logmap(R)));
Rot3M R = Rot3M::rodriguez(w);
CHECK(assert_equal(w, Rot3M::Logmap(R)));
Vector w5 = Vector_(3, 0.0, 0.0, 0.0);
Rot3 R5 = Rot3::rodriguez(w5);
CHECK(assert_equal(w5, Rot3::Logmap(R5)));
Rot3M R5 = Rot3M::rodriguez(w5);
CHECK(assert_equal(w5, Rot3M::Logmap(R5)));
Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0);
Rot3 R6 = Rot3::rodriguez(w6);
CHECK(assert_equal(w6, Rot3::Logmap(R6)));
Rot3M R6 = Rot3M::rodriguez(w6);
CHECK(assert_equal(w6, Rot3M::Logmap(R6)));
Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0);
Rot3 R7 = Rot3::rodriguez(w7);
CHECK(assert_equal(w7, Rot3::Logmap(R7)));
Rot3M R7 = Rot3M::rodriguez(w7);
CHECK(assert_equal(w7, Rot3M::Logmap(R7)));
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>());
Rot3 R8 = Rot3::rodriguez(w8);
CHECK(assert_equal(w8, Rot3::Logmap(R8)));
Rot3M R8 = Rot3M::rodriguez(w8);
CHECK(assert_equal(w8, Rot3M::Logmap(R8)));
}
/* ************************************************************************* */
TEST(Rot3, manifold)
TEST(Rot3M, manifold)
{
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 origin;
Rot3M gR1 = Rot3M::rodriguez(0.1, 0.4, 0.2);
Rot3M gR2 = Rot3M::rodriguez(0.3, 0.1, 0.7);
Rot3M origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2);
CHECK(assert_equal(gR2, gR1.retract(d12)));
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
CHECK(assert_equal(gR2, gR1*Rot3M::Expmap(d12)));
Vector d21 = gR2.localCoordinates(gR1);
CHECK(assert_equal(gR1, gR2.retract(d21)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
CHECK(assert_equal(gR1, gR2*Rot3M::Expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
@ -201,11 +198,11 @@ TEST(Rot3, manifold)
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = Vector_(3, 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
CHECK(assert_equal(Rot3M::Expmap(-d),Rot3M::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Rot3 R2 = Rot3::Expmap (2 * d);
Rot3 R3 = Rot3::Expmap (3 * d);
Rot3 R5 = Rot3::Expmap (5 * d);
Rot3M R2 = Rot3M::Expmap (2 * d);
Rot3M R3 = Rot3M::Expmap (3 * d);
Rot3M R5 = Rot3M::Expmap (5 * d);
CHECK(assert_equal(R5,R2*R3));
CHECK(assert_equal(R5,R3*R2));
}
@ -226,106 +223,106 @@ AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
}
/* ************************************************************************* */
TEST(Rot3, BCH)
TEST(Rot3M, BCH)
{
// Approximate exmap by BCH formula
AngularVelocity w1(0.2, -0.1, 0.1);
AngularVelocity w2(0.01, 0.02, -0.03);
Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
Rot3 R3 = R1 * R2;
Vector expected = Rot3::Logmap(R3);
Rot3M R1 = Rot3M::Expmap (w1.vector()), R2 = Rot3M::Expmap (w2.vector());
Rot3M R3 = R1 * R2;
Vector expected = Rot3M::Logmap(R3);
Vector actual = BCH(w1, w2).vector();
CHECK(assert_equal(expected, actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, rotate_derivatives)
TEST( Rot3M, rotate_derivatives)
{
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
R.rotate(P, actualDrotate1a, actualDrotate2);
R.inverse().rotate(P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P);
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P);
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3M,Point3>, R, P);
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3M,Point3>, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3M,Point3>, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error));
}
/* ************************************************************************* */
TEST( Rot3, unrotate)
TEST( Rot3M, unrotate)
{
Point3 w = R * P;
Matrix H1,H2;
Point3 actual = R.unrotate(w,H1,H2);
CHECK(assert_equal(P,actual));
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3,Point3>, R, w);
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3M,Point3>, R, w);
CHECK(assert_equal(numerical1,H1,error));
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3,Point3>, R, w);
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3M,Point3>, R, w);
CHECK(assert_equal(numerical2,H2,error));
}
/* ************************************************************************* */
TEST( Rot3, compose )
TEST( Rot3M, compose )
{
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3M R1 = Rot3M::rodriguez(0.1, 0.2, 0.3);
Rot3M R2 = Rot3M::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R1 * R2;
Rot3M expected = R1 * R2;
Matrix actualH1, actualH2;
Rot3 actual = R1.compose(R2, actualH1, actualH2);
Rot3M actual = R1.compose(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3>, R1,
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3M>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3>, R1,
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3M>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Rot3, inverse )
TEST( Rot3M, inverse )
{
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3M R = Rot3M::rodriguez(0.1, 0.2, 0.3);
Rot3 I;
Rot3M I;
Matrix actualH;
CHECK(assert_equal(I,R*R.inverse(actualH)));
CHECK(assert_equal(I,R.inverse()*R));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3M>, R);
CHECK(assert_equal(numericalH,actualH));
}
/* ************************************************************************* */
TEST( Rot3, between )
TEST( Rot3M, between )
{
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 origin;
Rot3M R = Rot3M::rodriguez(0.1, 0.4, 0.2);
Rot3M origin;
CHECK(assert_equal(R, origin.between(R)));
CHECK(assert_equal(R.inverse(), R.between(origin)));
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3M R1 = Rot3M::rodriguez(0.1, 0.2, 0.3);
Rot3M R2 = Rot3M::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R1.inverse() * R2;
Rot3M expected = R1.inverse() * R2;
Matrix actualH1, actualH2;
Rot3 actual = R1.between(R2, actualH1, actualH2);
Rot3M actual = R1.between(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3M> , R1, R2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2);
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3M> , R1, R2);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Rot3, xyz )
TEST( Rot3M, xyz )
{
double t = 0.1, st = sin(t), ct = cos(t);
@ -335,47 +332,47 @@ TEST( Rot3, xyz )
// z
// | * Y=(ct,st)
// x----y
Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
CHECK(assert_equal(expected1,Rot3::Rx(t)));
Rot3M expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
CHECK(assert_equal(expected1,Rot3M::Rx(t)));
// x
// | * Z=(ct,st)
// y----z
Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
CHECK(assert_equal(expected2,Rot3::Ry(t)));
Rot3M expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
CHECK(assert_equal(expected2,Rot3M::Ry(t)));
// y
// | X=* (ct,st)
// z----x
Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
CHECK(assert_equal(expected3,Rot3::Rz(t)));
Rot3M expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
CHECK(assert_equal(expected3,Rot3M::Rz(t)));
// Check compound rotation
Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1);
CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
Rot3M expected = Rot3M::Rz(0.3) * Rot3M::Ry(0.2) * Rot3M::Rx(0.1);
CHECK(assert_equal(expected,Rot3M::RzRyRx(0.1,0.2,0.3)));
}
/* ************************************************************************* */
TEST( Rot3, yaw_pitch_roll )
TEST( Rot3M, yaw_pitch_roll )
{
double t = 0.1;
// yaw is around z axis
CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t)));
CHECK(assert_equal(Rot3M::Rz(t),Rot3M::yaw(t)));
// pitch is around y axis
CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t)));
CHECK(assert_equal(Rot3M::Ry(t),Rot3M::pitch(t)));
// roll is around x axis
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
CHECK(assert_equal(Rot3M::Rx(t),Rot3M::roll(t)));
// Check compound rotation
Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
Rot3M expected = Rot3M::yaw(0.1) * Rot3M::pitch(0.2) * Rot3M::roll(0.3);
CHECK(assert_equal(expected,Rot3M::ypr(0.1,0.2,0.3)));
}
/* ************************************************************************* */
TEST( Rot3, RQ)
TEST( Rot3M, RQ)
{
// Try RQ on a pure rotation
Matrix actualK;
@ -385,18 +382,18 @@ TEST( Rot3, RQ)
CHECK(assert_equal(eye(3),actualK));
CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
// Try using xyz call, asserting that Rot3M::RzRyRx(x,y,z).xyz()==[x;y;z]
CHECK(assert_equal(expected,R.xyz(),1e-6));
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3M::RzRyRx(0.1,0.2,0.3).xyz()));
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try using ypr call, asserting that Rot3M::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3M::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3M::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3M::yaw (0.1).ypr()));
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3M::pitch(0.1).ypr()));
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3M::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
@ -407,60 +404,60 @@ TEST( Rot3, RQ)
}
/* ************************************************************************* */
TEST( Rot3, expmapStability ) {
TEST( Rot3M, expmapStability ) {
Vector w = Vector_(3, 78e-9, 5e-8, 97e-7);
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Rot3M actualR = Rot3M::Expmap(w);
Matrix W = Matrix_(3,3, 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
Rot3 expectedR( Rmat );
Rot3M expectedR( Rmat );
CHECK(assert_equal(expectedR, actualR, 1e-10));
}
/* ************************************************************************* */
TEST( Rot3, logmapStability ) {
TEST( Rot3M, logmapStability ) {
Vector w = Vector_(3, 1e-8, 0.0, 0.0);
Rot3 R = Rot3::Expmap(w);
Rot3M R = Rot3M::Expmap(w);
// double tr = R.r1().x()+R.r2().y()+R.r3().z();
// std::cout.precision(5000);
// std::cout << "theta: " << w.norm() << std::endl;
// std::cout << "trace: " << tr << std::endl;
// R.print("R = ");
Vector actualw = Rot3::Logmap(R);
Vector actualw = Rot3M::Logmap(R);
CHECK(assert_equal(w, actualw, 1e-15));
}
/* ************************************************************************* */
TEST(Rot3, quaternion) {
TEST(Rot3M, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3(Matrix_(3,3,
Rot3M R1 = Rot3M(Matrix_(3,3,
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219));
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3 R2 = Rot3(Matrix_(3,3,
Rot3M R2 = Rot3M(Matrix_(3,3,
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946));
// Check creating Rot3 from quaternion
EXPECT(assert_equal(R1, Rot3(q1)));
EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
EXPECT(assert_equal(R2, Rot3(q2)));
EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
// Check creating Rot3M from quaternion
EXPECT(assert_equal(R1, Rot3M(q1)));
EXPECT(assert_equal(R1, Rot3M::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
EXPECT(assert_equal(R2, Rot3M(q2)));
EXPECT(assert_equal(R2, Rot3M::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
// Check converting Rot3 to quaterion
// Check converting Rot3M to quaterion
EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));
EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs())));
// Check that quaternion and Rot3 represent the same rotation
// Check that quaternion and Rot3M represent the same rotation
Point3 p1(1.0, 2.0, 3.0);
Point3 p2(8.0, 7.0, 9.0);

518
gtsam/geometry/tests/testRot3Q.cpp Normal file
View File

@ -0,0 +1,518 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testRot3.cpp
* @brief Unit tests for Rot3Q class
* @author Alireza Fathi
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <boost/math/constants/constants.hpp>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3Q.h>
using namespace gtsam;
Rot3Q R = Rot3Q::rodriguez(0.1, 0.4, 0.2);
Point3 P(0.2, 0.7, -2.0);
double error = 1e-9, epsilon = 0.001;
/* ************************************************************************* */
TEST( Rot3Q, constructor)
{
Rot3Q expected(eye(3, 3));
Vector r1(3), r2(3), r3(3);
r1(0) = 1;
r1(1) = 0;
r1(2) = 0;
r2(0) = 0;
r2(1) = 1;
r2(2) = 0;
r3(0) = 0;
r3(1) = 0;
r3(2) = 1;
Rot3Q actual(r1, r2, r3);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3Q, constructor2)
{
Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Rot3Q actual(R);
Rot3Q expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3Q, constructor3)
{
Rot3Q expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9);
CHECK(assert_equal(Rot3Q(r1,r2,r3),expected));
}
/* ************************************************************************* */
TEST( Rot3Q, equals)
{
CHECK(R.equals(R));
Rot3Q zero;
CHECK(!R.equals(zero));
}
/* ************************************************************************* */
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3Q slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3Q();
Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
}
/* ************************************************************************* */
TEST( Rot3Q, rodriguez)
{
Rot3Q R1 = Rot3Q::rodriguez(epsilon, 0, 0);
Vector w = Vector_(3, epsilon, 0., 0.);
Rot3Q R2 = slow_but_correct_rodriguez(w);
Rot3Q expected2(Rot3M::rodriguez(epsilon, 0, 0));
CHECK(assert_equal(expected2,R1,1e-5));
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3Q, rodriguez2)
{
Vector axis = Vector_(3,0.,1.,0.); // rotation around Y
double angle = 3.14 / 4.0;
Rot3Q actual = Rot3Q::rodriguez(axis, angle);
Rot3Q expected(0.707388, 0, 0.706825,
0, 1, 0,
-0.706825, 0, 0.707388);
Rot3Q expected2(Rot3M::rodriguez(axis, angle));
CHECK(assert_equal(expected,actual,1e-5));
CHECK(assert_equal(expected2,actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3Q, rodriguez3)
{
Vector w = Vector_(3, 0.1, 0.2, 0.3);
Rot3Q R1 = Rot3Q::rodriguez(w / norm_2(w), norm_2(w));
Rot3Q R2 = slow_but_correct_rodriguez(w);
Rot3Q expected2(Rot3M::rodriguez(w / norm_2(w), norm_2(w)));
CHECK(assert_equal(expected2,R1));
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3Q, rodriguez4)
{
Vector axis = Vector_(3,0.,0.,1.); // rotation around Z
double angle = M_PI_2;
Rot3Q actual = Rot3Q::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle);
Rot3Q expected1(c,-s, 0,
s, c, 0,
0, 0, 1);
Rot3Q expected2(Rot3M::rodriguez(axis, angle));
CHECK(assert_equal(expected1,actual,1e-5));
CHECK(assert_equal(expected2,actual,1e-5));
CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3Q, expmap)
{
Vector v = zero(3);
CHECK(assert_equal(R.retract(v), R));
}
/* ************************************************************************* */
TEST(Rot3Q, log)
{
Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
Rot3Q R1 = Rot3Q::rodriguez(w1);
Rot3Q R1m = Rot3M::rodriguez(w1);
CHECK(assert_equal(w1, Rot3Q::Logmap(R1)));
CHECK(assert_equal(R1m, R1));
Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
Rot3Q R2 = Rot3Q::rodriguez(w2);
Rot3Q R2m = Rot3M::rodriguez(w2);
CHECK(assert_equal(w2, Rot3Q::Logmap(R2)));
CHECK(assert_equal(R2m, R2));
Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
Rot3Q R3 = Rot3Q::rodriguez(w3);
Rot3Q R3m = Rot3M::rodriguez(w3);
CHECK(assert_equal(w3, Rot3Q::Logmap(R3)));
CHECK(assert_equal(R3m, R3));
Vector w = Vector_(3, 0.1, 0.4, 0.2);
Rot3Q R = Rot3Q::rodriguez(w);
Rot3Q Rm = Rot3M::rodriguez(w);
CHECK(assert_equal(w, Rot3Q::Logmap(R)));
CHECK(assert_equal(Rm, R));
Vector w5 = Vector_(3, 0.0, 0.0, 0.0);
Rot3Q R5 = Rot3Q::rodriguez(w5);
Rot3Q R5m = Rot3M::rodriguez(w5);
CHECK(assert_equal(w5, Rot3Q::Logmap(R5)));
CHECK(assert_equal(R5m, R5));
Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0);
Rot3Q R6 = Rot3Q::rodriguez(w6);
Rot3Q R6m = Rot3M::rodriguez(w6);
CHECK(assert_equal(w6, Rot3Q::Logmap(R6)));
CHECK(assert_equal(R6m, R6));
Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0);
Rot3Q R7 = Rot3Q::rodriguez(w7);
Rot3Q R7m = Rot3M::rodriguez(w7);
CHECK(assert_equal(w7, Rot3Q::Logmap(R7)));
CHECK(assert_equal(R7m, R7));
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>());
Rot3Q R8 = Rot3Q::rodriguez(w8);
Rot3Q R8m = Rot3M::rodriguez(w8);
CHECK(assert_equal(w8, Rot3Q::Logmap(R8)));
CHECK(assert_equal(R8m, R8));
}
/* ************************************************************************* */
TEST(Rot3Q, manifold)
{
Rot3Q gR1 = Rot3Q::rodriguez(0.1, 0.4, 0.2);
Rot3Q gR2 = Rot3Q::rodriguez(0.3, 0.1, 0.7);
Rot3Q origin;
Rot3M gR1m = Rot3M::rodriguez(0.1, 0.4, 0.2);
Rot3M gR2m = Rot3M::rodriguez(0.3, 0.1, 0.7);
EXPECT(assert_equal(gR1m.matrix(), gR1.matrix()));
EXPECT(assert_equal(gR2m.matrix(), gR2.matrix()));
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2);
EXPECT(assert_equal(gR1m.localCoordinates(gR2m), d12));
CHECK(assert_equal(gR2, gR1.retract(d12)));
CHECK(assert_equal(gR2, gR1*Rot3Q::Expmap(d12)));
Vector d21 = gR2.localCoordinates(gR1);
EXPECT(assert_equal(gR2m.localCoordinates(gR1m), d21));
CHECK(assert_equal(gR1, gR2.retract(d21)));
CHECK(assert_equal(gR1, gR2*Rot3Q::Expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = Vector_(3, 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3Q::Expmap(-d),Rot3Q::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Rot3Q R2 = Rot3Q::Expmap (2 * d);
Rot3Q R3 = Rot3Q::Expmap (3 * d);
Rot3Q R5 = Rot3Q::Expmap (5 * d);
CHECK(assert_equal(R5,R2*R3));
CHECK(assert_equal(R5,R3*R2));
}
/* ************************************************************************* */
class AngularVelocity: public Point3 {
public:
AngularVelocity(const Point3& p) :
Point3(p) {
}
AngularVelocity(double wx, double wy, double wz) :
Point3(wx, wy, wz) {
}
};
AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
return X.cross(Y);
}
/* ************************************************************************* */
TEST(Rot3Q, BCH)
{
// Approximate exmap by BCH formula
AngularVelocity w1(0.2, -0.1, 0.1);
AngularVelocity w2(0.01, 0.02, -0.03);
Rot3Q R1 = Rot3Q::Expmap (w1.vector()), R2 = Rot3Q::Expmap (w2.vector());
Rot3Q R3 = R1 * R2;
Vector expected = Rot3Q::Logmap(R3);
Vector actual = BCH(w1, w2).vector();
CHECK(assert_equal(expected, actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3Q, rotate_derivatives)
{
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
R.rotate(P, actualDrotate1a, actualDrotate2);
R.inverse().rotate(P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3Q,Point3>, R, P);
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3Q,Point3>, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3Q,Point3>, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error));
}
/* ************************************************************************* */
TEST( Rot3Q, unrotate)
{
Point3 w = R * P;
Matrix H1,H2;
Point3 actual = R.unrotate(w,H1,H2);
CHECK(assert_equal(P,actual));
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3Q,Point3>, R, w);
CHECK(assert_equal(numerical1,H1,error));
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3Q,Point3>, R, w);
CHECK(assert_equal(numerical2,H2,error));
}
/* ************************************************************************* */
TEST( Rot3Q, compose )
{
Rot3Q R1 = Rot3Q::rodriguez(0.1, 0.2, 0.3);
Rot3Q R2 = Rot3Q::rodriguez(0.2, 0.3, 0.5);
Rot3Q expected = R1 * R2;
Matrix actualH1, actualH2;
Rot3Q actual = R1.compose(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3Q>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3Q>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Rot3Q, inverse )
{
Rot3Q R = Rot3Q::rodriguez(0.1, 0.2, 0.3);
Rot3Q I;
Matrix actualH;
CHECK(assert_equal(I,R*R.inverse(actualH)));
CHECK(assert_equal(I,R.inverse()*R));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3Q>, R);
CHECK(assert_equal(numericalH,actualH, 1e-4));
}
/* ************************************************************************* */
TEST( Rot3Q, between )
{
Rot3Q r1 = Rot3Q::Rz(M_PI/3.0);
Rot3Q r2 = Rot3Q::Rz(2.0*M_PI/3.0);
Matrix expectedr1 = Matrix_(3,3,
0.5, -sqrt(3.0)/2.0, 0.0,
sqrt(3.0)/2.0, 0.5, 0.0,
0.0, 0.0, 1.0);
EXPECT(assert_equal(expectedr1, r1.matrix()));
Rot3Q R = Rot3Q::rodriguez(0.1, 0.4, 0.2);
Rot3Q origin;
CHECK(assert_equal(R, origin.between(R)));
CHECK(assert_equal(R.inverse(), R.between(origin)));
Rot3Q R1 = Rot3Q::rodriguez(0.1, 0.2, 0.3);
Rot3Q R2 = Rot3Q::rodriguez(0.2, 0.3, 0.5);
Rot3Q expected = R1.inverse() * R2;
Matrix actualH1, actualH2;
Rot3Q actual = R1.between(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3Q> , R1, R2);
CHECK(assert_equal(numericalH1,actualH1, 1e-4));
Matrix numericalH1M = numericalDerivative21(testing::between<Rot3M> , Rot3M(R1.matrix()), Rot3M(R2.matrix()));
CHECK(assert_equal(numericalH1M,actualH1, 1e-4));
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3Q> , R1, R2);
CHECK(assert_equal(numericalH2,actualH2, 1e-4));
Matrix numericalH2M = numericalDerivative22(testing::between<Rot3M> , Rot3M(R1.matrix()), Rot3M(R2.matrix()));
CHECK(assert_equal(numericalH2M,actualH2, 1e-4));
}
/* ************************************************************************* */
TEST( Rot3Q, xyz )
{
double t = 0.1, st = sin(t), ct = cos(t);
// Make sure all counterclockwise
// Diagrams below are all from from unchanging axis
// z
// | * Y=(ct,st)
// x----y
Rot3Q expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
CHECK(assert_equal(expected1,Rot3Q::Rx(t)));
// x
// | * Z=(ct,st)
// y----z
Rot3Q expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
CHECK(assert_equal(expected2,Rot3Q::Ry(t)));
// y
// | X=* (ct,st)
// z----x
Rot3Q expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
CHECK(assert_equal(expected3,Rot3Q::Rz(t)));
// Check compound rotation
Rot3Q expected = Rot3Q::Rz(0.3) * Rot3Q::Ry(0.2) * Rot3Q::Rx(0.1);
CHECK(assert_equal(expected,Rot3Q::RzRyRx(0.1,0.2,0.3)));
}
/* ************************************************************************* */
TEST( Rot3Q, yaw_pitch_roll )
{
double t = 0.1;
// yaw is around z axis
CHECK(assert_equal(Rot3Q::Rz(t),Rot3Q::yaw(t)));
// pitch is around y axis
CHECK(assert_equal(Rot3Q::Ry(t),Rot3Q::pitch(t)));
// roll is around x axis
CHECK(assert_equal(Rot3Q::Rx(t),Rot3Q::roll(t)));
// Check compound rotation
Rot3Q expected = Rot3Q::yaw(0.1) * Rot3Q::pitch(0.2) * Rot3Q::roll(0.3);
CHECK(assert_equal(expected,Rot3Q::ypr(0.1,0.2,0.3)));
}
/* ************************************************************************* */
TEST( Rot3Q, RQ)
{
// Try RQ on a pure rotation
Matrix actualK;
Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = Vector_(3, 0.14715, 0.385821, 0.231671);
CHECK(assert_equal(eye(3),actualK));
CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3Q::RzRyRx(x,y,z).xyz()==[x;y;z]
CHECK(assert_equal(expected,R.xyz(),1e-6));
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3Q::RzRyRx(0.1,0.2,0.3).xyz()));
// Try using ypr call, asserting that Rot3Q::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3Q::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3Q::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3Q::yaw (0.1).ypr()));
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3Q::pitch(0.1).ypr()));
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3Q::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix A = K * R.matrix();
boost::tie(actualK, actual) = RQ(A);
CHECK(assert_equal(K,actualK));
CHECK(assert_equal(expected,actual,1e-6));
}
/* ************************************************************************* */
TEST( Rot3Q, expmapStability ) {
Vector w = Vector_(3, 78e-9, 5e-8, 97e-7);
double theta = w.norm();
double theta2 = theta*theta;
Rot3Q actualR = Rot3Q::Expmap(w);
Matrix W = Matrix_(3,3, 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
Rot3Q expectedR( Rmat );
CHECK(assert_equal(expectedR, actualR, 1e-10));
}
// Does not work with Quaternions
///* ************************************************************************* */
//TEST( Rot3Q, logmapStability ) {
// Vector w = Vector_(3, 1e-8, 0.0, 0.0);
// Rot3Q R = Rot3Q::Expmap(w);
//// double tr = R.r1().x()+R.r2().y()+R.r3().z();
//// std::cout.precision(5000);
//// std::cout << "theta: " << w.norm() << std::endl;
//// std::cout << "trace: " << tr << std::endl;
//// R.print("R = ");
// Vector actualw = Rot3Q::Logmap(R);
// CHECK(assert_equal(w, actualw, 1e-15));
//}
/* ************************************************************************* */
TEST(Rot3Q, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3Q R1 = Rot3Q(Matrix_(3,3,
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219));
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3Q R2 = Rot3Q(Matrix_(3,3,
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946));
// Check creating Rot3Q from quaternion
EXPECT(assert_equal(R1, Rot3Q(q1)));
EXPECT(assert_equal(R1, Rot3Q::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
EXPECT(assert_equal(R2, Rot3Q(q2)));
EXPECT(assert_equal(R2, Rot3Q::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
// Check converting Rot3Q to quaterion
EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));
EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs())));
// Check that quaternion and Rot3Q represent the same rotation
Point3 p1(1.0, 2.0, 3.0);
Point3 p2(8.0, 7.0, 9.0);
Point3 expected1 = R1*p1;
Point3 expected2 = R2*p2;
Point3 actual1 = Point3(q1*p1.vector());
Point3 actual2 = Point3(q2*p2.vector());
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(expected2, actual2));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

1
tests/Makefile.am
View File

@ -23,6 +23,7 @@ check_PROGRAMS += testNonlinearEqualityConstraint
check_PROGRAMS += testPose2SLAMwSPCG
check_PROGRAMS += testGaussianISAM2
check_PROGRAMS += testExtendedKalmanFilter
check_PROGRAMS += testRot3QOptimization
# only if serialization is available
if ENABLE_SERIALIZATION

130
tests/testRot3QOptimization.cpp Normal file
View File

@ -0,0 +1,130 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testRot3.cpp
* @brief Unit tests for Rot3Q class
* @author Richard Roberts
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <boost/math/constants/constants.hpp>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3Q.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
using namespace gtsam;
typedef TypedSymbol<Rot3Q, 'r'> KeyQ;
typedef Values<KeyQ> ValuesQ;
typedef PriorFactor<ValuesQ, KeyQ> PriorQ;
typedef BetweenFactor<ValuesQ, KeyQ> BetweenQ;
typedef NonlinearFactorGraph<ValuesQ> GraphQ;
typedef TypedSymbol<Rot3M, 'r'> KeyM;
typedef Values<KeyM> ValuesM;
typedef PriorFactor<ValuesM, KeyM> PriorM;
typedef BetweenFactor<ValuesM, KeyM> BetweenM;
typedef NonlinearFactorGraph<ValuesM> GraphM;
/* ************************************************************************* */
TEST(Rot3Q, optimize1) {
GraphQ fgQ;
fgQ.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01)));
fgQ.add(BetweenQ(0, 1, Rot3Q::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
fgQ.add(BetweenQ(1, 0, Rot3Q::Rz(5.0*M_PI/3.0), sharedSigma(3, 0.01)));
GraphM fgM;
fgM.add(PriorM(0, Rot3M(), sharedSigma(3, 0.01)));
fgM.add(BetweenM(0, 1, Rot3M::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
fgM.add(BetweenM(1, 0, Rot3M::Rz(5.0*M_PI/3.0), sharedSigma(3, 0.01)));
ValuesQ initialQ;
initialQ.insert(0, Rot3Q::Rz(0.0));
initialQ.insert(1, Rot3Q::Rz(M_PI/3.0 + 0.1));
ValuesM initialM;
initialM.insert(0, Rot3M::Rz(0.0));
initialM.insert(1, Rot3M::Rz(M_PI/3.0 + 0.1));
ValuesQ truthQ;
truthQ.insert(0, Rot3Q::Rz(0.0));
truthQ.insert(1, Rot3Q::Rz(M_PI/3.0));
ValuesM truthM;
truthM.insert(0, Rot3M::Rz(0.0));
truthM.insert(1, Rot3M::Rz(M_PI/3.0));
// Compare Matrix and Quaternion between
Matrix H1M, H2M;
Rot3M betwM = initialM[1].between(initialM[0], H1M, H2M);
Matrix H1Q, H2Q;
Rot3Q betwQ = initialM[1].between(initialM[0], H1Q, H2Q);
EXPECT(assert_equal(betwM.matrix(), betwQ.matrix()));
EXPECT(assert_equal(H1M, H1Q));
EXPECT(assert_equal(H2M, H2Q));
Point3 x1(1.0,0.0,0.0), x2(0.0,1.0,0.0);
EXPECT(assert_equal(betwM*x1, betwQ*x1));
EXPECT(assert_equal(betwM*x2, betwQ*x2));
// Compare Matrix and Quaternion logmap
Vector logM = initialM[1].localCoordinates(initialM[0]);
Vector logQ = initialQ[1].localCoordinates(initialQ[0]);
EXPECT(assert_equal(logM, logQ));
// Compare Matrix and Quaternion linear graph
Ordering ordering; ordering += KeyQ(0), KeyQ(1);
GaussianFactorGraph gfgQ(*fgQ.linearize(initialQ, ordering));
GaussianFactorGraph gfgM(*fgM.linearize(initialM, ordering));
EXPECT(assert_equal(gfgQ, gfgM, 1e-5));
NonlinearOptimizationParameters params;
//params.verbosity_ = NonlinearOptimizationParameters::TRYLAMBDA;
ValuesQ final = optimize(fgQ, initialQ, params);
EXPECT(assert_equal(truthQ, final, 1e-5));
}
/* ************************************************************************* */
TEST(Rot3Q, optimize) {
// Optimize a circle
ValuesQ truth;
ValuesQ initial;
GraphQ fg;
fg.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01)));
for(int j=0; j<6; ++j) {
truth.insert(j, Rot3Q::Rz(M_PI/3.0 * double(j)));
initial.insert(j, Rot3Q::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
fg.add(BetweenQ(j, (j+1)%6, Rot3Q::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
}
NonlinearOptimizationParameters params;
//params.verbosity_ = NonlinearOptimizationParameters::TRYLAMBDA;
ValuesQ final = optimize(fg, initial, params);
EXPECT(assert_equal(truth, final, 1e-5));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */