# Multi-Lidar System Localization and Mapping with Online Calibration
## 2 Front-End Laser Odometry and Online Calibration
### Point Cloud Preprocessing  
Ground point cloud screening, as depicted in Figure 2, involves two points, A and B, scanned by two adjacent laser beams on the same column. In the figure, h indicates the height difference between the points, d is the horizontal distance, and \(\theta\) represents the angle between the straight line formed by the points and the horizontal plane. The determination of whether points A and B belong to the ground point cloud hinges on comparing the horizontal angle with a predetermined threshold. If it surpasses the threshold, both points are considered non-ground. Otherwise, both points are considered as part of the ground point cloud.  
Figure 1: Laser odometer and online external parameter calibration process.  
The angle \(\theta\) can be computed as follows:  
\[\theta=\arctan\left(\frac{\left|z_{1}-z_{2}\right|}{\sqrt{\left(x_{1}-x_{2} \right)^{2}+\left(y_{1}-y_{2}\right)^{2}}}\right) \tag{1}\]  
When clustering the point cloud, the initial step involves traversing each point for processing. If the current point has not been marked, it is designated as a new cluster, and the current point is then used as a center for traversal of its four neighbors: up, down, left, and right. During this process, it is essential to calculate the angle between the laser radar, the current point, and the neighbor point. Should the angle exceed the designated threshold, both the current point and the neighbor point are categorized under the same point cloud. The calculation process is illustrated in Figure 3, where O represents the center point of the lidar, A is the current point of traversal, B denotes the neighbor of the current point, d1 is the length of line segment OA, d2 is the length of line segment OB, \(\alpha\) is the angle between OA and OB, and \(\theta\) is the angle to be calculated.  
Figure 3: Point cloud clustering model.  
Figure 2: Ground point cloud extraction process.  
The angle \(\theta\) can be computed as follows:  
\[\theta=\arctan\left(\frac{d_{2}*\sin\alpha}{d_{1}-d_{2}*\cos\alpha}\right) \tag{2}\]  
Neighbor points are continuously traversed until the current cluster no longer contains points that meet the requirements. At that point, the clustering process is deemed completed. The decision on whether to retain the cluster is based on the number of point clouds within it. If this count falls below a certain threshold, the cluster is considered invalid.  
In the process of correcting point cloud motion distortion, assuming constant lidar movement speed between two frames, the prediction of relative motion between the current frames hinges on the calculation results of the laser odometer from the preceding two frames, as expressed in the following equation:  
\[\left(T_{k-1}^{w}\right)^{-1}T_{k}^{w}=\left(T_{k-2}^{w}\right)^{-1}T_{k-1}^{w} \tag{3}\]  
where \(T_{k}^{w}\), \(T_{k-1}^{w}\), and \(T_{k-2}^{w}\) represent the laser odometer information for frames \(k\), \(k-1\), and \(k-2\), respectively, in the world coordinate system. In accordance with the assumption of a uniform motion model, the relative motion between frames \(k-2\) and \(k-1\) is consistent with the relative motion between frames \(k-1\) and \(k\). Consequently, the pose transformation for frame \(k\) can be computed. This approach effectively achieves motion distortion correction for the lidar point cloud.