A Mathematical Review

A.2 Rigid transforms

Rigid transforms are used to specify both the transformation of points and vectors between coordinate frames, as well as the relative position and orientation between coordinate frames.

Figure A.3: A position vector {\bf p}_{BA} and rotation matrix {\bf R}_{BA} describing the position and orientation of frame B with respect to frame A.

Consider two 3D coordinate frames in space, A and B (Figure A.3). The translational position of B with respect to A can be described by a vector {\bf p}_{BA} from the origin of A to the origin of B (described with respect to frame A). Meanwhile, the orientation of B with respect to A can be described by the 3\times 3 rotation matrix {\bf R}_{BA} (Section A.1). The combined position and orientation of B with respect to A is known as the pose of B with respect to A.

Figure A.4: Point vectors {}^{A}{\bf q} and {}^{B}{\bf q} describing the position of a point {\bf q} with respect to frames A and B.

Now, assume we have a 3D point {\bf q}, and consider its coordinates with respect to both frames A and B (Figure A.4). Given the pose descriptions given above, it is fairly straightforward to show that

{}^{A}{\bf q}={\bf R}_{BA}\,{}^{B}{\bf q}+{\bf p}_{BA}, (A.10)

and, given (A.2), that

{}^{B}{\bf q}={\bf R}_{BA}^{T}\,({}^{A}{\bf q}-{\bf p}_{BA}). (A.11)

If we extend our points into a 4D homogeneous coordinate space with the fourth coordinate w equal to 1, i.e.,

{\bf q}^{*}\equiv\left(\begin{matrix}{\bf q}\\
1\end{matrix}\right), (A.12)

then (A.10) and (A.11) can be simplified to

{}^{A}{\bf q}^{*}={\bf T}_{BA}\,{}^{B}{\bf q}^{*}\quad\text{and}\quad{}^{B}{%
\bf q}^{*}={\bf T}_{BA}^{-1}\,{}^{A}{\bf q}^{*}

where

{\bf T}_{BA}=\left(\begin{matrix}{\bf R}_{BA}&{\bf p}_{BA}\\
0&1\end{matrix}\right) (A.13)

and

{\bf T}_{BA}^{-1}=\left(\begin{matrix}{\bf R}_{BA}^{T}&-{\bf R}_{BA}^{T}{\bf p%
}_{BA}\\
0&1\end{matrix}\right). (A.14)

{\bf T}_{BA} is the 4\times 4 rigid transform matrix that transforms points from B to A and also describes the pose of B with respect to A (Figure A.5).

Figure A.5: The transform matrix {\bf T}_{BA} from B to A.

It is straightforward to show that {\bf R}_{BA}^{T} and -{\bf R}_{BA}^{T}{\bf p}_{BA} describe the orientation and position of A with respect to B, and so therefore

{\bf T}_{BA}^{-1}={\bf T}_{AB}. (A.15)

Note that if we are transforming a vector {\bf v} instead of a point between B and A, then we are only concerned about relative orientation and the vector transforms (A.3) and (A.4) should be used instead. However, we can express these using {\bf T}_{BA} if we embed vectors in a homogeneous coordinate space with the fourth coordinate w equal to 0, i.e.,

{\bf v}^{*}\equiv\left(\begin{matrix}{\bf v}\\
0\end{matrix}\right), (A.16)

so that

{}^{B}{\bf v}^{*}={\bf T}_{BA}\,{}^{A}{\bf v}^{*}\quad\text{and}\quad{}^{A}{%
\bf v}^{*}={\bf T}_{BA}^{-1}\,{}^{B}{\bf v}^{*}.
Figure A.6: Three coordinate frames A, B, and C and the transforms relating each one to the other.

Finally, we consider transform composition. Suppose we have three coordinate frames, A, B, and C, each related to the other by transforms {\bf T}_{BA}, {\bf T}_{CB}, and {\bf T}_{CA} (Figure A.6). Using the same reasoning used to derive (A.6) and (A.9), it is easy to show that

{\bf T}_{CB}={\bf T}_{BA}^{-1}\;{\bf T}_{CA}={\bf T}_{AB}\;{\bf T}_{CA}. (A.17)