This paper shows how to carry out batch continuous-time trajectory estimation for bodies translating and rotating in three-dimensional (3D) space, using a very efficient form of Gaussian-process (GP) regression. The method is fast, singularity-free, uses a physically motivated prior (the mean is constant body-centric velocity), and permits trajectory queries at arbitrary times through GP interpolation. Landmark estimation can be folded in to allow for simultaneous trajectory estimation and mapping (STEAM), a variant of SLAM.

This paper proposes using hierarchical wavelets as a basis in parametric continuous-time batch estimation. The need for a continuous-time robot pose in the simultaneous localization and mapping (SLAM) problem has arisen as state-of-the-art batch SLAM algorithms attempt to handle more challenging hardware; specifically, the continuous-time framework is particularly beneficial when using high-rate sensors, multiple unsynchronized sensors, or scanning sensors, such as lidar and rolling-shutter cameras, during motion. Although the traditional discrete-time SLAM formulation can be adapted by using temporal pose interpolation, approaches using the continuous-time framework are able to generate smooth robot trajectories with less state variables. In this paper, we focus on the parametric approach using temporal basis functions to develop a finite-element representation of the continuous-time robot trajectory. While the majority of current implementations have utilized a uniformly spaced B-spline basis, we note that trajectory richness is often quite variable; in this paper, we show how a hierarchical system of wavelet basis functions can be used to increase the resolution of the solution only in the temporally local regions of the trajectory that require additional detail. We validate our approach by contrasting uniform B-splines and wavelets in a six-dimensional pose-graph SLAM experiment, using both simulated and real data.

Appearance-based batch nonlinear optimization techniques for simultaneous localization and mapping (SLAM) have been highly successful in assisting robot motion estimation. Traditionally, these techniques are applied in a single privileged coordinate frame, which can become computationally expensive over long distances, particularly when a loop closure requires the adjustment of many pose variables. Recent approaches to the problem have shown that a completely relative coordinate framework can be used to incrementally find a close approximation of the full maximum likelihood solution in constant time. However, due to the nature of these discrete-time techniques, the state size becomes intractable when challenged with high-rate sensors. We propose moving the relative coordinate formulation of SLAM into continuous time by estimating the velocity profile of the robot. We derive the relative formulation of the continuous-time robot trajectory and formulate an estimator for the SLAM problem using temporal basis functions. Although we do not yet take advantage of large-scale loop closures, we intentionally use a relative formulation to set the stage for future work that will close loops in constant time. We show how the estimator can be used in a window-style filter to incrementally find the batch solution in constant time. The estimator is validated on a set of appearance-based feature measurements acquired using a two-axis scanning laser rangefinder over a 1.1km trajectory.

Accurate pose estimation relies on high-quality sensor measurements. Due to manufacturing tolerance, every sensor (camera or lidar) needs to be individually calibrated. Feature-based techniques using simple calibration targets (e.g., a checkerboard pattern) have become the dominant approach to camera sensor calibration. Existing lidar calibration methods require a controlled environment (e.g., a space of known dimension) or specific configurations of supporting hardware (e.g., coupled with GPS/IMU). Leveraging recent state estimation developments based on lidar intensity imagery, this paper presents a calibration procedure for a two-axis scanning lidar using only an inexpensive checkerboard calibration target. In addition, the proposed method generalizes a two-axis scanning lidar as an idealized spherical camera with additive measurement distortions. Conceptually, this is not unlike normal camera calibration in which an arbitrary camera is modelled as an idealized projective (pinhole) camera with tangential and radial distortions. The resulting calibration method, we believe, can be readily applied to a variety of two-axis scanning lidars. We present the measurement improvement quantitatively, as well as the impact of calibration on a 1.1-km visual odometry estimate.

Visual odometry (VO) is a highly efficient and powerful 6D motion estimation technique; state-of-the-art bundle adjustment algorithms now optimize over several frames of temporally tracked, appearance-based features in real time. It is well known that the temporal feature correspondence process is highly prone to mismatches. The standard technique used for outlier rejection in this process is random sample consensus (RANSAC), which is an iterative and non-deterministic process used to find the parameters of a mathematical model that best describe a likely set of inliers. The traditional model used for RANSAC in the visual odometry pipeline is a rigid transformation between two camera poses; this model has long assumed the use of an imaging sensor with a global shutter. In order to use imaging sensors that do not operate with a global shutter, it is proposed that the RANSAC algorithm be modified to use a constant-camera-velocity model. Specifically, this paper investigates the use of a two-axis scanning lidar in the visual-odometry pipeline. Images are formed using lidar intensity data, and due to the scanning-while-moving nature of the lidar, the behaviour of the sensor resembles that of a slow rolling-shutter camera. We formulate a Motion-Compensated RANSAC algorithm that uses a constant-velocity model and the individual timestamp of each extracted feature. The algorithm is validated using 6880 lidar frames with a resolution of 480 × 360, captured at 2 Hz, over a 1.1 km traversal. Our results show that the new algorithm results in far more inlying feature tracks for rolling-shutter-type images and ultimately higher-accuracy VO results.

Copyright © 2009 by the Association for Computing Machinery, Inc. Permission to make digital or hard copies of part or all of this work for personal or classroom use is granted without fee provided that copies are not made or distributed for commercial advantage and that copies bear this notice and the full citation on the first page. Copyrights for components of this work owned by others than ACM must be honored. Abstracting with credit is permitted. To copy otherwise, to republish, to post on servers, or to redistribute to lists, requires prior specific permission and/or a fee.