Seamless Integration of Fast LIO2 and Sophus for Advanced Autonomous Driving Capabilities

Abstract

FAST-LIO2 is a LiDAR-inertial odometry framework that is efficient, fast, resilient, and adaptable, utilizing a highly efficient tightly-coupled iterated Kalman filter. This post presents S-FAST-LIO2, which is a simplified version of FAST-LIO2 made using Sophus instead of the complex IK-FOM to define the state variables. Sophus, a widely used c++ implementation, is employed for solving 2d and 3d geometric problems in Computer Vision or Robotics applications. Dealing with non-linearities in the state space with Sophus can enhance filtering and estimation performance. Sophus makes it convenient to represent and manipulate transformations like rotations and translations, which is essential for tasks such as robot motion planning or visual odometry. S-FAST-LIO2 retains the key features of FAST-LIO2, offering fast, robust, and accurate LiDAR navigation and mapping. It allows direct registration of raw points to the map without the necessity to extract features. Additionally, it maintains a map using an incremental k-d tree data structure, known as ikd-Tree, allowing incremental updates and dynamic re-balancing. Compared to FAST-LIO2, S-FAST-LIO2 achieves an APE of 1.0578m and RPE of 0.24%, slightly surpassing FAST-LIO2’s APE of 1.0576m and RPE of 0.24%. Notably, S-FAST-LIO2 also outperforms FAST-LIO2 in average processing time, clocking in at 8.95ms compared to FAST-LIO2’s 9.75ms.