Rigid Transformations and Relative Pose

We introduce the terminology for the relationships between different reference frames that will be used throughout this book. Further terminology concerning coordinate systems will be provided in the later section 9.2, which should be consulted in part for certain terms.

Let $\prescript{w}{}{\mathbf{x}} \in \mathbb{R}^3$ be a point expressed in “global” or “world” coordinates (world coordinates), and let $\prescript{s}{}{\mathbf{x}}$ be the same point expressed instead in “local” or “sensor” coordinates (or body coordinates in the general case of a system moving relative to another). The coordinate $\prescript{s}{}{\mathbf{x}}$ represents spatial information measured solely by the sensor; it therefore contains no information about how the sensor is positioned and oriented in the world. Although they represent the same physical point, the coordinates of the two points differ because they are expressed in two different reference frames: one represents an absolute position (the world), while the other represents the point as seen by the sensor, with the sensor at the center of the reference frame aligned with the axes.

Definizione 4   The relationship between world and sensor coordinates is

$$\prescript{w}{}{\mathbf{x}} = \prescript{w}{}{\mathbf{R}_{s}} \prescript{s}{}{\mathbf{x}} + \prescript{w}{}{\mathbf{t}}$$ (1.99)

where $\prescript{w}{}{\mathbf{R}_{s}}$ is the rotation matrix that transforms a point from sensor coordinates to world coordinates and $\mathbf{t}$ is the sensor position relative to the origin of the reference frame.

Now let two generic sensors, numbered 1 and 2, be connected to the common world reference frame $w$ through the parameters $(\prescript{w}{}{\mathbf{R}}_1, \prescript{w}{}{\mathbf{t}}_1)$ and $(\prescript{w}{}{\mathbf{R}}_2, \prescript{w}{}{\mathbf{t}}_2)$, respectively, expressed as in definition 4.

Let $(\prescript{1}{}{\mathbf{R}}_2,\prescript{1}{}{\mathbf{t}}_{2,1})$ be the “relative” pose of sensor 2 with respect to sensor 1, a pose that makes it possible to convert a point from the reference frame of sensor 2 to that of sensor 1:

$$\prescript{1}{}{\mathbf{x}} = \prescript{1}{}{\mathbf{R}}_2 \prescript{2}{}{\mathbf{x}} + \prescript{1}{}{\mathbf{t}}_{2,1}$$ (1.100)

Matrix $\prescript{1}{}{\mathbf{R}}_2$, which represents the orientation of sensor 2 with respect to sensor 1, transforms the sensor coordinates, whereas $\prescript{1}{}{\mathbf{t}}_{2,1}$ is the pose of sensor 2 with respect to sensor 1 expressed in reference frame 1.

The relative-pose parameters are obtained from the poses of the individual sensors, expressed with respect to a third reference frame (the world frame), through the relations:

$$\begin{array}{l} \prescript{1}{}{\mathbf{R}}_2 = \mathbf{R... ... \mathbf{R}_1^{-1} ( \mathbf{t}_2 - \mathbf{t}_1 ) \end{array}$$ (1.101)

From now on, to simplify the notation, we omit the world reference frame $w$; therefore, when not otherwise indicated, coordinates refer to this frame, and the change of basis also maps into this frame.

The opposite relative pose $(\prescript{2}{}{\mathbf{R}}_1,\prescript{2}{}{\mathbf{t}}_{1,2})$, which transforms from frame 2 to frame 1, can be obtained from $(\prescript{1}{}{\mathbf{R}}_2,\prescript{1}{}{\mathbf{t}}_{2,1})$ as

$$\begin{array}{l} \prescript{2}{}{\mathbf{R}}_1 = \mathbf{R... ...hbf{R}}^{\top}_2 \prescript{1}{}{\mathbf{t}}_{2,1} \end{array}$$ (1.102)

Given the relative pose between the sensors and the absolute pose of one of them (in this case, for simplicity, sensor 1), it is possible to obtain the absolute pose of the second sensor through the transformation

$$\begin{array}{l} \mathbf{R}_2 = \mathbf{R}_1 \prescript{1}... ...1 \prescript{1}{}{\mathbf{t}}_{2,1} + \mathbf{t}_1 \end{array}$$ (1.103)