Least squares optimization: From theory to practice

Nowadays, Nonlinear Least-Squares embodies the foundation of many Robotics and Computer Vision systems. The research community deeply investigated this topic in the last few years, and this resulted in the development of several open-source solvers to approach constantly increasing classes of problems. In this work, we propose a unified methodology to design and develop efficient Least-Squares Optimization algorithms, focusing on the structures and patterns of each specific domain. Furthermore, we present a novel open-source optimization system that addresses problems transparently with a different structure and designed to be easy to extend. The system is written in modern C++ and runs efficiently on embedded systemsWe validated our approach by conducting comparative experiments on several problems using standard datasets. The results show that our system achieves state-of-the-art performances in all tested scenarios

Efficient Long-term Mapping in Dynamic Environments

As autonomous robots are increasingly being introduced in real-world environments operating for long periods of time, the difficulties of long-term mapping are attracting the attention of the robotics research community. This paper proposes a full SLAM system capable of handling the dynamics of the environment across a single or multiple mapping sessions. Using the pose graph SLAM paradigm, the system works on local maps in the form of 2D point cloud data which are updated over time to store the most up-to-date state of the environment. The core of our system is an efficient ICP-based alignment and merging procedure working on the clouds that copes with non-static entities of the environment. Furthermore, the system retains the graph complexity by removing out-dated nodes upon robust inter- and intra-session loop closure detections while graph coherency is preserved by using condensed measurements. Experiments conducted with real data from longterm SLAM datasets demonstrate the efficiency, accuracy and effectiveness of our system in the management of the mapping problem during long-term robot operation

Sparse Pose Graph Optimization in Cycle Space

The state-of-the-art modern pose-graph optimization (PGO) systems are vertex based. In this context, the number of variables might be high, albeit the number of cycles in the graph (loop closures) is relatively low. For sparse problems particularly, the cycle space has a significantly smaller dimension than the number of vertices. By exploiting this observation, in this article, we propose an alternative solution to PGO that directly exploits the cycle space. We characterize the topology of the graph as a cycle matrix, and reparameterize the problem using relative poses, which are further constrained by a cycle basis of the graph. We show that by using a minimum cycle basis, the cycle-based approach has superior convergence properties against its vertex-based counterpart, in terms of convergence speed and convergence to the global minimum. For sparse graphs, our cycle-based approach is also more time efficient than the vertex-based. As an additional contribution of this work, we present an effective algorithm to compute the minimum cycle basis. Albeit known in computer science, we believe that this algorithm is not familiar to the robotics community. All the claims are validated by experiments on both standard benchmarks and simulated datasets. To foster the reproduction of the results, we provide a complete open-source C++ implementation (1) of our approach

Low-cost Sonar Navigation System

In this paper, we present a sonar-based navigation system, designed to deploy a fleet of autonomous mobile platforms at a reasonable cost. In educational and hobbyist contexts, a large number of robots is required. By means of classical navigation approaches, every robot should be provided with accurate vision or range sensors. This limits the maximum number of robots in the fleet, due to the unaffordable cost of these sensors. In contrast to that, our system requires a single platform equipped with a higher quality sensor, used to perform calibration and mapping tasks. The rest of the fleet, able to localize and navigate, is equipped solely with low-cost sonars, providing a notable reduction in the overall cost. We achieve this task by presenting a novel calibration procedure to estimate the sonars extrinsic and adapting a classical monte-carlo localization algorithm to the sonar model, focusing on efficiency. We release an open source implementation of the system to the community

HiPE: Hierarchical Initialization for Pose Graphs

Pose graph optimization is a non-convex optimization problem encountered in many areas of robotics perception. Its convergence to an accurate solution is conditioned by two factors: the non-linearity of the cost function in use and the initial configuration of the pose variables. In this letter, we present HiPE, a novel hierarchical algorithm for pose graph initialization. Our approach exploits a coarse-grained graph that encodes an abstract representation of the problem geometry. We construct this graph by combining maximum likelihood estimates coming from local regions of the input. By leveraging the sparsity of this representation, we can initialize the pose graph in a non-linear fashion, without computational overhead compared to existing methods. The resulting initial guess can effectively bootstrap the fine-grained optimization that is used to obtain the final solution. In addition, we perform an empirical analysis on the impact of different cost functions on the final estimate. Our experimental evaluation shows that the usage of HiPE leads to a more efficient and robust optimization process, comparing favorably with state-of-the-art methods