LVIF: a lightweight tightly coupled stereo-inertial SLAM with fisheye camera

In the early days of SLAM, most systems used filtering as the back-end optimization. MonoSLAM [14] is the first real-time monocular SLAM system that utilizes Shi-Tomasi points to track very sparse feature points between frames. It also considered the current state of the camera and landmark as state variables. The extended Kalman filter (EKF) is used as the backend to optimize their mean and covariance. Although this approach had several drawbacks, it was a significant milestone at the time as no visual SLAM system could run in real time before MonoSLAM. In addition to EKF as a nonlinear optimization method, when there are outliers in the measurement results, [15] proposes a novel Masreliez–Martin filter to address robust parameter estimation in nonlinear systems with non-Gaussian noise.

Numerous studies [16, 17] have demonstrated that nonlinear optimization methods achieve higher accuracy than filtering methods, albeit at a slightly higher computational cost. Therefore, modern systems rely on MAP estimation, either through geometric MAP that minimizes feature reprojection error in feature-based methods, or through photometric MAP that minimizes the photometric error of a selected set of pixels in direct methods.

In feature-based methods, PTAM [18] is one of the most notable representatives. It selects some representative frames (keyframes) as input for bundle adjustment (BA) to reduce the computational complexity during nonlinear optimization. Additionally, PTAM divides the entire system into two parallel threads, one for camera pose tracking and the other for mapping. This approach enables PTAM to achieve short-term and medium-term data association. ORB-SLAM [19, 20] is one of the most prominent successors of PTAM. It supports monocular, stereo, and RGB-D sensor setups, and employs ORB features throughout the entire pipeline. The system is divided into three parallel threads for tracking, local mapping, and loop closure, representing short-term, medium-term, and long-term data association, respectively. In the short-term, ORB features are used to calculate the pose between frames. In the medium-term data association stage, keyframes and map points are utilized to minimize reprojection error with BA, refining the map. Finally, the loop closure thread employs the bag-of-words library DBoW2 [21] to achieve long-term data association. These methods result in ORB-SLAM achieving excellent accuracy.

In some large-scale scenarios, where there are a significant number of keyframes, the computational complexity of BA optimization can become substantial. Therefore, a common approach is to limit the number of keyframes used in the back-end optimization. Reference [22] proposes a new method called sliding window BA, which fixes the number of keyframes optimized for each back-end optimization. Reference [23] presents a double-window optimization with a co-visibility graph. Another approach presented in [24] is an RGB-D SLAM system designed for large-scale scenes. This system utilizes GPU acceleration to extract image features and track them at a very fast frame rate. Loop detection is implemented using pose-graph optimization with similarity constraints. These methods collectively contribute to more efficient and real-time performance of the SLAM system in large-scale scenes.

While feature-based methods can achieve good accuracy, the sparse distribution of feature points in each frame often leads to a sparse map representation. To address this limitation, direct method SLAM has been proposed as an alternative, aiming to construct a dense or semi-dense map. This approach leverages the intensity or photometric information from a set of selected pixels to build a more detailed and rich map representation. LSD-SLAM [8] is the most representative direct method, which constructs a large-scale semi-dense map using the optical flow method. This method initializes the depth of pixels with random numbers and normalizes the depth after triangulation. Because no feature points are extracted, only the pose graph can be optimized, which may result in lower accuracy compared to feature-based methods. SVO [25] is short for semi-direct visual odometry. It is a hybrid system that uses the FAST algorithm to extract feature points, tracks them by direct method, and optimizes camera pose and 3D map points by minimizing the reprojection error. The advantage of SVO is its high speed, capable of reaching 100 frames per second even on low-end computing platforms. Another fast semi-direct SLAM algorithm is [26], which uses a feature enhancement module based on subgraphs to extract image feature points more stably. Then, an apparent shape-weighted fusion method is proposed for camera pose estimation. Finally, an incremental dynamic covariance scaling algorithm is used to optimize the error of camera pose estimation. Compared with [8], the system has better accuracy.

Visual-inertial SLAM is an extension of pure visual SLAM that integrates inertial sensors to enhance robustness. By incorporating inertial information, it addresses challenges like fast rotations or poor texture that cannot be reliably tracked by pure visual SLAM. Moreover, this integration is achieved at a lower computational cost. However, inertial sensors also have their limitations. For instance, the measured angular velocity and acceleration may experience drift, which can result in unreliable calculations after multiple integration operations. Therefore, it is crucial to optimize the parameters of the IMU to ensure its reliable use. Some studies, such as [27], assume that the camera and IMU measurements are noise-free, and the external parameters of the IMU and camera are known. However, this assumption ignores the impact of bias on the solution process, which can have an unknown effect on the system’s performance. Reference [28] establishes a linear equation with the external parameter rotation as a variable through the rotation of the IMU and camera coordinate system in the same time period. However, if the IMU is zero and the initialization scale is inaccurate, the system may rely too heavily on the accuracy of the initial pose estimation of monocular SLAM.

The fusion of visual and inertial information can be classified into two main approaches: loosely coupled methods [29, 30]and tightly coupled methods. In loosely coupled methods, IMU is treated as an independent module that aids in the visual-only pose estimates obtained from visual structure from motion. However, this approach does not fully maximize the potential of the IMU. As a result, most current visual–inertial fusion SLAM systems employ tightly coupled methods, such as EKF or graph optimization. These methods jointly optimize camera and IMU measurements, leading to significantly improved accuracy and robustness compared to the loosely coupled approach.

The earliest SLAM system based on the EKF is MSCKF [31]. This system relies on the feature method and uses EKF filtering for optimization. It adds camera poses at different times to the state vector, and the feature points can be observed by multiple cameras, forming geometric constraints between multiple camera states to solve the problem of dimension explosion in optimization. This approach enables MSCKF to achieve accurate and robust performance in real-world scenarios.

Graph optimization or BA techniques can maintain and optimize all measurements to obtain the optimal state estimates. The most representative system is OKVIS [32], which supports both monocular and stereo vision. It is the first system to optimize the inertial measurements based on keyframes, nonlinear optimization, and error propagation models. However, OKVIS does not support re-localization. VINS-Mono [5] is an excellent open-source SLAM system that reduces the complexity of back-end computation using a sliding window, and solves place recognition using the DBoW2 library. The system improves stability using a 4 DoF pose graph with BA in loop closure. Its robust initialization can start the system from an unknown initial state. VINS-Fusion [33] extends VINS-Mono to stereo and stereo-inertial. ORB-SLAM3 [34] is a tightly coupled visual-inertial system that builds upon the success of [19, 20] by incorporating IMU information. It utilizes MAP estimation for IMU initialization after 2 and 15 s of system operation. For medium-term data association, ORB-SLAM3 combines inertial and visual residual terms to perform joint visual–inertial optimization with BA. It also utilizes the DBoW2 library for place recognition, which helps reduce the accumulated drift during long-term data association. Experimental results demonstrate that ORB-SLAM3 achieves higher accuracy.

The direct method has also found good application in visual–inertial SLAM. ROVIO [35] is an SLAM system that uses the direct method and EKF. It has a low computational cost, but its accuracy heavily relies on its parameters, which need to be adjusted for different scenarios. Additionally, it lacks loop closure, which results in significant accumulative drift. VI-DSO [36] extends the DSO [12] system by incorporating inertial information to extend the traditional photometric error optimization. Since scales are not immediately observable, VI-DSO is initialized at any scale, rather than lazily initialized until all variables are optimized. Moreover, it proposes a dynamic marginalization method to improve computational efficiency. These improvements enable VI-DSO to achieve high accuracy and robustness in real-world scenarios.

There is a point in the world reference \(P(X_w,Y_w,Z_w)\). Through SE(3) transformation as shown in Fig. 2, \(P_c(X_c,Y_c,Z_c)\) is the coordinate value of P in the camera referenceProject the \(P_c\) onto the normalized image planeSimilar to the pinhole model, the fisheye lens is also a distortion function of \(\theta \). The Kannala–Brandt model uses a polynomial containing only odd-order terms to describe the distortion process, and the distance r from the image point to the center of the image is compressed to \(r_d\)where \(r_d\) is undistorted projection length.

Through the similarity of the triangle, we can obtain the undistorted coordinate valuewhere \(x^,\), \(y^,\) are undistorted coordinate value in normalized image plane, respectively.

Finally, the pixel coordinate value of P is obtained through the camera internal parameterswhere \(f_x\), \(f_y\), \(c_x\), \(c_y \) are, respectively, camera internal parameters, and u, v are the pixel coordinate values, respectively Figure 3 illustrates the process of ORB feature extraction and matching for fisheye images.

Most SLAM systems use pinhole models for their camera components. In the case of stereo pinhole cameras, the two input images are rectified and their ORB features are extracted. Matching between the rectified left and right images can be done efficiently by searching for the same row as epipolar lines are horizontal. However, this approach is not suitable for fisheye lenses that have an FOV equal to or greater than 180\(^\circ \), as shown in Fig. 4. Rectifying images requires an FOV less than 180 degrees, and many computer vision algorithms assume that the reprojection error is uniform across the image. If the fisheye image is rectified, the resulting images need to be cropped shown in Fig. 4b. This will cause the system to lose the advantages of a large FOV, such as the ability to extract more feature points and improved occlusion robustness.

To overcome these difficulties, we adopt a novel method that treats the stereo-fisheye camera as two monocular cameras, extracting features separately. The camera only needs to be calibrated before the algorithm starts. Since it is not suitable to rectify the fisheye image to ensure that epipolar lines are horizontal, the time complexity of feature point matching may be high. Therefore, the SE(3) transformation between the two monocular cameras significantly reduces the time consumption caused by the matching of feature points. Furthermore, since the fisheye camera contains a large number of feature points, we divide them into close and far points, as described in [20]. The close points can be reliably estimated using disparity in the stereo camera if the depth is less than 40 times the stereo baseline. In this way, LVIF filters out those outliers that might negatively affect the localization accuracy. If the two cameras have a common area, image keypoints can be triangulated directly when they are first seen. The remaining landmarks that are not in the overlapping area can be used as monocular information, which can be triangulated from multiple views.

The FAST feature points are extracted from an 8-scale pyramid model with a scale factor of 1.2. Furthermore, LVIF uses the quadtree algorithm to recursively search for groups of points, and selects the point with the largest corresponding value of the FAST corner point in the neighborhood of local feature points to achieve non-maximum suppression and fast screening. The descriptor is calculated using the BRIEF algorithm to complete the matching of feature points.

From the parameters of inertial measurement between frame \(b_k\) and \(b_{k+1}\), the position, velocity, and rotation of the object are expressed bywhere \(p_{b_k}^\omega \), \(v_{b_k}^\omega \) are, respectively, the position and velocity of \(b_k\) frame in the world reference. \(q_{wb_{k+1}}\) is a quaternion which represents the transformation from \(b_{k+1}\) frame to the world reference, \(\bigotimes \) denotes the transformation operation, \(\omega ^{b_k}\), \(a^{b_k}\) represent angular velocity and acceleration in \(b_k\) frame reference respectively, g is the gravity vector, and t is time variation between frame \(b_k\) and \(b_{k+1}\).

Due to the high sampling frequency of the IMU, the IMU pre-integration method can convert the integration of multiple measurement values into a single one, which improves the calculation efficiency. Transform Eqs. (6–8) according to Eq. (9)From Eqs. (10–12), the IMU pre-integration between frame \(b_k\) and \(b_{k+1}\) can be expressed aswhere \(\Delta p_{b_kb_{k+1}},\Delta v_{b_kb_{k+1}},\Delta q_{b_kb_{k+1}}\) are the position, velocity, and rotation variation, respectively.

In the loop closing thread, the DBoW2 word-bag library is utilized to solve the position recognition problem, which converts the features in keyframes into bag-of-words vectors and queries the DBoW2 database to retrieve the most similar keyframes.

When a loop closure occurs, a portion of the sensor’s trajectory must be in close proximity to a ring structure. This means that if the sensor revisits a previous location, it must be moving in the opposite direction of some of its previous trajectory, as shown in Fig. 6. Taking advantage of this feature, we propose a method for coarse loop detection that fuses IMU information. Figure 6a is a 2D environment, which can be understood as the car running on the ground, and Fig. 6b is a three-dimensional space, representing the flight of the drone. We maintain an unit bearing vector [x, y] or [x,y,z] for 2D or 3D, respectively, which needs to be configured in code file. By analyzing the acceleration and angular velocity of the IMU for each frame, we can determine the direction of the robot’s motion. Specifically, we assign a value of 1 when the robot’s orientation aligns with the positive orientation of the IMU reference frame; otherwise, we assign a value of 0 if the orientation is opposite. By comparing the unit bearing vector maintained in the system, all keyframes that move in the opposite direction to the current keyframe can be identified. To expedite this process, a search threshold radius is set. In [40], a search radius of 10 m was used for Lidar SLAM, but this method is sensitive to point cloud density and depth accuracy. We set the threshold radius r to 35 times the stereo baseline, which prevents the system from missing loops and reduces unnecessary keyframe matching. This is the rough detection method using IMU data. If it is a 3D situation shown in Fig. 6b, the scene of the drone flying, the principle is similar to the 2D case. All the things to do are to expand the circle of radius r into a sphere and look for possible matching keyframes in it. If there are other frames within the threshold, loop detection will start, convert the features into bag-of-words vectors, and retrieve the DBoW2 database. This method can significantly reduce unnecessary calculations and CPU usage as the number of keyframes increases. Furthermore, the longer the system runs, the more evident this advantage becomes.

We select the most representative SLAM systems for each method. ROVIO utilizes the direct method and EKF, while BASALT relies on optical flow and BA optimization. OKVIS is considered a pioneering work in the field of visual–inertial SLAM, based on nonlinear optimization. ORB-SLAM3 is considered the most advanced system currently available. The absolute trajectory error (ATE) is a metric employed to assess the accuracy of trajectory estimation algorithms. It measures the absolute positional discrepancy between corresponding positions in the estimated trajectory and the ground-truth trajectory, typically expressed in meters or another suitable distance unit. In our evaluation, we benchmark our system against state-of-the-art SLAM systems using the ATE metric, and the results are showcased in Table 1. Overall, ROVIO and BASALT rely on direct methods and optical flow without feature point extraction, resulting in limited advantages in localization accuracy. Our aim is to propose an SLAM system that is real time and computationally efficient. The ATE of LVIF is significantly lower than that of the other methods, and comparable to that of ORB-SLAM3. This demonstrates the advantages of LVIF’s medium- and long-term data association method, which enables our system to maintain good localization accuracy under lightweight conditions. In the following sections, we will analyze the performance of LVIF in different sequences and various environments.

The room environment is the simplest of all environments and provides ground-truth data for the entire trajectory. In this sequence, the slow motion of the sensor leads to no tracking failures or re-localization. Moreover, it contains many identical scenes, allowing for multiple loop closings. All systems successfully complete all sequences in this environment. However, compared to other systems, our system and ORB-SLAM3 achieve extremely low RMS ATE (m) errors, demonstrating the system’s robust place recognition and loop closure capabilities. Figure 7 shows that our system almost perfectly coincides with the ground-truth trajectories and achieves very accurate tracking in all three axes of x, y, and z.

In medium indoor environments, the corridor sequences contain corridor and several offices where ground truth are available for the start and end segments. The fisheye camera used in LVIF can extract more feature points, resulting in a semi-dense map, as shown in Fig. 8. The map clearly outlines the entire corridor and office. LVIF’s accuracy is comparable to that of ORB-SLAM3, but it still outperforms several other systems. Multiple loops can be detected in these sequences, and each time a loop is detected, a global BA is performed to correct the poses of all keyframes in the map and reduce the accumulated drift. However, in corridor4 sequence, since the sensors do not revisit the mapped area and cannot take advantage of long-term data association, LVIF is slightly worse than ROVIO.

The Magistrale is sequence in the large hall, with the longest segment being over 900 m. All sequences are indoors and contain a lot of close points, most of which can be used for pose estimation. However, the scenes in these sequences are very long and will cause more accumulated drift if left uncorrected. The pre-integration of the IMU reduces the amount of calculation required and ensures real-time performance. Additionally, the joint optimization of vision and inertia also improves the accuracy of the system. Thanks to these contributions, LVIF achieves very good results in most scenarios, with an error of around 1 m. These sequences only have ground truth available at the beginning and end, resulting in a part of the graph with no fluctuations shown in Fig. 9b. Compared with ORB-SLAM3, the error fluctuation of our system is significantly smaller, and the mean, median, and minimum errors are relatively low (Fig. 9a).

In contrast, in the long outdoor sequences, there are many sky and cloud features which can cause severe accumulated drift. We use the strategy of separating the feature points into close and far categories based on their distance, as shown in Fig. 11. Although all systems presented in Table 1 suffer from severe accumulated drift, our system still achieves better results compared to other systems. This is due to our adoption of the MAP estimates of the inertia to rapidly optimize the parameters of the IMU, aligning the world reference frame with gravity. Furthermore, in the tracking thread, the rapidly optimized parameters replace the constant-velocity model, which increases the robustness when encountering poor textures.

The slides sequences in the TUM dataset are the most challenging environment, as the data collector traversed through a black tubular slide that was almost completely devoid of visual features, as shown in Fig. 10. This proves fatal for purely visual SLAMs, but visual–inertial SLAM systems can overcome this challenge. All systems listed in Table 1 complete the entire sequences, although some systems had larger errors. The inertial module can replace the camera for tracking, provided that the inertial variables are well optimized. Otherwise, serious drift will occur, and it is difficult to correct the drift due to the lack of feature points. When the sensor slips into the dark slide, where no feature points can be extracted, the optimized inertial sensor replaces the camera in a short time to calculate the pose transformation between frames. These sequences only have ground truth available for the start and end segments, and thus, one part of the graph does not show any fluctuations, as depicted in Fig. 12. Compared with ORB-SLAM3, the error fluctuation of our system is significantly smaller, and the mean, median, and minimum errors are relatively low, as shown in Fig. 12b.

As shown in the partial map in Fig. 13, a nearly closed trajectory shape is formed each time a loop closure occurs. We use IMU data to determine the direction of sensor displacement and employ the current keyframe’s camera center (represented in green) as the tangent point to draw a circle. This circle has a threshold radius of 35 times the stereo baseline and is used to segregate potentially matching keyframes.

The radius r is an effective filter that reduces unnecessary computational costs by eliminating many keyframes. To ensure that this approach does not miss any loops, we also conduct related experiments. Figure 14 shows the impact of various radii on loop closure. We used the mode as the evaluation criterion, since it can count the events with the highest probability. Our experiments indicate that when the radius is set to 40 times the stereo baseline, the results are identical to those achieved using a radius of 35 times, demonstrating that 40 times is too large for the TUM VI Benchmark dataset. A smaller radius could be chosen to further reduce CPU utilization. However, using a radius of 30 times resulted in the system missing several loop sequences, indicating that this radius will negatively affect localization accuracy. Therefore, a radius of 35 times the stereo baseline is appropriate, since it ensures accuracy and reduces CPU usage. From a macro- perspective, a larger radius of r is closer to the original DBoW2 calculation method, while a smaller radius can have a significant negative impact on loop detection.

LVIF has a significant advantage over ORB-SLAM3 in terms of CPU usage. In ORB-SLAM3, loop detection is initiated every time a keyframe enters the loop closure thread, and the DBoW2 database is searched to determine whether the robot revisits a previous location. As shown in Fig. 15, the CPU usage of ORB-SLAM3 fluctuates regularly. When the matching between the current frame and the database is successful, the CPU must process the transformations between them and use graph optimization to approximate the optimal solution, which leads to the highest CPU usage. Although LVIF has a similar trend to ORB-SLAM3, the coarse judgment of IMU data means that it is unnecessary to search the DBoW2 database every time, resulting in lower CPU usage. In Fig. 15d, the simple environment of the room2 sequence leads to low CPU usage. As the environment becomes more complex, the CPU usage increases. For instance, Magistrale6 contains indoor corridors and large halls, which are more complex than corridor3 and corridor4.

Table 2 shows the running time of the main operations in the tracking and local mapping thread between the stereo-inertial ORB-SLAM3 and LVIF in different environments.

Compared to ORB-SLAM3, LVIF demonstrates obvious advantages in running in real time with around 50 frames and approximately 8 keyframes per second. The time differences of operations, such as feature extraction and stereo matching, are within the allowable fluctuation range. However, the inertial-only MAP estimation renders the optimization more efficient. In the tracking thread, during feature point tracking, the optimized IMU variables produce an initial guess for camera pose from a very early stage, reducing the time cost for BA optimization, as demonstrated in the Track column of Table 2. Additionally, this approach reduces the burden on the CPU, as shown in Fig. 16. In all sequences, LVIF’s CPU usage is lower, which is very friendly to NUC minicomputers.