Drake
Drake C++ Documentation
RotationMatrix< T > Class Template Reference

Detailed Description

template<typename T>
class drake::math::RotationMatrix< T >

This class represents a 3x3 rotation matrix between two arbitrary frames A and B and helps ensure users create valid rotation matrices.

This class relates right-handed orthogonal unit vectors Ax, Ay, Az fixed in frame A to right-handed orthogonal unit vectors Bx, By, Bz fixed in frame B. The monogram notation for the rotation matrix relating A to B is R_AB. An example that gives context to this rotation matrix is v_A = R_AB * v_B, where v_B denotes an arbitrary vector v expressed in terms of Bx, By, Bz and v_A denotes vector v expressed in terms of Ax, Ay, Az. See Multibody Quantities for monogram notation for dynamics. See a discussion on rotation matrices.

Note
This class does not store the frames associated with a rotation matrix nor does it enforce strict proper usage of this class with vectors.
When assertions are enabled, several methods in this class perform a validity check and throw std::exception if the rotation matrix is invalid. When assertions are disabled, many of these validity checks are skipped (which helps improve speed). These validity tests are only performed for scalar types for which drake::scalar_predicate<T>::is_bool is true. For instance, validity checks are not performed when T is symbolic::Expression.
Authors
Paul Mitiguy (2018) Original author.
Drake team (see https://drake.mit.edu/credits).
Template Parameters
TThe scalar type, which must be one of the default scalars.

#include <drake/math/rotation_matrix.h>

Public Member Functions

 RotationMatrix ()
 Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz). More...
 
 RotationMatrix (const Matrix3< T > &R)
 Constructs a RotationMatrix from a Matrix3. More...
 
 RotationMatrix (const Eigen::Quaternion< T > &quaternion)
 Constructs a RotationMatrix from an Eigen::Quaternion. More...
 
 RotationMatrix (const Eigen::AngleAxis< T > &theta_lambda)
 Constructs a RotationMatrix from an Eigen::AngleAxis. More...
 
 RotationMatrix (const RollPitchYaw< T > &rpy)
 Constructs a RotationMatrix from an RollPitchYaw. More...
 
template<typename U >
RotationMatrix< U > cast () const requires is_default_scalar< U >
 Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T. More...
 
void set (const Matrix3< T > &R)
 Sets this RotationMatrix from a Matrix3. More...
 
RotationMatrix< T > inverse () const
 Returns R_BA = R_AB⁻¹, the inverse (transpose) of this RotationMatrix. More...
 
RotationMatrix< T > transpose () const
 Returns R_BA = R_AB⁻¹, the transpose of this RotationMatrix. More...
 
const Matrix3< T > & matrix () const
 Returns the Matrix3 underlying a RotationMatrix. More...
 
const Eigen::Block< const Matrix3< T >, 1, 3, false > row (int index) const
 Returns this rotation matrix's iᵗʰ row (i = 0, 1, 2). More...
 
const Eigen::Block< const Matrix3< T >, 3, 1, true > col (int index) const
 Returns this rotation matrix's iᵗʰ column (i = 0, 1, 2). More...
 
RotationMatrix< T > & operator *= (const RotationMatrix< T > &other)
 In-place multiply of this rotation matrix R_AB by other rotation matrix R_BC. More...
 
RotationMatrix< T > operator * (const RotationMatrix< T > &other) const
 Calculates this rotation matrix R_AB multiplied by other rotation matrix R_BC, returning the composition R_AB * R_BC. More...
 
RotationMatrix< T > InvertAndCompose (const RotationMatrix< T > &other) const
 Calculates the product of this inverted and another RotationMatrix. More...
 
Vector3< T > operator * (const Vector3< T > &v_B) const
 Calculates this rotation matrix R_AB multiplied by an arbitrary Vector3 expressed in the B frame. More...
 
template<typename Derived >
Eigen::Matrix< typename Derived::Scalar, 3, Derived::ColsAtCompileTime > operator * (const Eigen::MatrixBase< Derived > &v_B) const
 Multiplies this RotationMatrix R_AB by the n vectors v1, ... More...
 
boolean< T > IsValid () const
 Tests if this rotation matrix R is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality(). More...
 
boolean< T > IsExactlyIdentity () const
 Returns true if this is exactly equal to the identity matrix. More...
 
boolean< T > IsNearlyIdentity (double tolerance=get_internal_tolerance_for_orthonormality()) const
 Returns true if this is within tolerance of the identity RigidTransform. More...
 
boolean< T > IsNearlyEqualTo (const RotationMatrix< T > &other, double tolerance) const
 Compares each element of this to the corresponding element of other to check if they are the same to within a specified tolerance. More...
 
boolean< T > IsExactlyEqualTo (const RotationMatrix< T > &other) const
 Compares each element of this to the corresponding element of other to check if they are exactly the same. More...
 
GetMaximumAbsoluteDifference (const RotationMatrix< T > &other) const
 Computes the infinity norm of this - other (i.e., the maximum absolute value of the difference between the elements of this and other). More...
 
RollPitchYaw< T > ToRollPitchYaw () const
 Returns a RollPitchYaw that represents this RotationMatrix, with roll-pitch-yaw angles [r, p, y] in the range -π <= r <= π, -π/2 <= p <= π/2, -π <= y <= π. More...
 
Eigen::Quaternion< T > ToQuaternion () const
 Returns a quaternion q that represents this RotationMatrix. More...
 
Vector4< T > ToQuaternionAsVector4 () const
 Utility method to return the Vector4 associated with ToQuaternion(). More...
 
Eigen::AngleAxis< T > ToAngleAxis () const
 Returns an AngleAxis theta_lambda containing an angle theta and unit vector (axis direction) lambda that represents this RotationMatrix. More...
 
 RotationMatrix (internal::DoNotInitializeMemberFields)
 (Internal use only) Constructs a RotationMatrix without initializing the underlying 3x3 matrix. More...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 RotationMatrix (const RotationMatrix &)=default
 
RotationMatrixoperator= (const RotationMatrix &)=default
 
 RotationMatrix (RotationMatrix &&)=default
 
RotationMatrixoperator= (RotationMatrix &&)=default
 

Static Public Member Functions

static RotationMatrix< T > MakeUnchecked (const Matrix3< T > &R)
 (Advanced) Makes a RotationMatrix from a Matrix3. More...
 
static RotationMatrix< T > MakeFromOrthonormalColumns (const Vector3< T > &Bx, const Vector3< T > &By, const Vector3< T > &Bz)
 (Advanced) Makes the RotationMatrix R_AB from right-handed orthogonal unit vectors Bx, By, Bz so the columns of R_AB are [Bx, By, Bz]. More...
 
static RotationMatrix< T > MakeFromOrthonormalRows (const Vector3< T > &Ax, const Vector3< T > &Ay, const Vector3< T > &Az)
 (Advanced) Makes the RotationMatrix R_AB from right-handed orthogonal unit vectors Ax, Ay, Az so the rows of R_AB are [Ax, Ay, Az]. More...
 
static RotationMatrix< T > MakeXRotation (const T &theta)
 Makes the RotationMatrix R_AB associated with rotating a frame B relative to a frame A by an angle theta about unit vector Ax = Bx. More...
 
static RotationMatrix< T > MakeYRotation (const T &theta)
 Makes the RotationMatrix R_AB associated with rotating a frame B relative to a frame A by an angle theta about unit vector Ay = By. More...
 
static RotationMatrix< T > MakeZRotation (const T &theta)
 Makes the RotationMatrix R_AB associated with rotating a frame B relative to a frame A by an angle theta about unit vector Az = Bz. More...
 
static RotationMatrix< T > MakeFromOneVector (const Vector3< T > &b_A, int axis_index)
 Creates a 3D right-handed orthonormal basis B from a given vector b_A, returned as a rotation matrix R_AB. More...
 
static RotationMatrix< T > MakeFromOneUnitVector (const Vector3< T > &u_A, int axis_index)
 (Advanced) Creates a right-handed orthonormal basis B from a given unit vector u_A, returned as a rotation matrix R_AB. More...
 
static RotationMatrix< T > MakeClosestRotationToIdentityFromUnitZ (const Vector3< T > &u_A)
 Creates a 3D right-handed orthonormal basis B from a given unit vector u_A, returned as a rotation matrix R_AB. More...
 
static const RotationMatrix< T > & Identity ()
 Returns the 3x3 identity RotationMatrix. More...
 
static T GetMeasureOfOrthonormality (const Matrix3< T > &R)
 Returns how close the matrix R is to being a 3x3 orthonormal matrix by computing ‖R ⋅ Rᵀ - I‖∞ (i.e., the maximum absolute value of the difference between the elements of R ⋅ Rᵀ and the 3x3 identity matrix). More...
 
static boolean< T > IsOrthonormal (const Matrix3< T > &R, double tolerance)
 Tests if a generic Matrix3 has orthonormal vectors to within the threshold specified by tolerance. More...
 
static boolean< T > IsValid (const Matrix3< T > &R, double tolerance)
 Tests if a generic Matrix3 seems to be a proper orthonormal rotation matrix to within the threshold specified by tolerance. More...
 
static boolean< T > IsValid (const Matrix3< T > &R)
 Tests if a generic Matrix3 is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality(). More...
 
static RotationMatrix< T > ProjectToRotationMatrix (const Matrix3< T > &M, T *quality_factor=nullptr)
 Given an approximate rotation matrix M, finds the RotationMatrix R closest to M. More...
 
static constexpr double get_internal_tolerance_for_orthonormality ()
 Returns an internal tolerance that checks rotation matrix orthonormality. More...
 
static Eigen::Quaternion< T > ToQuaternion (const Eigen::Ref< const Matrix3< T >> &M)
 Returns a unit quaternion q associated with the 3x3 matrix M. More...
 
static Vector4< T > ToQuaternionAsVector4 (const Matrix3< T > &M)
 Utility method to return the Vector4 associated with ToQuaternion(M). More...
 

Friends

template<typename U >
class RotationMatrix
 
template<class HashAlgorithm >
void hash_append (HashAlgorithm &hasher, const RotationMatrix &R) noexcept
 Implements the hash_append generic hashing concept. More...
 

Related Functions

(Note that these are not member functions.)

using RotationMatrixd = RotationMatrix< double >
 Abbreviation (alias/typedef) for a RotationMatrix double scalar type. More...
 

(Internal use only) methods for axial rotations

(Internal use only) Axial rotations are RotationMatrix objects for which we know that the represented rotation is about one of the basis vectors (coordinate axes) x, y, or z. These are essentially 2d rotations and only four of the nine elements are "active", by which we mean that they change with the rotation angle θ. The other five elements are "inactive", meaning that they have known values and never change during a simulation. For best performance, the algorithms in this section are permitted to assume without looking that the inactive elements have their required values. However, those elements are still required to have the expected values, which are the values they would have in an identity rotation (that is, 1 on the diagonal and 0 off-diagonal). (Even when T is symbolic::Expression, we insist that the inactive elements have the correct numerical values and don't require an Environment for evaluation.) This ensures that general purpose (non-specialized) code can still use the resulting rotation matrices.

With general rotation matrices (nine active elements), re-expressing a vector takes 15 floating point operations, composing two rotations takes 45, and updating requires writing all nine elements. The methods in this section take advantage of the axial structure to reduce the number of operations required. To do that, they are templatized by the axis number 0, 1, or 2 (+x, +y, or +z axis, resp.). The number of operations required is documented with each method.

Notation: we denote axial rotations by preceding the usual "R" with a lower case "a" for axial, e.g. aR_BC is a rotation matrix R_BC but for which we have foreknowledge of its axial structure. In cases where we know the particular axis we'll replace the "a" with "x", "y", or "z".

Warning
We depend on the caller's promise that axial rotation matrix arguments are indeed axial; for performance reasons we do not verify that in Release builds although we may verify in Debug builds.
Note
There are no Python bindings for these methods since there is nothing to be gained by using them in Python. Use the general equivalent RotationMatrix methods instead.
template<int axis>
void PostMultiplyByAxialRotation (const RotationMatrix< T > &aR_BC, RotationMatrix< T > *R_AC) const
 (Internal use only) With this a general rotation R_AB, and given an axial rotation aR_BC, efficiently forms R_AC = R_AB * aR_BC. More...
 
template<int axis>
void PreMultiplyByAxialRotation (const RotationMatrix< T > &aR_AB, RotationMatrix< T > *R_AC) const
 (Internal use only) With this a general rotation R_BC, and given an axial rotation aR_AB, efficiently forms R_AC = aR_AB * R_BC. More...
 
template<int axis>
void IsAxialRotationOrThrow () const
 (Internal use only) Throws an exception if this RotationMatrix is not an axial rotation about the indicated axis. More...
 
template<int axis>
static RotationMatrix< T > MakeAxialRotation (const T &theta)
 (Internal use only) Creates an axial rotation aR_AB consisting of a rotation of theta radians about x, y, or z. More...
 
template<int axis>
static Vector3< T > ApplyAxialRotation (const RotationMatrix< T > &aR_BC, const Vector3< T > &v_C)
 (Internal use only) Given an axial rotation about a coordinate axis (x, y, or z), uses it to efficiently re-express a vector. More...
 
template<int axis>
static void UpdateAxialRotation (const T &theta, RotationMatrix< T > *aR_BC)
 (Internal use only) Given a new rotation angle θ, updates the axial rotation aR_BC to represent the new rotation angle. More...
 
template<int axis>
static void UpdateAxialRotation (const T &sin_theta, const T &cos_theta, RotationMatrix< T > *aR_BC)
 (Internal use only) Given sin(θ) and cos(θ), where θ is a new rotation angle, updates the axial rotation aR_BC to represent the new rotation angle. More...
 

Constructor & Destructor Documentation

◆ RotationMatrix() [1/8]

RotationMatrix ( const RotationMatrix< T > &  )
default

◆ RotationMatrix() [2/8]

RotationMatrix ( RotationMatrix< T > &&  )
default

◆ RotationMatrix() [3/8]

Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz).

◆ RotationMatrix() [4/8]

RotationMatrix ( const Matrix3< T > &  R)
explicit

Constructs a RotationMatrix from a Matrix3.

Parameters
[in]Ran allegedly valid rotation matrix.
Exceptions
std::exceptionin debug builds if R fails IsValid(R).

◆ RotationMatrix() [5/8]

RotationMatrix ( const Eigen::Quaternion< T > &  quaternion)
explicit

Constructs a RotationMatrix from an Eigen::Quaternion.

Parameters
[in]quaterniona non-zero, finite quaternion which may or may not have unit length [i.e., quaternion.norm() does not have to be 1].
Exceptions
std::exceptionin debug builds if the rotation matrix R that is built from quaternion fails IsValid(R). For example, an exception is thrown if quaternion is zero or contains a NaN or infinity.
Note
This method has the effect of normalizing its quaternion argument, without the inefficiency of the square-root associated with normalization.

◆ RotationMatrix() [6/8]

RotationMatrix ( const Eigen::AngleAxis< T > &  theta_lambda)
explicit

Constructs a RotationMatrix from an Eigen::AngleAxis.

Parameters
[in]theta_lambdaan Eigen::AngleAxis whose associated axis (vector direction herein called lambda) is non-zero and finite, but which may or may not have unit length [i.e., lambda.norm() does not have to be 1].
Exceptions
std::exceptionin debug builds if the rotation matrix R that is built from theta_lambda fails IsValid(R). For example, an exception is thrown if lambda is zero or contains a NaN or infinity.

◆ RotationMatrix() [7/8]

RotationMatrix ( const RollPitchYaw< T > &  rpy)
explicit

Constructs a RotationMatrix from an RollPitchYaw.

In other words, makes the RotationMatrix for a Space-fixed (extrinsic) X-Y-Z rotation by "roll-pitch-yaw" angles [r, p, y], which is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by "yaw-pitch-roll" angles [y, p, r].

Parameters
[in]rpya RollPitchYaw which is a Space-fixed (extrinsic) X-Y-Z rotation with "roll-pitch-yaw" angles [r, p, y] or equivalently a Body- fixed (intrinsic) Z-Y-X rotation with "yaw-pitch-roll" angles [y, p, r].
Return values
R_AD,rotationmatrix relating frame A to frame D.
Note
Denoting roll r, pitch p, yaw y, this method returns a rotation matrix R_AD equal to the matrix multiplication shown below.
⎡cos(y) -sin(y) 0⎤ ⎡ cos(p) 0 sin(p)⎤ ⎡1 0 0 ⎤
R_AD = ⎢sin(y) cos(y) 0⎥ * ⎢ 0 1 0 ⎥ * ⎢0 cos(r) -sin(r)⎥
⎣ 0 0 1⎦ ⎣-sin(p) 0 cos(p)⎦ ⎣0 sin(r) cos(r)⎦
= R_AB * R_BC * R_CD
In this discussion, A is the Space frame and D is the Body frame. One way to visualize this rotation sequence is by introducing intermediate frames B and C (useful constructs to understand this rotation sequence). Initially, the frames are aligned so Di = Ci = Bi = Ai (i = x, y, z). Then D is subjected to successive right-handed rotations relative to A.
  • 1st rotation R_CD: Frame D rotates relative to frames C, B, A by a roll angle r about Dx = Cx. Note: D and C are no longer aligned.
  • 2nd rotation R_BC: Frames D, C (collectively – as if welded together) rotate relative to frame B, A by a pitch angle p about Cy = By. Note: C and B are no longer aligned.
  • 3rd rotation R_AB: Frames D, C, B (collectively – as if welded) rotate relative to frame A by a roll angle y about Bz = Az. Note: B and A are no longer aligned.
This method constructs a RotationMatrix from a RollPitchYaw. Vice-versa, there are high-accuracy RollPitchYaw constructor/methods that form a RollPitchYaw from a rotation matrix.

◆ RotationMatrix() [8/8]

RotationMatrix ( internal::DoNotInitializeMemberFields  )
explicit

(Internal use only) Constructs a RotationMatrix without initializing the underlying 3x3 matrix.

For use by RigidTransform and RotationMatrix only.

Member Function Documentation

◆ ApplyAxialRotation()

static Vector3<T> ApplyAxialRotation ( const RotationMatrix< T > &  aR_BC,
const Vector3< T > &  v_C 
)
static

(Internal use only) Given an axial rotation about a coordinate axis (x, y, or z), uses it to efficiently re-express a vector.

This takes only 6 floating point operations.

Parameters
[in]aR_BCAn axial rotation about the indicated axis.
[in]v_CA vector expressed in frame C to be re-expressed in frame B.
Return values
v_BThe input vector re-expressed in frame B.
Template Parameters
axis0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
Precondition
aR_BC is an axial rotation matrix about the given axis.

◆ cast()

RotationMatrix<U> cast ( ) const

Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T.

For example,

RotationMatrix<double> source = RotationMatrix<double>::Identity();
RotationMatrix<AutoDiffXd> foo = source.cast<AutoDiffXd>();
Template Parameters
UScalar type on which the returned RotationMatrix is templated.
Note
RotationMatrix<From>::cast<To>() creates a new RotationMatrix<To> from a RotationMatrix<From> but only if type To is constructible from type From. This cast method works in accordance with Eigen's cast method for Eigen's Matrix3 that underlies this RotationMatrix. For example, Eigen currently allows cast from type double to AutoDiffXd, but not vice-versa.

◆ col()

const Eigen::Block<const Matrix3<T>, 3, 1, true> col ( int  index) const

Returns this rotation matrix's iᵗʰ column (i = 0, 1, 2).

For this rotation matrix R_AB (which relates right-handed sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),

  • col(0) returns Bx_A (Bx expressed in terms of Ax, Ay, Az).
  • col(1) returns By_A (By expressed in terms of Ax, Ay, Az).
  • col(2) returns Bz_A (Bz expressed in terms of Ax, Ay, Az).
    Parameters
    [in]indexrequested column index (0 <= index <= 2).
    See also
    row(), matrix()
    Exceptions
    InDebug builds, asserts (0 <= index <= 2).
    Note
    For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen's col() operator. The returned quantity can be assigned in various ways, e.g., as const auto& Bz_A = col(2); or Vector3<T> Bz_A = col(2);

◆ get_internal_tolerance_for_orthonormality()

static constexpr double get_internal_tolerance_for_orthonormality ( )
static

Returns an internal tolerance that checks rotation matrix orthonormality.

Returns
internal tolerance (small multiplier of double-precision epsilon) used to check whether or not a rotation matrix is orthonormal.
Note
The tolerance is chosen by developers to ensure a reasonably valid (orthonormal) rotation matrix.
To orthonormalize a 3x3 matrix, use ProjectToRotationMatrix().

◆ GetMaximumAbsoluteDifference()

T GetMaximumAbsoluteDifference ( const RotationMatrix< T > &  other) const

Computes the infinity norm of this - other (i.e., the maximum absolute value of the difference between the elements of this and other).

Parameters
[in]otherRotationMatrix to subtract from this.
Returns
‖this - other‖∞

◆ GetMeasureOfOrthonormality()

static T GetMeasureOfOrthonormality ( const Matrix3< T > &  R)
static

Returns how close the matrix R is to being a 3x3 orthonormal matrix by computing ‖R ⋅ Rᵀ - I‖∞ (i.e., the maximum absolute value of the difference between the elements of R ⋅ Rᵀ and the 3x3 identity matrix).

Parameters
[in]Rmatrix being checked for orthonormality.
Returns
‖R ⋅ Rᵀ - I‖∞

◆ Identity()

static const RotationMatrix<T>& Identity ( )
static

Returns the 3x3 identity RotationMatrix.

◆ inverse()

RotationMatrix<T> inverse ( ) const

Returns R_BA = R_AB⁻¹, the inverse (transpose) of this RotationMatrix.

Note
For a valid rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ.

◆ InvertAndCompose()

RotationMatrix<T> InvertAndCompose ( const RotationMatrix< T > &  other) const

Calculates the product of this inverted and another RotationMatrix.

If you consider this to be the rotation matrix R_AB, and other to be R_AC, then this method returns R_BC = R_AB⁻¹ * R_AC. For T==double, this method can be much faster than inverting first and then performing the composition because it can take advantage of the orthogonality of rotation matrices. On some platforms it can use SIMD instructions for further speedups.

Parameters
[in]otherRotationMatrix that post-multiplies this inverted.
Return values
R_BCwhere R_BC = this⁻¹ * other.
Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.

◆ IsAxialRotationOrThrow()

void IsAxialRotationOrThrow ( ) const

(Internal use only) Throws an exception if this RotationMatrix is not an axial rotation about the indicated axis.

See axial rotation matrix for the conditions that must be satisfied for a rotation matrix to be "axial". In addition, for numerical types T we check here that the active elements are reasonable: there should be two equal cos(θ) entries, ±sin(θ) entries, and sin²+cos²==1. (Numerical tests are done to a fairly loose tolerance of 16ε to avoid false negatives.)

Template Parameters
axis0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
Note
This is intended for Debug-mode checks that the other methods in this section are being used properly.

◆ IsExactlyEqualTo()

boolean<T> IsExactlyEqualTo ( const RotationMatrix< T > &  other) const

Compares each element of this to the corresponding element of other to check if they are exactly the same.

Parameters
[in]otherRotationMatrix to compare to this.
Returns
true if each element of this is exactly equal to the corresponding element in other.
See also
IsNearlyEqualTo().

◆ IsExactlyIdentity()

boolean<T> IsExactlyIdentity ( ) const

Returns true if this is exactly equal to the identity matrix.

See also
IsNearlyIdentity().

◆ IsNearlyEqualTo()

boolean<T> IsNearlyEqualTo ( const RotationMatrix< T > &  other,
double  tolerance 
) const

Compares each element of this to the corresponding element of other to check if they are the same to within a specified tolerance.

Parameters
[in]otherRotationMatrix to compare to this.
[in]tolerancemaximum allowable absolute difference between the matrix elements in this and other.
Returns
true if ‖this - other‖∞ <= tolerance.
See also
IsExactlyEqualTo().

◆ IsNearlyIdentity()

boolean<T> IsNearlyIdentity ( double  tolerance = get_internal_tolerance_for_orthonormality()) const

Returns true if this is within tolerance of the identity RigidTransform.

Parameters
[in]tolerancenon-negative number that is generally the default value, namely RotationMatrix::get_internal_tolerance_for_orthonormality().
See also
IsExactlyIdentity().

◆ IsOrthonormal()

static boolean<T> IsOrthonormal ( const Matrix3< T > &  R,
double  tolerance 
)
static

Tests if a generic Matrix3 has orthonormal vectors to within the threshold specified by tolerance.

Parameters
[in]Ran allegedly orthonormal rotation matrix.
[in]tolerancemaximum allowable absolute difference between R * Rᵀ and the identity matrix I, i.e., checks if ‖R ⋅ Rᵀ - I‖∞ <= tolerance.
Returns
true if R is an orthonormal matrix.

◆ IsValid() [1/3]

static boolean<T> IsValid ( const Matrix3< T > &  R,
double  tolerance 
)
static

Tests if a generic Matrix3 seems to be a proper orthonormal rotation matrix to within the threshold specified by tolerance.

Parameters
[in]Ran allegedly valid rotation matrix.
[in]tolerancemaximum allowable absolute difference of R * Rᵀ and the identity matrix I (i.e., checks if ‖R ⋅ Rᵀ - I‖∞ <= tolerance).
Returns
true if R is a valid rotation matrix.

◆ IsValid() [2/3]

static boolean<T> IsValid ( const Matrix3< T > &  R)
static

Tests if a generic Matrix3 is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().

Parameters
[in]Ran allegedly valid rotation matrix.
Returns
true if R is a valid rotation matrix.

◆ IsValid() [3/3]

boolean<T> IsValid ( ) const

Tests if this rotation matrix R is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().

Returns
true if this is a valid rotation matrix.

◆ MakeAxialRotation()

static RotationMatrix<T> MakeAxialRotation ( const T &  theta)
static

(Internal use only) Creates an axial rotation aR_AB consisting of a rotation of theta radians about x, y, or z.

Of the 9 entries in the rotation matrix, only 4 are active; the rest will be set to 0 or 1. This structure can be exploited for efficient updating and operating with this rotation matrix.

Parameters
[in]thetathe rotation angle.
Return values
aR_BCthe axial rotation (also known as R_BC(theta)).
Template Parameters
axis0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
See also
Axial rotations.

◆ MakeClosestRotationToIdentityFromUnitZ()

static RotationMatrix<T> MakeClosestRotationToIdentityFromUnitZ ( const Vector3< T > &  u_A)
static

Creates a 3D right-handed orthonormal basis B from a given unit vector u_A, returned as a rotation matrix R_AB.

It consists of orthogonal unit vectors [Bx, By, Bz] where Bz is u_A. The angle-axis representation of the resulting rotation is the one with the minimum rotation angle that rotates A to B. When u_A is not parallel or antiparallel to [0, 0, 1], such rotation is unique.

Parameters
[in]u_Aunit vector expressed in frame A that represents Bz.
Exceptions
std::exceptionif u_A is not a unit vector.
Return values
R_ABthe rotation matrix with properties as described above.

◆ MakeFromOneUnitVector()

static RotationMatrix<T> MakeFromOneUnitVector ( const Vector3< T > &  u_A,
int  axis_index 
)
static

(Advanced) Creates a right-handed orthonormal basis B from a given unit vector u_A, returned as a rotation matrix R_AB.

Parameters
[in]u_Aunit vector which is expressed in frame A and is either Bx or By or Bz (depending on the value of axis_index).
[in]axis_indexThe index ∈ {0, 1, 2} of the unit vector associated with u_A, 0 means u_A is Bx, 1 means u_A is By, and 2 means u_A is Bz.
Precondition
axis_index is 0 or 1 or 2.
Exceptions
std::exceptionif u_A is not a unit vector, i.e., |u_A| is not within a tolerance of 4ε ≈ 8.88E-16 to 1.0.
Note
This method is designed for speed and does not normalize u_A to ensure it is a unit vector. Alternatively, consider MakeFromOneVector().
Return values
R_ABthe rotation matrix whose properties are described in MakeFromOneVector().

◆ MakeFromOneVector()

static RotationMatrix<T> MakeFromOneVector ( const Vector3< T > &  b_A,
int  axis_index 
)
static

Creates a 3D right-handed orthonormal basis B from a given vector b_A, returned as a rotation matrix R_AB.

It consists of orthogonal unit vectors u_A, v_A, w_A where u_A is the normalized b_A in the axis_index column of R_AB and v_A has one element which is zero. If an element of b_A is zero, then one element of w_A is 1 and the other two elements are 0.

Parameters
[in]b_Avector expressed in frame A that when normalized as u_A = b_A.normalized() represents Bx, By, or Bz (depending on axis_index).
[in]axis_indexThe index ∈ {0, 1, 2} of the unit vector associated with u_A, 0 means u_A is Bx, 1 means u_A is By, and 2 means u_A is Bz.
Precondition
axis_index is 0 or 1 or 2.
Exceptions
std::exceptionif b_A cannot be made into a unit vector because b_A contains a NaN or infinity or |b_A| < 1.0E-10.
See also
MakeFromOneUnitVector() if b_A is known to already be unit length.
Return values
R_ABthe rotation matrix with properties as described above.

◆ MakeFromOrthonormalColumns()

static RotationMatrix<T> MakeFromOrthonormalColumns ( const Vector3< T > &  Bx,
const Vector3< T > &  By,
const Vector3< T > &  Bz 
)
static

(Advanced) Makes the RotationMatrix R_AB from right-handed orthogonal unit vectors Bx, By, Bz so the columns of R_AB are [Bx, By, Bz].

Parameters
[in]Bxfirst unit vector in right-handed orthogonal set.
[in]Bysecond unit vector in right-handed orthogonal set.
[in]Bzthird unit vector in right-handed orthogonal set.
Exceptions
std::exceptionin debug builds if R_AB fails IsValid(R_AB).
Note
In release builds, the caller can subsequently test if R_AB is, in fact, a valid RotationMatrix by calling R_AB.IsValid().
The rotation matrix R_AB relates two sets of right-handed orthogonal unit vectors, namely Ax, Ay, Az and Bx, By, Bz. The rows of R_AB are Ax, Ay, Az expressed in frame B (i.e.,Ax_B, Ay_B, Az_B). The columns of R_AB are Bx, By, Bz expressed in frame A (i.e., Bx_A, By_A, Bz_A).

◆ MakeFromOrthonormalRows()

static RotationMatrix<T> MakeFromOrthonormalRows ( const Vector3< T > &  Ax,
const Vector3< T > &  Ay,
const Vector3< T > &  Az 
)
static

(Advanced) Makes the RotationMatrix R_AB from right-handed orthogonal unit vectors Ax, Ay, Az so the rows of R_AB are [Ax, Ay, Az].

Parameters
[in]Axfirst unit vector in right-handed orthogonal set.
[in]Aysecond unit vector in right-handed orthogonal set.
[in]Azthird unit vector in right-handed orthogonal set.
Exceptions
std::exceptionin debug builds if R_AB fails IsValid(R_AB).
Note
In release builds, the caller can subsequently test if R_AB is, in fact, a valid RotationMatrix by calling R_AB.IsValid().
The rotation matrix R_AB relates two sets of right-handed orthogonal unit vectors, namely Ax, Ay, Az and Bx, By, Bz. The rows of R_AB are Ax, Ay, Az expressed in frame B (i.e.,Ax_B, Ay_B, Az_B). The columns of R_AB are Bx, By, Bz expressed in frame A (i.e., Bx_A, By_A, Bz_A).

◆ MakeUnchecked()

static RotationMatrix<T> MakeUnchecked ( const Matrix3< T > &  R)
static

(Advanced) Makes a RotationMatrix from a Matrix3.

No check is performed to test whether or not the parameter R is a valid rotation matrix.

◆ MakeXRotation()

static RotationMatrix<T> MakeXRotation ( const T &  theta)
static

Makes the RotationMatrix R_AB associated with rotating a frame B relative to a frame A by an angle theta about unit vector Ax = Bx.

Parameters
[in]thetaradian measure of rotation angle about Ax.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitX().
R_AB relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially, Bx = Ax, By = Ay, Bz = Az, then B undergoes a right-handed rotation relative to A by an angle theta about Ax = Bx.
⎡ 1 0 0 ⎤
R_AB = ⎢ 0 cos(theta) -sin(theta) ⎥
⎣ 0 sin(theta) cos(theta) ⎦

◆ MakeYRotation()

static RotationMatrix<T> MakeYRotation ( const T &  theta)
static

Makes the RotationMatrix R_AB associated with rotating a frame B relative to a frame A by an angle theta about unit vector Ay = By.

Parameters
[in]thetaradian measure of rotation angle about Ay.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitY().
R_AB relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially, Bx = Ax, By = Ay, Bz = Az, then B undergoes a right-handed rotation relative to A by an angle theta about Ay = By.
cos(theta) 0 sin(theta) ⎤
R_AB = ⎢ 0 1 0 ⎥
⎣ -sin(theta) 0 cos(theta) ⎦

◆ MakeZRotation()

static RotationMatrix<T> MakeZRotation ( const T &  theta)
static

Makes the RotationMatrix R_AB associated with rotating a frame B relative to a frame A by an angle theta about unit vector Az = Bz.

Parameters
[in]thetaradian measure of rotation angle about Az.
Note
Orientation is same as Eigen::AngleAxis<T>(theta, Vector3d::UnitZ().
R_AB relates two frames A and B having unit vectors Ax, Ay, Az and Bx, By, Bz. Initially, Bx = Ax, By = Ay, Bz = Az, then B undergoes a right-handed rotation relative to A by an angle theta about Az = Bz.
cos(theta) -sin(theta) 0 ⎤
R_AB = ⎢ sin(theta) cos(theta) 0 ⎥
⎣ 0 0 1 ⎦

◆ matrix()

const Matrix3<T>& matrix ( ) const

Returns the Matrix3 underlying a RotationMatrix.

See also
col(), row()

◆ operator *() [1/3]

RotationMatrix<T> operator * ( const RotationMatrix< T > &  other) const

Calculates this rotation matrix R_AB multiplied by other rotation matrix R_BC, returning the composition R_AB * R_BC.

Parameters
[in]otherRotationMatrix that post-multiplies this.
Returns
rotation matrix that results from this multiplied by other.
Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.

◆ operator *() [2/3]

Vector3<T> operator * ( const Vector3< T > &  v_B) const

Calculates this rotation matrix R_AB multiplied by an arbitrary Vector3 expressed in the B frame.

Parameters
[in]v_B3x1 vector that post-multiplies this.
Returns
3x1 vector v_A = R_AB * v_B.

◆ operator *() [3/3]

Eigen::Matrix<typename Derived::Scalar, 3, Derived::ColsAtCompileTime> operator * ( const Eigen::MatrixBase< Derived > &  v_B) const

Multiplies this RotationMatrix R_AB by the n vectors v1, ...

vn, where each vector has 3 elements and is expressed in frame B.

Parameters
[in]v_B3 x n matrix whose n columns are regarded as arbitrary vectors v1, ... vn expressed in frame B.
Return values
v_A3 x n matrix whose n columns are vectors v1, ... vn expressed in frame A.
const RollPitchYaw<double> rpy(0.1, 0.2, 0.3);
const RotationMatrix<double> R_AB(rpy);
Eigen::Matrix<double, 3, 2> v_B;
v_B.col(0) = Vector3d(4, 5, 6);
v_B.col(1) = Vector3d(9, 8, 7);
const Eigen::Matrix<double, 3, 2> v_A = R_AB * v_B;

◆ operator *=()

RotationMatrix<T>& operator *= ( const RotationMatrix< T > &  other)

In-place multiply of this rotation matrix R_AB by other rotation matrix R_BC.

On return, this is set to equal R_AB * R_BC.

Parameters
[in]otherRotationMatrix that post-multiplies this.
Returns
this rotation matrix which has been multiplied by other.
Note
It is possible (albeit improbable) to create an invalid rotation matrix by accumulating round-off error with a large number of multiplies.

◆ operator=() [1/2]

RotationMatrix& operator= ( RotationMatrix< T > &&  )
default

◆ operator=() [2/2]

RotationMatrix& operator= ( const RotationMatrix< T > &  )
default

◆ PostMultiplyByAxialRotation()

void PostMultiplyByAxialRotation ( const RotationMatrix< T > &  aR_BC,
RotationMatrix< T > *  R_AC 
) const

(Internal use only) With this a general rotation R_AB, and given an axial rotation aR_BC, efficiently forms R_AC = R_AB * aR_BC.

This requires only 18 floating point operations.

Parameters
[in]aR_BCAn axial rotation about the indicated axis.
[out]R_ACThe result, which will be a general rotation matrix. Must not overlap with this in memory.
Template Parameters
axis0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
Precondition
aR_BC is an axial rotation matrix about the given axis.
R_AC does not overlap with this in memory.

◆ PreMultiplyByAxialRotation()

void PreMultiplyByAxialRotation ( const RotationMatrix< T > &  aR_AB,
RotationMatrix< T > *  R_AC 
) const

(Internal use only) With this a general rotation R_BC, and given an axial rotation aR_AB, efficiently forms R_AC = aR_AB * R_BC.

This requires only 18 floating point operations.

Parameters
[in]aR_ABAn axial rotation about the indicated axis.
[out]R_ACThe result, which will be a general rotation matrix. Must not overlap with this in memory.
Template Parameters
axis0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
Precondition
aR_AB is an axial rotation matrix about the given axis.
R_AC does not overlap with this in memory.

◆ ProjectToRotationMatrix()

static RotationMatrix<T> ProjectToRotationMatrix ( const Matrix3< T > &  M,
T *  quality_factor = nullptr 
)
static

Given an approximate rotation matrix M, finds the RotationMatrix R closest to M.

Closeness is measured with a matrix-2 norm (or equivalently with a Frobenius norm). Hence, this method creates a RotationMatrix R from a 3x3 matrix M by minimizing ‖R - M‖₂ (the matrix-2 norm of (R-M)) subject to R * Rᵀ = I, where I is the 3x3 identity matrix. For this problem, closeness can also be measured by forming the orthonormal matrix R whose elements minimize the double-summation ∑ᵢ ∑ⱼ (R(i,j) - M(i,j))² where i = 1:3, j = 1:3, subject to R * Rᵀ = I. The square-root of this double-summation is called the Frobenius norm.

Parameters
[in]Ma 3x3 matrix.
[out]quality_factorThe quality of M as a rotation matrix. quality_factor = 1 is perfect (M = R). quality_factor = 1.25 means that when M multiplies a unit vector (magnitude 1), a vector of magnitude as large as 1.25 may result. quality_factor = 0.8 means that when M multiplies a unit vector, a vector of magnitude as small as 0.8 may result. quality_factor = 0 means M is singular, so at least one of the bases related by matrix M does not span 3D space (when M multiples a unit vector, a vector of magnitude as small as 0 may result).
Returns
proper orthonormal matrix R that is closest to M.
Exceptions
std::exceptionif R fails IsValid(R).
Note
William Kahan (UC Berkeley) and Hongkai Dai (Toyota Research Institute) proved that for this problem, the same R that minimizes the Frobenius norm also minimizes the matrix-2 norm (a.k.a an induced-2 norm), which is defined [Dahleh, Section 4.2] as the column matrix u which maximizes ‖(R - M) u‖ / ‖u‖, where u ≠ 0. Since the matrix-2 norm of any matrix A is equal to the maximum singular value of A, minimizing the matrix-2 norm of (R - M) is equivalent to minimizing the maximum singular value of (R - M).

◆ row()

const Eigen::Block<const Matrix3<T>, 1, 3, false> row ( int  index) const

Returns this rotation matrix's iᵗʰ row (i = 0, 1, 2).

For this rotation matrix R_AB (which relates right-handed sets of orthogonal unit vectors Ax, Ay, Az to Bx, By, Bz),

  • row(0) returns Ax_B (Ax expressed in terms of Bx, By, Bz).
  • row(1) returns Ay_B (Ay expressed in terms of Bx, By, Bz).
  • row(2) returns Az_B (Az expressed in terms of Bx, By, Bz).
    Parameters
    [in]indexrequested row index (0 <= index <= 2).
    See also
    col(), matrix()
    Exceptions
    InDebug builds, asserts (0 <= index <= 2).
    Note
    For efficiency and consistency with Eigen, this method returns the same quantity returned by Eigen's row() operator. The returned quantity can be assigned in various ways, e.g., as const auto& Az_B = row(2); or RowVector3<T> Az_B = row(2);

◆ set()

void set ( const Matrix3< T > &  R)

Sets this RotationMatrix from a Matrix3.

Parameters
[in]Ran allegedly valid rotation matrix.
Exceptions
std::exceptionin debug builds if R fails IsValid(R).

◆ ToAngleAxis()

Eigen::AngleAxis<T> ToAngleAxis ( ) const

Returns an AngleAxis theta_lambda containing an angle theta and unit vector (axis direction) lambda that represents this RotationMatrix.

Note
The orientation and RotationMatrix associated with theta * lambda is identical to that of (-theta) * (-lambda). The AngleAxis returned by this method chooses to have 0 <= theta <= pi.
Returns
an AngleAxis with 0 <= theta <= pi and a unit vector lambda.

◆ ToQuaternion() [1/2]

Eigen::Quaternion<T> ToQuaternion ( ) const

Returns a quaternion q that represents this RotationMatrix.

Since the quaternion q and -q represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.

◆ ToQuaternion() [2/2]

static Eigen::Quaternion<T> ToQuaternion ( const Eigen::Ref< const Matrix3< T >> &  M)
static

Returns a unit quaternion q associated with the 3x3 matrix M.

Since the quaternion q and -q represent the same RotationMatrix, this method chooses to return a canonical quaternion, i.e., with q(0) >= 0.

Parameters
[in]M3x3 matrix to be made into a quaternion.
Returns
a unit quaternion q in canonical form, i.e., with q(0) >= 0.
Exceptions
std::exceptionin debug builds if the quaternion q returned by this method cannot construct a valid RotationMatrix. For example, if M contains NaNs, q will not be a valid quaternion.

◆ ToQuaternionAsVector4() [1/2]

Vector4<T> ToQuaternionAsVector4 ( ) const

Utility method to return the Vector4 associated with ToQuaternion().

See also
ToQuaternion().

◆ ToQuaternionAsVector4() [2/2]

static Vector4<T> ToQuaternionAsVector4 ( const Matrix3< T > &  M)
static

Utility method to return the Vector4 associated with ToQuaternion(M).

Parameters
[in]M3x3 matrix to be made into a quaternion.
See also
ToQuaternion().

◆ ToRollPitchYaw()

RollPitchYaw<T> ToRollPitchYaw ( ) const

Returns a RollPitchYaw that represents this RotationMatrix, with roll-pitch-yaw angles [r, p, y] in the range -π <= r <= π, -π/2 <= p <= π/2, -π <= y <= π.

Note
This new high-accuracy algorithm avoids numerical round-off issues encountered by some algorithms when pitch is within 1E-6 of π/2 or -π/2.

◆ transpose()

RotationMatrix<T> transpose ( ) const

Returns R_BA = R_AB⁻¹, the transpose of this RotationMatrix.

Note
For a valid rotation matrix R_BA = R_AB⁻¹ = R_ABᵀ.

◆ UpdateAxialRotation() [1/2]

static void UpdateAxialRotation ( const T &  theta,
RotationMatrix< T > *  aR_BC 
)
static

(Internal use only) Given a new rotation angle θ, updates the axial rotation aR_BC to represent the new rotation angle.

We expect that aR_BC was already such an axial rotation about the given x, y, or z axis. Only the 4 active elements are modified; the other 5 remain unchanged. (However, execution time is likely to be dominated by the time to calculate sine and cosine.)

Parameters
[in]thetathe new rotation angle in radians.
[in,out]aR_BCthe axial rotation matrix to be updated.
Template Parameters
axis0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
See also
the overloaded signature if you already have sin(θ) & cos(θ).
Precondition
aR_BC is an axial rotation matrix about the given axis.

◆ UpdateAxialRotation() [2/2]

static void UpdateAxialRotation ( const T &  sin_theta,
const T &  cos_theta,
RotationMatrix< T > *  aR_BC 
)
static

(Internal use only) Given sin(θ) and cos(θ), where θ is a new rotation angle, updates the axial rotation aR_BC to represent the new rotation angle.

We expect that aR_BC was already such a rotation (about the given x, y, or z axis). Only the 4 active elements are modified; the other 5 remain unchanged.

Parameters
[in]sin_thetasin(θ), where θ is the new rotation angle.
[in]cos_thetacos(θ), where θ is the new rotation angle.
[in,out]aR_BCthe axial rotation matrix to be updated.
Template Parameters
axis0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
See also
the overloaded signature if you just have the angle θ.
Precondition
aR_BC is an axial rotation matrix about the given axis.
sin_theta and cos_theta are sine and cosine of the same angle.

Friends And Related Function Documentation

◆ hash_append

void hash_append ( HashAlgorithm &  hasher,
const RotationMatrix< T > &  R 
)
friend

Implements the hash_append generic hashing concept.

Precondition
T implements the hash_append concept.

◆ RotationMatrix

friend class RotationMatrix
friend

◆ RotationMatrixd

Abbreviation (alias/typedef) for a RotationMatrix double scalar type.


The documentation for this class was generated from the following files: