With growing demands for accuracy in sensor fusion, increasing attention is being paid to temporal offsets as a source of deterministic error when processing data from multiple devices. Established approaches for the calibration of temporal offsets exploit domain-specific heuristics of common sensor suites and utilize simplifications to circumvent some of the challenges arising when both temporal and spatial parameters are not accurately known a priori. These properties make it difficult to generalize the work to other applications or different combinations of sensors. This work presents a general and principled approach to joint estimation of temporal offsets and spatial transformations between sensors. Our framework exploits recent advances in continuous-time batch estimation and thus exists within the rigorous theoretical framework of maximum likelihood estimation. The derivation is presented without relying on unique properties of specific sensors and, therefore, represents the first general technique for temporal calibration in robotics. The broad applicability of this approach is demonstrated through spatiotemporal calibration of a camera with respect to an inertial measurement unit as well as between a stereo camera and a laser range finder. The method is shown to be more repeatable and accurate than the current state of the art, estimating spatial displacements to millimeter precision and temporal offsets to a fraction of the fastest measurement interval.

M-estimators are the de-facto standard method of robust estimation in robotics. They are easily incorporated into iterative non-linear least-squares estimation and provide seamless and effective handling of outliers in data. However, every M-estimator's robust loss function has one or more tuning parameters that control the influence of different data. The choice of M-estimator and the manual tuning of these parameters is always a source of uncertainty when applying the technique to new data or a new problem. In this paper we develop the concept of self-tuning M-estimators. We first make the connection between many common M-estimators and elliptical probability distributions. This connection shows that the choice of M-estimator is an assumption that the residuals belong to a well-defined elliptical distribution. We exploit this implication in two ways. First, we develop an algorithm for tuning the M-estimator parameters during iterative optimization. Second, we show how to choose the correct M-estimator for your data by examining the likelihood of the data given the model. We fully derive these algorithms and show their behavior on a representative example of visual simultaneous localization and mapping.

Robust, scalable place recognition is a core competency for many robotic applications. However, when revisiting places over and over, many state-of-the-art approaches exhibit reduced performance in terms of computation and memory complexity and in terms of accuracy. For successful deployment of robots over long time scales, we must develop algorithms that get better with repeated visits to the same environment, while still working within a fixed computational budget. This paper presents and evaluates an algorithm that alternates between online place recognition and offline map maintenance with the goal of producing the best performance with a fixed map size. At the core of the algorithm is the concept of a Summary Map, a reduced map representation that includes only the landmarks that are deemed most useful for place recognition. To assign landmarks to the map, we use a scoring function that ranks the utility of each landmark and a sampling policy that selects the landmarks for each place. The Summary Map can then be used by any descriptor-based inference method for constant-complexity online place recognition. We evaluate a number of scoring functions and sampling policies and show that it is possible to build and maintain maps of a constant size and that place-recognition performance improves over multiple visits.

We present a framework for integrating two layers of map which are often required for fully automated operation: metric and semantic. Metric maps are likely to improve with subsequent visitations to the same place, while semantic maps can comprise both permanent and fluctuating features of the environment. However, it is not clear from the state of the art how to update the semantic layer as the metric map evolves. The strengths of our method are threefold: the framework allows for the unsupervised evolution of both maps as the environment is revisited by the robot; it uses vision-only sensors, making it appropriate for production cars; and the human labelling effort is minimised as far as possible while maintaining high fidelity. We evaluate this on two different car parks with a fully automated car, performing repeated automated parking manoeuvres to demonstrate the robustness of the system.

This paper presents a fast collision-detection method for sampling-based motion planners based on bounding volume hierarchies in workspace-time space. By introducing time as an additional dimension to the robot's workspace, the method is able to quickly evaluate time-indexed candidate trajectories for collision with the known future motions of other agents. The approach makes no assumptions on the shape of the objects and is able to handle arbitrary motions. We highlight implementation details regarding the application of the collision detection technique within an online planning framework for automated driving. Furthermore, we give detailed profiling information to show the capability for real-time operation.

Place recognition is a core competency for any visual simultaneous localization and mapping system. Identifying previously visited places enables the creation of globally accurate maps, robust relocalization, and multi-user mapping. To match one place to another, most state-of-the-art approaches must decide a priori what constitutes a place, often in terms of how many consecutive views should overlap, or how many consecutive images should be considered together. Unfortunately, depending on thresholds such as these, limits their generality to different types of scenes. In this paper, we present a placeless place recognition algorithm using a novel vote-density estimation technique that avoids heuristically discretizing the space. Instead, our approach considers place recognition as a problem of continuous matching between image streams, automatically discovering regions of high vote density that represent overlapping trajectory segments. The resulting algorithm has a single free parameter and all remaining thresholds are set automatically using well-studied statistical tests. We demonstrate the efficiency and accuracy of our methodology on three outdoor sequences: A comprehensive evaluation against ground-truth from publicly available datasets shows that our approach outperforms several state-of-the-art algorithms for place recognition.

The online recalibration of multi-sensor systems is a fundamental problem that must be solved before complex automated systems are deployed in situations such as automated driving. In such situations, accurate knowledge of calibration parameters is critical for the safe operation of automated systems. However, most existing calibration methods for multisensor systems are computationally expensive, use installations of known fiducial patterns, and require expert supervision. We propose an alternative approach called infrastructure-based calibration that is efficient, requires no modification of the infrastructure, and is completely unsupervised. In a survey phase, a computationally expensive simultaneous localization and mapping (SLAM) method is used to build a highly accurate map of a calibration area. Once the map is built, many other vehicles are able to use it for calibration as if it were a known fiducial pattern. We demonstrate the effectiveness of this method to calibrate the extrinsic parameters of a multi-camera system. The method does not assume that the cameras have an overlapping field of view and it does not require an initial guess. As the camera rig moves through the previously mapped area, we match features between each set of synchronized camera images and the map. Subsequently, we find the camera poses and inlier 2D-3D correspondences. From the camera poses, we obtain an initial estimate of the camera extrinsics and rig poses, and optimize these extrinsics and rig poses via non-linear refinement. The calibration code is publicly available as a standalone C++ package.

OpenGV is a new C++ library for calibrated realtime 3D geometric vision. It unifies both central and non-central absolute and relative camera pose computation algorithms within a single library. Each problem type comes with minimal and non-minimal closed-form solvers, as well as non-linear iterative optimization and robust sample consensus methods. OpenGV therefore contains an unprecedented level of completeness with regard to calibrated geometric vision algorithms, and it is the first library with a dedicated focus on a unified real-time usage of non-central multi-camera systems, which are increasingly popular in robotics and in the automotive industry. This paper introduces OpenGV's flexible interface and abstraction for multi-camera systems, and outlines the performance of all contained algorithms. It is our hope that the introduction of this open-source platform will motivate people to use it and potentially also include more algorithms, which would further contribute to the general accessibility of geometric vision algorithms, and build a common playground for the fair comparison of different solutions.

Robust, accurate pose estimation and mapping at real-time in six dimensions is a primary need of mobile robots, in particular flying Micro Aerial Vehicles (MAVs), which still perform their impressive maneuvers mostly in controlled environments. This work presents a visual-inertial sensor unit aimed at effortless deployment on robots in order to equip them with robust real-time Simultaneous Localization and Mapping (SLAM) capabilities, and to facilitate research on this important topic at a low entry barrier. Up to four cameras are interfaced through a modern ARM-FPGA system, along with an Inertial Measurement Unit (IMU) providing high-quality rate gyro and accelerometer measurements, calibrated and hardware-synchronized with the images. This facilitates a tight fusion of visual and inertial cues that leads to a level of robustness and accuracy which is difficult to achieve with purely visual SLAM systems. In addition to raw data, the sensor head provides FPGA-pre-processed data such as visual keypoints, reducing the computational complexity of SLAM algorithms significantly and enabling employment on resource-constrained platforms. Sensor selection, hardware and firmware design, as well as intrinsic and extrinsic calibration are addressed in this work. Results from a tightly coupled reference visual-inertial motion estimation framework demonstrate the capabilities of the presented system.

New applications of mobile robotics in dynamic urban areas require more than the single-session geometric maps that have dominated simultaneous localization and mapping (SLAM) research to date; maps must be updated as the environment changes and include a semantic layer (such as road network information) to aid motion planning in dynamic environments. We present an algorithm for long-term localization and mapping in real time using a three-dimensional (3D) laser scanner. The system infers the static or dynamic state of each 3D point in the environment based on repeated observations. The velocity of each dynamic point is estimated without requiring object models or explicit clustering of the points. At any time, the system is able to produce a most-likely representation of underlying static scene geometry. By storing the time history of velocities, we can infer the dominant motion patterns within the map. The result is an online mapping and localization system specifically designed to enable long-term autonomy within highly dynamic environments. We validate the approach using data collected around the campus of ETH Zurich over seven months and several kilometers of navigation. To the best of our knowledge, this is the first work to unify long-term map update with tracking of dynamic objects.

This work presents a novel approach to spatio-temporal calibration of a laser range finder (LRF) with respect to a combination of a stereo camera and an inertial measurement unit (IMU). Spatial calibration between an LRF and a camera has been extensively studied, but so far the temporal relationship between the two has largely been neglected. While this may be sufficient for applications where the setup is mounted on a vehicle, which imposes bounds on the dynamics, we aim for employment on a hand-held scanning device, where angular velocities can easily exceed hundreds of degrees per second. Employing a continuous-time batch estimation framework, this work demonstrates that the transformation between the LRF and the visual/inertial setup-but also its temporal relationship-can be estimated accurately. In contrast to the majority of established calibration approaches, our approach does not require an overlap in the field of view of the LRF and camera, allowing for previously infeasible sensor configurations to be calibrated. Preliminary results for a novel hand-held scanning device suggest improvements in 3D reconstructions and image based point cloud coloring, especially for highly dynamic motions.

In this paper, we provide specific and practical approaches to associate uncertainty with 4 ×4 transformation matrices, which is a common representation for pose variables in 3-D space. We show constraint-sensitive means of perturbing transformation matrices using their associated exponential-map generators and demonstrate these tools on three simple-yet-important estimation problems: 1) propagating uncertainty through a compound pose change, 2) fusing multiple measurements of a pose (e.g., for use in pose-graph relaxation), and 3) propagating uncertainty on poses (and landmarks) through a nonlinear camera model. The contribution of the paper is the presentation of the theoretical tools, which can be applied in the analysis of many problems involving 3-D pose and point variables.

Rolling Shutter (RS) cameras are used across a wide range of consumer electronic devices-from smart-phones to high-end cameras. It is well known, that if a RS camera is used with a moving camera or scene, significant image distortions are introduced. The quality or even success of structure from motion on rolling shutter images requires the usual intrinsic parameters such as focal length and distortion coefficients as well as accurate modelling of the shutter timing. The current state-of-the-art technique for calibrating the shutter timings requires specialised hardware. We present a new method that only requires video of a known calibration pattern. Experimental results on over 60 real datasets show that our method is more accurate than the current state of the art.

This paper introduces two novel solutions to the generalized-camera exterior orientation problem, which has a vast number of potential applications in robotics: (i) a minimal solution requiring only three point correspondences, and (ii) gPnP, an efficient, non-iterative n-point solution with linear complexity in the number of points. Already existing minimal solutions require exhaustive algebraic derivations. In contrast, our novel minimal solution is solved in a straightforward manner using the Gröbner basis method. Existing n-point solutions are mostly based on iterative optimization schemes. Our n-point solution is non-iterative and outperforms existing algorithms in terms of computational efficiency. Our results present an evaluation against state-of-the-art single-camera algorithms, and a comparison of different multi-camera setups. It demonstrates the superior noise resilience achieved when using multi-camera configurations, and the efficiency of our algorithms. As a further contribution, we illustrate a possible robotic use-case of our non-perspective orientation computation algorithms by presenting visual odometry results on real data with a non-overlapping multi-camera configuration, including a comparison to a loosely coupled alternative.

In order to increase accuracy and robustness in state estimation for robotics, a growing number of applications rely on data from multiple complementary sensors. For the best performance in sensor fusion, these different sensors must be spatially and temporally registered with respect to each other. To this end, a number of approaches have been developed to estimate these system parameters in a two stage process, first estimating the time offset and subsequently solving for the spatial transformation between sensors. In this work, we present on a novel framework for jointly estimating the temporal offset between measurements of different sensors and their spatial displacements with respect to each other. The approach is enabled by continuous-time batch estimation and extends previous work by seamlessly incorporating time offsets within the rigorous theoretical framework of maximum likelihood estimation. Experimental results for a camera to inertial measurement unit (IMU) calibration prove the ability of this framework to accurately estimate time offsets up to a fraction of the smallest measurement period.

Future requirements for drastic reduction of CO2production and energy consumption will lead to significant changes in the way we see mobility in the years to come. However, the automotive industry has identified significant barriers to the adoption of electric vehicles, including reduced driving range and greatly increased refueling times. Automated cars have the potential to reduce the environmental impact of driving, and increase the safety of motor vehicle travel. The current state-of-the-art in vehicle automation requires a suite of expensive sensors. While the cost of these sensors is decreasing, integrating them into electric cars will increase the price and represent another barrier to adoption. The V-Charge Project, funded by the European Commission, seeks to address these problems simultaneously by developing an electric automated car, outfitted with close-to-market sensors, which is able to automate valet parking and recharging for integration into a future transportation system. The final goal is the demonstration of a fully operational system including automated navigation and parking. This paper presents an overview of the V-Charge system, from the platform setup to the mapping, perception, and planning sub-systems.

We present a generic algorithm for self calibration of robotic systems that utilizes two key innovations. First, it uses information theoretic measures to automatically identify and store novel measurement sequences. This keeps the computation tractable by discarding redundant information and allows the system to build a sparse but complete calibration dataset from data collected at different times. Second, as the full observability of the calibration parameters may not be guaranteed for an arbitrary measurement sequence, the algorithm detects and locks unobservable directions in parameter space using a truncated QR decomposition of the Gauss-Newton system. The result is an algorithm that listens to an incoming sensor stream, builds a minimal set of data for estimating the calibration parameters, and updates parameters as they become observable, leaving the others locked at their initial guess. Through an extensive set of simulated and real-world experiments, we demonstrate that our method outperforms state-of-the-art algorithms in terms of stability, accuracy, and computational efficiency.

The European V-Charge project seeks to develop fully automated valet parking and charging of electric vehicles using only low-cost sensors. One of the challenges is to implement robust visual localization using only cameras and stock vehicle sensors. We integrated four monocular, wide-angle, fisheye cameras on a consumer car and implemented a mapping and localization pipeline. Visual features and odometry are combined to build and localize against a keyframe-based three dimensional map. We report results for the first stage of the project, based on two months worth of data acquired under varying conditions, with the objective of localizing against a map created offline.

In this paper a generic framework for sampling-based partial motion planning along a reference path is presented. The sampling mechanism builds on the specification of a vehicle model and a control law, both of which are freely selectable. Via a closed-loop forward simulation, the vehicle model is regulated onto a carefully chosen set of terminal states aligned with the reference path, generating system-compliant sample trajectories in accordance with the specified system and environmental constraints. The consideration of arbitrary state and input limits make this framework appealing to nonholonomic systems. The rich trajectory set is evaluated in an online sampling-based planning framework, targeting realtime motion planning in dynamic environments. In an example application, a Volkswagen Golf is modeled via a kinodynamic single-track system that is further constrained by steering angle/rate and velocity/acceleration limits. Control is implemented via state-feedback onto piecewise C0-continuous reference paths. Experiments demonstrate the planner's applicability to online operation, its ability to cope with discontinuous reference paths as well as its capability to navigate in a realistic traffic scenario.

Roboticists often formulate estimation problems in discrete time for the practical reason of keeping the state size tractable. However, the discrete-time approach does not scale well for use with high-rate sensors, such as inertial measurement units or sweeping laser imaging sensors. The difficulty lies in the fact that a pose variable is typically included for every time at which a measurement is acquired, rendering the dimension of the state impractically large for large numbers of measurements. This issue is exacerbated for the simultaneous localization and mapping (SLAM) problem, which further augments the state to include landmark variables. To address this tractability issue, we propose to move the full maximum likelihood estimation (MLE) problem into continuous time and use temporal basis functions to keep the state size manageable. We present a full probabilistic derivation of the continuous-time estimation problem, derive an estimator based on the assumption that the densities and processes involved are Gaussian, and show how coefficients of a relatively small number of basis functions can form the state to be estimated, making the solution efficient. Our derivation is presented in steps of increasingly specific assumptions, opening the door to the development of other novel continuous-time estimation algorithms through the application of different assumptions at any point. We use the SLAM problem as our motivation throughout the paper, although the approach is not specific to this application. Results from a self-calibration experiment involving a camera and a high-rate inertial measurement unit are provided to validate the approach.

Visual Teach and Repeat (VT&R) has proven to be an effective method to allow a vehicle to autonomously repeat any previously driven route without the need for a global positioning system. One of the major challenges for a method that relies on visual input to recognize previously visited places is lighting change, as this can make the appearance of a scene look drastically different. For this reason, passive sensors, such as cameras, are not ideal for outdoor environments with inconsistent/inadequate light. However, camera-based systems have been very successful for localization and mapping in outdoor, unstructured terrain, which can be largely attributed to the use of sparse, appearance-based computer vision techniques. Thus, in an effort to achieve lighting invariance and to continue to exploit the heritage of the appearance-based vision techniques traditionally used with cameras, this paper presents the first VT&R system that uses appearance-based techniques with laser scanners for motion estimation. The system has been field tested in a planetary analogue environment for an entire diurnal cycle, covering more than 11km with an autonomy rate of 99.7% of the distance traveled.

Cameras have emerged as the dominant sensor modality for localization and mapping in three-dimensional, unstructured terrain, largely due to the success of sparse, appearance-based techniques, such as visual odometry. However, the Achilles' heel for all camera-based systems is their dependence on consistent ambient lighting, which poses a serious problem in outdoor environments that lack adequate or consistent light, such as the Moon. Actively illuminated sensors on the other hand, such as a light detection and ranging (lidar) device, use their own light source to illuminate the scene, making them a favourable alternative in light-denied environments. The purpose of this paper is to demonstrate that the largely successful appearance-based methods traditionally used with cameras can be applied to laser-based sensors, such as a lidar. We present two experiments that are vital to understanding and enabling appearance-based methods for lidar sensors. In the first experiment, we explore the stability of a representative keypoint detection and description algorithm on both camera images and lidar intensity images collected over a 24 hour period. In the second experiment, we validate our approach by implementing visual odometry based on sparse bundle adjustment on a sequence of lidar intensity images.

This paper describes the design and testing of a technique to enable long-range autonomous navigation using a stereo camera as the only sensor. During a learning phase, the rover is piloted along a route capturing stereo images. The images are processed into a manifold map of topologically-connected submaps that may be used for localization during an autonomous repeat traverse. Path following in non-planar terrain is handled by moving from localization in three dimensions, to path following in two dimensions using a local ground plane associated with each submap. The use of small submaps decouples the computational complexity of route repeating from the length of the path. We validate the algorithm by demonstrating its performance on a difficult three-dimensional route. Using this technique, a rover may autonomously traverse a multi-kilometer route in unstructured, three-dimensional terrain, without an accurate global reconstruction.

Visual teach-and-repeat navigation enables long-range rover autonomy without solving the simultaneous localization and mapping problem or requiring an accurate global reconstruction. During a learning phase, the rover is piloted along a route, logging images. After post-processing, the rover is able to repeat the route in either direction any number of times. This paper describes and evaluates the localization algorithm at the core of a teach-and-repeat system that has been tested on over 32 kilometers of autonomous driving in an urban environment and at a planetary analog site in the High Arctic. We show how a stereo visual odometry pipeline can be extended to become a mapping and localization system, then evaluate the performance of the algorithm with respect to accuracy, robustness to path-tracking error, and the effects of lighting.

Global localization of a planetary-exploration rover in the absence of a satellite-based global positioning system (GPS) is still an open problem. Although a satellite network is not available for localization around any near-term exploration targets, topographic maps derived from satellite imagery are available. This has spurred the development of several algorithms that perform global localization by matching data collected from onboard sensors to a global digital elevation map (DEM). This paper reviews two of these algorithms-Multiple-frame Odometry-compensated Global Alignment (MOGA) and VIsual Position Estimation for Rovers (VIPER)-and compares their performance on a common dataset, collected in a planetary analog environment. The comparison demonstrates the common factors limiting the performance of these algorithms, but also highlights the benefits and drawbacks of each method. Overall, the MOGA algorithm performed significantly better; however, running both algorithms is seen to be the best option as the computational cost of VIPER is low and it may succeed in some situations wherein MOGA will fail.