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.
true
. For instance, validity checks are not performed when T is symbolic::Expression.T | The 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 |
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... | |
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 ). 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 | |
RotationMatrix & | operator= (const RotationMatrix &)=default |
RotationMatrix (RotationMatrix &&)=default | |
RotationMatrix & | operator= (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... | |
|
default |
|
default |
RotationMatrix | ( | ) |
Constructs a 3x3 identity RotationMatrix – which corresponds to aligning two frames (so that unit vectors Ax = Bx, Ay = By, Az = Bz).
|
explicit |
Constructs a RotationMatrix from a Matrix3.
[in] | R | an allegedly valid rotation matrix. |
std::exception | in debug builds if R fails IsValid(R). |
|
explicit |
Constructs a RotationMatrix from an Eigen::Quaternion.
[in] | quaternion | a non-zero, finite quaternion which may or may not have unit length [i.e., quaternion.norm() does not have to be 1]. |
std::exception | in 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. |
quaternion
argument, without the inefficiency of the square-root associated with normalization.
|
explicit |
Constructs a RotationMatrix from an Eigen::AngleAxis.
[in] | theta_lambda | an 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]. |
std::exception | in 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. |
|
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]
.
[in] | rpy | radian measures of three angles [roll, pitch, yaw]. |
[in] | rpy | a 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] . |
R_AD,rotation | matrix relating frame A to frame D. |
r
, pitch p
, yaw y
, this method returns a rotation matrix R_AD
equal to the matrix multiplication shown below. Di = Ci = Bi = Ai (i = x, y, z)
. Then D is subjected to successive right-handed rotations relative to A. r
about Dx = Cx
. Note: D and C are no longer aligned. p
about Cy = By
. Note: C and B are no longer aligned. y
about Bz = Az
. Note: B and A are no longer aligned.
|
explicit |
(Internal use only) Constructs a RotationMatrix without initializing the underlying 3x3 matrix.
For use by RigidTransform and RotationMatrix only.
RotationMatrix<U> cast | ( | ) | const |
Creates a RotationMatrix templatized on a scalar type U from a RotationMatrix templatized on scalar type T.
For example,
U | Scalar type on which the returned RotationMatrix is templated. |
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. 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),
[in] | index | requested column index (0 <= index <= 2). |
In | Debug builds, asserts (0 <= index <= 2). |
const auto& Bz_A = col(2);
or Vector3<T> Bz_A = col(2);
|
static |
Returns an internal tolerance that checks rotation matrix orthonormality.
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
).
[in] | other | RotationMatrix to subtract from this . |
‖this - other‖∞
|
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).
[in] | R | matrix being checked for orthonormality. |
‖R ⋅ Rᵀ - I‖∞
|
static |
Returns the 3x3 identity RotationMatrix.
RotationMatrix<T> inverse | ( | ) | const |
Returns R_BA = R_AB⁻¹
, the inverse (transpose) of this RotationMatrix.
R_BA = R_AB⁻¹ = R_ABᵀ
. 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.
[in] | other | RotationMatrix that post-multiplies this inverted. |
R_BC | where R_BC = this⁻¹ * other. |
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.
[in] | other | RotationMatrix to compare to this . |
this
is exactly equal to the corresponding element in other
. boolean<T> IsExactlyIdentity | ( | ) | const |
Returns true
if this
is exactly equal to the identity matrix.
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
.
[in] | other | RotationMatrix to compare to this . |
[in] | tolerance | maximum allowable absolute difference between the matrix elements in this and other . |
true
if ‖this - other‖∞ <= tolerance
. boolean<T> IsNearlyIdentity | ( | double | tolerance = get_internal_tolerance_for_orthonormality() | ) | const |
Returns true if this
is within tolerance of the identity RigidTransform.
[in] | tolerance | non-negative number that is generally the default value, namely RotationMatrix::get_internal_tolerance_for_orthonormality(). |
Tests if a generic Matrix3 has orthonormal vectors to within the threshold specified by tolerance
.
[in] | R | an allegedly orthonormal rotation matrix. |
[in] | tolerance | maximum allowable absolute difference between R * Rᵀ and the identity matrix I, i.e., checks if ‖R ⋅ Rᵀ - I‖∞ <= tolerance . |
true
if R is an orthonormal matrix. Tests if a generic Matrix3 seems to be a proper orthonormal rotation matrix to within the threshold specified by tolerance
.
[in] | R | an allegedly valid rotation matrix. |
[in] | tolerance | maximum allowable absolute difference of R * Rᵀ and the identity matrix I (i.e., checks if ‖R ⋅ Rᵀ - I‖∞ <= tolerance ). |
true
if R is a valid rotation matrix. Tests if a generic Matrix3 is a proper orthonormal rotation matrix to within the threshold of get_internal_tolerance_for_orthonormality().
[in] | R | an allegedly valid rotation matrix. |
true
if R is a valid rotation matrix. 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().
true
if this
is a valid rotation matrix.
|
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.
[in] | u_A | unit vector expressed in frame A that represents Bz. |
std::exception | if u_A is not a unit vector. |
R_AB | the rotation matrix with properties as described above. |
|
static |
(Advanced) Creates a right-handed orthonormal basis B from a given unit vector u_A, returned as a rotation matrix R_AB.
[in] | u_A | unit vector which is expressed in frame A and is either Bx or By or Bz (depending on the value of axis_index). |
[in] | axis_index | The 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. |
std::exception | if u_A is not a unit vector, i.e., |u_A| is not within a tolerance of 4ε ≈ 8.88E-16 to 1.0. |
R_AB | the rotation matrix whose properties are described in MakeFromOneVector(). |
|
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.
[in] | b_A | vector expressed in frame A that when normalized as u_A = b_A.normalized() represents Bx, By, or Bz (depending on axis_index). |
[in] | axis_index | The 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. |
std::exception | if b_A cannot be made into a unit vector because b_A contains a NaN or infinity or |b_A| < 1.0E-10. |
R_AB | the rotation matrix with properties as described above. |
|
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]
.
[in] | Bx | first unit vector in right-handed orthogonal set. |
[in] | By | second unit vector in right-handed orthogonal set. |
[in] | Bz | third unit vector in right-handed orthogonal set. |
std::exception | in debug builds if R_AB fails IsValid(R_AB). |
R_AB
is, in fact, a valid RotationMatrix by calling R_AB.IsValid()
. 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
).
|
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]
.
[in] | Ax | first unit vector in right-handed orthogonal set. |
[in] | Ay | second unit vector in right-handed orthogonal set. |
[in] | Az | third unit vector in right-handed orthogonal set. |
std::exception | in debug builds if R_AB fails IsValid(R_AB). |
R_AB
is, in fact, a valid RotationMatrix by calling R_AB.IsValid()
. 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
).
|
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.
|
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
.
[in] | theta | radian measure of rotation angle about Ax. |
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
.
|
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
.
[in] | theta | radian measure of rotation angle about Ay. |
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
.
|
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
.
[in] | theta | radian measure of rotation angle about Az. |
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
. const Matrix3<T>& matrix | ( | ) | const |
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
.
[in] | other | RotationMatrix that post-multiplies this . |
this
multiplied by other
. Calculates this
rotation matrix R_AB
multiplied by an arbitrary Vector3 expressed in the B frame.
[in] | v_B | 3x1 vector that post-multiplies this . |
v_A = R_AB * v_B
. 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.
[in] | v_B | 3 x n matrix whose n columns are regarded as arbitrary vectors v1 , ... vn expressed in frame B. |
v_A | 3 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; |
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
.
[in] | other | RotationMatrix that post-multiplies this . |
this
rotation matrix which has been multiplied by other
.
|
default |
|
default |
|
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.
[in] | M | a 3x3 matrix. |
[out] | quality_factor. | The 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). |
std::exception | if R fails IsValid(R). |
‖(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).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),
[in] | index | requested row index (0 <= index <= 2). |
In | Debug builds, asserts (0 <= index <= 2). |
const auto& Az_B = row(2);
or RowVector3<T> Az_B = row(2);
void set | ( | const Matrix3< T > & | R | ) |
Sets this
RotationMatrix from a Matrix3.
[in] | R | an allegedly valid rotation matrix. |
std::exception | in debug builds if R fails IsValid(R). |
Eigen::AngleAxis<T> ToAngleAxis | ( | ) | const |
Returns an AngleAxis theta_lambda
containing an angle theta
and unit vector (axis direction) lambda
that represents this
RotationMatrix.
theta * lambda
is identical to that of (-theta) * (-lambda)
. The AngleAxis returned by this method chooses to have 0 <= theta <= pi
. 0 <= theta <= pi
and a unit vector lambda
. 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.
|
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.
[in] | M | 3x3 matrix to be made into a quaternion. |
std::exception | in 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. |
Vector4<T> ToQuaternionAsVector4 | ( | ) | const |
Utility method to return the Vector4 associated with ToQuaternion().
Utility method to return the Vector4 associated with ToQuaternion(M).
[in] | M | 3x3 matrix to be made into a quaternion. |
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 <= π
.
RotationMatrix<T> transpose | ( | ) | const |
Returns R_BA = R_AB⁻¹
, the transpose of this RotationMatrix.
R_BA = R_AB⁻¹ = R_ABᵀ
.
|
friend |
Implements the hash_append generic hashing concept.
|
friend |
|
related |
Abbreviation (alias/typedef) for a RotationMatrix double scalar type.