gtsam  4.1.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 
22 // \callgraph
23 
24 #pragma once
25 
26 #include <gtsam/geometry/Unit3.h>
28 #include <gtsam/geometry/SO3.h>
29 #include <gtsam/base/concepts.h>
30 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
31 
32 #include <random>
33 
34 // You can override the default coordinate mode using this flag
35 #ifndef ROT3_DEFAULT_COORDINATES_MODE
36  #ifdef GTSAM_USE_QUATERNIONS
37  // Exponential map is very cheap for quaternions
38  #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
39  #else
40  // If user doesn't require GTSAM_ROT3_EXPMAP in cmake when building
41  #ifndef GTSAM_ROT3_EXPMAP
42  // For rotation matrices, the Cayley transform is a fast retract alternative
43  #define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY
44  #else
45  #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
46  #endif
47  #endif
48 #endif
49 
50 namespace gtsam {
51 
59  class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
60 
61  private:
62 
63 #ifdef GTSAM_USE_QUATERNIONS
64 
65  gtsam::Quaternion quaternion_;
66 #else
67  SO3 rot_;
68 #endif
69 
70  public:
71 
74 
76  Rot3();
77 
84  Rot3(const Point3& col1, const Point3& col2, const Point3& col3);
85 
87  Rot3(double R11, double R12, double R13,
88  double R21, double R22, double R23,
89  double R31, double R32, double R33);
90 
98  template <typename Derived>
99 #ifdef GTSAM_USE_QUATERNIONS
100  explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
101  quaternion_ = Matrix3(R);
102  }
103 #else
104  explicit Rot3(const Eigen::MatrixBase<Derived>& R) : rot_(R) {
105  }
106 #endif
107 
112 #ifdef GTSAM_USE_QUATERNIONS
113  explicit Rot3(const Matrix3& R) : quaternion_(R) {}
114 #else
115  explicit Rot3(const Matrix3& R) : rot_(R) {}
116 #endif
117 
121 #ifdef GTSAM_USE_QUATERNIONS
122  explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {}
123 #else
124  explicit Rot3(const SO3& R) : rot_(R) {}
125 #endif
126 
131  Rot3(const Quaternion& q);
132  Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
133 
140  static Rot3 Random(std::mt19937 & rng);
141 
143  virtual ~Rot3() {}
144 
145  /* Static member function to generate some well known rotations */
146 
148  static Rot3 Rx(double t);
149 
151  static Rot3 Ry(double t);
152 
154  static Rot3 Rz(double t);
155 
157  static Rot3 RzRyRx(double x, double y, double z,
158  OptionalJacobian<3, 1> Hx = boost::none,
159  OptionalJacobian<3, 1> Hy = boost::none,
160  OptionalJacobian<3, 1> Hz = boost::none);
161 
163  inline static Rot3 RzRyRx(const Vector& xyz,
164  OptionalJacobian<3, 3> H = boost::none) {
165  assert(xyz.size() == 3);
166  Rot3 out;
167  if (H) {
168  Vector3 Hx, Hy, Hz;
169  out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
170  (*H) << Hx, Hy, Hz;
171  } else
172  out = RzRyRx(xyz(0), xyz(1), xyz(2));
173  return out;
174  }
175 
177  static Rot3 Yaw (double t) { return Rz(t); }
178 
180  static Rot3 Pitch(double t) { return Ry(t); }
181 
183  static Rot3 Roll (double t) { return Rx(t); }
184 
199  static Rot3 Ypr(double y, double p, double r,
200  OptionalJacobian<3, 1> Hy = boost::none,
201  OptionalJacobian<3, 1> Hp = boost::none,
202  OptionalJacobian<3, 1> Hr = boost::none) {
203  return RzRyRx(r, p, y, Hr, Hp, Hy);
204  }
205 
207  static Rot3 Quaternion(double w, double x, double y, double z) {
208  gtsam::Quaternion q(w, x, y, z);
209  return Rot3(q);
210  }
211 
218  static Rot3 AxisAngle(const Point3& axis, double angle) {
219  // Convert to unit vector.
220  Vector3 unitAxis = Unit3(axis).unitVector();
221 #ifdef GTSAM_USE_QUATERNIONS
222  return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
223 #else
224  return Rot3(SO3::AxisAngle(unitAxis,angle));
225 #endif
226  }
227 
234  static Rot3 AxisAngle(const Unit3& axis, double angle) {
235  return AxisAngle(axis.unitVector(),angle);
236  }
237 
243  static Rot3 Rodrigues(const Vector3& w) {
244  return Rot3::Expmap(w);
245  }
246 
254  static Rot3 Rodrigues(double wx, double wy, double wz) {
255  return Rodrigues(Vector3(wx, wy, wz));
256  }
257 
259  static Rot3 AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p);
260 
262  static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
263  const Unit3& a_q, const Unit3& b_q);
264 
274  static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); }
275 
286  Rot3 normalized() const;
287 
291 
293  void print(const std::string& s="") const;
294 
296  bool equals(const Rot3& p, double tol = 1e-9) const;
297 
301 
303  inline static Rot3 identity() {
304  return Rot3();
305  }
306 
308  Rot3 operator*(const Rot3& R2) const;
309 
311  Rot3 inverse() const {
312 #ifdef GTSAM_USE_QUATERNIONS
313  return Rot3(quaternion_.inverse());
314 #else
315  return Rot3(rot_.matrix().transpose());
316 #endif
317  }
318 
324  Rot3 conjugate(const Rot3& cRb) const {
325  // TODO: do more efficiently by using Eigen or quaternion properties
326  return cRb * (*this) * cRb.inverse();
327  }
328 
332 
344 #ifndef GTSAM_USE_QUATERNIONS
346 #endif
347  };
348 
349 #ifndef GTSAM_USE_QUATERNIONS
350 
351  // Cayley chart around origin
352  struct CayleyChart {
353  static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none);
354  static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none);
355  };
356 
358  Rot3 retractCayley(const Vector& omega) const {
359  return compose(CayleyChart::Retract(omega));
360  }
361 
363  Vector3 localCayley(const Rot3& other) const {
364  return CayleyChart::Local(between(other));
365  }
366 
367 #endif
368 
372 
377  static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) {
378  if(H) *H = Rot3::ExpmapDerivative(v);
379 #ifdef GTSAM_USE_QUATERNIONS
381 #else
382  return Rot3(traits<SO3>::Expmap(v));
383 #endif
384  }
385 
390  static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none);
391 
393  static Matrix3 ExpmapDerivative(const Vector3& x);
394 
396  static Matrix3 LogmapDerivative(const Vector3& x);
397 
399  Matrix3 AdjointMap() const { return matrix(); }
400 
401  // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
402  struct ChartAtOrigin {
403  static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none);
404  static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none);
405  };
406 
407  using LieGroup<Rot3, 3>::inverse; // version with derivative
408 
412 
416  Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
417  OptionalJacobian<3,3> H2 = boost::none) const;
418 
420  Point3 operator*(const Point3& p) const;
421 
423  Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
424  OptionalJacobian<3,3> H2=boost::none) const;
425 
429 
431  Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
432  OptionalJacobian<2,2> Hp = boost::none) const;
433 
435  Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
436  OptionalJacobian<2,2> Hp = boost::none) const;
437 
439  Unit3 operator*(const Unit3& p) const;
440 
444 
446  Matrix3 matrix() const;
447 
451  Matrix3 transpose() const;
452 
454  Point3 column(int index) const;
455 
456  Point3 r1() const;
457  Point3 r2() const;
458  Point3 r3() const;
459 
464  Vector3 xyz(OptionalJacobian<3, 3> H = boost::none) const;
465 
470  Vector3 ypr(OptionalJacobian<3, 3> H = boost::none) const;
471 
476  Vector3 rpy(OptionalJacobian<3, 3> H = boost::none) const;
477 
484  double roll(OptionalJacobian<1, 3> H = boost::none) const;
485 
492  double pitch(OptionalJacobian<1, 3> H = boost::none) const;
493 
500  double yaw(OptionalJacobian<1, 3> H = boost::none) const;
501 
505 
514  std::pair<Unit3, double> axisAngle() const;
515 
519  gtsam::Quaternion toQuaternion() const;
520 
525  Vector quaternion() const;
526 
532  Rot3 slerp(double t, const Rot3& other) const;
533 
535  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
536 
538 
539  private:
541  friend class boost::serialization::access;
542  template <class ARCHIVE>
543  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
544 #ifndef GTSAM_USE_QUATERNIONS
545  Matrix3& M = rot_.matrix_;
546  ar& boost::serialization::make_nvp("rot11", M(0, 0));
547  ar& boost::serialization::make_nvp("rot12", M(0, 1));
548  ar& boost::serialization::make_nvp("rot13", M(0, 2));
549  ar& boost::serialization::make_nvp("rot21", M(1, 0));
550  ar& boost::serialization::make_nvp("rot22", M(1, 1));
551  ar& boost::serialization::make_nvp("rot23", M(1, 2));
552  ar& boost::serialization::make_nvp("rot31", M(2, 0));
553  ar& boost::serialization::make_nvp("rot32", M(2, 1));
554  ar& boost::serialization::make_nvp("rot33", M(2, 2));
555 #else
556  ar& boost::serialization::make_nvp("w", quaternion_.w());
557  ar& boost::serialization::make_nvp("x", quaternion_.x());
558  ar& boost::serialization::make_nvp("y", quaternion_.y());
559  ar& boost::serialization::make_nvp("z", quaternion_.z());
560 #endif
561  }
562 
563 #ifdef GTSAM_USE_QUATERNIONS
564  // only align if quaternion, Matrix3 has no alignment requirements
565  public:
567 #endif
568  };
569 
580  GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
581  const Matrix3& A, OptionalJacobian<3, 9> H = boost::none);
582 
583  template<>
584  struct traits<Rot3> : public internal::LieGroup<Rot3> {};
585 
586  template<>
587  struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
588 }
589 
gtsam::Rot3::CayleyChart
Definition: Rot3.h:352
gtsam::SO< 3 >::ClosestTo
static SO ClosestTo(const MatrixNN &M)
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
gtsam::SO< 3 >::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::SO< 3 >
gtsam::Rot3::Rot3
Rot3(const Eigen::MatrixBase< Derived > &R)
Constructor from a rotation matrix Version for generic matrices.
Definition: Rot3.h:104
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:228
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Rot3::EXPMAP
@ EXPMAP
Use the Lie group exponential map to retract.
Definition: Rot3.h:343
gtsam::RQ
pair< Matrix3, Vector3 > RQ(const Matrix3 &A, OptionalJacobian< 3, 9 > H)
[RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles correspo...
Definition: Rot3.cpp:258
gtsam::Rot3::CoordinatesMode
CoordinatesMode
The method retract() is used to map from the tangent space back to the manifold.
Definition: Rot3.h:342
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:311
gtsam::Rot3::Rot3
Rot3(const Matrix3 &R)
Constructor from a rotation matrix Overload version for Matrix3 to avoid casting in quaternion mode.
Definition: Rot3.h:115
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Rot3::ExpmapDerivative
static Matrix3 ExpmapDerivative(const Vector3 &x)
Derivative of Expmap.
Definition: Rot3.cpp:248
gtsam::Rot3::ClosestTo
static Rot3 ClosestTo(const Matrix3 &M)
Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
Definition: Rot3.h:274
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:152
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(const Vector3 &w)
Rodrigues' formula to compute an incremental rotation.
Definition: Rot3.h:243
gtsam::Rot3::retractCayley
Rot3 retractCayley(const Vector &omega) const
Retraction from R^3 to Rot3 manifold using the Cayley transform.
Definition: Rot3.h:358
gtsam::Rot3::RzRyRx
static Rot3 RzRyRx(const Vector &xyz, OptionalJacobian< 3, 3 > H=boost::none)
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix,...
Definition: Rot3.h:163
gtsam::Rot3::Ypr
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy=boost::none, OptionalJacobian< 3, 1 > Hp=boost::none, OptionalJacobian< 3, 1 > Hr=boost::none)
Returns rotation nRb from body to nav frame.
Definition: Rot3.h:199
gtsam::Rot3::identity
static Rot3 identity()
identity rotation for group operation
Definition: Rot3.h:303
gtsam::Rot3
Definition: Rot3.h:59
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(double wx, double wy, double wz)
Rodrigues' formula to compute an incremental rotation.
Definition: Rot3.h:254
gtsam::Rot3::AxisAngle
static Rot3 AxisAngle(const Point3 &axis, double angle)
Convert from axis/angle representation.
Definition: Rot3.h:218
gtsam::Rot3::AxisAngle
static Rot3 AxisAngle(const Unit3 &axis, double angle)
Convert from axis/angle representation.
Definition: Rot3.h:234
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:36
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Rot3::localCayley
Vector3 localCayley(const Rot3 &other) const
Inverse of retractCayley.
Definition: Rot3.h:363
gtsam::Rot3::ChartAtOrigin
Definition: Rot3.h:402
gtsam::Rot3::CAYLEY
@ CAYLEY
Retract and localCoordinates using the Cayley transform.
Definition: Rot3.h:345
gtsam::Rot3::Pitch
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:180
gtsam::SO::matrix_
MatrixNN matrix_
Rotation matrix.
Definition: SOn.h:60
gtsam::Rot3::conjugate
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:324
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:45
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::Unit3::unitVector
GTSAM_EXPORT Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition: Unit3.cpp:143
gtsam::Rot3::Rot3
Rot3(const SO3 &R)
Constructor from an SO3 instance.
Definition: Rot3.h:124
Quaternion.h
Lie Group wrapper for Eigen Quaternions.
SO3.h
3*3 matrix representation of SO(3)
gtsam::Rot3::Quaternion
static Rot3 Quaternion(double w, double x, double y, double z)
Create from Quaternion coefficients.
Definition: Rot3.h:207
gtsam::Rot3::AdjointMap
Matrix3 AdjointMap() const
Calculate Adjoint map.
Definition: Rot3.h:399
gtsam::column
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Extracts a column view from a matrix that avoids a copy.
Definition: Matrix.h:214
gtsam::Rot3::Expmap
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:377
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:177
gtsam::Rot3::~Rot3
virtual ~Rot3()
Virtual destructor.
Definition: Rot3.h:143