gtsam  4.0.0
gtsam
Rot3.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
21 // \callgraph
22 
23 #pragma once
24 
25 #include <gtsam/geometry/Unit3.h>
27 #include <gtsam/geometry/SO3.h>
28 #include <gtsam/base/concepts.h>
29 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
30 
31 // You can override the default coordinate mode using this flag
32 #ifndef ROT3_DEFAULT_COORDINATES_MODE
33  #ifdef GTSAM_USE_QUATERNIONS
34  // Exponential map is very cheap for quaternions
35  #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
36  #else
37  // If user doesn't require GTSAM_ROT3_EXPMAP in cmake when building
38  #ifndef GTSAM_ROT3_EXPMAP
39  // For rotation matrices, the Cayley transform is a fast retract alternative
40  #define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY
41  #else
42  #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
43  #endif
44  #endif
45 #endif
46 
47 namespace gtsam {
48 
56  class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
57 
58  private:
59 
60 #ifdef GTSAM_USE_QUATERNIONS
61 
62  gtsam::Quaternion quaternion_;
63 #else
64  Matrix3 rot_;
65 #endif
66 
67  public:
68 
71 
73  Rot3();
74 
81  Rot3(const Point3& col1, const Point3& col2, const Point3& col3);
82 
84  Rot3(double R11, double R12, double R13,
85  double R21, double R22, double R23,
86  double R31, double R32, double R33);
87 
95  template<typename Derived>
96  inline explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
97  #ifdef GTSAM_USE_QUATERNIONS
98  quaternion_=Matrix3(R);
99  #else
100  rot_ = R;
101  #endif
102  }
103 
108  inline explicit Rot3(const Matrix3& R) {
109  #ifdef GTSAM_USE_QUATERNIONS
110  quaternion_=R;
111  #else
112  rot_ = R;
113  #endif
114  }
115 
120  Rot3(const Quaternion& q);
121  Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
122 
124  static Rot3 Random(boost::mt19937 & rng);
125 
127  virtual ~Rot3() {}
128 
129  /* Static member function to generate some well known rotations */
130 
132  static Rot3 Rx(double t);
133 
135  static Rot3 Ry(double t);
136 
138  static Rot3 Rz(double t);
139 
141  static Rot3 RzRyRx(double x, double y, double z);
142 
144  inline static Rot3 RzRyRx(const Vector& xyz) {
145  assert(xyz.size() == 3);
146  return RzRyRx(xyz(0), xyz(1), xyz(2));
147  }
148 
150  static Rot3 Yaw (double t) { return Rz(t); }
151 
153  static Rot3 Pitch(double t) { return Ry(t); }
154 
156  static Rot3 Roll (double t) { return Rx(t); }
157 
172  static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
173 
175  static Rot3 Quaternion(double w, double x, double y, double z) {
176  gtsam::Quaternion q(w, x, y, z);
177  return Rot3(q);
178  }
179 
186  static Rot3 AxisAngle(const Point3& axis, double angle) {
187 #ifdef GTSAM_USE_QUATERNIONS
188  return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
189 #else
190  return Rot3(SO3::AxisAngle(axis,angle));
191 #endif
192  }
193 
200  static Rot3 AxisAngle(const Unit3& axis, double angle) {
201  return AxisAngle(axis.unitVector(),angle);
202  }
203 
209  static Rot3 Rodrigues(const Vector3& w) {
210  return Rot3::Expmap(w);
211  }
212 
220  static Rot3 Rodrigues(double wx, double wy, double wz) {
221  return Rodrigues(Vector3(wx, wy, wz));
222  }
223 
225  static Rot3 AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p);
226 
228  static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
229  const Unit3& a_q, const Unit3& b_q);
230 
234 
236  void print(const std::string& s="R") const;
237 
239  bool equals(const Rot3& p, double tol = 1e-9) const;
240 
244 
246  inline static Rot3 identity() {
247  return Rot3();
248  }
249 
251  Rot3 operator*(const Rot3& R2) const;
252 
254  Rot3 inverse() const {
255  return Rot3(Matrix3(transpose()));
256  }
257 
263  Rot3 conjugate(const Rot3& cRb) const {
264  // TODO: do more efficiently by using Eigen or quaternion properties
265  return cRb * (*this) * cRb.inverse();
266  }
267 
271 
283 #ifndef GTSAM_USE_QUATERNIONS
285 #endif
286  };
287 
288 #ifndef GTSAM_USE_QUATERNIONS
289 
290  // Cayley chart around origin
291  struct CayleyChart {
292  static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none);
293  static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none);
294  };
295 
297  Rot3 retractCayley(const Vector& omega) const {
298  return compose(CayleyChart::Retract(omega));
299  }
300 
302  Vector3 localCayley(const Rot3& other) const {
303  return CayleyChart::Local(between(other));
304  }
305 
306 #endif
307 
311 
316  static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) {
317  if(H) *H = Rot3::ExpmapDerivative(v);
318 #ifdef GTSAM_USE_QUATERNIONS
320 #else
321  return Rot3(traits<SO3>::Expmap(v));
322 #endif
323  }
324 
329  static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none);
330 
332  static Matrix3 ExpmapDerivative(const Vector3& x);
333 
335  static Matrix3 LogmapDerivative(const Vector3& x);
336 
338  Matrix3 AdjointMap() const { return matrix(); }
339 
340  // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
341  struct ChartAtOrigin {
342  static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none);
343  static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none);
344  };
345 
346  using LieGroup<Rot3, 3>::inverse; // version with derivative
347 
351 
355  Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
356  OptionalJacobian<3,3> H2 = boost::none) const;
357 
359  Point3 operator*(const Point3& p) const;
360 
362  Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
363  OptionalJacobian<3,3> H2=boost::none) const;
364 
368 
370  Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
371  OptionalJacobian<2,2> Hp = boost::none) const;
372 
374  Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
375  OptionalJacobian<2,2> Hp = boost::none) const;
376 
378  Unit3 operator*(const Unit3& p) const;
379 
383 
385  Matrix3 matrix() const;
386 
390  Matrix3 transpose() const;
391  // TODO: const Eigen::Transpose<const Matrix3> transpose() const;
392 
394  Point3 column(int index) const;
395 
396  Point3 r1() const;
397  Point3 r2() const;
398  Point3 r3() const;
399 
404  Vector3 xyz() const;
405 
410  Vector3 ypr() const;
411 
416  Vector3 rpy() const;
417 
424  inline double roll() const { return ypr()(2); }
425 
432  inline double pitch() const { return ypr()(1); }
433 
440  inline double yaw() const { return ypr()(0); }
441 
445 
449  gtsam::Quaternion toQuaternion() const;
450 
455  Vector quaternion() const;
456 
462  Rot3 slerp(double t, const Rot3& other) const;
463 
465  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
466 
468 
469 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
470  static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); }
473  static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); }
474  static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); }
475  static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); }
476  static Rot3 yaw (double t) { return Yaw(t); }
477  static Rot3 pitch(double t) { return Pitch(t); }
478  static Rot3 roll (double t) { return Roll(t); }
479  static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);}
480  static Rot3 quaternion(double w, double x, double y, double z) {
481  return Rot3::Quaternion(w, x, y, z);
482  }
484 #endif
485 
486  private:
487 
489  friend class boost::serialization::access;
490  template<class ARCHIVE>
491  void serialize(ARCHIVE & ar, const unsigned int /*version*/)
492  {
493 #ifndef GTSAM_USE_QUATERNIONS
494  ar & boost::serialization::make_nvp("rot11", rot_(0,0));
495  ar & boost::serialization::make_nvp("rot12", rot_(0,1));
496  ar & boost::serialization::make_nvp("rot13", rot_(0,2));
497  ar & boost::serialization::make_nvp("rot21", rot_(1,0));
498  ar & boost::serialization::make_nvp("rot22", rot_(1,1));
499  ar & boost::serialization::make_nvp("rot23", rot_(1,2));
500  ar & boost::serialization::make_nvp("rot31", rot_(2,0));
501  ar & boost::serialization::make_nvp("rot32", rot_(2,1));
502  ar & boost::serialization::make_nvp("rot33", rot_(2,2));
503 #else
504  ar & boost::serialization::make_nvp("w", quaternion_.w());
505  ar & boost::serialization::make_nvp("x", quaternion_.x());
506  ar & boost::serialization::make_nvp("y", quaternion_.y());
507  ar & boost::serialization::make_nvp("z", quaternion_.z());
508 #endif
509  }
510 
511 #ifdef GTSAM_USE_QUATERNIONS
512  // only align if quaternion, Matrix3 has no alignment requirements
513  public:
514  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
515 #endif
516  };
517 
528  GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
529 
530  template<>
531  struct traits<Rot3> : public internal::LieGroup<Rot3> {};
532 
533  template<>
534  struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
535 }
536 
static Rot3 Rodrigues(const Vector3 &w)
Rodrigues' formula to compute an incremental rotation.
Definition: Rot3.h:209
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Extracts a column view from a matrix that avoids a copy.
Definition: Matrix.h:213
Rot3 retractCayley(const Vector &omega) const
Retraction from R^3 to Rot3 manifold using the Cayley transform.
Definition: Rot3.h:297
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:150
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
Definition: Point3.h:45
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:153
double yaw() const
Accessor to get to component of angle representations NOTE: these are not efficient to get to multipl...
Definition: Rot3.h:440
Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition: Unit3.cpp:149
static Rot3 AxisAngle(const Unit3 &axis, double angle)
Convert from axis/angle representation.
Definition: Rot3.h:200
Both LieGroupTraits and Testable.
Definition: Lie.h:237
Template to create a binary predicate.
Definition: Testable.h:110
3*3 matrix representation of SO(3)
Matrix3 AdjointMap() const
Calculate Adjoint map.
Definition: Rot3.h:338
Rot3(const Matrix3 &R)
Constructor from a rotation matrix Overload version for Matrix3 to avoid casting in quaternion mode.
Definition: Rot3.h:108
double roll() const
Accessor to get to component of angle representations NOTE: these are not efficient to get to multipl...
Definition: Rot3.h:424
static Rot3 Ypr(double y, double p, double r)
Returns rotation nRb from body to nav frame.
Definition: Rot3.h:172
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
Definition: Rot3.h:56
Retract and localCoordinates using the Cayley transform.
Definition: Rot3.h:284
Rot3 inverse() const
inverse of a rotation, TODO should be different for M/Q
Definition: Rot3.h:254
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:36
static Matrix3 ExpmapDerivative(const Vector3 &x)
Derivative of Expmap.
Definition: Rot3.cpp:188
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
static Rot3 identity()
identity rotation for group operation
Definition: Rot3.h:246
Definition: Rot3.h:341
CoordinatesMode
The method retract() is used to map from the tangent space back to the manifold.
Definition: Rot3.h:281
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
virtual ~Rot3()
Virtual destructor.
Definition: Rot3.h:127
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates using Rodrigues' formula.
Definition: Rot3.h:316
Definition: Rot3.h:291
Lie Group wrapper for Eigen Quaternions.
double pitch() const
Accessor to get to component of angle representations NOTE: these are not efficient to get to multipl...
Definition: Rot3.h:432
pair< Matrix3, Vector3 > RQ(const Matrix3 &A)
[RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles correspo...
Definition: Rot3.cpp:198
static SO3 AxisAngle(const Vector3 &axis, double theta)
Static, named constructor TODO think about relation with above.
Definition: SO3.cpp:115
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
static Rot3 Rodrigues(double wx, double wy, double wz)
Rodrigues' formula to compute an incremental rotation.
Definition: Rot3.h:220
Rot3(const Eigen::MatrixBase< Derived > &R)
Constructor from a rotation matrix Version for generic matrices.
Definition: Rot3.h:96
static Rot3 AxisAngle(const Point3 &axis, double angle)
Convert from axis/angle representation.
Definition: Rot3.h:186
Vector3 localCayley(const Rot3 &other) const
Inverse of retractCayley.
Definition: Rot3.h:302
static Rot3 Quaternion(double w, double x, double y, double z)
Create from Quaternion coefficients.
Definition: Rot3.h:175
Use the Lie group exponential map to retract.
Definition: Rot3.h:282
Point2 operator *(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:170
Rot3 conjugate(const Rot3 &cRb) const
Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C.
Definition: Rot3.h:263
static Rot3 RzRyRx(const Vector &xyz)
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix,...
Definition: Rot3.h:144