|
Rosetta 3.3
|
|
Rosetta 3.3
|
#include <HomogeneousTransform.hh>
Public Types | |
| typedef T | value_type |
Public Member Functions | |
| HomogeneousTransform () | |
| Default constructor: axis aligned with the global coordinate frame and the point located at the origin. | |
| HomogeneousTransform (xyzVector< T > const &p1, xyzVector< T > const &p2, xyzVector< T > const &p3) | |
| Construct the coordinate frame from the points defined such that 1. the point is at p3, 2. the z axis points along p2 to p3. 3. the y axis is in the p1-p2-p3 plane 4. the x axis is the cross product of y and z. | |
| HomogeneousTransform (xyzVector< T > const &xaxis, xyzVector< T > const &yaxis, xyzVector< T > const &zaxis, xyzVector< T > const &point) | |
| Constructor from xyzVectors: trust that the axis are indeed orthoganol. | |
| HomogeneousTransform (xyzMatrix< T > const &axes, xyzVector< T > const &point) | |
| Constructor from an xyzMatrix and xyzVectors: trust that the axis are indeed orthoganol. | |
| ~HomogeneousTransform () | |
| HomogeneousTransform< T > const & | operator= (HomogeneousTransform< T > const &rhs) |
| value_type | xx () const |
| value_type | xy () const |
| value_type | xz () const |
| value_type | yx () const |
| value_type | yy () const |
| value_type | yz () const |
| value_type | zx () const |
| value_type | zy () const |
| value_type | zz () const |
| value_type | px () const |
| value_type | py () const |
| value_type | pz () const |
| xyzVector< T > | xaxis () const |
| xyzVector< T > | yaxis () const |
| xyzVector< T > | zaxis () const |
| xyzVector< T > | point () const |
| void | set_identity_rotation () |
| Mutators. | |
| void | set_identity_transform () |
| void | set_identity () |
| void | set_xaxis_rotation_deg (T angle) |
| void | set_yaxis_rotation_deg (T angle) |
| void | set_zaxis_rotation_deg (T angle) |
| void | set_xaxis_rotation_rad (T angle) |
| void | set_yaxis_rotation_rad (T angle) |
| void | set_zaxis_rotation_rad (T angle) |
| void | set_transform (xyzVector< T > const &t) |
| Set this HT to describe a transformation along the three axes. This has the size effect of setting the axes to the global axes. | |
| void | set_point (xyzVector< T > const &p) |
| Set the point that this HT is centered at. This leaves the axes untouched. | |
| void | walk_along_x (T delta) |
| Right multiply this coordinate frame by the frame 1 0 0 delta 0 1 0 0 0 0 1 0. | |
| void | walk_along_y (T delta) |
| Right multiply this coordinate frame by the frame 1 0 0 0 0 1 0 delta 0 0 1 0. | |
| void | walk_along_z (T delta) |
| Right multiply this coordinate frame by the frame 1 0 0 0 0 1 0 0 0 0 1 delta. | |
| HomogeneousTransform< T > | operator* (HomogeneousTransform< T > const &rmat) const |
| Multiplication. | |
| xyzVector< T > | operator* (xyzVector< T > const &vect) const |
| Transform a point. The input point is a location in this coordinate frame the output point is the location in global coordinate frame. | |
| HomogeneousTransform< T > | inverse () const |
| Invert this matrix. | |
| xyzVector< T > | to_local_coordinate (xyzVector< T > const &v) |
| Convert a point in the global coordinate system to a point in this coordinate frame. If this frame is F and the point is p, then this solves for x st: F x = p. Equivalent to computing (F.inverse() * p).point(). | |
| xyzVector< T > | euler_angles_rad () const |
| Return the three euler angles (in radians) that describe this HomogeneousTransform as the series of a Z axis rotation by the angle phi (returned in position 1 of the output vector), followed by an X axis rotation by the angle theta (returned in position 3 of the output vector), followed by another Z axis rotation by the angle psi (returned in position 2 of the output vector). This code is a modified version of Alex Z's code from r++. | |
| xyzVector< T > | euler_angles_deg () const |
| void | from_euler_angles_rad (xyzVector< T > const &euler) |
| Construct the coordinate frame from three euler angles that describe the frame. Keep the point fixed. See the description for euler_angles_rad() to understand the Z-X-Z transformation convention. | |
| void | from_euler_angles_deg (xyzVector< T > const &euler) |
| typedef T numeric::HomogeneousTransform< T >::value_type |
| numeric::HomogeneousTransform< T >::HomogeneousTransform | ( | ) | [inline] |
Default constructor: axis aligned with the global coordinate frame and the point located at the origin.
| numeric::HomogeneousTransform< T >::HomogeneousTransform | ( | xyzVector< T > const & | p1, |
| xyzVector< T > const & | p2, | ||
| xyzVector< T > const & | p3 | ||
| ) | [inline] |
Construct the coordinate frame from the points defined such that 1. the point is at p3, 2. the z axis points along p2 to p3. 3. the y axis is in the p1-p2-p3 plane 4. the x axis is the cross product of y and z.
| numeric::HomogeneousTransform< T >::HomogeneousTransform | ( | xyzVector< T > const & | xaxis, |
| xyzVector< T > const & | yaxis, | ||
| xyzVector< T > const & | zaxis, | ||
| xyzVector< T > const & | point | ||
| ) | [inline] |
Constructor from xyzVectors: trust that the axis are indeed orthoganol.
| numeric::HomogeneousTransform< T >::HomogeneousTransform | ( | xyzMatrix< T > const & | axes, |
| xyzVector< T > const & | point | ||
| ) | [inline] |
| numeric::HomogeneousTransform< T >::~HomogeneousTransform | ( | ) | [inline] |
| xyzVector< T > numeric::HomogeneousTransform< T >::euler_angles_deg | ( | ) | const [inline] |
| xyzVector< T > numeric::HomogeneousTransform< T >::euler_angles_rad | ( | ) | const [inline] |
Return the three euler angles (in radians) that describe this HomogeneousTransform as the series of a Z axis rotation by the angle phi (returned in position 1 of the output vector), followed by an X axis rotation by the angle theta (returned in position 3 of the output vector), followed by another Z axis rotation by the angle psi (returned in position 2 of the output vector). This code is a modified version of Alex Z's code from r++.
The range of phi is [ -pi, pi ]; The range of psi is [ -pi, pi ]; The range of theta is [ 0, pi ];
The function pretends that this HomogeneousTransform is the result of these three transformations; if it were, then the rotation matrix would be
FIGURE 1: R = [ cos(psi)cos(phi)-cos(theta)sin(phi)sin(psi) cos(psi)sin(phi)+cos(theta)cos(phi)sin(psi) sin(psi)sin(theta) -sin(psi)cos(phi)-cos(theta)sin(phi)cos(psi) -sin(psi)sin(phi)+cos(theta)cos(phi)cos(psi) cos(psi)sin(theta) sin(theta)sin(phi) -sin(theta)cos(phi) cos(theta) ]
where each axis above is represented as a ROW VECTOR (to be distinguished from the HomogeneousTransform's representation of axes as COLUMN VECTORS).
The zz_ coordinate gives away theta. Theta may be computed as acos( zz_ ), or, as Alex does it, asin( sqrt( 1 - zz^2)) Since there is redundancy in theta, this function chooses a theta with a positive sin(theta): i.e. quadrants I and II. Assuming we have a positive sin theta pushes phi and psi into conforming angles.
NOTE on theta: asin returns a value in the range [ -pi/2, pi/2 ], and we have artificially created a positive sin(theta), so we will get a asin( pos_sin_theta ), we have a value in the range [ 0, pi/2 ]. To convert this into the actual angle theta, we examine the zz sign. If zz is negative, we chose the quadrant II theta. That is, asin( pos_sin_theta) returned an angle, call it theta'. Now, if cos( theta ) is negative, then we want to choose the positive x-axis rotation that's equivalent to -theta'. To do so, we reflect q through the y axis (See figure 2 below) to get p and then measure theta as pi - theta'.
FIGURE 2:
II | I | p. | .q (cos(-theta'), abs(sin(theta'))) . | . theta'( . | . ) theta' = asin( abs(sin(theta)) ----------------------- | | | III | IV | The angle between the positive x axis and p is pi - theta'.
Since zx and zy contain only phi terms and a constant sin( theta ) term, phi is given by atan2( sin_phi, cos_phi ) = atan2( c*sin_phi, c*cos_phi ) = atan2( zx, -zy ) for c positive and non-zero. If sin_theta is zero, or very close to zero, we're at gimbal lock.
Moreover, since xz and yz contain only psi terms, psi may also be deduced using atan2.
There are 2 degenerate cases (gimbal lock) 1. theta close to 0 (North Pole singularity), or 2. theta close to pi (South Pole singularity) For these, we take: phi=acos(xx), theta = 0 (resp. Pi/2), psi = 0
Referenced by numeric::HomogeneousTransform< double >::euler_angles_deg().
| void numeric::HomogeneousTransform< T >::from_euler_angles_deg | ( | xyzVector< T > const & | euler | ) | [inline] |
| void numeric::HomogeneousTransform< T >::from_euler_angles_rad | ( | xyzVector< T > const & | euler | ) | [inline] |
Construct the coordinate frame from three euler angles that describe the frame. Keep the point fixed. See the description for euler_angles_rad() to understand the Z-X-Z transformation convention.
Referenced by numeric::HomogeneousTransform< double >::from_euler_angles_deg().
| HomogeneousTransform< T > numeric::HomogeneousTransform< T >::inverse | ( | ) | const [inline] |
Invert this matrix.
| xyzVector< T > numeric::HomogeneousTransform< T >::operator* | ( | xyzVector< T > const & | vect | ) | const [inline] |
Transform a point. The input point is a location in this coordinate frame the output point is the location in global coordinate frame.
| HomogeneousTransform< T > numeric::HomogeneousTransform< T >::operator* | ( | HomogeneousTransform< T > const & | rmat | ) | const [inline] |
Multiplication.
the main operation of a homogeneous transform: matrix multiplication. Note: matrix multiplication is transitive, so rotation/translation matrices may be pre-multiplied before being applied to a set of points.
| HomogeneousTransform< T > const& numeric::HomogeneousTransform< T >::operator= | ( | HomogeneousTransform< T > const & | rhs | ) | [inline] |
| xyzVector< T > numeric::HomogeneousTransform< T >::point | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::px | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::py | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::pz | ( | ) | const [inline] |
| void numeric::HomogeneousTransform< T >::set_identity | ( | ) | [inline] |
| void numeric::HomogeneousTransform< T >::set_identity_rotation | ( | ) | [inline] |
Mutators.
Referenced by numeric::HomogeneousTransform< double >::set_identity(), and numeric::HomogeneousTransform< double >::set_transform().
| void numeric::HomogeneousTransform< T >::set_identity_transform | ( | ) | [inline] |
Referenced by numeric::HomogeneousTransform< double >::set_identity().
| void numeric::HomogeneousTransform< T >::set_point | ( | xyzVector< T > const & | p | ) | [inline] |
Set the point that this HT is centered at. This leaves the axes untouched.
| void numeric::HomogeneousTransform< T >::set_transform | ( | xyzVector< T > const & | t | ) | [inline] |
Set this HT to describe a transformation along the three axes. This has the size effect of setting the axes to the global axes.
| void numeric::HomogeneousTransform< T >::set_xaxis_rotation_deg | ( | T | angle | ) | [inline] |
| void numeric::HomogeneousTransform< T >::set_xaxis_rotation_rad | ( | T | angle | ) | [inline] |
| void numeric::HomogeneousTransform< T >::set_yaxis_rotation_deg | ( | T | angle | ) | [inline] |
| void numeric::HomogeneousTransform< T >::set_yaxis_rotation_rad | ( | T | angle | ) | [inline] |
| void numeric::HomogeneousTransform< T >::set_zaxis_rotation_deg | ( | T | angle | ) | [inline] |
| void numeric::HomogeneousTransform< T >::set_zaxis_rotation_rad | ( | T | angle | ) | [inline] |
| xyzVector< T > numeric::HomogeneousTransform< T >::to_local_coordinate | ( | xyzVector< T > const & | v | ) | [inline] |
Convert a point in the global coordinate system to a point in this coordinate frame. If this frame is F and the point is p, then this solves for x st: F x = p. Equivalent to computing (F.inverse() * p).point().
| void numeric::HomogeneousTransform< T >::walk_along_x | ( | T | delta | ) | [inline] |
Right multiply this coordinate frame by the frame 1 0 0 delta 0 1 0 0 0 0 1 0.
| void numeric::HomogeneousTransform< T >::walk_along_y | ( | T | delta | ) | [inline] |
Right multiply this coordinate frame by the frame 1 0 0 0 0 1 0 delta 0 0 1 0.
| void numeric::HomogeneousTransform< T >::walk_along_z | ( | T | delta | ) | [inline] |
Right multiply this coordinate frame by the frame 1 0 0 0 0 1 0 0 0 0 1 delta.
| xyzVector< T > numeric::HomogeneousTransform< T >::xaxis | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::xx | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::xy | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::xz | ( | ) | const [inline] |
| xyzVector< T > numeric::HomogeneousTransform< T >::yaxis | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::yx | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::yy | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::yz | ( | ) | const [inline] |
| xyzVector< T > numeric::HomogeneousTransform< T >::zaxis | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::zx | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::zy | ( | ) | const [inline] |
| value_type numeric::HomogeneousTransform< T >::zz | ( | ) | const [inline] |
1.7.4
