|
|
void | print (const std::string &s="") const |
|
bool | equals (const Rot3 &p, double tol=1e-9) const |
|
|
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 More...
|
|
Point3 | unrotate (const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const |
| rotate point from world to rotated frame More...
|
|
|
Unit3 | rotate (const Unit3 &p, OptionalJacobian< 2, 3 > HR={}, OptionalJacobian< 2, 2 > Hp={}) const |
| rotate 3D direction from rotated coordinate frame to world frame More...
|
|
Unit3 | unrotate (const Unit3 &p, OptionalJacobian< 2, 3 > HR={}, OptionalJacobian< 2, 2 > Hp={}) const |
| unrotate 3D direction from world frame to rotated coordinate frame More...
|
|
Unit3 | operator* (const Unit3 &p) const |
| rotate 3D direction from rotated coordinate frame to world frame More...
|
|
|
Matrix3 | matrix () const |
|
Matrix3 | transpose () const |
|
Point3 | column (int index) const |
|
Point3 | r1 () const |
| first column More...
|
|
Point3 | r2 () const |
| second column More...
|
|
Point3 | r3 () const |
| third column More...
|
|
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 |
|
Rot3 | between (const Rot3 &g) const |
|
Rot3 | between (const Rot3 &g, ChartJacobian H1, ChartJacobian H2={}) const |
|
SOn | between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
|
GTSAM_EXPORT SOn | between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
|
Rot3 | compose (const Rot3 &g) const |
|
Rot3 | compose (const Rot3 &g, ChartJacobian H1, ChartJacobian H2={}) const |
|
SOn | compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
|
GTSAM_EXPORT SOn | compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
|
const Rot3 & | derived () const |
|
Rot3 | expmap (const TangentVector &v) const |
|
Rot3 | expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
| expmap with optional derivatives More...
|
|
Rot3 | inverse (ChartJacobian H) const |
|
TangentVector | localCoordinates (const Rot3 &g) const |
| localCoordinates as required by manifold concept: finds tangent vector between *this and g More...
|
|
TangentVector | localCoordinates (const Rot3 &g, ChartJacobian H1, ChartJacobian H2={}) const |
| localCoordinates with optional derivatives More...
|
|
TangentVector | logmap (const Rot3 &g) const |
|
TangentVector | logmap (const Rot3 &g, ChartJacobian H1, ChartJacobian H2={}) const |
| logmap with optional derivatives More...
|
|
Rot3 | retract (const TangentVector &v) const |
| retract as required by manifold concept: applies v at *this More...
|
|
Rot3 | retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
| retract with optional derivatives More...
|
|
|
| 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 !!! More...
|
|
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. More...
|
|
static Rot3 | Ry (double t) |
| Rotation around Y axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. More...
|
|
static Rot3 | Rz (double t) |
| Rotation around Z axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. More...
|
|
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. More...
|
|
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. More...
|
|
static Rot3 | Yaw (double t) |
| Positive yaw is to right (as in aircraft heading). See ypr. More...
|
|
static Rot3 | Pitch (double t) |
| Positive pitch is up (increasing aircraft altitude).See ypr. More...
|
|
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. More...
|
|
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. More...
|
|
static Rot3 | ClosestTo (const Matrix3 &M) |
|
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.
Definition at line 58 of file Rot3.h.
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.
|
Definition at line 339 of file Rot3.h.
static Rot3 gtsam::Rot3::ClosestTo |
( |
const Matrix3 & |
M | ) |
|
|
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.
Definition at line 271 of file Rot3.h.
Rot3 gtsam::Rot3::normalized |
( |
| ) |
const |
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).
Definition at line 196 of file Rot3.h.