Occasionally, we discover that the LiDAR extrinsic parameters are inaccurate. In such cases, we aim to recalibrate the SLAM poses and maps based on the updated parameters, without the need to rerun the entire SLAM process. By doing so, we can keep the annotations and the original SLAM map, which saves human effort and computational resources.
Coordinate Systems
A coordinate system consists of an origin and three axes used to express a point’s position. We distinguish three coordinate systems:
- LiDAR coordinate system ($l$): the sensor’s measurement frame. The raw points $\mathbf{p}_{t,i}^{l}$ are expressed in this coordinate system.
- Ego coordinate system ($e$): the vehicle’s own reference frame, with an origin and axes fixed to the vehicle body. The updated extrinsics $\mathbf{T}_{l\to e}$ express LiDAR points in this frame.
- Virtual coordinate system ($v$): the frame obtained by applying the original extrinsics $\mathbf{T}_{l\to v}$ to the LiDAR points before SLAM. Its origin and axes may differ from those of the ego frame.
These frames move with the vehicle, and their relationships are fixed when the extrinsics are constant. The local SLAM map uses the virtual frame at the first scan, $v_0$, as a stationary reference. At time $t$, the vehicle frame is $v_t$, while the map frame remains $v_0$.
We write $\mathbf{T}_{a\to b}$ for a transform that maps coordinates from frame $a$ into frame $b$, using homogeneous column vectors $[x,y,z,1]^\top$. This follows the conventions in Lynch and Park’s Modern Robotics, §3.3.1.
Raw Data
Given LiDAR points in the LiDAR coordinate system
$$ \mathcal{P}_t^l = \{\mathbf{p}_{t,i}^l\}_{i=1}^{N_t}, \qquad \mathcal{P}^{\mathrm{orig}} = \bigcup_{t=0}^{T}\mathcal{P}_t^l. $$Here, $t\in\{0,\ldots,T\}$ is the scan time index, $T$ is the final scan index, $i$ is the point index at time $t$, and $N_t$ is the number of points at time $t$. Calligraphic $\mathcal{P}$ denotes a point set, while bold lowercase $\mathbf{p}$ denotes an individual point. We retain the scan and point indices throughout the processing.
Transform to Virtual Coordinate System
Transform the LiDAR points into a virtual coordinate system
$$ \mathbf{p}_{t,i}^v = \mathbf{T}_{l\to v}\mathbf{p}_{t,i}^l, \qquad \mathcal{P}_t^v = \{\mathbf{p}_{t,i}^v\}_{i=1}^{N_t}, \qquad \mathcal{P}^v = \bigcup_{t=0}^{T}\mathcal{P}_t^v. $$Perform SLAM
Perform SLAM on these points to obtain the poses and points
$$ \mathcal{D}^v = \bigcup_{t=0}^{T}\left\{\left(\mathbf{P}_t^v,\mathcal{P}_t^v\right)\right\}. $$Here, $\mathbf{P}_t^v = \mathbf{T}_{v_t\to v_0}$ is the position and orientation of the virtual vehicle frame at time $t$, expressed in the fixed initial frame $v_0$. It maps each scan point into the local map: $\mathbf{P}_t^v\mathbf{p}_{t,i}^v$. Its rotation block gives the orientation of $v_t$ in $v_0$, and its translation column gives the position of the origin of $v_t$ in $v_0$. We initialize $\mathbf{P}_0^v=\mathbf{I}$.
Align with RTK
In the next step, we will align the poses with the RTK poses to obtain their global positions. Here, we only perform the alignment on the x-y plane due to the noisy attitude of the RTK.
Suppose the RTK poses are $\{\mathbf{P}_t^{\mathrm{RTK}}\}$. Write the virtual and RTK poses in block form:
$$ \mathbf{P}_t^f = \begin{bmatrix} \mathbf{R}_t^f & \mathbf{d}_t^f\\ \mathbf{0}_{1\times3} & 1 \end{bmatrix}, \qquad f\in\{v,\mathrm{RTK}\}, $$where $\mathbf{d}_t^f$ is the translation vector. Define the horizontal positions as
$$ \mathbf{x}_t\triangleq(\mathbf{d}_t^v)_{xy}, \qquad \mathbf{y}_t\triangleq(\mathbf{d}_t^{\mathrm{RTK}})_{xy}, $$where $(\mathbf{d})_{xy}=[d_x,d_y]^\top$.
Align the x-y coordinates between virtual poses and RTK by solving the following optimization problem
$$ (\mathbf{R}^*,\mathbf{t}^*) = \underset{\mathbf{R}\in SO(2),\,\mathbf{t}\in\mathbb{R}^2}{\operatorname{argmin}}\sum_t\|\mathbf{R}\mathbf{x}_t+\mathbf{t}-\mathbf{y}_t\|_2^2. $$Construct the aligned virtual pose as
$$\mathbf{P}_t^{va} = \mathbf{Q} \mathbf{P}_t^v$$where
$$\mathbf{Q} \triangleq \begin{bmatrix} \mathbf{R}^* & \mathbf{0}_{2\times1} & \mathbf{t}^*\\ \mathbf{0}_{1\times2} & 1 & 0\\ \mathbf{0}_{1\times2} & 0 & 1 \end{bmatrix} $$Thus the x-y-z coordinates of $\mathbf{P}_t^{va}$ will be $[(\mathbf{R}^* \mathbf{x}_t+\mathbf{t}^*)^\top, z]^\top$, where $z$ is the original z coordinate of $\mathbf{P}_t^v$. By doing so, the x-y positions are aligned with the RTK position, and z remains unchanged.
Then, we get the new RTK aligned poses and corresponding points
$$ \mathcal{D}^{va} = \bigcup_{t=0}^{T}\left\{\left(\mathbf{P}_t^{va},\mathcal{P}_t^v\right)\right\}. $$If we only care about the local structure of the SLAM map, we can use the first frame as the reference frame. So, we have the poses and points as
$$ \bigcup_{t=0}^{T}\left\{\left((\mathbf{P}_0^{va})^{-1}\mathbf{P}_t^{va},\mathcal{P}_t^v\right)\right\}. $$Map Annotation
We perform annotation on the local SLAM map. The annotated points can be represented as
$$ \bigcup_{t=0}^{T}\left\{\left((\mathbf{P}_0^{va})^{-1}\mathbf{P}_t^{va},\{\mathbf{p}_{t,i}^v\}_{i\in S_t}\right)\right\}. $$where $S_t\subseteq\{1,\ldots,N_t\}$ is the set of annotated point indices at time $t$.
Relocating and Mapping
When another LiDAR car generates LiDAR points and poses as
$$ \mathcal{P}_\tau^r = \{\mathbf{p}_{\tau,j}^r\}_j, \qquad \mathcal{D}^r = \bigcup_{\tau=0}^{T_r}\left\{\left(\mathbf{P}_\tau^r,\mathcal{P}_\tau^r\right)\right\}, $$where $\tau\in\{0,\ldots,T_r\}$ and $j$ index the second vehicle’s scans and points, respectively, and $T_r$ is its final scan index.
First, we register the second vehicle’s map against the RTK-aligned reference map represented by $\mathcal{D}^{va}=\bigcup_{t=0}^{T}\left\{\left(\mathbf{P}_t^{va},\mathcal{P}_t^v\right)\right\}$. The two maps contain different samples of the scene, so we express their relationship through matched points and registration residuals.
Let $\mathcal{C}$ contain accepted point-index correspondences $((\tau,j),(t,i))$ between the two maps. For fixed correspondences, the point-to-point alignment problem is
$$ \min_{\mathbf{T}\in SE(3)} \sum_{((\tau,j),(t,i))\in\mathcal{C}} \left\| \mathbf{P}_\tau^r\mathbf{p}_{\tau,j}^r -\mathbf{T}\mathbf{P}_t^{va}\mathbf{p}_{t,i}^v \right\|_2^2. $$ICP alternates between updating the correspondences and estimating the transform. Denote the estimated transform by $\mathbf{T}_{\mathrm{rel}}$. For each accepted correspondence, we can write
$$ \mathbf{P}_\tau^r\mathbf{p}_{\tau,j}^r =\mathbf{T}_{\mathrm{rel}}\mathbf{P}_t^{va}\mathbf{p}_{t,i}^v +\boldsymbol{\varepsilon}_{(\tau,j),(t,i)}, \qquad ((\tau,j),(t,i))\in\mathcal{C}, $$where $\boldsymbol{\varepsilon}_{(\tau,j),(t,i)}$ is the registration residual. Because the points use homogeneous coordinates, the residual’s last component is zero, and its norm measures the distance in the three spatial coordinates.
With this convention, $\mathbf{T}_{\mathrm{rel}}$ maps the annotated reference map into the second vehicle’s map. Its inverse maps back to the reference map, so the second vehicle’s pose expressed in the reference map is
$$ \widehat{\mathbf{P}}_\tau^r = \mathbf{T}_{\mathrm{rel}}^{-1}\mathbf{P}_\tau^r. $$If $\mathbf{P}_\tau^r$ indicates the ego pose, then the new ego pose is $\widehat{\mathbf{P}}_\tau^r$. If it instead describes the second vehicle’s virtual frame, we also need to convert to that vehicle’s ego frame.
- For a local trajectory initialized at identity, compute the ego pose using the derivation below. This also rebases the local map to the second vehicle’s initial ego frame:
where $\mathbf{T}_{e\to v}$ is the transformation matrix from the second vehicle’s ego coordinate system to its virtual coordinate system.
- Update the registration transform for the rebased local map:
The ego pose expressed in the annotated reference map is then
$$ \widehat{\mathbf{P}}_\tau^{re} =(\mathbf{T}_{\mathrm{rel}}^e)^{-1}\mathbf{P}_\tau^{re} =\mathbf{T}_{\mathrm{rel}}^{-1}\mathbf{P}_\tau^r\mathbf{T}_{e\to v}. $$Impact of Change of Virtual Coordinate
We will discuss the impact if we change the extrinsic parameters and the correlation of the new poses and the original poses.
LiDAR SLAM Pose
Suppose we change the original points using another transformation matrix, e.g., transform into ego coordinate. We have the new transformed LiDAR points:
$$ \mathbf{p}_{t,i}^e = \mathbf{T}_{l\to e}\mathbf{p}_{t,i}^l, \qquad \mathcal{P}_t^e = \{\mathbf{p}_{t,i}^e\}_{i=1}^{N_t}, \qquad \mathcal{P}^e = \bigcup_{t=0}^{T}\mathcal{P}_t^e. $$Using these LiDAR points to perform SLAM, we obtain the new poses and points
$$ \mathcal{D}^e = \bigcup_{t=0}^{T}\left\{\left(\mathbf{P}_t^e,\mathcal{P}_t^e\right)\right\}. $$Since we only change the reference coordinate system of the ego vehicle, the 3D reconstruction is invariant to rigid body transformations. Thus, we have
$$ \mathbf{P}_t^e \mathbf{p}_{t,i}^e = \mathbf{T} \mathbf{P}_t^v \mathbf{p}_{t,i}^v, $$for all $t$ and $i$. This equation leads to
$$ \mathbf{P}_t^e \mathbf{T}_{l\to e} \mathbf{p}_{t,i}^{l} = \mathbf{T} \mathbf{P}_t^v \mathbf{T}_{l\to v} \mathbf{p}_{t,i}^{l} $$Thus we have
$$ \mathbf{P}_t^e \mathbf{T}_{l\to e} = \mathbf{T} \mathbf{P}_t^v \mathbf{T}_{l\to v} $$where $\mathbf{T}$ is an unknown rigid body transformation. This means the new and old 3D reconstructions are invariant to rigid body transformations.
When doing SLAM, we initialize the pose with an identity matrix, i.e., $\mathbf{P}_0^e = \mathbf{I}$ and $\mathbf{P}_0^v = \mathbf{I}$, we have
$$ \mathbf{I} \mathbf{T}_{l\to e} = \mathbf{T} \mathbf{I} \mathbf{T}_{l\to v} $$which results in
$$ \mathbf{T} = \mathbf{T}_{l\to e} \mathbf{T}_{l\to v}^{-1}, $$and
$$ \mathbf{P}_t^e = \mathbf{T}_{l\to e} \mathbf{T}_{l\to v}^{-1} \mathbf{P}_t^v \left(\mathbf{T}_{l\to v} \mathbf{T}_{l\to e}^{-1}\right) $$Note that $\mathbf{T}_{l\to e}^{-1} = \mathbf{T}_{e\to l}$, so we have
$$ \mathbf{T}_{l\to v} \mathbf{T}_{l\to e}^{-1} = \mathbf{T}_{e\to v}, $$$$ \begin{aligned} \mathbf{P}_t^e &= \mathbf{T}_{v\to e} \mathbf{P}_t^v \mathbf{T}_{e\to v}\\ &= \mathbf{T}_{e\to v}^{-1} \mathbf{P}_t^v \mathbf{T}_{e\to v} \end{aligned}. $$This conjugation has the same algebraic form as the hand–eye calibration relation $\mathbf{A}\mathbf{X}=\mathbf{X}\mathbf{B}$ studied by Park and Martin (1994): here, $\mathbf{P}_t^v\mathbf{T}_{e\to v}=\mathbf{T}_{e\to v}\mathbf{P}_t^e$. We use the known transform $\mathbf{T}_{e\to v}$ to relate the two pose representations.
This tells us that the new pose is not a simple right or left multiplication by any rigid body transformation of the original pose.
Aligned Poses
Suppose the new alignment has planar rotation $\mathbf{Q}^*\in SO(2)$ and translation $\mathbf{s}^*\in\mathbb{R}^2$. We have
$$ \mathbf{P}_t^{ea} = \mathbf{W} \mathbf{P}_t^e $$where
$$ \mathbf{W} \triangleq \begin{bmatrix} \mathbf{Q}^* & \mathbf{0}_{2\times1} & \mathbf{s}^*\\ \mathbf{0}_{1\times2} & 1 & 0\\ \mathbf{0}_{1\times2} & 0 & 1 \end{bmatrix} $$Relative Poses
$$ \begin{aligned} (\mathbf{P}_s^{ea})^{-1}\mathbf{P}_t^{ea} &= (\mathbf{P}_s^{e})^{-1}\mathbf{P}_t^{e}\\ &= \mathbf{T}_{e\to v}^{-1} (\mathbf{P}_s^v)^{-1} \mathbf{P}_t^v \mathbf{T}_{e\to v}\\ &= \mathbf{T}_{e\to v}^{-1} (\mathbf{P}_s^{va})^{-1} \mathbf{P}_t^{va} \mathbf{T}_{e\to v} \end{aligned} $$From this equation, we know that the relative poses follow a similar transformation to the individual poses.
Annotations
Since we perform relocation and re-mapping using the RTK-aligned poses and their corresponding maps, the LiDAR points carry the properties of the target annotations. Any reference coordinate system change does not affect the SLAM map structure, so the annotations remain unchanged.
References
- Park, F. C., and Martin, B. J. (1994). Robot Sensor Calibration: Solving AX=XB on the Euclidean Group. IEEE Transactions on Robotics and Automation, 10(5), 717–721.
- Lynch, K. M., and Park, F. C. (2017). Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press. §3.3.1: Homogeneous Transformation Matrices.