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.
Raw Data
Given LiDAR points in the LiDAR coordinate system
$$\mathbf{p}^{orig} = \bigcup_{t=0:T}\{\mathbf{p}_{t,i}\}_{i=1}^{N_t}$$Where $t$ is the time index, $i$ is the point index at time $t$, and $N_t$ is the number of points at time $t$.
Transform to Virtual Coordinate System
Transform the LiDAR points into a virtual coordinate system
$$\mathbf{p}^v = \bigcup_{t} \{\mathbf{p}^v_{t,i}|\mathbf{p}^v_{t,i} = \mathbf{T}_{l\to v}\mathbf{p}_{t,i}\}_{i=1}^{N_t}$$Perform SLAM
Perform SLAM on these points to obtain the poses and points
$$\bigcup_{t} \left(pose_t^v, \{\mathbf{p}_{t,i}^v\}_i\right)$$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 $\{pose_t^{RTK}\}$, and define $\mathbf{y}_t \triangleq pose_t^{RTK}[1:2, 3]$, where $\mathbf{y}_t$ represents the x-y coordinates of the RTK poses. Define $\mathbf{x}_t\triangleq pose_t^v[1:2, 3]$ as the x-y coordinates of the virtual poses.
Align the x-y coordinates between virtual poses and RTK by solving the following optimization problem
$$\mathbf{R}^*, \mathbf{t}^* = \text{argmin}_{\mathbf R, \mathbf t}\sum_t \|\mathbf R*\mathbf{x}_t + \mathbf{t} -\mathbf{y}_t \|_2^2$$Construct the aligned virtual pose as
$$pose_t^{va} = \mathbf{Q}*pose_t^v$$where
$$\mathbf{Q} \triangleq \begin{bmatrix} \mathbf{R}^*& 0&\mathbf{t}^*\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix} $$Thus the x-y-z coordinates of $pose_t^{va}$ will be $[(\mathbf{R}^**\mathbf{x}_t+\mathbf{t}^*)^\top, z]^\top$, where $z$ is the original z coordinate of $pose_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
$$ \bigcup_t \left(pose_{t}^{va}, \{\mathbf{p}_{t,i}^v\}_i\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 \left((pose_0^{va})^{-1}*pose_t^{va}, \{\mathbf{p}_{t,i}^{v}\}_i\right). $$Map Annotation
We perform annotation on the local SLAM map. The annotated points can be represented as
$$ \bigcup_t \left((pose_0^{va})^{-1}*pose_t^{va}, \{\mathbf{p}_{t,i}^v\}_{i\in S_t}\right). $$where $S_t$ is the set of annotated data points at time $t$.
Relocating and Mapping
When another LiDAR car generates LiDAR points and poses as
$$\bigcup_\tau \left(pose_\tau^r, \{\mathbf{p}_{\tau,j}^r\}_j\right)$$Firstly, we do ICP matching on the RTK aligned maps, $\bigcup_t \left(pose_{t}^{va}, \{\mathbf{p}_{t,i}^v\}_i\right)$, thus we have
$$ \{pose_\tau^r * \mathbf{p}_{\tau,j}^r\}_{\tau,j} \approx \{\mathbf{T}_{rel}*pose_t^{va}*\mathbf{p}_{t,i}^v\}_{t,i} $$where $\mathbf{T}_{rel}$ is the transformation matrix between the relocated map and RTK annotated map, and the approximation is under the least squares error.
Then the revised pose can be computed as
$$ \widehat{pose}^r_t = \mathbf{T}_{rel}*pose^r_t $$If $pose^r_t$ indicates the ego pose, then the new ego pose is $\widehat{pose}^r_t$. Otherwise, we need to do further transformation.
- First, compute the ego pose according to (will show later)
where $\mathbf{T}_{e\to v}$ is the transformation matrix from the ego coordinate to the virtual coordinate.
- Compute the new ego pose
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:
$$ \bigcup_{t, i} \{\mathbf{p}^e_{t,i}|\mathbf{p}^e_{t,i} = \mathbf{T}_{l\to e}\mathbf{p}_{t,i}\} $$Using these LiDAR points to perform SLAM, we obtain the new poses and points
$$ \bigcup_t \left(pose_t^e, \{\mathbf{p}_{t,i}^e\}_i\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
$$ pose^e_{t}*\mathbf{p}^e_{t,i} = \mathbf{T}*pose^v_{t}*\mathbf{p}^v_{t,i}, $$for all $t$ and $i$. This equation leads to
$$ pose^e_{t}*\mathbf{T}_{l\to e}*\mathbf{p}_{t,i} = \mathbf{T}*pose^v_{t}*\mathbf{T}_{l\to v}*\mathbf{p}_{t,i} $$Thus we have
$$ pose^e_{t}*\mathbf{T}_{l\to e} = \mathbf{T}*pose^v_{t}*\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., $pose_0^e = \mathbf{I}$ and $pose_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
$$ pose_{t}^e = \mathbf{T}_{l\to e}*\mathbf{T}_{l\to v}^{-1} * pose_{t,i}^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} pose_{t}^e &= \mathbf{T}_{v\to e} * pose_{t}^v * \mathbf{T}_{e\to v}\\ &= \mathbf{T}_{e\to v}^{-1} * pose_{t}^v * \mathbf{T}_{e\to v} \end{aligned}. $$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 aligned pose transformation matrix is $\mathbf{Q}^*, \mathbf{s}^*$, we have
$$ pose_t^{ea} = \mathbf{W}*pose_t^e $$where
$$ \mathbf{W} \triangleq \begin{bmatrix} \mathbf{Q}^*& 0&\mathbf{s}^*\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix} $$Relative Poses
$$ \begin{aligned} (pose_{i}^{ea})^{-1}pose_{j}^{ea} &= (pose_{i}^{e})^{-1}pose_{j}^{e}\\ &= T_{e\to v}^{-1} * (pose_{i}^v)^{-1}* pose_{j}^v* T_{e\to v}\\ &= T_{e\to v}^{-1} * (pose_{i}^{va})^{-1}* pose_{j}^{va}* 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.