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 
58 class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
59  private:
60 
61 #ifdef GTSAM_USE_QUATERNIONS
62 
63  gtsam::Quaternion quaternion_;
64 #else
66 #endif
67 
68  public:
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 #ifdef GTSAM_USE_QUATERNIONS
97  explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
98  quaternion_ = Matrix3(R);
99  }
100 #else
101  explicit Rot3(const Eigen::MatrixBase<Derived>& R) : rot_(R) {
102  }
103 #endif
104 
109 #ifdef GTSAM_USE_QUATERNIONS
110  explicit Rot3(const Matrix3& R) : quaternion_(R) {}
111 #else
112  explicit Rot3(const Matrix3& R) : rot_(R) {}
113 #endif
114 
118 #ifdef GTSAM_USE_QUATERNIONS
119  explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {}
120 #else
121  explicit Rot3(const SO3& R) : rot_(R) {}
122 #endif
123 
128  Rot3(const Quaternion& q);
129  Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
130 
137  static Rot3 Random(std::mt19937 & rng);
138 
140  virtual ~Rot3() {}
141 
142  /* Static member function to generate some well known rotations */
143 
145  static Rot3 Rx(double t);
146 
148  static Rot3 Ry(double t);
149 
151  static Rot3 Rz(double t);
152 
154  static Rot3 RzRyRx(double x, double y, double z,
155  OptionalJacobian<3, 1> Hx = {},
156  OptionalJacobian<3, 1> Hy = {},
157  OptionalJacobian<3, 1> Hz = {});
158 
160  inline static Rot3 RzRyRx(const Vector& xyz,
161  OptionalJacobian<3, 3> H = {}) {
162  assert(xyz.size() == 3);
163  Rot3 out;
164  if (H) {
165  Vector3 Hx, Hy, Hz;
166  out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
167  (*H) << Hx, Hy, Hz;
168  } else
169  out = RzRyRx(xyz(0), xyz(1), xyz(2));
170  return out;
171  }
172 
174  static Rot3 Yaw (double t) { return Rz(t); }
175 
177  static Rot3 Pitch(double t) { return Ry(t); }
178 
180  static Rot3 Roll (double t) { return Rx(t); }
181 
196  static Rot3 Ypr(double y, double p, double r,
197  OptionalJacobian<3, 1> Hy = {},
198  OptionalJacobian<3, 1> Hp = {},
199  OptionalJacobian<3, 1> Hr = {}) {
200  return RzRyRx(r, p, y, Hr, Hp, Hy);
201  }
202 
204  static Rot3 Quaternion(double w, double x, double y, double z) {
205  gtsam::Quaternion q(w, x, y, z);
206  return Rot3(q);
207  }
208 
215  static Rot3 AxisAngle(const Point3& axis, double angle) {
216  // Convert to unit vector.
217  Vector3 unitAxis = Unit3(axis).unitVector();
218 #ifdef GTSAM_USE_QUATERNIONS
219  return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
220 #else
221  return Rot3(SO3::AxisAngle(unitAxis,angle));
222 #endif
223  }
224 
231  static Rot3 AxisAngle(const Unit3& axis, double angle) {
232  return AxisAngle(axis.unitVector(),angle);
233  }
234 
240  static Rot3 Rodrigues(const Vector3& w) {
241  return Rot3::Expmap(w);
242  }
243 
251  static Rot3 Rodrigues(double wx, double wy, double wz) {
252  return Rodrigues(Vector3(wx, wy, wz));
253  }
254 
256  static Rot3 AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p);
257 
259  static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
260  const Unit3& a_q, const Unit3& b_q);
261 
271  static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); }
272 
283  Rot3 normalized() const;
284 
288 
290  void print(const std::string& s="") const;
291 
293  bool equals(const Rot3& p, double tol = 1e-9) const;
294 
298 
300  inline static Rot3 Identity() {
301  return Rot3();
302  }
303 
305  Rot3 operator*(const Rot3& R2) const;
306 
308  Rot3 inverse() const {
309 #ifdef GTSAM_USE_QUATERNIONS
310  return Rot3(quaternion_.inverse());
311 #else
312  return Rot3(rot_.matrix().transpose());
313 #endif
314  }
315 
321  Rot3 conjugate(const Rot3& cRb) const {
322  // TODO: do more efficiently by using Eigen or quaternion properties
323  return cRb * (*this) * cRb.inverse();
324  }
325 
329 
341 #ifndef GTSAM_USE_QUATERNIONS
343 #endif
344  };
345 
346 #ifndef GTSAM_USE_QUATERNIONS
347 
348  // Cayley chart around origin
349  struct CayleyChart {
350  static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = {});
351  static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = {});
352  };
353 
355  Rot3 retractCayley(const Vector& omega) const {
356  return compose(CayleyChart::Retract(omega));
357  }
358 
360  Vector3 localCayley(const Rot3& other) const {
361  return CayleyChart::Local(between(other));
362  }
363 
364 #endif
365 
369 
374  static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {}) {
375  if(H) *H = Rot3::ExpmapDerivative(v);
376 #ifdef GTSAM_USE_QUATERNIONS
378 #else
379  return Rot3(traits<SO3>::Expmap(v));
380 #endif
381  }
382 
387  static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = {});
388 
390  static Matrix3 ExpmapDerivative(const Vector3& x);
391 
393  static Matrix3 LogmapDerivative(const Vector3& x);
394 
396  Matrix3 AdjointMap() const { return matrix(); }
397 
398  // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
399  struct ChartAtOrigin {
400  static Rot3 Retract(const Vector3& v, ChartJacobian H = {});
401  static Vector3 Local(const Rot3& r, ChartJacobian H = {});
402  };
403 
404  using LieGroup<Rot3, 3>::inverse; // version with derivative
405 
409 
413  Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = {},
414  OptionalJacobian<3,3> H2 = {}) const;
415 
417  Point3 operator*(const Point3& p) const;
418 
420  Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = {},
421  OptionalJacobian<3,3> H2={}) const;
422 
426 
428  Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = {},
429  OptionalJacobian<2,2> Hp = {}) const;
430 
433  OptionalJacobian<2,2> Hp = {}) const;
434 
436  Unit3 operator*(const Unit3& p) const;
437 
441 
443  Matrix3 matrix() const;
444 
448  Matrix3 transpose() const;
449 
451  Point3 column(int index) const;
452 
453  Point3 r1() const;
454  Point3 r2() const;
455  Point3 r3() const;
456 
461  Vector3 xyz(OptionalJacobian<3, 3> H = {}) const;
462 
467  Vector3 ypr(OptionalJacobian<3, 3> H = {}) const;
468 
473  Vector3 rpy(OptionalJacobian<3, 3> H = {}) const;
474 
481  double roll(OptionalJacobian<1, 3> H = {}) const;
482 
489  double pitch(OptionalJacobian<1, 3> H = {}) const;
490 
497  double yaw(OptionalJacobian<1, 3> H = {}) const;
498 
502 
511  std::pair<Unit3, double> axisAngle() const;
512 
516  gtsam::Quaternion toQuaternion() const;
517 
523  Rot3 slerp(double t, const Rot3& other) const;
524 
526  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
527 
529 
530  private:
531 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
532 
533  friend class boost::serialization::access;
534  template <class ARCHIVE>
535  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
536 #ifndef GTSAM_USE_QUATERNIONS
537  Matrix3& M = rot_.matrix_;
538  ar& boost::serialization::make_nvp("rot11", M(0, 0));
539  ar& boost::serialization::make_nvp("rot12", M(0, 1));
540  ar& boost::serialization::make_nvp("rot13", M(0, 2));
541  ar& boost::serialization::make_nvp("rot21", M(1, 0));
542  ar& boost::serialization::make_nvp("rot22", M(1, 1));
543  ar& boost::serialization::make_nvp("rot23", M(1, 2));
544  ar& boost::serialization::make_nvp("rot31", M(2, 0));
545  ar& boost::serialization::make_nvp("rot32", M(2, 1));
546  ar& boost::serialization::make_nvp("rot33", M(2, 2));
547 #else
548  ar& boost::serialization::make_nvp("w", quaternion_.w());
549  ar& boost::serialization::make_nvp("x", quaternion_.x());
550  ar& boost::serialization::make_nvp("y", quaternion_.y());
551  ar& boost::serialization::make_nvp("z", quaternion_.z());
552 #endif
553  }
554 #endif
555 
556 #ifdef GTSAM_USE_QUATERNIONS
557  // only align if quaternion, Matrix3 has no alignment requirements
558  public:
560 #endif
561  };
562 
564  using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3>>;
565 
576  GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
577  const Matrix3& A, OptionalJacobian<3, 9> H = {});
578 
579  template<>
580  struct traits<Rot3> : public internal::LieGroup<Rot3> {};
581 
582  template<>
583  struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
584 
585 } // namespace gtsam
586 
Point3_ unrotate(const Rot3_ &x, const Point3_ &p)
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
static Rot3 RzRyRx(const Vector &xyz, OptionalJacobian< 3, 3 > H={})
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
Definition: Rot3.h:160
SO3 rot_
Definition: Rot3.h:65
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={})
Definition: Rot3.h:196
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
EIGEN_DEVICE_FUNC CoeffReturnType x() const
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > Rot3Vector
std::vector of Rot3s, used in Matlab wrapper
Definition: Rot3.h:564
Scalar * y
virtual ~Rot3()
Definition: Rot3.h:140
Rot3(const Eigen::MatrixBase< Derived > &R)
Definition: Rot3.h:101
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
Eigen::Vector3d Vector3
Definition: Vector.h:43
std::string serialize(const T &input)
serializes to a string
std::ofstream out("Result.txt")
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:177
Use the Lie group exponential map to retract.
Definition: Rot3.h:340
static std::mt19937 rng
EIGEN_DEVICE_FUNC CoeffReturnType z() const
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
Definition: Unit3.cpp:151
Rot2 R(Rot2::fromAngle(0.1))
static Rot3 Identity()
identity rotation for group operation
Definition: Rot3.h:300
Rot3 retractCayley(const Vector &omega) const
Retraction from R^3 to Rot3 manifold using the Cayley transform.
Definition: Rot3.h:355
Pose2_ Expmap(const Vector3_ &xi)
EIGEN_DEVICE_FUNC CoeffReturnType w() const
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Rot3(const SO3 &R)
Definition: Rot3.h:121
3*3 matrix representation of SO(3)
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
gtsam::Rot3 cRb
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Rot3(double w, double x, double y, double z)
Definition: Rot3.h:129
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
pair< Matrix3, Vector3 > RQ(const Matrix3 &A, OptionalJacobian< 3, 9 > H)
Definition: Rot3.cpp:246
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
Retract and localCoordinates using the Cayley transform.
Definition: Rot3.h:342
static Rot3 AxisAngle(const Unit3 &axis, double angle)
Definition: Rot3.h:231
static Rot3 Quaternion(double w, double x, double y, double z)
Definition: Rot3.h:204
MatrixNN matrix_
Rotation matrix.
Definition: SOn.h:64
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Vector3 localCayley(const Rot3 &other) const
Inverse of retractCayley.
Definition: Rot3.h:360
static Rot3 Rodrigues(const Vector3 &w)
Definition: Rot3.h:240
Rot3 conjugate(const Rot3 &cRb) const
Definition: Rot3.h:321
Matrix3 AdjointMap() const
Definition: Rot3.h:396
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
Eigen::VectorXd Vector
Definition: Vector.h:38
static Rot3 Roll(double t)
Definition: Rot3.h:180
static Matrix3 ExpmapDerivative(const Vector3 &x)
Derivative of Expmap.
Definition: Rot3.cpp:236
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
Array< int, Dynamic, 1 > v
EIGEN_DEVICE_FUNC Quaternion< Scalar > inverse() const
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
static const double r2
EIGEN_DEVICE_FUNC const Scalar & q
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
EIGEN_DEVICE_FUNC CoeffReturnType y() const
static const double r3
RowVector3d w
static Rot3 AxisAngle(const Point3 &axis, double angle)
Definition: Rot3.h:215
traits
Definition: chartTesting.h:28
static const double r1
ofstream os("timeSchurFactors.csv")
Both LieGroupTraits and Testable.
Definition: Lie.h:229
The quaternion class used to represent 3D orientations and rotations.
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
#define HR
Definition: mincover.c:26
float * p
static Rot3 ClosestTo(const Matrix3 &M)
Definition: Rot3.h:271
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:284
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
static Rot3 Rodrigues(double wx, double wy, double wz)
Definition: Rot3.h:251
static SO ClosestTo(const MatrixNN &M)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Rot3(const Matrix3 &R)
Definition: Rot3.h:112
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Definition: base/Matrix.h:210
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Point2 t(10, 10)
CoordinatesMode
Definition: Rot3.h:339
Eigen::Quaternion< double, Eigen::DontAlign > Quaternion


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:35