Loading...
Searching...
No Matches
Rot3.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
// \callgraph
#pragma once
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
#include <random>
// You can override the default coordinate mode using this flag
#ifndef ROT3_DEFAULT_COORDINATES_MODE
#ifdef GTSAM_USE_QUATERNIONS
// Exponential map is very cheap for quaternions
#define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
#else
// If user doesn't require GTSAM_ROT3_EXPMAP in cmake when building
#ifndef GTSAM_ROT3_EXPMAP
// For rotation matrices, the Cayley transform is a fast retract alternative
#define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY
#else
#define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
#endif
#endif
#endif
namespace gtsam {
class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
private:
#ifdef GTSAM_USE_QUATERNIONS
gtsam::Quaternion quaternion_;
#else
SO3 rot_;
#endif
public:
Rot3();
Rot3(const Point3& col1, const Point3& col2, const Point3& col3);
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33);
template <typename Derived>
#ifdef GTSAM_USE_QUATERNIONS
explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
quaternion_ = Matrix3(R);
}
#else
explicit Rot3(const Eigen::MatrixBase<Derived>& R) : rot_(R) {
}
#endif
#ifdef GTSAM_USE_QUATERNIONS
explicit Rot3(const Matrix3& R) : quaternion_(R) {}
#else
explicit Rot3(const Matrix3& R) : rot_(R) {}
#endif
#ifdef GTSAM_USE_QUATERNIONS
explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {}
#else
explicit Rot3(const SO3& R) : rot_(R) {}
#endif
Rot3(const Quaternion& q);
Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
static Rot3 Random(std::mt19937 & rng);
virtual ~Rot3() {}
/* Static member function to generate some well known rotations */
static Rot3 Rx(double t);
static Rot3 Ry(double t);
static Rot3 Rz(double t);
static Rot3 RzRyRx(double x, double y, double z,
OptionalJacobian<3, 1> Hx = boost::none,
OptionalJacobian<3, 1> Hy = boost::none,
OptionalJacobian<3, 1> Hz = boost::none);
inline static Rot3 RzRyRx(const Vector& xyz,
OptionalJacobian<3, 3> H = boost::none) {
assert(xyz.size() == 3);
Rot3 out;
if (H) {
Vector3 Hx, Hy, Hz;
out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
(*H) << Hx, Hy, Hz;
} else
out = RzRyRx(xyz(0), xyz(1), xyz(2));
return out;
}
static Rot3 Yaw (double t) { return Rz(t); }
static Rot3 Pitch(double t) { return Ry(t); }
static Rot3 Roll (double t) { return Rx(t); }
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) {
return RzRyRx(r, p, y, Hr, Hp, Hy);
}
static Rot3 Quaternion(double w, double x, double y, double z) {
gtsam::Quaternion q(w, x, y, z);
return Rot3(q);
}
static Rot3 AxisAngle(const Point3& axis, double angle) {
// Convert to unit vector.
Vector3 unitAxis = Unit3(axis).unitVector();
#ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
#else
return Rot3(SO3::AxisAngle(unitAxis,angle));
#endif
}
static Rot3 AxisAngle(const Unit3& axis, double angle) {
return AxisAngle(axis.unitVector(),angle);
}
static Rot3 Rodrigues(const Vector3& w) {
return Rot3::Expmap(w);
}
static Rot3 Rodrigues(double wx, double wy, double wz) {
return Rodrigues(Vector3(wx, wy, wz));
}
static Rot3 AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p);
static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
const Unit3& a_q, const Unit3& b_q);
static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); }
Rot3 normalized() const;
void print(const std::string& s="") const;
bool equals(const Rot3& p, double tol = 1e-9) const;
inline static Rot3 Identity() {
return Rot3();
}
Rot3 operator*(const Rot3& R2) const;
Rot3 inverse() const {
#ifdef GTSAM_USE_QUATERNIONS
return Rot3(quaternion_.inverse());
#else
return Rot3(rot_.matrix().transpose());
#endif
}
Rot3 conjugate(const Rot3& cRb) const {
// TODO: do more efficiently by using Eigen or quaternion properties
return cRb * (*this) * cRb.inverse();
}
enum CoordinatesMode {
EXPMAP,
#ifndef GTSAM_USE_QUATERNIONS
CAYLEY,
#endif
};
#ifndef GTSAM_USE_QUATERNIONS
// Cayley chart around origin
struct CayleyChart {
static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none);
static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none);
};
Rot3 retractCayley(const Vector& omega) const {
return compose(CayleyChart::Retract(omega));
}
Vector3 localCayley(const Rot3& other) const {
return CayleyChart::Local(between(other));
}
#endif
static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) {
if(H) *H = Rot3::ExpmapDerivative(v);
#ifdef GTSAM_USE_QUATERNIONS
return traits<gtsam::Quaternion>::Expmap(v);
#else
return Rot3(traits<SO3>::Expmap(v));
#endif
}
static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none);
static Matrix3 ExpmapDerivative(const Vector3& x);
static Matrix3 LogmapDerivative(const Vector3& x);
Matrix3 AdjointMap() const { return matrix(); }
// Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
struct ChartAtOrigin {
static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none);
static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none);
};
using LieGroup<Rot3, 3>::inverse; // version with derivative
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
Point3 operator*(const Point3& p) const;
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
Unit3 operator*(const Unit3& p) const;
Matrix3 matrix() const;
Matrix3 transpose() const;
Point3 column(int index) const;
Point3 r1() const;
Point3 r2() const;
Point3 r3() const;
Vector3 xyz(OptionalJacobian<3, 3> H = boost::none) const;
Vector3 ypr(OptionalJacobian<3, 3> H = boost::none) const;
Vector3 rpy(OptionalJacobian<3, 3> H = boost::none) const;
double roll(OptionalJacobian<1, 3> H = boost::none) const;
double pitch(OptionalJacobian<1, 3> H = boost::none) const;
double yaw(OptionalJacobian<1, 3> H = boost::none) const;
std::pair<Unit3, double> axisAngle() const;
gtsam::Quaternion toQuaternion() const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
Vector GTSAM_DEPRECATED quaternion() const;
#endif
Rot3 slerp(double t, const Rot3& other) const;
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
#ifndef GTSAM_USE_QUATERNIONS
Matrix3& M = rot_.matrix_;
ar& boost::serialization::make_nvp("rot11", M(0, 0));
ar& boost::serialization::make_nvp("rot12", M(0, 1));
ar& boost::serialization::make_nvp("rot13", M(0, 2));
ar& boost::serialization::make_nvp("rot21", M(1, 0));
ar& boost::serialization::make_nvp("rot22", M(1, 1));
ar& boost::serialization::make_nvp("rot23", M(1, 2));
ar& boost::serialization::make_nvp("rot31", M(2, 0));
ar& boost::serialization::make_nvp("rot32", M(2, 1));
ar& boost::serialization::make_nvp("rot33", M(2, 2));
#else
ar& boost::serialization::make_nvp("w", quaternion_.w());
ar& boost::serialization::make_nvp("x", quaternion_.x());
ar& boost::serialization::make_nvp("y", quaternion_.y());
ar& boost::serialization::make_nvp("z", quaternion_.z());
#endif
}
#ifdef GTSAM_USE_QUATERNIONS
// only align if quaternion, Matrix3 has no alignment requirements
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
const Matrix3& A, OptionalJacobian<3, 9> H = boost::none);
template<>
struct traits<Rot3> : public internal::LieGroup<Rot3> {};
template<>
struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
} // namespace gtsam
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Quaternion.h
Lie Group wrapper for Eigen Quaternions.
SO3.h
3*3 matrix representation of SO(3)
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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:260
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
gtsam::Rot3Vector
std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > Rot3Vector
std::vector of Rot3s, mainly for wrapper
Definition Rot3.h:573
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:36
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::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
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::axisAngle
std::pair< Unit3, double > axisAngle() const
Compute the Euler axis and angle (in radians) representation of this rotation.
Definition Rot3.cpp:244
gtsam::Rot3::Identity
static Rot3 Identity()
identity rotation for group operation
Definition Rot3.h:300
gtsam::Rot3::pitch
double pitch(OptionalJacobian< 1, 3 > H=boost::none) const
Accessor to get to component of angle representations NOTE: these are not efficient to get to multipl...
Definition Rot3.cpp:207
gtsam::Rot3::operator<<
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Rot3 &p)
Output stream operator.
Definition Rot3.cpp:320
gtsam::Rot3::retractCayley
Rot3 retractCayley(const Vector &omega) const
Retraction from R^3 to Rot3 manifold using the Cayley transform.
Definition Rot3.h:355
gtsam::Rot3::slerp
Rot3 slerp(double t, const Rot3 &other) const
Spherical Linear intERPolation between *this and other.
Definition Rot3.cpp:326
gtsam::Rot3::AdjointMap
Matrix3 AdjointMap() const
Calculate Adjoint map.
Definition Rot3.h:396
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:271
gtsam::Rot3::~Rot3
virtual ~Rot3()
Virtual destructor.
Definition Rot3.h:140
gtsam::Rot3::RzRyRx
static Rot3 RzRyRx(double x, double y, double z, OptionalJacobian< 3, 1 > Hx=boost::none, OptionalJacobian< 3, 1 > Hy=boost::none, OptionalJacobian< 3, 1 > Hz=boost::none)
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix,...
Definition Rot3M.cpp:85
gtsam::Rot3::transpose
Matrix3 transpose() const
Return 3*3 transpose (inverse) rotation matrix.
Definition Rot3M.cpp:144
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
Definition Rot3.cpp:136
gtsam::Rot3::column
Point3 column(int index) const
Definition Rot3.cpp:149
gtsam::Rot3::r1
Point3 r1() const
first column
Definition Rot3M.cpp:224
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition Rot3.h:174
gtsam::Rot3::r2
Point3 r2() const
second column
Definition Rot3M.cpp:227
gtsam::Rot3::toQuaternion
gtsam::Quaternion toQuaternion() const
Compute the quaternion representation of this rotation.
Definition Rot3M.cpp:233
gtsam::Rot3::AxisAngle
static Rot3 AxisAngle(const Unit3 &axis, double angle)
Convert from axis/angle representation.
Definition Rot3.h:231
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:112
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(const Vector3 &w)
Rodrigues' formula to compute an incremental rotation.
Definition Rot3.h:240
gtsam::Rot3::xyz
Vector3 xyz(OptionalJacobian< 3, 3 > H=boost::none) const
Use RQ to calculate xyz angle representation.
Definition Rot3.cpp:161
gtsam::Rot3::Rot3
Rot3()
default constructor, unit rotation
Definition Rot3M.cpp:35
gtsam::Rot3::Ry
static Rot3 Ry(double t)
Rotation around Y axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when look...
Definition Rot3M.cpp:66
gtsam::Rot3::AxisAngle
static Rot3 AxisAngle(const Point3 &axis, double angle)
Convert from axis/angle representation.
Definition Rot3.h:215
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition Rot3M.cpp:149
gtsam::Rot3::operator*
Rot3 operator*(const Rot3 &R2) const
Syntatic sugar for composing two rotations.
Definition Rot3M.cpp:139
gtsam::Rot3::yaw
double yaw(OptionalJacobian< 1, 3 > H=boost::none) const
Accessor to get to component of angle representations NOTE: these are not efficient to get to multipl...
Definition Rot3.cpp:219
gtsam::Rot3::Pitch
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition Rot3.h:177
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:160
gtsam::Rot3::localCayley
Vector3 localCayley(const Rot3 &other) const
Inverse of retractCayley.
Definition Rot3.h:360
gtsam::Rot3::Quaternion
static Rot3 Quaternion(double w, double x, double y, double z)
Create from Quaternion coefficients.
Definition Rot3.h:204
gtsam::Rot3::r3
Point3 r3() const
third column
Definition Rot3M.cpp:230
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:321
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(double wx, double wy, double wz)
Rodrigues' formula to compute an incremental rotation.
Definition Rot3.h:251
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition Rot3.h:308
gtsam::Rot3::Rot3
Rot3(const SO3 &R)
Constructor from an SO3 instance.
Definition Rot3.h:121
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:374
gtsam::Rot3::rpy
Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const
Use RQ to calculate roll-pitch-yaw angle representation.
Definition Rot3.cpp:192
gtsam::Rot3::ExpmapDerivative
static Matrix3 ExpmapDerivative(const Vector3 &x)
Derivative of Expmap.
Definition Rot3.cpp:250
gtsam::Rot3::roll
double roll(OptionalJacobian< 1, 3 > H=boost::none) const
Accessor to get to component of angle representations NOTE: these are not efficient to get to multipl...
Definition Rot3.cpp:195
gtsam::Rot3::Rz
static Rot3 Rz(double t)
Rotation around Z axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when look...
Definition Rot3M.cpp:75
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:196
gtsam::Rot3::CoordinatesMode
CoordinatesMode
The method retract() is used to map from the tangent space back to the manifold.
Definition Rot3.h:339
gtsam::Rot3::CAYLEY
@ CAYLEY
Retract and localCoordinates using the Cayley transform.
Definition Rot3.h:342
gtsam::Rot3::EXPMAP
@ EXPMAP
Use the Lie group exponential map to retract.
Definition Rot3.h:340
gtsam::Rot3::Rot3
Rot3(const Eigen::MatrixBase< Derived > &R)
Constructor from a rotation matrix Version for generic matrices.
Definition Rot3.h:101
gtsam::Rot3::matrix
Matrix3 matrix() const
return 3*3 rotation matrix
Definition Rot3M.cpp:219
gtsam::Rot3::ypr
Vector3 ypr(OptionalJacobian< 3, 3 > H=boost::none) const
Use RQ to calculate yaw-pitch-roll angle representation.
Definition Rot3.cpp:184
gtsam::Rot3::CayleyChart
Definition Rot3.h:349
gtsam::Rot3::ChartAtOrigin
Definition Rot3.h:399
gtsam::SO< 3 >::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Definition SO3.cpp:138
gtsam::SO< 3 >::ClosestTo
static SO ClosestTo(const MatrixNN &M)
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition Unit3.cpp:151