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 GTSAM_EXPORT 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 
432  Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = {},
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 
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
H
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
Definition: gnuplot_common_settings.hh:74
gtsam::SO< 3 >::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
gtsam::Quaternion
Eigen::Quaternion< double, Eigen::DontAlign > Quaternion
Definition: geometry/Quaternion.h:182
gtsam::Rot3::RzRyRx
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,...
Definition: Rot3.h:160
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::column
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Definition: base/Matrix.h:210
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
gtsam::Rot3::Rot3
Rot3(const SO3 &R)
Definition: Rot3.h:121
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(double wx, double wy, double wz)
Definition: Rot3.h:251
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
gtsam::Rot3::Quaternion
static Rot3 Quaternion(double w, double x, double y, double z)
Definition: Rot3.h:204
gtsam::Rot3::Identity
static Rot3 Identity()
identity rotation for group operation
Definition: Rot3.h:300
gtsam::Rot3::ClosestTo
static Rot3 ClosestTo(const Matrix3 &M)
Definition: Rot3.h:271
x
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
Definition: gnuplot_common_settings.hh:12
Eigen::AngleAxis
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition: ForwardDeclarations.h:290
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
concepts.h
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
os
ofstream os("timeSchurFactors.csv")
gtsam::Rot3::retractCayley
Rot3 retractCayley(const Vector &omega) const
Retraction from R^3 to Rot3 manifold using the Cayley transform.
Definition: Rot3.h:355
r1
static const double r1
Definition: testSmartRangeFactor.cpp:32
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Rot3::Ypr
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
gtsam::Rot3::Roll
static Rot3 Roll(double t)
Definition: Rot3.h:180
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
Unit3.h
gtsam::SO::matrix_
MatrixNN matrix_
Rotation matrix.
Definition: SOn.h:64
gtsam::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
SO3.h
3*3 matrix representation of SO(3)
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
A
Definition: test_numpy_dtypes.cpp:298
gtsam::unrotate
Point3_ unrotate(const Rot3_ &x, const Point3_ &p)
Definition: slam/expressions.h:109
gtsam::rotate
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
Definition: slam/expressions.h:97
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:229
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(const Vector3 &w)
Definition: Rot3.h:240
gtsam::Rot3::conjugate
Rot3 conjugate(const Rot3 &cRb) const
Definition: Rot3.h:321
gtsam::Rot3Vector
std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > Rot3Vector
std::vector of Rot3s, used in Matlab wrapper
Definition: Rot3.h:564
gtsam::Rot3::AdjointMap
Matrix3 AdjointMap() const
Definition: Rot3.h:396
gtsam::Rot3::Pitch
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:177
gtsam::Rot3::~Rot3
virtual ~Rot3()
Definition: Rot3.h:140
r3
static const double r3
Definition: testSmartRangeFactor.cpp:32
gtsam::Rot3::ChartAtOrigin
Definition: Rot3.h:399
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
Eigen::internal::omega
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
gtsam::Rot3::CayleyChart
Definition: Rot3.h:349
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
out
std::ofstream out("Result.txt")
gtsam::Rot3::AxisAngle
static Rot3 AxisAngle(const Unit3 &axis, double angle)
Definition: Rot3.h:231
gtsam::equals
Definition: Testable.h:112
gtsam::Rot3::EXPMAP
@ EXPMAP
Use the Lie group exponential map to retract.
Definition: Rot3.h:340
y
Scalar * y
Definition: level1_cplx_impl.h:124
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
gtsam::Rot3::localCayley
Vector3 localCayley(const Rot3 &other) const
Inverse of retractCayley.
Definition: Rot3.h:360
gtsam::Rot3::Rot3
Rot3(const Matrix3 &R)
Definition: Rot3.h:112
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
so3::R2
SO3 R2
Definition: testShonanFactor.cpp:43
gtsam
traits
Definition: chartTesting.h:28
gtsam::Rot3::rot_
SO3 rot_
Definition: Rot3.h:65
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Rot3::ExpmapDerivative
static Matrix3 ExpmapDerivative(const Vector3 &x)
Derivative of Expmap.
Definition: Rot3.cpp:236
Quaternion.h
Lie Group wrapper for Eigen Quaternions.
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Rot3::Rot3
Rot3(const Eigen::MatrixBase< Derived > &R)
Definition: Rot3.h:101
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::LieGroup
Definition: Lie.h:37
gtsam::SO< 3 >
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Rot3::CoordinatesMode
CoordinatesMode
Definition: Rot3.h:339
gtsam::SO< 3 >::ClosestTo
static SO ClosestTo(const MatrixNN &M)
Eigen::MatrixBase
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
Definition: Unit3.cpp:151
align_3::t
Point2 t(10, 10)
cRb
gtsam::Rot3 cRb
Definition: testEssentialMatrixFactor.cpp:33
so3::R12
SO3 R12
Definition: testShonanFactor.cpp:44
gtsam::Rot3::CAYLEY
@ CAYLEY
Retract and localCoordinates using the Cayley transform.
Definition: Rot3.h:342
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::RQ
pair< Matrix3, Vector3 > RQ(const Matrix3 &A, OptionalJacobian< 3, 9 > H)
Definition: Rot3.cpp:246
gtsam::Rot3::AxisAngle
static Rot3 AxisAngle(const Point3 &axis, double angle)
Definition: Rot3.h:215
gtsam::Rot3::Rot3
Rot3(double w, double x, double y, double z)
Definition: Rot3.h:129
HR
#define HR
Definition: mincover.c:26
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:04:58