26#include <gtsam/geometry/Unit3.h>
29#include <gtsam/base/concepts.h>
30#include <gtsam/config.h>
35#ifndef ROT3_DEFAULT_COORDINATES_MODE
36 #ifdef GTSAM_USE_QUATERNIONS
38 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
41 #ifndef GTSAM_ROT3_EXPMAP
43 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY
45 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
61#ifdef GTSAM_USE_QUATERNIONS
63 gtsam::Quaternion quaternion_;
84 Rot3(
double R11,
double R12,
double R13,
85 double R21,
double R22,
double R23,
86 double R31,
double R32,
double R33);
95 template <
typename Derived>
96#ifdef GTSAM_USE_QUATERNIONS
97 explicit Rot3(
const Eigen::MatrixBase<Derived>& R) {
98 quaternion_ = Matrix3(R);
101 explicit Rot3(
const Eigen::MatrixBase<Derived>& R) : rot_(R) {
109#ifdef GTSAM_USE_QUATERNIONS
110 explicit Rot3(
const Matrix3& R) : quaternion_(R) {}
112 explicit Rot3(
const Matrix3& R) : rot_(R) {}
118#ifdef GTSAM_USE_QUATERNIONS
119 explicit Rot3(
const SO3& R) : quaternion_(R.matrix()) {}
128 Rot3(
const Quaternion& q);
129 Rot3(
double w,
double x,
double y,
double z) :
Rot3(Quaternion(w, x, y, z)) {}
137 static Rot3 Random(std::mt19937 & rng);
145 static Rot3 Rx(
double t);
148 static Rot3 Ry(
double t);
151 static Rot3 Rz(
double t);
154 static Rot3 RzRyRx(
double x,
double y,
double z,
162 assert(xyz.size() == 3);
166 out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
169 out = RzRyRx(xyz(0), xyz(1), xyz(2));
180 static Rot3 Roll (
double t) {
return Rx(t); }
200 return RzRyRx(r, p, y, Hr, Hp, Hy);
205 gtsam::Quaternion q(w, x, y, z);
218#ifdef GTSAM_USE_QUATERNIONS
219 return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
221 return Rot3(SO3::AxisAngle(unitAxis,angle));
241 return Rot3::Expmap(w);
252 return Rodrigues(Vector3(wx, wy, wz));
283 Rot3 normalized()
const;
290 void print(
const std::string& s=
"")
const;
293 bool equals(
const Rot3& p,
double tol = 1e-9)
const;
309#ifdef GTSAM_USE_QUATERNIONS
310 return Rot3(quaternion_.inverse());
323 return cRb * (*this) * cRb.
inverse();
341#ifndef GTSAM_USE_QUATERNIONS
346#ifndef GTSAM_USE_QUATERNIONS
356 return compose(CayleyChart::Retract(omega));
361 return CayleyChart::Local(between(other));
375 if(H) *H = Rot3::ExpmapDerivative(v);
376#ifdef GTSAM_USE_QUATERNIONS
390 static Matrix3 ExpmapDerivative(
const Vector3& x);
393 static Matrix3 LogmapDerivative(
const Vector3& x);
400 static Rot3 Retract(
const Vector3& v, ChartJacobian H = boost::none);
401 static Vector3 Local(
const Rot3& r, ChartJacobian H = boost::none);
443 Matrix3 matrix()
const;
448 Matrix3 transpose()
const;
511 std::pair<Unit3, double> axisAngle()
const;
516 gtsam::Quaternion toQuaternion()
const;
518#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
526 Vector GTSAM_DEPRECATED quaternion()
const;
534 Rot3 slerp(
double t,
const Rot3& other)
const;
537 GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os,
const Rot3& p);
543 friend class boost::serialization::access;
544 template <
class ARCHIVE>
545 void serialize(ARCHIVE& ar,
const unsigned int ) {
546#ifndef GTSAM_USE_QUATERNIONS
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));
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());
565#ifdef GTSAM_USE_QUATERNIONS
573 using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
585 GTSAM_EXPORT std::pair<Matrix3, Vector3>
RQ(
#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
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