Localization of Autonomous Vehicles: Proof of Concept for A Computer Vision Approach (2104.02785v1)

Abstract: This paper introduces a visual-based localization method for autonomous vehicles (AVs) that operate in the absence of any complicated hardware system but a single camera. Visual localization refers to techniques that aim to find the location of an object based on visual information of its surrounding area. The problem of localization has been of interest for many years. However, visual localization is a relatively new subject in the literature of transportation. Moreover, the inevitable application of this type of localization in the context of autonomous vehicles demands special attention from the transportation community to this problem. This study proposes a two-step localization method that requires a database of geotagged images and a camera mounted on a vehicle that can take pictures while the car is moving. The first step which is image retrieval uses SIFT local feature descriptor to find an initial location for the vehicle using image matching. The next step is to utilize the Kalman filter to estimate a more accurate location for the vehicle as it is moving. All stages of the introduced method are implemented as a complete system using different Python libraries. The proposed system is tested on the KITTI dataset and has shown an average accuracy of 2 meters in finding the final location of the vehicle.

• The paper presents a proof-of-concept that uses a single camera for visual localization, eliminating the need for complex sensors.
• It employs a two-step approach with SIFT-based image retrieval followed by Kalman filtering to refine vehicle location estimates.
• The method achieves an average localization accuracy of approximately 2 meters, offering a viable alternative in GPS-denied environments.

This paper (2104.02785) presents a proof-of-concept for a visual localization method for autonomous vehicles (AVs) using minimal hardware: specifically, only a single camera. The method aims to provide an alternative or backup localization system when GPS is unreliable (e.g., in urban canyons or tunnels) without requiring complex sensors like lidar or high-precision IMUs. The core idea is to use computer vision techniques to match current camera images to a pre-built database of geotagged images and then refine the location estimate using a statistical filter.

The proposed system follows a two-step approach:

1. Image Retrieval: Given a query image taken by the vehicle's camera, the system searches a database of geotagged images to find the one that depicts the most similar scene. The geotag of the best-matching image provides an initial, approximate location for the vehicle.
2. Location Estimation using Kalman Filter: The sequence of approximate locations obtained from image retrieval over time is combined with a simple motion model (constant velocity) using a Kalman filter to produce a more accurate and smoothed estimate of the vehicle's location.

Here's a breakdown of the implementation details and practical considerations discussed in the paper:

1. Image Retrieval (Offline and Online Stages)

• Offline Stage: A database of geotagged images is created. For each image in the database, local features are extracted and stored. The paper uses the Scale-Invariant Feature Transform (SIFT) algorithm [14] for this purpose. SIFT extracts distinctive keypoints and their 128-dimensional descriptors, which are robust to changes in scale, rotation, and illumination. Only these keypoints and descriptors (along with their associated geotags) need to be stored, not the original images, which is efficient for storage.
• Online Stage: When a query image is captured by the vehicle's camera, SIFT is applied to extract its keypoints and descriptors. These query descriptors are then compared against the descriptors in the database.
• Matching Criteria: To determine if a query keypoint matches a database keypoint, two criteria are used based on the descriptor similarity:
  • Distance Ratio Test: The ratio of the distance to the closest match and the distance to the second closest match in the database must be below a threshold $T_1$. This helps filter out ambiguous matches.
  • Cosine Similarity: The cosine similarity between the query descriptor and the database descriptor must be above a threshold $T_2$.
  • $d_2(g, f_{fast}) / d_2(g, f_{2nd}) < T_1$ (Eq. 1)
  • $\cos(g, f) = \frac{f^T g}{\|f\| \|g\|} > T_2$ (Eq. 2)
  • where $g$ is the query descriptor, $f_{fast}$ is the descriptor of the closest match in the database, and $f_{2nd}$ is the descriptor of the second closest match.
• Finding the Best Match: The database image with the highest number of keypoint correspondences to the query image is selected as the best match. Its geotag is used as the approximate location measurement.
• Efficiency Considerations: Searching a large database can be computationally expensive. Practical methods to improve efficiency include:
  • Limiting the search space based on the vehicle's last known location (e.g., from the previous visual localization estimate or a fallback GPS reading). The paper demonstrates limiting the search to images within a ±20-second time window relative to the match of the previous query image.
  • Using advanced search algorithms (e.g., k-d trees, hierarchical clustering) for faster descriptor matching, although the paper primarily focuses on demonstrating the core concept rather than optimizing large-scale search.

2. Location Estimation using Kalman Filter

• Purpose: The approximate locations from image retrieval are noisy and discrete. The Kalman filter is used to integrate these measurements over time and estimate a more continuous and accurate vehicle state.
• Kalman Filter Model: A simple linear Kalman filter is employed, assuming a constant velocity motion model.
  • State Vector: $X_k = [x_k, y_k, \dot{x}_k, \dot{y}_k]^T$, representing the vehicle's location (latitude $x$, longitude $y$) and velocities in the $x$ and $y$ directions at time step $k$.
  • State Transition Model ($F_k$): Describes how the state evolves over time. With constant velocity, the location at $k$ is the location at $k-1$ plus velocity times time step $\Delta t$.
  • $$F_k = \begin{pmatrix} 1 & 0 & \Delta t & 0 \ 0 & 1 & 0 & \Delta t \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{pmatrix}$$ (Eq. 6)
  • Measurement Model ($H_k$): Relates the state vector to the observation (measurement). The measurement $y_k$ is the approximate location $[x_k, y_k]^T$ obtained from image retrieval.
  • $$H_k = \begin{pmatrix} 1 & 0 & 0 & 0 \ 0 & 1 & 0 & 0 \end{pmatrix}$$ (Eq. 7)
  • Noise: The process noise ($W_k$) and measurement noise ($V_k$) are assumed to be independent, white, and normally distributed with covariance matrices $Q_k$ and $R_k$, respectively. The paper sets $Q_k$ as an identity matrix scaled by a parameter and $R_k$ as a small constant value (0.0001 in decimal degrees).
• Operation: The filter predicts the state at time $k$ based on the state at $k-1$ and then updates the state estimate using the measurement from image retrieval at time $k$, minimizing the mean square error.

Implementation and Testing

• The entire system is implemented in Python.
• Testing is performed using the KITTI dataset [33]. A subset of the KITTI data (images from the right grayscale camera) is used to create the geotagged database, associating images with their timestamps and high-precision GPS/IMU ground truth locations. SIFT keypoints and descriptors are pre-extracted for the database images.
• Query image sequences are created from subsequent frames in the KITTI data.
• Parameters used in the paper: $T_1 = 0.8$, $T_2 = 0.97$, $R = 0.0001$ (in decimal degrees), $Q = I \times q$ (where $q$ is a parameter, initialized based on assumed velocity uncertainty), $\Delta t = 1$ second (interval between query images).
• Results: The system achieved an average final localization accuracy of approximately 2 meters on the tested sequences. The Kalman filter was shown to improve the initial, noisy measurement accuracy (which could be around 16-18 meters) over several time steps (as illustrated in Table 3 and Figure 4). The measurement noise observed (16-18m) is consistent with the distance the vehicle travels in 1 second at typical road speeds, reflecting the discrete nature and inherent inaccuracy of image matching alone.

Practical Implications and Limitations

• The paper demonstrates the feasibility of vision-only localization with minimal hardware, serving as a robust alternative or complement to GPS, especially in challenging environments.
• The achieved accuracy of ~2 meters is promising for certain AV tasks but may not be sufficient for precise lane-level navigation without further refinement or integration with other sensors.
• The current implementation uses a very basic constant velocity model for the Kalman filter, which might not accurately represent real-world vehicle motion.
• The efficiency of image retrieval for a massive database in real-time remains a challenge, although techniques like database partitioning based on location and efficient search algorithms can mitigate this.
• The SIFT feature matching is sensitive to significant changes in viewpoint, lighting, and occlusions.

Future Improvements Suggested (leading to more practical systems)

• Integrate information about the relative pose (rotation and translation) between the query camera and the database image cameras, potentially using 3D information or spatial geometry, rather than just relying on the database camera's location.
• Use a more sophisticated state estimation filter, such as an Extended Kalman Filter (EKF), that can incorporate additional vehicle data like speed, acceleration, and odometry measurements if available.
• Improve image retrieval efficiency and robustness by pre-selecting more consistent and reliable keypoints during the offline database construction. Techniques like RANSAC can be used to find robust inliers when matching database images to their neighbors.

In summary, the paper provides a foundational system architecture and demonstrates that a basic visual localization approach using SIFT feature matching and a Kalman filter can achieve meter-level accuracy with minimal hardware, highlighting its potential for AVs operating in GPS-denied or degraded environments. While the proof-of-concept has limitations for immediate real-world deployment in complex scenarios, it lays out a clear path for future improvements using more advanced computer vision techniques (pose estimation, robust feature selection) and state estimation filters (EKF, particle filters).