This paper reports on a system for dynamic obstacle tracking for autonomous vehicles. In this work, we seek to simultaneously estimate both the trajectory of the obstacle and the obstacle's shape. These two tasks are inherently coupled-given only noisy partial views, one cannot accurately estimate the trajectory of an obstacle if its shape is unknown, nor can one estimate its shape without knowing its trajectory. To address this challenge, we note that simultaneous localization and mapping (SLAM), where a robot must build a map of the environment while localizing itself within the map, presents similar challenges. By treating the obstacle's shape as a “map” in the obstacle's moving reference frame, we can formulate the obstacle tracking and shape estimation similarly to SLAM. Additionally, we use a continuous time estimation framework to incorporate sensor data that is collected at a fast rate (e.g., light detection and ranging (LIDAR)). Using these methods, we are able to obtain smooth trajectories and crisp point clouds for tracked obstacles. We test our proposed tracker on real-world data collected by our autonomous vehicle platform and demonstrate that it produces improved results when compared to a standard centroid-based extended Kalman filter (EKF) tracker.

This paper reports on optimization-based methods for producing a sparse, conservative approximation of the dense potentials induced by node marginalization in simultaneous localization and mapping (SLAM) factor graphs. The proposed methods start with a sparse, but overconfident, Chow-Liu tree approximation of the marginalization potential and then use optimization-based methods to adjust the approximation so that it is conservative subject to minimizing the Kullback-Leibler divergence (KLD) from the true marginalization potential. Results are presented over multiple real-world SLAM graphs and show that the proposed methods enforce a conservative approximation, while achieving low KLD from the true marginalization potential.

In many robotic applications, especially long-term outdoor deployments, the success or failure of feature-based image registration is largely determined by changes in lighting. This paper reports on a method to learn visual feature point descriptors that are more robust to changes in scene lighting than standard hand-designed features. We demonstrate that, by tracking feature points in time-lapse videos, one can easily generate training data that captures how the visual appearance of interest points changes with lighting over time. This training data is used to learn feature descriptors that map the image patches associated with feature points to a lower-dimensional feature space where Euclidean distance provides good discrimination between matching and non-matching image patches. Results showing that the learned descriptors increase the ability to register images under varying lighting conditions are presented for a challenging indoor-outdoor dataset spanning 27 mapping sessions over a period of 15 months, containing a wide variety of lighting changes.

This paper reports on a generic factor-based method for node removal in factor-graph simultaneous localization and mapping (SLAM), which we call generic linear constraints (GLCs). The need for a generic node removal tool is motivated by long-term SLAM applications, whereby nodes are removed in order to control the computational cost of graph optimization. GLC is able to produce a new set of linearized factors over the elimination clique that can represent either the true marginalization (i.e., dense GLC) or a sparse approximation of the true marginalization using a ChowLiu tree (i.e., sparse GLC). The proposed algorithm improves upon commonly used methods in two key ways: First, it is not limited to graphs with strictly full-state relative-pose factors and works equally well with other low-rank factors, such as those produced by monocular vision. Second, the new factors are produced in such a way that accounts for measurement correlation, which is a problem encountered in other methods that rely strictly upon pairwise measurement composition. We evaluate the proposed method over multiple real-world SLAM graphs and show that it outperforms other recently proposed methods in terms of Kullback-Leibler divergence. Additionally, we experimentally demonstrate that the proposed GLC method provides a principled and flexible tool to control the computational complexity of long-term graph SLAM, with results shown for 34.9 h of real-world indoor-outdoor data covering 147.4 km collected over 27 mapping sessions spanning a period of 15 months.

This paper reports on a factor-based method for node marginalization in simultaneous localization and mapping (SLAM) pose-graphs. Node marginalization in a pose-graph induces fill-in and leads to computational challenges in performing inference. The proposed method is able to produce a new set of constraints over the elimination clique that can represent either the true marginalization, or a sparse approximation of the true marginalization using a Chow-Liu tree. The proposed algorithm improves upon existing methods in two key ways: First, it is not limited to strictly full-state relative-pose constraints and works equally well with other low-rank constraints such as those produced by monocular vision. Second, the new factors are produced in a way that accounts for measurement correlation, a problem ignored in other methods that rely upon measurement composition. We evaluate the proposed method over several real-world SLAM graphs and show that it outperforms other state-of-the-art methods in terms of Kullback-Leibler divergence.

This paper reports on the use of generic linear constraint (GLC) node removal as a method to control the computational complexity of long-term simultaneous localization and mapping. We experimentally demonstrate that GLC provides a principled and flexible tool enabling a wide variety of complexity management schemes. Specifically, we consider two main classes: batch multi-session node removal, in which nodes are removed in a batch operation between mapping sessions, and online node removal, in which nodes are removed as the robot operates. Results are shown for 34.9 h of real-world indoor-outdoor data covering 147.4 km collected over 27 mapping sessions spanning a period of 15 months.

This paper reports an algorithm for the registration of images with low overlap and low visual feature density- a typical characteristic of down-looking underwater imagery. Our algorithm exploits locally accurate temporal motion-priors and pairwise image correspondences to aggregate semi-rigid sets of sequential images. These sets are then used to search for visual correspondences across sets instead of between individual pairs of images. By simultaneously searching over multiple views, we increase the physical area seen by more than one image, effectively increasing the "field of view" of the image correspondence search. This increases the probability that the area viewed by both sets will contain enough visual features to register the sets. Our algorithm systematically reduces the uncertainty in the motion prior between the two sets resulting in a refined motion prior that is used to geometrically constrain the correspondence search between sets. This geometric constraint allows us to confidently identify local correspondences that would not be possible globally, further increasing our ability to register images in feature poor environments. We present results using a real-world ship hull inspection data set collected by an autonomous underwater vehicle.

This paper reports on a method for tracking a camera system within an a priori known map constructed from co-registered 3D light detection and ranging (LIDAR) and omnidirectional image data. Our method pre-processes the raw 3D LIDAR and camera data to produce a sparse map that can scale to city-size environments. From the original LIDAR and camera data we extract visual features and identify those that are most robust to varying viewpoint. This allows us to include only the visual features that are most useful for localization in the map. Additionally, we quantize the visual features using a vocabulary tree to further reduce the map's file size. We then use vision-based localization to track the vehicle's motion through the map. We present results on urban data collected with Ford Motor Company's autonomous vehicle testbed. In our experiments the map is built using urban data from winter 2009, and localization is performed using data collected in fall 2010 and winter 2011. This demonstrates our algorithm's robustness to temporal changes in the environment.