#include <so3.h>
Classes | |
struct | Invert |
Public Member Functions | |
template<int S, typename A > | |
Vector< 3, Precision > | adjoint (const Vector< S, Precision, A > &vect) const |
void | coerce () |
Modifies the matrix to make sure it is a valid rotation matrix. | |
template<int S, typename VP , typename VA > | |
SO3< Precision > | exp (const Vector< S, VP, VA > &w) |
const Matrix< 3, 3, Precision > & | get_matrix () const |
Returns the SO3 as a Matrix<3> | |
SO3 | inverse () const |
Returns the inverse of this matrix (=the transpose, so this is a fast operation). | |
Vector< 3, Precision > | ln () const |
SO3 | operator* (const SO3 &rhs) const |
Right-multiply by another rotation matrix. | |
SO3 & | operator*= (const SO3 &rhs) |
Right-multiply by another rotation matrix. | |
template<int R, int C, typename P , typename A > | |
SO3 & | operator= (const Matrix< R, C, P, A > &rhs) |
template<int S1, int S2, typename P1 , typename P2 , typename A1 , typename A2 > | |
SO3 (const Vector< S1, P1, A1 > &a, const Vector< S2, P2, A2 > &b) | |
template<int R, int C, typename P , typename A > | |
SO3 (const Matrix< R, C, P, A > &rhs) | |
Construct from a rotation matrix. | |
template<int S, typename P , typename A > | |
SO3 (const Vector< S, P, A > &v) | |
Construct from the axis of rotation (and angle given by the magnitude). | |
SO3 () | |
Default constructor. Initialises the matrix to the identity (no rotation). | |
Static Public Member Functions | |
template<int S, typename VP , typename A > | |
static SO3 | exp (const Vector< S, VP, A > &vect) |
static Matrix< 3, 3, Precision > | generator (int i) |
template<typename Base > | |
static Vector< 3, Precision > | generator_field (int i, const Vector< 3, Precision, Base > &pos) |
Returns the i-th generator times pos. | |
Private Member Functions | |
SO3 (const SO3 &a, const SO3 &b) | |
SO3 (const SO3 &so3, const Invert &) | |
Private Attributes | |
Matrix< 3, 3, Precision > | my_matrix |
Friends | |
std::istream & | operator>> (std::istream &is, SE3< Precision > &rhs) |
std::istream & | operator>> (std::istream &is, SO3< Precision > &rhs) |
class | SE3< Precision > |
Related Functions | |
(Note that these are not member functions.) | |
template<int R, int C, typename P , typename PM , typename A > | |
Matrix< R, 3, typename Internal::MultiplyType< PM, P > ::type > | operator* (const Matrix< R, C, PM, A > &lhs, const SO3< P > &rhs) |
template<int R, int C, typename P , typename PM , typename A > | |
Matrix< 3, C, typename Internal::MultiplyType< P, PM > ::type > | operator* (const SO3< P > &lhs, const Matrix< R, C, PM, A > &rhs) |
template<int S, typename P , typename PV , typename A > | |
Vector< 3, typename Internal::MultiplyType< PV, P > ::type > | operator* (const Vector< S, PV, A > &lhs, const SO3< P > &rhs) |
template<int S, typename P , typename PV , typename A > | |
Vector< 3, typename Internal::MultiplyType< P, PV > ::type > | operator* (const SO3< P > &lhs, const Vector< S, PV, A > &rhs) |
template<typename Precision > | |
std::ostream & | operator<< (std::ostream &os, const SO3< Precision > &rhs) |
template<typename Precision , typename VP , typename VA , typename MA > | |
void | rodrigues_so3_exp (const Vector< 3, VP, VA > &w, const Precision A, const Precision B, Matrix< 3, 3, Precision, MA > &R) |
Class to represent a three-dimensional rotation matrix. Three-dimensional rotation matrices are members of the Special Orthogonal Lie group SO3. This group can be parameterised three numbers (a vector in the space of the Lie Algebra). In this class, the three parameters are the finite rotation vector, i.e. a three-dimensional vector whose direction is the axis of rotation and whose length is the angle of rotation in radians. Exponentiating this vector gives the matrix, and the logarithm of the matrix gives this vector.
Definition at line 53 of file so3.h.
TooN::SO3< Precision >::SO3 | ( | const Vector< S1, P1, A1 > & | a, | |
const Vector< S2, P2, A2 > & | b | |||
) | [inline] |
void TooN::SO3< Precision >::coerce | ( | ) | [inline] |
static SO3 TooN::SO3< Precision >::exp | ( | const Vector< S, VP, A > & | vect | ) | [inline, static] |
Exponentiate a vector in the Lie algebra to generate a new SO3. See the Detailed Description for details of this vector.
Matrix< R, 3, typename Internal::MultiplyType< PM, P >::type > operator* | ( | const Matrix< R, C, PM, A > & | lhs, | |
const SO3< P > & | rhs | |||
) | [related] |
Matrix< 3, C, typename Internal::MultiplyType< P, PM >::type > operator* | ( | const SO3< P > & | lhs, | |
const Matrix< R, C, PM, A > & | rhs | |||
) | [related] |
Vector< 3, typename Internal::MultiplyType< PV, P >::type > operator* | ( | const Vector< S, PV, A > & | lhs, | |
const SO3< P > & | rhs | |||
) | [related] |
Vector< 3, typename Internal::MultiplyType< P, PV >::type > operator* | ( | const SO3< P > & | lhs, | |
const Vector< S, PV, A > & | rhs | |||
) | [related] |
std::ostream & operator<< | ( | std::ostream & | os, | |
const SO3< Precision > & | rhs | |||
) | [related] |
std::istream & operator>> | ( | std::istream & | is, | |
SE3< Precision > & | rhs | |||
) | [friend] |
std::istream & operator>> | ( | std::istream & | is, | |
SO3< Precision > & | rhs | |||
) | [friend] |
void rodrigues_so3_exp | ( | const Vector< 3, VP, VA > & | w, | |
const Precision | A, | |||
const Precision | B, | |||
Matrix< 3, 3, Precision, MA > & | R | |||
) | [related] |
Compute a rotation exponential using the Rodrigues Formula. The rotation axis is given by , and the rotation angle must be computed using . This is provided as a separate function primarily to allow fast and rough matrix exponentials using fast and rough approximations to A and B.
friend class SE3< Precision > [friend] |