This paper proposes G-VIDO, a vehicle dynamics, and intermittent Global Navigation Satellite System (GNSS)-aided visual-inertial state estimator, to address the state estimation problem of autonomous vehicle localization (i.e., position and orientation estimation in the global coordinate system) under various GNSS states. A dynamics pre-integration theory is proposed on the basis of a two-degree-of-freedom (DOF) vehicle dynamics model, and dynamics constraints are built in the optimization back-end, considering the unobservable problem of the monocular visual-inertial system under degenerate motions. The proposed highly nonlinear system can be robustly initialized by loosely aligning the monocular structure from motion (SfM) results, pre-integrated IMU measurements, and vehicle motion information. GNSS is used for reference frame transformation and constraint construction in the sliding window. The cumulative error can be corrected with the aid of GNSS, and the vehicle’s position in the global coordinate system can be determined. A GNSS anomaly detection algorithm is proposed to improve the system robustness under intermittent GNSS. Experiments have shown that G-VIDO can provide real-time, robust, and seamless localization in multiple GNSS states, with an RMSE of less than 30 cm (with GNSS). Moreover, we proved that the initialization and local odometry modules in G-VIDO outperform several state-of-the-art VIO systems and our preliminary work VINS-Vehicle.

Ego-pose estimation and 3D object tracking are two critical problems that autonomous driving systems must solve. The solutions to these problems are usually based on their respective assumptions, i.e., the static world assumption for simultaneous localization and mapping (SLAM) and the accurate ego-pose assumption in object tracking. However, these assumptions may not hold in dynamic road scenarios, where SLAM and object tracking are closely correlated. Therefore, we propose DL-SLOT, a tightly-coupled dynamic LiDAR SLAM and 3D object tracking method that addresses both problems simultaneously. This method integrates the state estimations of both the autonomous vehicle and the stationary and dynamic objects in the environment in a unified optimization framework. First, we use object detection to identify all points belonging to potentially dynamic objects. Subsequently, LiDAR odometry is performed using the filtered point cloud. Simultaneously, we propose a sliding window-based 3D multi-object tracking method considering the historical trajectories of the tracked objects. The states of the ego-vehicle, stationary objects, and dynamic objects are jointly estimated by the sliding window-based collaborative graph optimization to achieve SLOT. True stationary objects are restored from the potentially dynamic object set to join the optimization. Finally, a global pose graph is implemented to eliminate the accumulated error. Experiments on the KITTI datasets demonstrate that our method achieves better accuracy than SLAM and 3D object tracking baseline methods. This confirms that solving SLAM and object tracking simultaneously is mutually beneficial, and dramatically improves the robustness and accuracy of SLAM and object tracking in dynamic road scenarios.

The localization system is one of the key components of autonomous vehicles. Widely-used GNSS (low-cost) alone can not meet the centimeter accuracy and is unstable in urban environment. Typically LiDAR-based localization system is very computationally demanding due to the usage of the dense point cloud map. In this paper, we propose a novel lightweight LiDAR-based localization system for autonomous vehicle in this paper. The proposed system only relies on lightweight poles and curbs landmark map, which is highly robust and efficient compared to other localization systems. Poles and curbs are selected as landmarks because of their commonality and stability. We novelly propose a Branch-and-Bound (BnB)-based global optimization method to tackle the data association problem of poles. Motion decoupling is adopted to decouple translation and rotation to improve the efficiency of the BnB-based algorithm. Besides, we propose a new local grid map-based representation for curbs to make better use of curb information. Cost functions for pole and curb are defined respectively and then fused for the subsequent non-linear optimization method to obtain the vehicle location. Experiments on KITTI dataset and our self-collected dataset demonstrate the efficiency and accuracy of our system.

Recent deep convolutional neural network-based object detectors have shown promising performance when detecting large objects, but they are still limited in detecting small or partially occluded ones-in part because such objects convey limited information due to the small areas they occupy in images. Consequently, it is difficult for deep neural networks to extract sufficient distinguishing fine-grained features for high-level feature maps, which are crucial for the network to precisely locate small or partially occluded objects. There are two ways to alleviate this problem: the first is to use lower-level but larger feature maps to improve location accuracy and the second is to use context information to increase classification accuracy. In this paper, we combine both methods by first constructing larger and more meaningful feature maps in top-down order and concatenating them and subsequently fusing multilevel contextual information through pyramid pooling to construct context aware features. We propose a unified framework called the Semantic Context Aware Network (SCAN) to enhance object detection accuracy. SCAN is simple to implement and can be trained from end to end. We evaluate the proposed network on the KITTI challenge benchmark and present an improvement of the precision.

Local geometric information, i.e., normal and distribution of points, is crucial for LiDAR-based simultaneous localization and mapping (SLAM) because it provides constraints for data association, which further determines the direction of optimization and ultimately affects the accuracy of localization. However, estimating normal and distribution of points are time-consuming tasks even with the assistance of kdtree or volumetric maps. To achieve fast normal estimation, we look into the structure of LiDAR scan and propose a ring-based fast approximate least squares (Ring FALS) method. With the Ring structural information, estimating the normal requires only the range information of the points when a new scan arrives. To efficiently estimate the distribution of points, we extend the ikd-tree to manage the map in voxels and update the distribution of points in each voxel incrementally while maintaining its consistency with the normal estimation. We further fix the distribution after its convergence to balance the time consumption and the correctness of representation. Based on the extracted and maintained local geometric information, we devise a robust and accurate hierarchical data association scheme where point-to-surfel association is prioritized over point-to-plane. Extensive experiments on diverse public datasets demonstrate the advantages of our system compared to other state-of-the-art methods.

We propose LIO-Vehicle, a new tightly-coupled vehicle dynamics extension of LiDAR inertial odometry (LIO) method that provides highly accurate, robust, and real-time vehicle trajectory estimation. Since most existing LiDAR-based localization methods are not specifically proposed for vehicles, they do not take the motion constraints of ground robots into account. And they may not work well in structure-less areas, such as tunnels and narrow corridors. For LIO in these LiDAR-degraded circumstances, inertial sensors alone are unable to sustain reliable long-term accuracy due to the accumulation of errors without external periodic corrections. Therefore, it is necessary to introduce other low-cost sensors and vehicle motion constraints to build a more accurate and robust odometry algorithm. In this letter, we use wheel speedometer and steering angle sensor measurements to establish a two-degree-of-freedom vehicle dynamics model and then construct a preintegration factor based on the model’s output. At the backend, we add the vehicle dynamics preintegration results, IMU preintegration measurements, and LiDAR odometry results to a factor graph and get the optimized result with the help of sliding window optimization. The experiments show that the proposed method can achieve higher positioning accuracy compared with the existing LiDAR inertial odometry methods and it can significantly mitigate navigation error in harsh areas where environmental features are insufficient for LiDAR odometry.