We propose a novel method for fitting multiple geometric models to multi-structural data via convex relaxation. Unlike greedy methods - which maximise the number of inliers - our approach efficiently searches for a soft assignment of points to geometric models by minimising the energy of the overall assignment. The inherently parallel nature of our approach, as compared to the sequential approach found in state-of-the-art energy minimisation techniques, allows for the elegant treatment of a scaling factor that occurs as the number of features in the data increases. This results in an energy minimisation that, per iteration, is as much as two orders of magnitude faster on comparable architectures thus bringing real-time, robust performance to a wider set of geometric multi-model fitting problems. We demonstrate the versatility of our approach on two canonical problems in estimating structure from images: plane extraction from RGB-D images and homography estimation from pairs of images. Our approach seamlessly adapts to the different metrics brought forth in these distinct problems. In both cases, we report results on publicly available data-sets that in most instances outperform the state-of-the-art while simultaneously presenting run-times that are as much as an order of magnitude faster.

Depth map estimation techniques from cameras often struggle to accurately estimate the depth of large textureless regions. In this work we present a vision-only method that accurately extracts planar priors from a viewed scene without making any assumptions of the underlying scene layout. Through a fast global labelling, these planar priors can be associated to the individual pixels leading to more complete depth-maps specifically over large, plain and planar regions that tend to dominate the urban environment. When these depth-maps are deployed to the creation of a vision only dense reconstruction over large scales, we demonstrate reconstructions that yield significantly better results in terms of coverage while still maintaining high accuracy.

This paper is about discovering and leveraging architectural constraints in large scale 3D reconstructions using laser. Our contribution is to offer a formulation of the problem which naturally and in a unified way, captures the variety of architectural constraints that can be discovered and applied in urban reconstructions. We focus in particular on the case of survey construction with a push broom laser + VO system. Here visual odometry is combined with vertical 2D scans to create a 3D picture of the environment. A key characteristic here is that the sensors pass/sweep swiftly through the environment such that elements of the scene are seen only briefly by cameras and scanned just once by the laser. These qualities make for a an ill-constrained optimisation problem which is greatly aided if architectural constraints can be discovered and appropriately applied. We demonstrate our approach in an end-to-end implementation which discovers salient architectural constraints and rejects false loop closures before invoking an optimisation to return a 3D model of the workspace. We evaluate the precision of this model by comparison to a ground truth provided by a 3rd party professional survey using highend (static) 3D laser scanners.

We propose a calibration method that automatically estimates the extrinsic calibration between a sensor pose-graph from natural scenes. The sensor pose-graph represents a system of sensors comprising of lidars and cameras, without sensor co-visibility constraints. The method addresses the fact that each scene contributes differently to the calibration problem by introducing a diligent scene selection scheme. The algorithm searches over all scenes to extract a subset of exemplars, whose joint optimisation yields progressively better calibration estimates. This non-parametric method requires no knowledge of the physical world, and continuously finds scenes that better constrain the optimisation parameters. We explain the theory, implement the method, and provide detailed performance analyses with experiments on real-world data.

In mobile robotics applications, generation of accurate static maps is encumbered by the presence of ephemeral objects such as vehicles, pedestrians, or bicycles. We propose a method to process a sequence of laser point clouds and back-fill dense surfaces into gaps caused by removing objects from the scene - a valuable tool in scenarios where resource constraints permit only one mapping pass in a particular region. Our method processes laser scans in a three-dimensional voxel grid using the Truncated Signed Distance Function (TSDF) and then uses a Total Variation (TV) regulariser with a Kernel Conditional Density Estimation (KCDE) “soft” data term to interpolate missing surfaces. Using four scenarios captured with a push-broom 2D laser, our technique infills approximately 20 m2 of missing surface area for each removed object. Our reconstruction's median error ranges between 5.64 cm – 9.24 cm with standard deviations between 4.57 cm – 6.08 cm.

In this paper we propose an on-line system that discovers and drives collision-free traversable paths, using a variational approach to dense stereo vision. Our system is light weight, can be run on low cost hardware and is remarkably quick to predict the semantics. In addition to the scene's path affordance it yields a segmentation of the local scene as a composite of distinctive labels - e.g, ground, sky, obstacles and vegetation. To estimate the labels, we combine a very fast and light weight (shallow) image classifier which considers informative feature channels derived from colour images and dense depth maps estimates. Unlike other approaches, we do not use local descriptors around pixel features. Instead, we encompass label-predicted probabilities with a variational approach for image segmentation. Akin to dense depth map estimation, we obtain semantically segmented images by means of convex regularisation. We show how our system can rapidly obtain the required semantics and paths at VGA resolution. Extensive experiments on the KITTI dataset support the robustness of our system to derive collision-free local routes. An accompanied video supports the robustness of the system at live execution in an outdoor experiment.

Automated valet parking services provide great potential to increase the attractiveness of electric vehicles by mitigating their two main current deficiencies: reduced driving ranges and prolonged refueling times. The European research project V-Charge aims at providing this service on designated parking lots using close-to-market sensors only. For this purpose the project developed a prototype capable of performing fully automated navigation in mixed traffic on designated parking lots and GPS-denied parking garages with cameras and ultrasonic sensors only. This paper summarizes the work of the project, comprising advances in network communication and parking space scheduling, multi-camera calibration, semantic mapping concepts, visual localization and motion planning. The project pushed visual localization, environment perception and automated parking to centimetre precision. The developed infrastructure-based camera calibration and semi-supervised semantic mapping concepts greatly reduce maintenance efforts. Results are presented from extensive month-long field tests.

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.

In this paper we present an online approach to segmenting roads on large scale trajectories using only a monocular camera mounted on a car. We differ from popular 2D segmentation solutions which use single colour images and machine learning algorithms that require supervised training on huge image databases. Instead, we propose a novel approach that fuses 3D geometric data with appearance-based segmentation of 2D information in an automatic system. Our contribution is twofold: first, we propagate labels from frame to frame using depth priors of the segmented road avoiding user interaction most of the time; second, we transfer the segmented road labels to 3D laser point clouds. This reduces the complexity of state-of-the-art segmentation algorithms running on 3D Lidar data. Segmentation fails is in only 3% of the cases over a sequence of 13,600 monocular images spanning an urban trajectory of more than 10km.

In this paper we address the problem of dense depth map estimation from sparse noisy range data to reconstruct large heterogeneous outdoor scenes. We propose a surface inpainting solution through energy minimisation with an adaptive selection of surface regularisers among a set of well known convex and non-convex regularisers. In fact, the selection of norm is pivotal with respect to the intrinsic surface characteristics. Our goal is to show how dense interpolation of sparse range data can be leveraged of more exotic and non-convex regularisers such as the log and logTGV [1] which can better capture the scene geometry. In contrast to state of the art solutions, we do not restrict ourselves to this set of norms, instead we search for the most apt norm for each semantically segmented part of the scene. Our energy model selection use Bayesian optimisation to learn the best choice of free parameters. This results in an adaptive model selection and the generalisation of well studied regularisation norms. We conclude with a detailed experimental analysis of our approach using a basis of four norms over a set of challenging outdoor scenes.

This paper is about dense depthmap estimation using a monocular camera in workspaces with extensive textureless surfaces. Current state of the art techniques have been shown to work in real time with an admirable performance in desktop-size environments. Unfortunately, as we show in this paper, when applied to larger indoor environments, performance often degrades. A common cause is the presence of large affine texture-less areas like by walls, floors, ceilings and drab objects such as chairs and tables. These produce noisy and worse still, grossly erroneous initial seeds for the depthmap that greatly impede successful optimisation. We solve this problem via the introduction of a new non-local higher-order regularisation term that enforces piecewise affine constraints between image pixels that are far apart in the image. This property leverages the observation that the depth at the edges of bland regions are often well estimated whereas their inner pixels are deeply problematic. A welcome by-product of our proposed technique is an estimate of the surface normals at each pixel. We will show that in terms of implementation, our algorithm is a natural extension of the often used variational approaches. We evaluate the proposed technique using real datasets for which we have ground truth models.

We propose an automatic, targetless, data-driven, extrinsic calibration method to calibrate push-broom 2D lidars with a multi-camera system. The calibration problem is decoupled into alternating optimisers over two hierarchical levels, where both levels are linked with a penalty term. The lower-level optimises the six degrees-of-freedom (DoF) rigid-body transforms between the lidar and each camera of the multi-camera unit by minimising the Normalised Information Distance between intensity measurements obtained from both sensor modalities. The upper-level minimises a nonlinear least squares error between the lower-level solutions. We describe the theory, implement the method, and provide a detailed performance analysis with experiments on real-world data.

Graphical models jointly with non linear optimization have become the most popular approaches for solving SLAM and Bundle Adjustment problems: using a non linear least squares (NLSQs) description of the problem, these math tools serve to formalize the minimization of an error cost function that relates state variables through relative sensor observations. The simplest case just considers as state variables the locations of the sensor/robot in the environment deriving in a pose graph subproblem. In general, the cost function is based on the L2norm whose principal iterative solutions exploit the sparse connectivity of the corresponding Gaussian Markov Field (GMRF) or the Factor Graph, whose adjacency matrices are given by the fill-in of the Hessian and Jacobian of the cost function respectively. In this paper we propose a novel solution based on the L1norm as a back-end to the pose graph subproblem. In contrast to other NLSQs approaches, we formulate an iterative algorithm inspired directly on the Factor Graph structure to solve for the linearized residual ∥Ax - b∥1. Under the presence of spurious measurements the L1based solution can achieve similar results to the robust Huber norm. Indeed, our main interest in L1optimization is that it opens the door to the set of more robust non-convex Lpnorms where p ≤ 1. Since our approach depends on the minimization of a non differentiable function, we provide the theoretical insights to solve for the L1norm. Our optimization is based on a primal-dual formulation successfully applied for solving variational convex problems in computer vision. We show the effectiveness of the L1norm to produce both a robust initial seed and a final optimized solution on challenging and well known datasets widely used in other state of the art works.

In this paper we describe a Simultaneous Localization and Mapping (SLAM) approach specifically designed to address the communication and computational issues that affect multi-robot systems. Our method utilizes condensed measurements to exchange map information between the robots. These measurements can effectively compress relevant portions of a map in a few data. This results in a substantial reduction of both the data to be transmitted and processed, that renders the system more robust and efficient. As documented by our simulated and real world experiments, these advantages come with a very little decrease in accuracy compared to ideal (but not realistic) methods that share the full data among all the robots.

The Sparse Bundle Adjustment (SBA) algorithm is a widely used method to solve multi-view reconstruction problems in vision. The critical cost of SBA depends on the fill in of the reduced camera matrix whose pattern is known as the Secondary structure of the problem. In centered object applications where a large number of images are taken in a small area the camera matrix obtained when points are eliminated is dense. On the contrary, visual mapping systems where long trajectories are traversed yield sparse matrices. In this paper, we propose a Decomposable Bundle Adjustment (DBA) method which naturally adapts to the fill in pattern of the camera matrix improving the performance on visual mapping systems. The proposed algorithm is able to decompose the normal equations into small subsystems which are ordered in a junction tree structure. To solve the original system, local factorizations of the small dense matrices are passed between clusters in the tree. The DBA algorithm has been tested for simulated and real data experiments for different environment configurations showing good performance.

When solving the Simultaneous Localization and Mapping (SLAM) problem, submapping and graphical methods have shown to be valuable approaches that provide significant advantages over the standard EKF solution: they are faster and can produce more consistent estimates when using local coordinates. In this paper we present CI-Graph, a submapping method for SLAM that uses a graph structure to efficiently solve complex trajectories reducing the computational cost. Unlike other submapping SLAM approaches, we are able to transmit and share information through maps in the graph in a consistent manner by using conditionally independent submaps. In addition, the current submap always summarizes, without further computations, all information available making CI-Graph be an intrinsically “up to date” algorithm. Moreover, the technique is also efficient in memory requirements since it does not need to recover the full covariance matrix. To evaluate CI-Graph performance, the method has been tested using a synthetic Manhattan world and Victoria Park data set.

In this paper, we describe a system that can carry out simultaneous localization and mapping (SLAM) in large indoor and outdoor environments using a stereo pair moving with 6 DOF as the only sensor. Unlike current visual SLAM systems that use either bearing-only monocular information or 3-D stereo information, our system accommodates both monocular and stereo. Textured point features are extracted from the images and stored as 3-D points if seen in both images with sufficient disparity, or stored as inverse depth points otherwise. This allows the system to map both near and far features: the first provide distance and orientation, and the second provide orientation information. Unlike other vision-only SLAM systems, stereo does not suffer from “scale drift” because of unobservability problems, and thus, no other information such as gyroscopes or accelerometers is required in our system. Our SLAM algorithm generates sequences of conditionally independent local maps that can share information related to the camera motion and common features being tracked. The system computes the full map using the novel conditionally independent divide and conquer algorithm, which allows constant time operation most of the time, with linear time updates to compute the full map. To demonstrate the robustness and scalability of our system, we show experimental results in indoor andoutdoor urban environments of 210 m and 140 m loop trajectories, with the stereo camera being carried in hand by a person walking at normal walking speeds of 4--5 km/h.

In this paper, we show thatallprocesses associated with the move-sense-update cycle of extended Kalman filter (EKF) Simultaneous Localization and Mapping (SLAM) can be carried out in timelinearwith the number of map features. We describeDivide and ConquerSLAM, which is an EKF SLAM algorithm in which the computational complexity per step is reduced from$O(n^2)$to$O(n)$, and the total cost of SLAM is reduced from$O(n^3)$to$O(n^2)$. Unlike many current large-scale EKF SLAM techniques, this algorithm computes a solution without relying on approximations or simplifications (other than linearizations) to reduce computational complexity. Also, estimates and covariances are available when needed by data association without any further computation. Furthermore, as the method works most of the time in local maps, where angular errors remain small, the effect of linearization errors is limited. The resulting vehicle and map estimates are more precise than those obtained with standard EKF SLAM. The errors with respect to the true value are smaller, and the computed state covariance is consistent with the real error in the estimation. Both simulated experiments and the Victoria Park dataset are used to provide evidence of the advantages of this algorithm.

In this paper we describe divide and conquer SLAM (D&C SLAM), an algorithm for performing simultaneous localization and mapping using the extended Kalman filter. D&C SLAM overcomes the two fundamental limitations of standard EKF SLAM: 1.) the computational cost per step is reduced from O(n2) to O(n) (the cost full SLAM is reduced from O(n3) to O(n2)); 2.) the resulting vehicle and map estimates have better consistency properties than standard EKF SLAM in the sense that the computed state covariance adequately represents the real error in the estimation. Unlike many current large scale EKF SLAM techniques, this algorithm computes an exact solution, without relying on approximations or simplifications to reduce computational complexity. Also, estimates and covariances are available when needed by data association without any further computation. Empirical results show that, as a bi-product of reduced computations, and without losing precision because of approximations, D&C SLAM has better consistency properties than standard EKF SLAM. Both characteristics allow to extend the range of environments that can be mapped in real time using EKF. We describe the algorithm and study its computational cost and consistency properties.

In this paper we show how to optimize the computational cost and maximize consistency in EKF-based SLAM for large environments. We combine local mapping with map joining in a way that the total cost of computing the final map is minimized compared to full global EKF-SLAM. This solution is not now only shown to be (1) computationally optimal, but in addition, it is empirically shown that (2) it also produces the most consistent environment map. For a given environment size and sensor range, we can determine the optimal size of the local maps required to minimize the total computational cost and maximize map consistency. The motivation of this work is described in a map building experiment in our lab, and the statistical significance of the proposed method is validated using Monte Carlo simulations

In this paper we study the global localization problem in SLAM: the determination of the vehicle location in a previously mapped environment with no other prior information. We show that, using a grid sampling representation of the configuration space, it is possible to evaluate all vehicle location hypotheses in the environment (up to a certain resolution) with a computational cost that is bilinear: linear both in the number of map features and in the number of sensor measurements. We propose a pairing-driven algorithm that considers only individual measurement-feature pairings and thus, in contrast with current correspondence space algorithms, it avoids searching in the exponential correspondence space. It uses a voting strategy that accumulates evidence for each vehicle location hypothesis, assuring robustness to noise in the sensor measurements and environment models. The general nature of the proposed strategy allows the consideration of different types of features and sensor measurements. Using the popular Victoria Park dataset, we compare its performance with location-driven algorithms where the solution space is usually randomly sampled. We show that the proposed pairing-driven technique is computationally more efficient in proportion to the density of features in the environment.