Implement ICP Algorithm for SLAM Using Relative Motion Threshold and Robust Kernel

Simultaneous Localization and Mapping (SLAM) stands as a crucial technology for Autonomous Vehicles, enabling the construction of an environment map while concurrently localizing the vehicle or robot within it. Leveraging sensor data such as LiDAR and wheel odometry, the environment map is progressively developed through a scan-matching algorithm. The Iterative Closest Point (ICP) algorithm plays an important role in finding relative transformations between point clouds, ensuring robust alignment and refining map accuracy. In this paper, two novel techniques for outlier rejection are implemented: Relative Motion Threshold (RMT) and Robust Kernel. RMT detects outliers based on error threshold calculated by a ratio which depends on the convergence rate. While Robust Kernel reduce the influence of outliers. Through simulation, our findings reveal that the incorporation of RMT and Robust Kernel outperforms traditional techniques, demonstrating map-building capabilities. The application of ICP, RMT, and Robust Kernel showcases the potential for enhancing SLAM performance, promising advancements in autonomous vehicle navigation and mapping.