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.

We propose a direct monocular SLAM algorithm based on the Normalised Information Distance (NID) metric. In contrast to current state-of-the-art direct methods based on photometric error minimisation, our information-theoretic NID metric provides robustness to appearance variation due to lighting, weather and structural changes in the scene. We demonstrate successful localisation and mapping across changes in lighting with a synthetic indoor scene, and across changes in weather (direct sun, rain, snow) using real-world data collected from a vehicle-mounted camera. Our approach runs in real-time on a consumer GPU using OpenGL, and provides comparable localisation accuracy to state-of-the-art photometric methods but significantly outperforms both direct and feature-based methods in robustness to appearance changes.

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.

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.

Simultaneous localization and mapping (SLAM) algorithms based on local maps have been demonstrated to be well suited for mapping large environments as they reduce the computational cost and improve the consistency of the final estimation. The main contribution of this paper is a novel submapping technique that does not require independence between maps. The technique is based on the intrinsic structure of the SLAM problem that allows the building of submaps that can share information, remainingconditionally independent. The resulting algorithm obtains local maps in constant time during the exploration of new terrain and recovers the global map in linear time after simple loop closures without introducing any approximations besides the inherent extended Kalman filter linearizations. The memory requirements are also linear with the size of the map. As the algorithm works in a covariance form, well-known data-association techniques can be used in the usual manner. We present experimental results using a handheld monocular camera, building a map along a closed-loop trajectory of 140 m in a public square, with people and other clutter. Our results show that the combination of conditional independence, which enables the system to share the camera and feature states between submaps, and local coordinates, which reduce the effects of linearization errors, allow us to obtain precise maps of large areas with pure monocular SLAM in real time.

This paper presents the benefits of using a low cost inertial measurement unit to aid in an implementation of inverse depth initialized SLAM using a hand-held monocular camera. Results are presented with and without inertial observations for different assumed initial ranges to features on the same dataset. When using only the camera, the scale of the scene is not observable. As expected, the scale of the map depends on the prior used to initialize the depth of the features and may drift when exploring new terrain, precluding loop closure. The results show that the inertial observations help to improve the estimated trajectory of the camera leading to a better estimation of the map scale and a more accurate localization of features.

Local maps algorithms have demonstrated to be well suited for mapping large environments as can reduce the computational cost and improve the consistency of the final estimation. In this paper we present a new technique that allows the use of local mapping algorithms in the context of EKF SLAM but without the constrain of probabilistic independence between local maps. By means of this procedure, salient features of the environment or vehicle state components as velocity or global attitude, can be shared between local maps without affecting the posterior joining process or introducing any undesirable approximations in the final global map estimate. The overload cost introduced by the technique is minimum since building up local maps does not require any additional operations apart from the usual EKF steps. As the algorithm works with covariance matrices, well-known data association techniques can be used in the usual manner. To test the technique, experimental results using a monocular camera in an outdoor environment are provided. The initialization of the features is based on the inverse depth algorithm.

The probability of finding alive a person buried by a snow avalanche decreases dramatically with time. The best chance for the victims is to carry an avalanche beacon or ARVA (from the french: Appareilde Recherche de Victimes d'Avalanche), that transmits a magnetic field that can be detected by the rescuer's ARVA. However, the signals received are difficult to interpret and require people with good training on the actual searching techniques. In this paper we propose to address the search for victims as a SLAM problem: tracking the rescuer location while building a map of the locations of the victims under the snow, using 3D measurements of the magnetic field produced by their ARVAs. Given the high non-linearity of the problem, we propose a technique based on sum of Gaussians (SOGs) and extended Kalman filter. We present preliminary simulation results showing that this technique is faster and more accurate than the classical ones

A person buried by a snow avalanche can be found by measuring the magnetic field generated by an avalanche beacon or ARVA carried by the victim. However, the signals received are difficult to interpret and require people with good training on the actual searching techniques. In this paper we show that the search can be automated using SLAM techniques. The rescuer is equipped with an inertial sensor to estimate its own motion and a triple antenna to obtain 3D measurements of the magnetic field generated by the victim's ARVA. Both measurements are used to build a "robocentric" map that contains the location of the victim relative to the rescuer. To solve this highly non-linear SLAM problem we propose and compare two alternative solutions based on a sum of Gaussians (SOGs) filter and a particle filter. We present simulation results showing that, for comparable computing times, the SOGs solution gives more accurate results

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.