LVIO-SAM: A Multi-sensor Fusion Odometry via Smoothing and Mapping.

State estimation with sensors is critical for the mapping and navigation of mobile robots. Since different sensors have different performances in the environment, how to fuse different sensors together will be a problem. In this paper, we propose a multi-sensor fusion odometry, LVIOSAM, which fuses LiDAR, stereo camera and inertial measurement unit (IMU) via smoothing and mapping. The pre-integration motion estimation from the IMU eliminates the skew of the point cloud and produces initial guesses for the optimization of lidar odometry. The obtained lidar odometry is used to estimate the bias of the IMU and as the initial value of the triangulation of the visual odometry. The visual odometry is used as a between factor for the motion estimation of the entire system. To ensure the real-time performance, we separately marginalize the old lidar scans and visual 3D points for pose optimization. The method has been widely evaluated on datasets gathered from simulation environment and public datasets. To benefit the community, we open source our simulation environment and codes on our Github(1).