Probabilistic cooperative mobile robot area coverage and its application to autonomous seabed mapping

There are many applications that require mobile robots to autonomously cover an entire area with a sensor or end effector. The vast majority of the literature on this subject is focused on addressing path planning for area coverage under the assumption that the robot’s pose is known or that error is bounded. In this work, we remove this assumption and develop a completely probabilistic representation of coverage. We show that coverage is guaranteed as long as the robot pose estimates are consistent, a much milder assumption than zero or bounded error. After formally connecting robot sensor uncertainty with area coverage, we propose an adaptive sliding window filter pose estimator that provides a close approximation to the full maximum a posteriori estimate with a computation cost that is bounded over time. Subsequently, an adaptive planning strategy is presented that automatically exploits conditions of low vehicle uncertainty to more efficiently cover an area. We further extend this approach to the multi-robot case where robots can communicate through a (possibly faulty and low-bandwidth) channel and make relative measurements of one another. In this case, area coverage is achieved more quickly since the uncertainty over the robots’ trajectories is reduced. We apply the framework to the scenario of mapping an area of seabed with an autonomous underwater vehicle. Experimental results support the claim that our method achieves guaranteed complete coverage notwithstanding poor navigational sensors and that resulting path lengths required to cover the entire area are shortest using the proposed cooperative and adaptive approach.

Model-Based Real-Time Non-Rigid Tracking

This paper presents a sequential non-rigid reconstruction method that recovers the 3D shape and the camera pose of a deforming object from a video sequence and a previous shape model of the object. We take PTAM (Parallel Mapping and Tracking), a state-of-the-art sequential real-time SfM (Structure-from-Motion) engine, and we upgrade it to solve non-rigid reconstruction. Our method provides a good trade-off between processing time and reconstruction error without the need for specific processing hardware, such as GPUs. We improve the original PTAM matching by using descriptor-based features, as well as smoothness priors to better constrain the 3D error. This paper works with perspective projection and deals with outliers and missing data. We evaluate the tracking algorithm performance through different tests over several datasets of non-rigid deforming objects. Our method achieves state-of-the-art accuracy and can be used as a real-time method suitable for being embedded in portable devices.

Graph-Based Cooperative Localization Using Symmetric Measurement Equations

Precise localization is a key requirement for the success of highly assisted or autonomous vehicles. The diminishing cost of hardware has resulted in a proliferation of the number of sensors in the environment. Cooperative localization (CL) presents itself as a feasible and effective solution for localizing the ego-vehicle and its neighboring vehicles. However, one of the major challenges to fully realize the effective use of infrastructure sensors for jointly estimating the state of a vehicle in cooperative vehicle-infrastructure localization is an effective data association. In this paper, we propose a method which implements symmetric measurement equations within factor graphs in order to overcome the data association challenge with a reduced bandwidth overhead. Simulated results demonstrate the benefits of the proposed approach in comparison with our previously proposed approach of topology factors.

Analytical SLAM without linearization

This paper solves the classical problem of simultaneous localization and mapping (SLAM) in a fashion that avoids linearized approximations altogether. Based on the creation of virtual synthetic measurements, the algorithm uses a linear time-varying Kalman observer, bypassing errors and approximations brought by the linearization process in traditional extended Kalman filtering SLAM. Convergence rates of the algorithm are established using contraction analysis. Different combinations of sensor information can be exploited, such as bearing measurements, range measurements, optical flow, or time-to-contact. SLAM-DUNK, a more advanced version of the algorithm in global coordinates, exploits the conditional independence property of the SLAM problem, decoupling the covariance matrices between different landmarks and reducing computational complexity to O(n). As illustrated in simulations, the proposed algorithm can solve SLAM problems in both 2D and 3D scenarios with guaranteed convergence rates in a full nonlinear context.

SLAM++-A highly efficient and temporally scalable incremental SLAM framework

The most common way to deal with the uncertainty present in noisy sensorial perception and action is to model the problem with a probabilistic framework. Maximum likelihood estimation is a well-known estimation method used in many robotic and computer vision applications. Under Gaussian assumption, the maximum likelihood estimation converts to a nonlinear least squares problem. Efficient solutions to nonlinear least squares exist and they are based on iteratively solving sparse linear systems until convergence. In general, the existing solutions provide only an estimation of the mean state vector, the resulting covariance being computationally too expensive to recover. Nevertheless, in many simultaneous localization and mapping (SLAM) applications, knowing only the mean vector is not enough. Data association, obtaining reduced state representations, active decisions and next best view are only a few of the applications that require fast state covariance recovery. Furthermore, computer vision and robotic applications are in general performed online. In this case, the state is updated and recomputed every step and its size is continuously growing, therefore, the estimation process may become highly computationally demanding.

This paper introduces a general framework for incremental maximum likelihood estimation called SLAM++, which fully benefits from the incremental nature of the online applications, and provides efficient estimation of both the mean and the covariance of the estimate. Based on that, we propose a strategy for maintaining a sparse and scalable state representation for large scale mapping, which uses information theory measures to integrate only informative and non-redundant contributions to the state representation. SLAM++ differs from existing implementations by performing all the matrix operations by blocks. This led to extremely fast matrix manipulation and arithmetic operations used in nonlinear least squares. Even though this paper tests SLAM++ efficiency on SLAM problems, its applicability remains general.

A critique of current developments in simultaneous localization and mapping

The number of research publications dealing with the simultaneous localization and mapping problem has grown significantly over the past 15 years. Many fundamental and practical aspects of simultaneous localization and mapping have been addressed, and some efficient algorithms and practical solutions have been demonstrated. The aim of this paper is to provide a critical review of current theoretical understanding of the fundamental properties of the SLAM problem, such as observability, convergence, achievable accuracy and consistency. Recent research outcomes associated with these topics are briefly discussed together with potential future research directions.

An improved FastSLAM 2.0 algorithm based on FC&ASD-PSO

FastSLAM 2.0 is a popular framework which uses a Rao-Blackwellized particle filter to solve the simultaneous localization and mapping problem. The sampling process is one of the most important phases in the FastSLAM 2.0 framework. Its estimation accuracy depends heavily on a correct prior knowledge about the control and observation noise statistics (the covariance matrices Q and R). Without the correct prior knowledge about these matrices, the estimation accuracy of the robot path and landmark positions may degrade seriously. However in many applications, the prior knowledge is unknown, or these noises are non-stationary. In this paper, these covariance matrices are supposed to be dynamic and denoted as Qt and Rt. Since there are noises, time-adjacent observations are inconsistent with each other. This inconsistency can reflect the real value of the covariance matrices. By the inconsistency, an extra step is introduced to the FastSLAM 2.0 framework. This step makes Qt and Rt match with their real value by using a particle swarm optimization method based on fractional calculus and alpha stable distribution (FC&ASD-PSO). Both simulation and experimental results show that the proposed algorithm improves the accuracy by the more accurate estimation on the noise covariance matrices.

Robotic path-finding in inverse treatment planning for stereotactic radiosurgery with continuous dose delivery

PURPOSE

Continuous dose delivery in radiation therapy treatments has been shown to decrease total treatment time while improving the dose conformity and distribution homogeneity over the conventional step-and-shoot approach. The authors develop an inverse treatment planning method for Gamma Knife® Perfexion™ that continuously delivers dose along a path in the target.

METHODS

The authors' method is comprised of two steps: find a path within the target, then solve a mixed integer optimization model to find the optimal collimator configurations and durations along the selected path. Robotic path-finding techniques, specifically, simultaneous localization and mapping (SLAM) using an extended Kalman filter, are used to obtain a path that travels sufficiently close to selected isocentre locations. SLAM is novelly extended to explore a 3D, discrete environment, which is the target discretized into voxels. Further novel extensions are incorporated into the steering mechanism to account for target geometry.

RESULTS

The SLAM method was tested on seven clinical cases and compared to clinical, Hamiltonian path continuous delivery, and inverse step-and-shoot treatment plans. The SLAM approach improved dose metrics compared to the clinical plans and Hamiltonian path continuous delivery plans. Beam-on times improved over clinical plans, and had mixed performance compared to Hamiltonian path continuous plans. The SLAM method is also shown to be robust to path selection inaccuracies, isocentre selection, and dose distribution.

CONCLUSIONS

The SLAM method for continuous delivery provides decreased total treatment time and increased treatment quality compared to both clinical and inverse step-and-shoot plans, and outperforms existing path methods in treatment quality. It also accounts for uncertainty in treatment planning by accommodating inaccuracies.

Relative continuous-time SLAM

Appearance-based techniques for simultaneous localization and mapping (SLAM) have been highly successful in assisting robot-motion estimation; however, these vision-based technologies have long assumed the use of imaging sensors with a global shutter, which are well suited to the traditional, discrete-time formulation of visual problems. In order to adapt these technologies to use scanning sensors, we propose novel methods for both outlier rejection and batch nonlinear estimation. Traditionally, the SLAM problem has been formulated 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 discrete-time estimators 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. In order to use scanning 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 using temporal basis functions. A motion-compensated outlier rejection scheme is proposed by using a constant-velocity model for the random sample consensus algorithm. Our experimental results use intensity imagery from a two-axis scanning lidar; due to the sensors’ scanning nature, it behaves similarly to a slow rolling-shutter camera. Both algorithms are validated using a sequence of 6880 lidar frames acquired over a 1.1 km traversal.

Simultaneous Localization and Mapping with Iterative Sparse Extended Information Filter for Autonomous Vehicles

In this paper, a novel iterative sparse extended information filter (ISEIF) was proposed to solve the simultaneous localization and mapping problem (SLAM), which is very crucial for autonomous vehicles. The proposed algorithm solves the measurement update equations with iterative methods adaptively to reduce linearization errors. With the scalability advantage being kept, the consistency and accuracy of SEIF is improved. Simulations and practical experiments were carried out with both a land car benchmark and an autonomous underwater vehicle. Comparisons between iterative SEIF (ISEIF), standard EKF and SEIF are presented. All of the results convincingly show that ISEIF yields more consistent and accurate estimates compared to SEIF and preserves the scalability advantage over EKF, as well.

Continuous-time batch trajectory estimation using temporal basis functions

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, rolling-shutter cameras, 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 problem, which further augments the state to include landmark variables. To address this tractability issue, we propose to move the full Maximum-a-Posteriori estimation 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 the 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 simultaneous localization and mapping problem as our motivation throughout the paper, although the approach is not specific to this application. Results from two experiments are provided to validate the approach: (i) self-calibration involving a camera and a high-rate inertial measurement unit, and (ii) perspective localization with a rolling-shutter camera.

Graph Structure-Based Simultaneous Localization and Mapping Using a Hybrid Method of 2D Laser Scan and Monocular Camera Image in Environments with Laser Scan Ambiguity

Localization is an essential issue for robot navigation, allowing the robot to perform tasks autonomously. However, in environments with laser scan ambiguity, such as long corridors, the conventional SLAM (simultaneous localization and mapping) algorithms exploiting a laser scanner may not estimate the robot pose robustly. To resolve this problem, we propose a novel localization approach based on a hybrid method incorporating a 2D laser scanner and a monocular camera in the framework of a graph structure-based SLAM. 3D coordinates of image feature points are acquired through the hybrid method, with the assumption that the wall is normal to the ground and vertically flat. However, this assumption can be relieved, because the subsequent feature matching process rejects the outliers on an inclined or non-flat wall. Through graph optimization with constraints generated by the hybrid method, the final robot pose is estimated. To verify the effectiveness of the proposed method, real experiments were conducted in an indoor environment with a long corridor. The experimental results were compared with those of the conventional GMappingapproach. The results demonstrate that it is possible to localize the robot in environments with laser scan ambiguity in real time, and the performance of the proposed method is superior to that of the conventional approach.

Nonlinear factor recovery for long-term SLAM

For long-term operations, graph-based simultaneous localization and mapping (SLAM) approaches require nodes to be marginalized in order to control the computational cost. In this paper, we present a method to recover a set of nonlinear factors that best represents the marginal distribution in terms of Kullback–Leibler divergence. The proposed method, which we call nonlinear factor recovery (NFR), estimates both the mean and the information matrix of the set of nonlinear factors, where the recovery of the latter is equivalent to solving a convex optimization problem. NFR is able to provide either the dense distribution or a sparse approximation of it. In contrast to previous algorithms, our method does not necessarily require a global linearization point and can be used with any nonlinear measurement function. Moreover, we are not restricted to only using tree-based sparse approximations and binary factors, but we can include any topology and correlations between measurements. Experiments performed on several publicly available datasets demonstrate that our method outperforms the state of the art with respect to the Kullback–Leibler divergence and the sparsity of the solution.

WiFi SLAM algorithms: an experimental comparison

Localization and mapping in indoor environments, such as airports and hospitals, are key tasks for almost every robotic platform. Some researchers suggest the use of Range-Only (RO) sensors based on WiFi (Wireless Fidelity) technology with SLAM (Simultaneous Localization And Mapping) techniques to solve both problems. The current state of the art in RO SLAM is mainly focused on the filtering approach, while the study of smoothing approaches with RO sensors is quite incomplete. This paper presents a comparison between filtering algorithms, such as EKF and FastSLAM, and a smoothing algorithm, the SAM (Smoothing And Mapping). Experimental results are obtained in indoor environments using WiFi sensors. The results demonstrate the feasibility of the smoothing approach using WiFi sensors in an indoor environment.

Concurrent filtering and smoothing: A parallel architecture for real-time navigation and full smoothing

We present a parallelized navigation architecture that is capable of running in real-time and incorporating long-term loop closure constraints while producing the optimal Bayesian solution. This architecture splits the inference problem into a low-latency update that incorporates new measurements using just the most recent states (filter), and a high-latency update that is capable of closing long loops and smooths using all past states (smoother). This architecture employs the probabilistic graphical models of factor graphs, which allows the low-latency inference and high-latency inference to be viewed as sub-operations of a single optimization performed within a single graphical model. A specific factorization of the full joint density is employed that allows the different inference operations to be performed asynchronously while still recovering the optimal solution produced by a full batch optimization. Due to the real-time, asynchronous nature of this algorithm, updates to the state estimates from the high-latency smoother will naturally be delayed until the smoother calculations have completed. This architecture has been tested within a simulated aerial environment and on real data collected from an autonomous ground vehicle. In all cases, the concurrent architecture is shown to recover the full batch solution, even while updated state estimates are produced in real-time.

Online temporal calibration for camera–IMU systems: Theory and algorithms

When fusing visual and inertial measurements for motion estimation, each measurement’s sampling time must be precisely known. This requires knowledge of the time offset that inevitably exists between the two sensors’ data streams. The first contribution of this work is an online approach for estimating this time offset, by treating it as an additional state variable to be estimated along with all other variables of interest (inertial measurement unit (IMU) pose and velocity, biases, camera-to-IMU transformation, feature positions). We show that this approach can be employed in pose-tracking with mapped features, in simultaneous localization and mapping, and in visual–inertial odometry. The second main contribution of this paper is an analysis of the identifiability of the time offset between the visual and inertial sensors. We show that the offset is locally identifiable, except in a small number of degenerate motion cases, which we characterize in detail. These degenerate cases are either (i) cases known to cause loss of observability even when no time offset exists, or (ii) cases that are unlikely to occur in practice. Our simulation and experimental results validate these theoretical findings, and demonstrate that the proposed approach yields high-precision, consistent estimates, in scenarios involving either known or unknown features, with both constant and time-varying offsets.

Coordination strategies for multi-robot exploration and mapping

Situational awareness in rescue operations can be provided by teams of autonomous mobile robots. Human operators are required to teleoperate the current generation of mobile robots for such applications; however, teleoperation is increasingly difficult as the number of robots is expanded. As the number of robots is increased, each robot may also interfere with one another and eventually decrease mapping performance. As presented here, through careful consideration of robot team coordination and exploration strategy, large numbers of mobile robots can be allocated to accomplish the mapping task more quickly and accurately. We present both the coordination and exploration strategies and present results from experiments in simulation as well as with up to nine mobile platforms.

SfM with MRFs: discrete-continuous optimization for large-scale structure from motion

Recent work in structure from motion (SfM) has built 3D models from large collections of images downloaded from the Internet. Many approaches to this problem use incremental algorithms that solve progressively larger bundle adjustment problems. These incremental techniques scale poorly as the image collection grows, and can suffer from drift or local minima. We present an alternative framework for SfM based on finding a coarse initial solution using hybrid discrete-continuous optimization and then improving that solution using bundle adjustment. The initial optimization step uses a discrete Markov random field (MRF) formulation, coupled with a continuous Levenberg-Marquardt refinement. The formulation naturally incorporates various sources of information about both the cameras and points, including noisy geotags and vanishing point (VP) estimates. We test our method on several large-scale photo collections, including one with measured camera positions, and show that it produces models that are similar to or better than those produced by incremental bundle adjustment, but more robustly and in a fraction of the time.