GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it is defined. More...
#include <Rot3.h>
Classes | |
struct | CayleyChart |
struct | ChartAtOrigin |
Public Types | |
enum | |
typedef OptionalJacobian< N, N > | ChartJacobian |
typedef Eigen::Matrix< double, N, N > | Jacobian |
typedef Eigen::Matrix< double, N, 1 > | TangentVector |
Public Member Functions | |
const Rot3 & | derived () const |
Rot3 | compose (const Rot3 &g) const |
Rot3 | compose (const Rot3 &g, ChartJacobian H1, ChartJacobian H2={}) const |
GTSAM_EXPORT SOn | compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
Rot3 | between (const Rot3 &g) const |
Rot3 | between (const Rot3 &g, ChartJacobian H1, ChartJacobian H2={}) const |
GTSAM_EXPORT SOn | between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
Rot3 | inverse (ChartJacobian H) const |
Rot3 | expmap (const TangentVector &v) const |
Rot3 | expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
expmap with optional derivatives | |
TangentVector | logmap (const Rot3 &g) const |
TangentVector | logmap (const Rot3 &g, ChartJacobian H1, ChartJacobian H2={}) const |
logmap with optional derivatives | |
Rot3 | retract (const TangentVector &v) const |
retract as required by manifold concept: applies v at *this | |
Rot3 | retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
retract with optional derivatives | |
TangentVector | localCoordinates (const Rot3 &g) const |
localCoordinates as required by manifold concept: finds tangent vector between *this and g | |
TangentVector | localCoordinates (const Rot3 &g, ChartJacobian H1, ChartJacobian H2={}) const |
localCoordinates with optional derivatives | |
Testable | |
void | print (const std::string &s="") const |
bool | equals (const Rot3 &p, double tol=1e-9) const |
Group Action on Point3 | |
Point3 | rotate (const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const |
Point3 | operator* (const Point3 &p) const |
rotate point from rotated coordinate frame to world = R*p | |
Point3 | unrotate (const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const |
rotate point from world to rotated frame \( p^c = (R_c^w)^T p^w \) | |
Group Action on Unit3 | |
Unit3 | rotate (const Unit3 &p, OptionalJacobian< 2, 3 > HR={}, OptionalJacobian< 2, 2 > Hp={}) const |
rotate 3D direction from rotated coordinate frame to world frame | |
Unit3 | unrotate (const Unit3 &p, OptionalJacobian< 2, 3 > HR={}, OptionalJacobian< 2, 2 > Hp={}) const |
unrotate 3D direction from world frame to rotated coordinate frame | |
Unit3 | operator* (const Unit3 &p) const |
rotate 3D direction from rotated coordinate frame to world frame | |
Standard Interface | |
Matrix3 | matrix () const |
Matrix3 | transpose () const |
Point3 | column (int index) const |
Point3 | r1 () const |
first column | |
Point3 | r2 () const |
second column | |
Point3 | r3 () const |
third column | |
Vector3 | xyz (OptionalJacobian< 3, 3 > H={}) const |
Vector3 | ypr (OptionalJacobian< 3, 3 > H={}) const |
Vector3 | rpy (OptionalJacobian< 3, 3 > H={}) const |
double | roll (OptionalJacobian< 1, 3 > H={}) const |
double | pitch (OptionalJacobian< 1, 3 > H={}) const |
double | yaw (OptionalJacobian< 1, 3 > H={}) const |
Static Public Member Functions | |
static Rot3 | Retract (const TangentVector &v) |
Retract at origin: possible in Lie group because it has an identity. | |
static Rot3 | Retract (const TangentVector &v, ChartJacobian H) |
Retract at origin with optional derivative. | |
static TangentVector | LocalCoordinates (const Rot3 &g) |
LocalCoordinates at origin: possible in Lie group because it has an identity. | |
static TangentVector | LocalCoordinates (const Rot3 &g, ChartJacobian H) |
LocalCoordinates at origin with optional derivative. | |
Constructors and named constructors | |
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) | |
Construct from a rotation matrix, as doubles in row-major order !!! | |
template<typename Derived > | |
Rot3 (const Eigen::MatrixBase< Derived > &R) | |
Rot3 (const Matrix3 &R) | |
Rot3 (const SO3 &R) | |
Rot3 (const Quaternion &q) | |
Rot3 (double w, double x, double y, double z) | |
virtual | ~Rot3 () |
Rot3 | normalized () const |
static Rot3 | Random (std::mt19937 &rng) |
static Rot3 | Rx (double t) |
Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. | |
static Rot3 | Ry (double t) |
Rotation around Y axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. | |
static Rot3 | Rz (double t) |
Rotation around Z axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. | |
static Rot3 | RzRyRx (double x, double y, double z, OptionalJacobian< 3, 1 > Hx={}, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hz={}) |
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. | |
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. | |
static Rot3 | Yaw (double t) |
Positive yaw is to right (as in aircraft heading). See ypr. | |
static Rot3 | Pitch (double t) |
Positive pitch is up (increasing aircraft altitude).See ypr. | |
static Rot3 | Roll (double t) |
static Rot3 | Ypr (double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={}) |
static Rot3 | Quaternion (double w, double x, double y, double z) |
static Rot3 | AxisAngle (const Point3 &axis, double angle) |
static Rot3 | AxisAngle (const Unit3 &axis, double angle) |
static Rot3 | Rodrigues (const Vector3 &w) |
static Rot3 | Rodrigues (double wx, double wy, double wz) |
static Rot3 | AlignPair (const Unit3 &axis, const Unit3 &a_p, const Unit3 &b_p) |
Determine a rotation to bring two vectors into alignment, using the rotation axis provided. | |
static Rot3 | AlignTwoPairs (const Unit3 &a_p, const Unit3 &b_p, const Unit3 &a_q, const Unit3 &b_q) |
Calculate rotation from two pairs of homogeneous points using two successive rotations. | |
static Rot3 | ClosestTo (const Matrix3 &M) |
Group | |
Rot3 | operator* (const Rot3 &R2) const |
Syntatic sugar for composing two rotations. | |
Rot3 | inverse () const |
inverse of a rotation | |
Rot3 | conjugate (const Rot3 &cRb) const |
static Rot3 | Identity () |
identity rotation for group operation | |
Manifold | |
enum | CoordinatesMode { EXPMAP, CAYLEY } |
Rot3 | retractCayley (const Vector &omega) const |
Retraction from R^3 to Rot3 manifold using the Cayley transform. | |
Vector3 | localCayley (const Rot3 &other) const |
Inverse of retractCayley. | |
Lie Group | |
Matrix3 | AdjointMap () const |
static Rot3 | Expmap (const Vector3 &v, OptionalJacobian< 3, 3 > H={}) |
static Vector3 | Logmap (const Rot3 &R, OptionalJacobian< 3, 3 > H={}) |
static Matrix3 | ExpmapDerivative (const Vector3 &x) |
Derivative of Expmap. | |
static Matrix3 | LogmapDerivative (const Vector3 &x) |
Derivative of Logmap. | |
Advanced Interface | |
std::pair< Unit3, double > | axisAngle () const |
gtsam::Quaternion | toQuaternion () const |
Rot3 | slerp (double t, const Rot3 &other) const |
Spherical Linear intERPolation between *this and other. More... | |
GTSAM_EXPORT friend std::ostream & | operator<< (std::ostream &os, const Rot3 &p) |
Output stream operator. | |
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it is defined.
The method retract() is used to map from the tangent space back to the manifold. Its inverse, is localCoordinates(). For Lie groups, an obvious retraction is the exponential map, but this can be expensive to compute. The following Enum is used to indicate which method should be used. The default is determined by ROT3_DEFAULT_COORDINATES_MODE, which may be set at compile time, and itself defaults to Rot3::CAYLEY, or if GTSAM_USE_QUATERNIONS is defined, to Rot3::EXPMAP.
Enumerator | |
---|---|
EXPMAP | Use the Lie group exponential map to retract. |
CAYLEY | Retract and localCoordinates using the Cayley transform. |
gtsam::Rot3::Rot3 | ( | ) |
default constructor, unit rotation
Constructor from columns
r1 | X-axis of rotated frame |
r2 | Y-axis of rotated frame |
r3 | Z-axis of rotated frame |
|
inlineexplicit |
Constructor from a rotation matrix Version for generic matrices. Need casting to Matrix3 in quaternion mode, since Eigen's quaternion doesn't allow assignment/construction from a generic matrix. See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
|
inlineexplicit |
Constructor from a rotation matrix Overload version for Matrix3 to avoid casting in quaternion mode.
|
inlineexplicit |
Constructor from an SO3 instance
gtsam::Rot3::Rot3 | ( | const Quaternion & | q | ) |
Constructor from a quaternion. This can also be called using a plain Vector, due to implicit conversion from Vector to Quaternion
q | The quaternion |
|
inlinevirtual |
Virtual destructor
|
inline |
Calculate Adjoint map
Convert from axis/angle representation
axis | is the rotation axis, unit length |
angle | rotation angle |
Convert from axis/angle representation
axis | is the rotation axis |
angle | rotation angle |
std::pair<Unit3, double> gtsam::Rot3::axisAngle | ( | ) | const |
Compute the Euler axis and angle (in radians) representation of this rotation. The angle is in the range [0, π]. If the angle is not in the range, the axis is flipped around accordingly so that the returned angle is within the specified range.
|
inlinestatic |
Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
Uses Full SVD to compute the orthogonal matrix, thus is highly accurate and robust.
N. J. Higham. Matrix nearness problems and applications. In M. J. C. Gover and S. Barnett, editors, Applications of Matrix Theory, pages 1–27. Oxford University Press, 1989.
Point3 gtsam::Rot3::column | ( | int | index | ) | const |
Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C
cRb | rotation from B frame to C frame |
bool gtsam::Rot3::equals | ( | const Rot3 & | p, |
double | tol = 1e-9 |
||
) | const |
equals with an tolerance
|
inlineinherited |
expmap as required by manifold concept Applies exponential map to v and composes with *this
|
inlinestatic |
Exponential map at identity - create a rotation from canonical coordinates \( [R_x,R_y,R_z] \) using Rodrigues' formula
|
inlineinherited |
logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g
|
static |
Log map at identity - returns the canonical coordinates \( [R_x,R_y,R_z] \) of this rotation
Matrix3 gtsam::Rot3::matrix | ( | ) | const |
return 3*3 rotation matrix
Rot3 gtsam::Rot3::normalized | ( | ) | const |
Normalize rotation so that its determinant is 1. This means either re-orthogonalizing the Matrix representation or normalizing the quaternion representation.
This method is akin to ClosestTo
but uses a computationally cheaper algorithm.
Ref: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view
double gtsam::Rot3::pitch | ( | OptionalJacobian< 1, 3 > | H = {} | ) | const |
void gtsam::Rot3::print | ( | const std::string & | s = "" | ) | const |
|
inlinestatic |
Create from Quaternion coefficients
|
static |
Random, generates a random axis, then random angle \(\in\) [-pi,pi] Example: std::mt19937 engine(42); Unit3 unit = Unit3::Random(engine);
|
inlinestatic |
Rodrigues' formula to compute an incremental rotation
w | a vector of incremental roll,pitch,yaw |
|
inlinestatic |
Rodrigues' formula to compute an incremental rotation
wx | Incremental roll (about X) |
wy | Incremental pitch (about Y) |
wz | Incremental yaw (about Z) |
double gtsam::Rot3::roll | ( | OptionalJacobian< 1, 3 > | H = {} | ) | const |
Point3 gtsam::Rot3::rotate | ( | const Point3 & | p, |
OptionalJacobian< 3, 3 > | H1 = {} , |
||
OptionalJacobian< 3, 3 > | H2 = {} |
||
) | const |
rotate point from rotated coordinate frame to world \( p^w = R_c^w p^c \)
Vector3 gtsam::Rot3::rpy | ( | OptionalJacobian< 3, 3 > | H = {} | ) | const |
Use RQ to calculate roll-pitch-yaw angle representation
Spherical Linear intERPolation between *this and other.
t | a value between 0 and 1 |
other | final point of iterpolation geodesic on manifold |
gtsam::Quaternion gtsam::Rot3::toQuaternion | ( | ) | const |
Compute the quaternion representation of this rotation.
Matrix3 gtsam::Rot3::transpose | ( | ) | const |
Return 3*3 transpose (inverse) rotation matrix
Vector3 gtsam::Rot3::xyz | ( | OptionalJacobian< 3, 3 > | H = {} | ) | const |
Use RQ to calculate xyz angle representation
double gtsam::Rot3::yaw | ( | OptionalJacobian< 1, 3 > | H = {} | ) | const |
|
inlinestatic |
Returns rotation nRb from body to nav frame. For vehicle coordinate frame X forward, Y right, Z down: Positive yaw is to right (as in aircraft heading). Positive pitch is up (increasing aircraft altitude). Positive roll is to right (increasing yaw in aircraft). Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw) as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf.
For vehicle coordinate frame X forward, Y left, Z up: Positive yaw is to left (as in aircraft heading). Positive pitch is down (decreasing aircraft altitude). Positive roll is to right (decreasing yaw in aircraft).
Vector3 gtsam::Rot3::ypr | ( | OptionalJacobian< 3, 3 > | H = {} | ) | const |
Use RQ to calculate yaw-pitch-roll angle representation