Performance Index for Extrinsic Calibration of LiDAR and Motion Sensor for Mapping and Localization

. 2021 Dec 24;22(1):106. doi: 10.3390/s22010106

Algorithm 2 Evaluation of Matching Error

Inputs

Motion data in time i: $g y r o_{m o t i o n_{i}}$ , $v e l_{m o t i o n_{i}}$

Point cloud in time i: $p_{l i d a r_{i}}$

Calibration transformation matrix: $T_{l i d a r}^{m o t i o n}$

Accumulated point cloud map: $P C M$

2: Output

Performance index according to calibration parameter: $P I_{d i s t}, P I_{r o t}$

for i = 1 to N do

4: ${T_{m o t i o n}^{w o r l d}}_{i} \leftarrow V e h i c l e M o d e l (g y r o_{m o t i o n_{i}}, v e l_{m o t i o n_{i}})$

${T_{l i d a r}^{w o r l d}}_{i} \leftarrow {T_{m o t i o n}^{w o r l d}}_{i} T_{l i d a r}^{m o t i o n}$

6: ${T_{r a n d}}_{i} \leftarrow R a n d o m G e n e r a t o r$

${T_{i n i t}}_{i} \leftarrow T_{l i d a r}^{w o r l d} T_{r a n d_{i}}$

8: ${T_{p r e d_l i d a r}}_{i} \leftarrow M a t c h i n g b t w P C M a n d P C (P C M, p_{l i d a r_{i}}, T_{i n i t_{i}})$

${T_{p r e d_m o t i o n}}_{i} \leftarrow T_{p r e d_l i d a r_{i}} {(T_{l i d a r}^{m o t i o n})}^{- 1}$

10: $d i s t_e r r_{i} \leftarrow t_{p r e d_m o t i o n_{i}} - t_{m o t i o n_{i}}$

$r o t_e r r_{i} \leftarrow R_{p r e d_m o t i o n_{i}} ⊖ R_{m o t i o n_{i}}$

12: end for

$P I_{d i s t} \leftarrow R M S E (d i s t_e r r)$

14: $P I_{r o t} \leftarrow R M S E (r o t_e r r)$