trnorm
Normalize an SO(3) or SE(3) matrix
TRNORM(R) is guaranteed to be a proper orthogonal matrix rotation matrix (3×3) which is “close” to the input matrix R (3×3). If R = [N,O,A] the O and A vectors are made unit length and the normal vector is formed from N = O x A, and then we ensure that O and A are orthogonal by O = A x N.
TRNORM(T) as above but the rotational submatrix of the homogeneous transformation T (4×4) is normalised while the translational part is unchanged.
If R (3×3× K) or T (4×4× K) representing a sequence then the normalisation is performed on each of the K planes.
Notes
- Only the direction of A (the z-axis) is unchanged.
- Used to prevent finite word length arithmetic causing transforms to become `unnormalized’.
- There is no Toolbox function for SO(2) or SE(2).