gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
50namespace gtsam {
51
58class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
59 private:
60
61#ifdef GTSAM_USE_QUATERNIONS
63 gtsam::Quaternion quaternion_;
64#else
65 SO3 rot_;
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 = boost::none,
156 OptionalJacobian<3, 1> Hy = boost::none,
157 OptionalJacobian<3, 1> Hz = boost::none);
158
160 inline static Rot3 RzRyRx(const Vector& xyz,
161 OptionalJacobian<3, 3> H = boost::none) {
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 = boost::none,
198 OptionalJacobian<3, 1> Hp = boost::none,
199 OptionalJacobian<3, 1> Hr = boost::none) {
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 = boost::none);
351 static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none);
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 = boost::none) {
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 = boost::none);
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
400 static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none);
401 static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none);
402 };
403
404 using LieGroup<Rot3, 3>::inverse; // version with derivative
405
409
413 Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
414 OptionalJacobian<3,3> H2 = boost::none) const;
415
417 Point3 operator*(const Point3& p) const;
418
420 Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
421 OptionalJacobian<3,3> H2=boost::none) const;
422
426
428 Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
429 OptionalJacobian<2,2> Hp = boost::none) const;
430
432 Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
433 OptionalJacobian<2,2> Hp = boost::none) 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 = boost::none) const;
462
467 Vector3 ypr(OptionalJacobian<3, 3> H = boost::none) const;
468
473 Vector3 rpy(OptionalJacobian<3, 3> H = boost::none) const;
474
481 double roll(OptionalJacobian<1, 3> H = boost::none) const;
482
489 double pitch(OptionalJacobian<1, 3> H = boost::none) const;
490
497 double yaw(OptionalJacobian<1, 3> H = boost::none) const;
498
502
511 std::pair<Unit3, double> axisAngle() const;
512
516 gtsam::Quaternion toQuaternion() const;
517
518#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
526 Vector GTSAM_DEPRECATED quaternion() const;
527#endif
528
534 Rot3 slerp(double t, const Rot3& other) const;
535
537 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
538
540
541 private:
543 friend class boost::serialization::access;
544 template <class ARCHIVE>
545 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
546#ifndef GTSAM_USE_QUATERNIONS
547 Matrix3& M = rot_.matrix_;
548 ar& boost::serialization::make_nvp("rot11", M(0, 0));
549 ar& boost::serialization::make_nvp("rot12", M(0, 1));
550 ar& boost::serialization::make_nvp("rot13", M(0, 2));
551 ar& boost::serialization::make_nvp("rot21", M(1, 0));
552 ar& boost::serialization::make_nvp("rot22", M(1, 1));
553 ar& boost::serialization::make_nvp("rot23", M(1, 2));
554 ar& boost::serialization::make_nvp("rot31", M(2, 0));
555 ar& boost::serialization::make_nvp("rot32", M(2, 1));
556 ar& boost::serialization::make_nvp("rot33", M(2, 2));
557#else
558 ar& boost::serialization::make_nvp("w", quaternion_.w());
559 ar& boost::serialization::make_nvp("x", quaternion_.x());
560 ar& boost::serialization::make_nvp("y", quaternion_.y());
561 ar& boost::serialization::make_nvp("z", quaternion_.z());
562#endif
563 }
564
565#ifdef GTSAM_USE_QUATERNIONS
566 // only align if quaternion, Matrix3 has no alignment requirements
567 public:
569#endif
570 };
571
573 using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
574
585 GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
586 const Matrix3& A, OptionalJacobian<3, 9> H = boost::none);
587
588 template<>
589 struct traits<Rot3> : public internal::LieGroup<Rot3> {};
590
591 template<>
592 struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
593
594} // namespace gtsam
595
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
3*3 matrix representation of SO(3)
Lie Group wrapper for Eigen Quaternions.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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:260
std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > Rot3Vector
std::vector of Rot3s, mainly for wrapper
Definition Rot3.h:573
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Extracts a column view from a matrix that avoids a copy.
Definition Matrix.h:211
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
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:36
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
Both LieGroupTraits and Testable.
Definition Lie.h:229
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
Template to create a binary predicate.
Definition Testable.h:111
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
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
Matrix3 AdjointMap() const
Calculate Adjoint map.
Definition Rot3.h:396
static Rot3 ClosestTo(const Matrix3 &M)
Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
Definition Rot3.h:271
virtual ~Rot3()
Virtual destructor.
Definition Rot3.h:140
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition Rot3.h:174
static Rot3 AxisAngle(const Unit3 &axis, double angle)
Convert from axis/angle representation.
Definition Rot3.h:231
Rot3(const Matrix3 &R)
Constructor from a rotation matrix Overload version for Matrix3 to avoid casting in quaternion mode.
Definition Rot3.h:112
static Rot3 Rodrigues(const Vector3 &w)
Rodrigues' formula to compute an incremental rotation.
Definition Rot3.h:240
static Rot3 AxisAngle(const Point3 &axis, double angle)
Convert from axis/angle representation.
Definition Rot3.h:215
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition Rot3.h:177
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:160
Vector3 localCayley(const Rot3 &other) const
Inverse of retractCayley.
Definition Rot3.h:360
static Rot3 Quaternion(double w, double x, double y, double z)
Create from Quaternion coefficients.
Definition Rot3.h:204
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:321
static Rot3 Rodrigues(double wx, double wy, double wz)
Rodrigues' formula to compute an incremental rotation.
Definition Rot3.h:251
Rot3 inverse() const
inverse of a rotation
Definition Rot3.h:308
Rot3(const SO3 &R)
Constructor from an SO3 instance.
Definition Rot3.h:121
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:374
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:196
CoordinatesMode
The method retract() is used to map from the tangent space back to the manifold.
Definition Rot3.h:339
@ CAYLEY
Retract and localCoordinates using the Cayley transform.
Definition Rot3.h:342
@ EXPMAP
Use the Lie group exponential map to retract.
Definition Rot3.h:340
Rot3(const Eigen::MatrixBase< Derived > &R)
Constructor from a rotation matrix Version for generic matrices.
Definition Rot3.h:101
Definition Rot3.h:349
Definition Rot3.h:399
MatrixNN matrix_
Rotation matrix.
Definition SOn.h:62
const MatrixNN & matrix() const
Return matrix.
Definition SOn.h:155
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition Unit3.cpp:151