Algorithm 3: Improved weighted iterative closest point (ICP)

Input: V-SLAM track $p_i$, GPS track $q_i$, weights on timestamps $\left\{\left.w_i\right|w_i\ge 0\right\}$, $i=1,\dots ,N$ Output: Rotation matrix and translation vector that minimises ${{\displaystyle {\sum }_{i=1}^N{w}_i‖q_i-\left(R{p}_i+t\right)‖}}^2$ 1: Centroids $\overline{p}=\left({\displaystyle {\sum }_{i=1}^N{w}_i{p}_i}\right)/\left({\displaystyle {\sum }_{i=1}^N{w}_i}\right)$, $\overline{q}=\left({\displaystyle {\sum }_{i=1}^N{w}_i{q}_i}\right)/\left({\displaystyle {\sum }_{i=1}^N{w}_i}\right)$ 2: Centred vectors $x_i=q_i-\overline{q}$, $y_i=p_i-\overline{p}$ 3: Covariance matrix $S=XW{Y}^T$, where $X$ and $Y$ have $x_i$ and $y_i$ as columns, respectively, and $W$ is a diagonal matrix with $w_i$ on the diagonal 4: Singular value decomposition $S=U\mathsf{\Sigma }{V}^T$ 5: $R=U{V}^T$ and $t=\overline{q}-R\overline{p}$ 6: V-SLAM track after calibration in global coordinates ${p^{\prime }}_i=R{p}_i+t$