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 1 Point Cloud Mapping

1: 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}$

2: Output

Accumulated point cloud map: $P C M$

3: 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}})$

5: ${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: $p_{w o r l d_{i}} \leftarrow {T_{l i d a r}^{w o r l d}}_{i} p_{l i d a r_{i}}$

7: $P C M \leftarrow A c c u m (P C M, p_{w o r l d_{i}})$

8: end for