Relative Positioning Between Sensors

We introduce for nomenclature the relationships that exist between various reference systems, which will be used throughout this book. Further nomenclature regarding coordinate systems will be found in the upcoming section 8.2, which should be referred to for some terms.

Let $\prescript{w}{}{\mathbf{x}} \in \mathbb{R}^3$ be a point expressed in "global" coordinates, "world" (world coordinates), and let $\prescript{s}{}{\mathbf{x}}$ be the same point expressed in "local" coordinates, "sensor" (or body in the generic case of a system in motion relative to another). The coordinate $\prescript{s}{}{\mathbf{x}}$ represents spatial information captured by the mere sensor: this coordinate therefore does not possess knowledge of how the sensor is positioned and oriented in world space. Although representing the same physical point, the coordinates expressed by the two points are indeed different as they are represented in two different reference systems: one represents a position understood as absolute (the world), while the second represents the point as seen from the sensor, where the sensor is at the center of the reference system aligned with respect to the axes.

Definition 1   The relation that links world coordinates to sensor coordinates is

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

with $\prescript{w}{}{\mathbf{R}_{s}}$ being the rotation matrix that allows the transformation of a point from sensor coordinates to world coordinates and $\mathbf{t}$ representing the position of the sensor with respect to the origin of the reference system.

Let us now denote by the numbers 1 and 2 two generic sensors connected to the common reference frame world $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 1.

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 allows for the conversion of a point from the reference frame of sensor 2 to the reference frame of sensor 1:

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

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

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

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

From now on, to simplify the notation, we will implicitly understand the world reference frame $w$ and therefore, when not specified, the coordinates are referred to this system and the change of basis also leads to this one.

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

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

Given the knowledge of the relative pose between the sensors and the absolute pose of one of the two (for simplicity, sensor 1), it is possible to derive 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.67)