Robust and Efficient Geometric Methods for Registration, Localization and Mapping

In the last couple of decades, robotics has developed vertically towards a growing number of fields. Nowadays, robotics engineering encompasses a wide set of diverse tasks, spanning from the strictly electrical or mechanical ones, to multi-robot fleet coordination, or autonomous and intelligent operations. The rise among the computer science community of the artificial intelligence techniques has caused this trend to accelerate even further. This is due to the tendency of applying machine learning techniques to an ever increasing number of fields. However, even today many industrial and service robotics tasks are solved through model-based approaches. For this relevant number of use cases, it is important to keep the research going on improving robustness, efficiency and diversity of robotic primitives. Primitives can be defined as operations that are relevant in a wide number of robotic applications. A robotic primitive tackled in this dissertation is point cloud registration, which is the estimation of the rigid motion that better aligns different views of the same scene. Registration has an important role in addressing simultaneous localization and mapping (SLAM), which can be decomposed in subproblems, including alignment of consecutive sensor measurements, loop closure, and place recognition. Clearly, registration can be seen as one of the cornerstones for performing solid SLAM, as it allows comparison and merging of partially overlapping sensory data into a consistent map, or the assessment of the motion of a mobile robot. Another important spatial estimation problem is the cross-localization between sensors or robots within a fleet. It is an instance of pose graph problems involving a network of reference frames constrained by relative measurements. The specific issues are the density of relative constraints, as well as the emphasis on pose synchronization. These pose estimation problems are usually defined in the form of an optimization problem on variables, features and transformations described by Lie groups and algebras. The most common registration methods solve the problem with local search methods relying on an initial guess provided by odometry or coarse-to-fine assessment. However, more robust evaluation can be guaranteed by global search or, when possible, by globally optimal methods exploiting optimization on Riemannian manifolds in order to certify the behavior of a system. Robustness is achieved by independence from an initial assessment of the solution, and by certifiable properties such as global optimality. However, most of the time removing assumptions from a problem can lead to an increased computational complexity of the algorithms that solve it globally. Because of this, it is paramount to keep an eye on the computational efficiency of these approaches e.g., by simplifying input data eliminating redundancies, or parallelizing as many operations as possible. This dissertation presents two main contributions related to the geometric approach in the solution of pose estimation problems. The first main contribution is the proposal of Angular Radon Spectrum (ARS) for point cloud registration. Angular Radon Spectrum is a descriptor that captures the collinearity (in 2D domain) or the coplanarity (in 3D domain) of a point cloud to a pencil of lines or planes. The point clouds are represented in the form of Gaussian Mixture Models (GMMs) that effectively model point position uncertainty. ARS is translation-invariant, and expresses rotation-shifts straightforwardly. This allows decoupling of rotation and translation estimation. As such, ARS can be used to perform pairwise rotation estimation through maximization of the correlation function between the two ARS spectra. There are three original results about ARS presented in this dissertation. The first one is the algorithm for efficient computation of ARS in planar domain, with general anisotropic Gaussian kernels that compose input GMMs. This algorithm also performs GMM simplification in order to reduce the number of input kernels. The second outcome is a parallel algorithm for planar ARS computation suitable for GPU-based implementation. The third specific contribution is the derivation of ARS for 3D point clouds, and the derivation of closed-form series expansion in spherical harmonics. Full pose registration is achieved through a branch-and-bound translation estimation algorithm. Experiments show the high accuracy and improved computational speed of the ARS-based rotation estimation, while concisely assessing the potential of ARS in a SLAM scan-based registration and mapping pipeline. The second main contribution presented is the pose averaging, or cross-localization, of a sensor fleet through a formulation based on the Shape of Motion (SOM) matrix. The novelty introduced by this approach regards the reformulation of the Riemannian Staircase (RS) algorithm to the specific case needed. RS is used to exit local minima in which solvers stop with a given dimensionality of the problem, by iteratively increasing the dimension of the cost function members and their associated search space. Optimization has been performed through Riemannian Geometry and Netwonian solvers. A relevant part of the work regards the search of an exit direction leading to a point with cost lower than the one where the optimization stopped at the previous staircase step, based on searching a negative eigenvalue of the Riemannian Hessian of the cost function, after increasing the dimensionality of the problem.