EMV-LIO: An Efficient Multiple Vision aided LiDAR-Inertial Odometry

The early research work on LiDAR-visual-inertial fusion was V-LOAM [5] proposed by the author of LOAM [6], Zhang et al, based on their previous research. The issue of robots encountering positioning failures during high-motion scenarios was examined, highlighting its resemblance to the degeneracy challenge stemming from sparse feature data. This observation offers valuable insight for addressing SLAM difficulties in environments characterized by degeneration. In 2019, Wang [7] combined VINS-mono [9] and LOAM in a novel approach. Here, VINS-mono functioned as a Visual-Inertial Odometry (VIO) subsystem to provide the initial pose value. Subsequently, the outcome is fed into LOAM to further optimize the pose by minimizing the residual of extracted feature points. In 2020, Khattack introduced CompSLAM, which is a loosely coupled filter combining LOAM, ROVIO [26], and kinematic-inertial odometry (TSIF [27]), thereby demonstrating a comprehensive fusion of these components. More recently, Wang [25] proposed an advanced SLAM framework that directly fuses visual and LIDAR data. This framework comprises three integral components: a frame-by-frame tracking module, an enhanced sliding window-based refinement module, and a parallel global and local search loop closure detection (PGLS-LCD) module. It ingeniously combines the bag-of-visual-words (BoW) technique with LIDAR iris features to achieve effective location recognition.

Given the fact that the above fusion methods are loosely-coupled, where the LiDAR measurements are not jointly optimized with the visual or inertial measurements. In order to enhance the anti-degeneration ability and robustness of the system, an increasing number of scholars have directed their efforts towards these tightly-coupled approaches. In 2018, Graeter launched LIMO [8], pioneering the concept of leveraging LiDAR point depth information to estimate the depth of visual feature points. This innovative approach effectively creates a system similar to RGB-D cameras for the first time. Apart from this, LIC-Fusion [28] presents a distinctive real-time spatial and temporal multi-sensor calibration, utilizing MSCKF [29] framework as its foundation. Nonetheless, it is worth mentioning that this work is not publicly available. Inspired by the aforementioned idea, Shan developed LVI-SAM [4], which combines LIO-SAM [10] with VINS-mono. Notably, in scenarios where the LIO subsystem or the VIO subsystem fails, the other system can still remain functional autonomously. This dual-system setup safeguards the overall system’s robustness. At the same time, Lin introduced R2LIVE[12] based on FAST-LIO2 [11] to fuse the measurements of LiDAR, IMU, and camera within the framework of ES-IKF (Error State Iterative Kalman Filter), which achieves better real-time performance compared with LVI-SAM. Additionally, R2LIVE adopts a comprehensive approach by incorporating all measurements into a factor graph. It optimizes the visual landmarks present within the local sliding window, thereby contributing to the refinement of visual features within the map, and consequently enhancing accuracy. Then in 2021, they launched R3LIVE [13] on the basis of the above work. This iteration refined the VIO algorithm and rendered the LiDAR point cloud. Similar two submodules, LIO and VIO are also used in FAST-LIVO [14] and achieve real-time performance at the level of state-of-the-art.

We assume that the intrinsic parameters of each camera are known. The extrinsic parameters between the three kinds of sensors are calibrated, and they are time synchronized. The relationship of different frames is shown in Fig. 2.

Let us denote the pose of the robot under the World coordinate system at each timestamp $t_{k}$ as ${\chi}_{k}$ as follows:

where ${R_{B}^{O}}_{k}$ refer to the 3D rotations from base to odometry at timestamp k while ${p_{B}^{O}}_{k}$ refer to the 3D translations.

The Multi-camera VIO in our system develops from VINS-mono. All visual features from multiple cameras are detected using a corner detector [15] first and tracked by the Kanade–Lucas–Tomasi algorithm [16]. Each feature point is endowed with a unique ID which is managed by establishing a one-way mapping between the feature point set and the camera set. After that, either camera will be selected for initialization by performing Structure from Motion (SfM), including estimating the camera’s motion based on epipolar geometry constraint, estimating the depth of feature points by triangulation, and optimizing the camera’s pose and the position of feature points by solving the Perspective-n-Point (PnP) problem by Bundle Adjustment (BA). By setting the position of the first frame of the camera $c_{o}$ as the origin of the reference coordinate system of SfM, we can calculate the pose of the camera for initialization in the IMU coordinate system on the premise of knowing the camera-IMU external parameters ($\bm{R}_{C}^{B},\bm{p}_{C}^{B}$).

Assuming that there are $n$ cameras in the system, we perform bundle adjustment in a sliding-window which adds the observation residuals of feature points from all cameras into the following cost function:

After obtaining the estimation from VIO as mentioned before, it is feasible for us to utilize it as the initial guess of scan-matching in LIO, which can be solved by minimizing the following cost function [4].

• •

  Removing LiDAR’s noise via range image: It’s well known that the raw data processing including feature extraction and distortion removal is typically the first step in the LiDAR-based SLAM system before further analysis is performed. Considering that the noise in the raw point cloud will decrease the accuracy of odometry while increasing the processing time for the back-end optimization, it is essential to remove the noise at the very beginning of the LIO. In this paper, a method referred from [17] is adopted. With knowing the vertical scanning angles of a LiDAR, we can project the raw point cloud onto a range image where each valid point is represented by a pixel and the value of it records the Euclidean distance from the point to the LiDAR’s origin. Supposing that the horizontal angular resolutions is $\alpha$, clustering is performed assuming that two neighboring points in the horizontal or vertical direction belong to the same object if the angle $\gamma$ between their connected line and the laser beam is above a certain threshold $\theta$. $\gamma$ can be calculated by applying trigonometric equations.

  $$\gamma=artan\frac{d_{2}sin\alpha}{d_{1}-d_{2}cos\alpha}$$

  (5)

  where $d_{1}$ and $d_{2}$ are the depths of two adjacent points belonging to the same ring which can be obtained from the range image. Then we discard small clusters since noise may be contained and it will offer unreliable constraints in optimization.

• •

  Setting condition for nearest neighbor search: Most of the LiDAR-based SLAM perform Nearest Neighbor Search (NNS) to find the nearest neighbor point in the previous frame or local map for each point in the current scan during each iteration, which is the most time-consuming step during the whole optimization process. Considering that when the optimization process is about to end, chances are that the result of the current NNS is the same as the result of the previous iteration since the optimization result tends to converge, which leads to unnecessary searches. In this paper, we propose to skip this round of search and adopt the result of the previous round of search once $\Delta\bm{\chi}_{ki}<{\alpha}\Delta\bm{\chi}_{thres}$, where $\Delta\bm{\chi}_{ki}$ is the incremental iteration result of the $i^{th}$ round at timestamp $t_{k}$. $\Delta\bm{\chi}_{thres}$ is the convergence threshold of iteration and $\alpha$ is a ratio which can be set as 2.

• •

  Replacing kd-Tree with ikd-Tree: The data structure used for NNS is usually kd-Tree. However, it does not support incremental updates, which means that after building kd-Tree for the current frame point cloud, new points can be added to the frame point cloud only by building a new kd-Tree, which adds additional computing overhead. Therefore, we adopt incremental kd-Tree (ikd-Tree) proposed by Fast-LIO2 [11], which can incrementally insert and delete points and be automatically re-balanced by partial re-building which enables efficient NNS in later stages.

Experiments on LIO-SAM, LVI-SAM using different cameras, and our proposed EMV-LIO using different combinations of cameras were conducted and Relative Pose Errors (RPE) described in [19] were adopted to evaluate the performances. The RPE information estimated by different methods is summarized in Table II. Since there are many challenging scenes in the two sequences like fast motion or being occluded by the wall, the failure of VIO is likely to occur in LVI-SAM depending on a monocular camera so that a wrong initial guess may be provided for LIO, leading to a worse result compared with LIO-SAM. Nevertheless, for EMV-LIO using multiple cameras proposed in this paper, when one of the above cameras fails due to inadequate trackable features caused by challenging scenes, the feature points tracked by the other cameras are relatively stable at this time, which can provide more accurate visual-inertial odometry result for the LiDAR-inertial odometry as a priori, thus improving the robustness of the overall system, which is a vivid annotation of Do not put all your eggs into one basket. In addition, the introduction of more visual re-projection error items during optimization can bring a certain degree of accuracy improvement.

We divide the LIO subsystem into two main modules, which are the prehandle module and the optimization module. In order to evaluate the efficiency performance, we report the number of processing points in the prehandle module, the time usage of NNS, the time usage of extracting keyframes in the optimization module, and the total time usage and the CPU utilization percentage of the two modules in the LIO subsystem while the total time usage and the CPU utilization percentage of the VIO subsystem are also recorded. For the sake of convenience, different methods will be defined as follows:

Method A: The original LVI-SAM [4].

Method B: The method with the addition of removing LiDAR’s noise via range image based on Method A

Method C: The method with the addition of setting condition for nearest neighbor search based on Method B

Method D: The method with the addition of replacing based on Method C.

The efficiency information based on different methods is summarized in Table III. The result of Method A and Method B indicates that removing LiDAR’s noise via range image can reduce the number of processing points at the cost of increased time consumption in prehandle step, which can decrease the time consumption in optimization. As shown in Fig. 5 circled by white ovals, the point cloud of the bamboo grove after noise removal is obviously clearer than that of before. Nevertheless, it tends to underperform in reconstructing objects at high places circled by black ovals for instance, since their values in range image change greatly and will be treated as different kinds of objects during clustering and then filtered out. The introduction of setting condition for nearest neighbor search can reduce the search times to help improve the efficiency of the whole system, which can be concluded from the comparison between Method B and Method C. Besides, replacing kd-Tree with ikd-Tree can significantly decrease the CPU utilization percentage and the optimization time consumption. Since the emphasis is only put on the efficiency improvement of the LIO subsystem, the total time usage and the CPU utilization percentage of the VIO subsystem remain the same throughout the experiment.

The relative odometry frame average rate based on different methods is summarized in Table IV. We set the odometry frame average rate of LIO-SAM as the baseline and report the relative values compared with it. The result of LIO-SAM and Method A shows that the introduction of visual observation can reduce the odometry frame rate intuitively. However, our proposed method can help compensate for the reduction and even perform better than LIO-SAM which does not take visual observation into account.