Exactly sparse delayed state filter on Lie groups for long-term pose graph SLAM

In this paper we propose a simultaneous localization and mapping (SLAM) back-end solution called the exactly sparse delayed state filter on Lie groups (LG-ESDSF). We derive LG-ESDSF and demonstrate that it retains all the good characteristics of the classic Euclidean ESDSF, the main advantage being the exact sparsity of the information matrix. The key advantage of LG-ESDSF in comparison with the classic ESDSF lies in the ability to respect the state space geometry by negotiating uncertainties and employing filtering equations directly on Lie groups. We also exploit the special structure of the information matrix in order to allow long-term operation while the robot is moving repeatedly through the same environment. To prove the effectiveness of the proposed SLAM solution, we conducted extensive experiments on two different publicly available datasets, namely the KITTI and EuRoC datasets, using two front-ends: one based on the stereo camera and the other on the 3D LIDAR. We compare LG-ESDSF with the general graph optimization framework ([Formula: see text]) when coupled with the same front-ends. Similarly to [Formula: see text] the proposed LG-ESDSF is front-end agnostic and the comparison demonstrates that our solution can match the accuracy of [Formula: see text], while maintaining faster computation times. Furthermore, the proposed back-end coupled with the stereo camera front-end forms a complete visual SLAM solution dubbed LG-SLAM. Finally, we evaluated LG-SLAM using the online KITTI protocol and at the time of writing it achieved the second best result among the stereo odometry solutions and the best result among the tested SLAM algorithms.

Orientation Uncertainty Characteristics of Some Pose Measuring Systems

We investigate the performance of pose measuring systems which determine an object's pose from measurement of a few fiducial markers attached to the object. Such systems use point-based, rigid body registration to get the orientation matrix. Uncertainty in the fiducials' measurement propagates to the uncertainty of the orientation matrix. This orientation uncertainty then propagates to points on the object's surface. This propagation is anisotropic, and the direction along which the uncertainty is the smallest is determined by the eigenvector associated with the largest eigenvalue of the orientation data's covariance matrix. This eigenvector in the coordinate frame defined by the fiducials remains almost fixed for any rotation of the object. However, the remaining two eigenvectors vary widely and the direction along which the propagated uncertainty is the largest cannot be determined from the object's pose. Conditions that result in such a behavior and practical consequences of it are presented.

Toward On-line Parameter Estimation of Concentric Tube Robots Using a Mechanics-based Kinematic Model

Although existing mechanics-based models of concentric tube robots have been experimentally demonstrated to approximate the actual kinematics, determining accurate estimates of model parameters remains difficult due to the complex relationship between the parameters and available measurements. Further, because the mechanics-based models neglect some phenomena like friction, nonlinear elasticity, and cross section deformation, it is also not clear if model error is due to model simplification or to parameter estimation errors. The parameters of the superelastic materials used in these robots can be slowly time-varying, necessitating periodic re-estimation. This paper proposes a method for estimating the mechanics-based model parameters using an extended Kalman filter as a step toward on-line parameter estimation. Our methodology is validated through both simulation and experiments.

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.

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.

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.