An SE(3) Noise Model for Range-Azimuth-Elevation Sensors

Scan matching is a widely used technique in state estimation. Point-cloud alignment, one of the most popular methods for scan matching, is a weighted least-squares problem in which the weights are determined from the inverse covariance of the measured points. An inaccurate representation of the covariance will affect the weighting of the least-squares problem. For example, if ellipsoidal covariance bounds are used to approximate the curved, "banana-shaped" noise characteristics of many scanning sensors, the weighting in the least-squares problem may be overconfident. Additionally, sensor-to-vehicle extrinsic uncertainty and odometry uncertainty during submap formation are two sources of uncertainty that are often overlooked in scan matching applications, also likely contributing to overconfidence on the scan matching estimate. This paper attempts to address these issues by developing a model for range-azimuth-elevation sensors on matrix Lie groups. The model allows for the seamless incorporation of extrinsic and odometry uncertainty. Illustrative results are shown both for a simulated example and for a real point-cloud submap collected with an underwater laser scanner.

Laser-to-Vehicle Extrinsic Calibration in Low-Observability Scenarios for Subsea Mapping

Laser line scanners are increasingly being used in the subsea industry for high-resolution mapping and infrastructure inspection. However, calibrating the 3D pose of the scanner relative to the vehicle is a perennial source of confusion and frustration for industrial surveyors. This work describes three novel algorithms for laser-to-vehicle extrinsic calibration using naturally occurring features. Each algorithm makes a different assumption on the quality of the vehicle trajectory estimate, enabling good calibration results in a wide range of situations. A regularization technique is used to address low-observability scenarios frequently encountered in practice with large, rotationally stable subsea vehicles. Experimental results are provided for two field datasets, including the recently discovered wreck of the Endurance.

Combining DVL-INS and Laser-Based Loop Closures in a Batch Estimation Framework for Underwater Positioning

Correcting gradual position drift is a challenge in long-term subsea navigation. Though highly accurate, modern inertial navigation system (INS) estimates will drift over time due to the accumulated effects of sensor noise and biases, even with acoustic aiding from a Doppler velocity log (DVL). The raw sensor measurements and estimation algorithms used by the DVL-aided INS are often proprietary, which restricts the fusion of additional sensors that could bound navigation drift over time. In this letter, the raw sensor measurements and their respective covariances are estimated from the DVL-aided INS output using semidefinite programming tools. The estimated measurements are then augmented with laser-based loop-closure measurements in a batch state estimation framework to correct planar position errors. The heading uncertainty from the DVL-aided INS is also considered in the estimation of the updated positions. The pipeline is tested in simulation and on experimental field data. The proposed methodology reduces the long-term navigation drift by more than 30 times compared to the DVL-aided INS estimate.

Adaptive Graduated Nonconvexity Loss

Many problems in robotics, such as estimating the state from noisy sensor data or aligning two LiDAR point clouds, can be posed and solved as least-squares problems. Unfortunately, vanilla nonminimal solvers for least-squares problems are notoriously sensitive to outliers. As such, various robust loss functions have been proposed to reduce the sensitivity to outliers. Examples of loss functions include pseudo-Huber, Cauchy, and Geman-McClure. Recently, these loss functions have been generalized into a single loss function that enables the best loss function to be found adaptively based on the distribution of the residuals. However, even with the generalized robust loss function, most nonminimal solvers can only be solved locally given a prior state estimate due to the nonconvexity of the problem. The first contribution of this paper is to combine graduated nonconvexity (GNC) with the generalized robust loss function to solve least-squares problems without a prior state estimate and without the need to specify a loss function. Moreover, existing loss functions, including the generalized loss function, are based on Gaussian-like distribution. However, residuals are often defined as the squared norm of a multivariate error and distributed in a Chi-like fashion. The second contribution of this paper is to apply a norm-aware adaptive robust loss function within a GNC framework. This leads to additional robustness when compared with state-of-the-art methods. Simulations and experiments demonstrate that the proposed approach is more robust and yields faster convergence times compared to other GNC formulations.

Improving Self-Consistency in Underwater Mapping through Laser-Based Loop Closure (Extended)

Accurate, self-consistent bathymetric maps are needed to monitor changes in subsea environments and infrastructure. These maps are increasingly collected by underwater vehicles, and mapping requires an accurate vehicle navigation solution. Commercial off-the-shelf (COTS) navigation solutions for underwater vehicles often rely on external acoustic sensors for localization, however survey-grade acoustic sensors are expensive to deploy and limit the range of the vehicle. Techniques from the field of simultaneous localization and mapping, particularly loop closures, can improve the quality of the navigation solution over dead-reckoning, but are difficult to integrate into COTS navigation systems. This work presents a method to improve the self-consistency of bathymetric maps by smoothly integrating loop-closure measurements into the state estimate produced by a commercial subsea navigation system. Integration is done using a white-noise-on-acceleration motion prior, without access to raw sensor measurements or proprietary models. Improvements in map self-consistency are shown for both simulated and experimental datasets, including a 3D scan of an underwater shipwreck in Wiarton, Ontario, Canada.

Performance Evaluation of 3D Keypoint Detectors and Descriptors on Coloured Point Clouds in Subsea Environments

The recent development of high-precision subsea optical scanners allows for 3D keypoint detectors and feature descriptors to be leveraged on point cloud scans from subsea environments. However, the literature lacks a comprehensive survey to identify the best combination of detectors and descriptors to be used in these challenging and novel environments. This paper aims to identify the best detector/descriptor pair using a challenging field dataset collected using a commercial underwater laser scanner. Furthermore, studies have shown that incorporating texture information to extend geometric features adds robustness to feature matching on synthetic datasets. This paper also proposes a novel method of fusing images with underwater laser scans to produce coloured point clouds, which are used to study the effectiveness of 6D point cloud descriptors.

Mind the Gap: Norm-Aware Adaptive Robust Loss for Multivariate Least-Squares Problems

Measurement outliers are unavoidable when solving real-world robot state estimation problems. A large family of robust loss functions (RLFs) exists to mitigate the effects of outliers, including newly developed adaptive methods that do not require parameter tuning. All of these methods assume that residuals follow a zero-mean Gaussian-like distribution. However, in multivariate problems the residual is often defined as a norm, and norms follow a Chi-like distribution with a non-zero mode value. This produces a ''mode gap'' that impacts the convergence rate and accuracy of existing RLFs. The proposed approach, ''Adaptive MB,'' accounts for this gap by first estimating the mode of the residuals using an adaptive Chi-like distribution. Applying an existing adaptive weighting scheme only to residuals greater than the mode leads to more robust performance and faster convergence times in two fundamental state estimation problems, point cloud alignment and pose averaging.