Jose‐Luis Blanco

José-Luis Blanco-Claraco, Antonio Leanza, Giulio Reina

In this paper, we present a novel general framework grounded in the factor graph theory to solve kinematic and dynamic problems for multi-body systems. Although the motion of multi-body systems is considered to be a well-studied problem and various methods have been proposed for its solution, a unified approach providing an intuitive interpretation is still pursued. We describe how to build factor graphs to model and simulate multibody systems using both, independent and dependent coordinates. Then, batch optimization or a fixed-lag-smoother can be applied to solve the underlying optimization problem that results in a highly-sparse nonlinear minimization problem. The proposed framework has been tested in extensive simulations and validated against a commercial multibody software. We release a reference implementation as an open-source C++ library, based on the GTSAM framework, a well-known estimation library. Simulations of forward and inverse dynamics are presented, showing comparable accuracy with classical approaches. The proposed factor graph-based framework has the potential to be integrated into applications related with motion estimation and parameter identification of complex mechanical systems, ranging from mechanisms to vehicles, or robot manipulators.

Jose Luis Blanco-Claraco

The problems of point-cloud registration and attitude estimation from vector observations (Wahba's problem) have widespread applications in computer vision and mobile robotics. This work introduces a simple approach for integrating sets of geometric feature observations (points, lines, and planes) in such a way that any solution to either point-cloud registration or to Wahba's problem can be used to find the SE(3) transformation between the two sets that minimizes the corresponding cost function. We compare the performance of three solutions: classic Horn's optimal quaternion method, Optimal Linear Attitude Estimator (OLAE) that efficiently recovers the optimal Gibbs-Rodrigues vector solving a small linear system, and an iterative non-linear Gauss-Newton solver. Special care is given to explain how to overcome the Gibbs vector singularity for OLAE by using the method of sequential rotations. Gross outliers in point-to-point correspondences can be discarded by means of detecting transformation scale mismatches. The approach also allows the introduction of per-primitive relative weights, including an optional robust loss function that is applicable only if an initial guess for the solution is known in advance. Experiments are presented to evaluate how the three solutions tolerate noise in the input data for different kinds of geometric primitives. Finally, experiments with real datasets validate the suitability of the optimal alignment algorithm as the core of an Iterative Closest Point/Primitive (ICP) algorithm. An open-source implementation of all the described algorithms is provided in https://github.com/MOLAorg/mp2p_icp

Rubén Antonio García-Ruiz, José Luis Blanco-Claraco, Javier López-Martínez et al.

Expensive ultrasonic anemometers are usually required to measure wind speed accurately. The aim of this work is to overcome the loss of accuracy of a low cost hot-wire anemometer caused by the changes of air temperature, by means of a probabilistic calibration using Gaussian Process Regression. Gaussian Process Regression is a non-parametric, Bayesian, and supervised learning method designed to make predictions of an unknown target variable as a function of one or more known input variables. Our approach is validated against real datasets, obtaining a good performance in inferring the actual wind speed values. By performing, before its real use in the field, a calibration of the hot-wire anemometer taking into account air temperature, permits that the wind speed can be estimated for the typical range of ambient temperatures, including a grounded uncertainty estimation for each speed measure.

Antonio Leanza, Giulio Reina, Jose-Luis Blanco-Claraco

Sideslip angle is an important variable for understanding and monitoring vehicle dynamics but it lacks an inexpensive method for direct measurement. Therefore, it is typically estimated from inertial and other proprioceptive sensors onboard using filtering methods from the family of the Kalman Filter. As a novel alternative, this work proposes modelling the problem directly as a graphical model (factor graph), which can then be optimized using a variety of methods, such as whole dataset batch optimization for offline processing or fixed-lag smoother for on-line operation. Experimental results on real vehicle datasets validate the proposal with a good agreement between estimated and actual sideslip angle, showing similar performance than the state-of-the-art with a great potential for future extensions due to the flexible mathematical framework.

José Luis Blanco-Claraco

An arbitrary rigid transformation in $\mathbf{SE}(3)$ can be separated into two parts, namely, a translation and a rigid rotation. This technical report reviews, under a unifying viewpoint, three common alternatives to representing the rotation part: sets of three (yaw-pitch-roll) Euler angles, orthogonal rotation matrices from $\mathbf{SO}(3)$ and quaternions. It will be described: (i) the equivalence between these representations and the formulas for transforming one to each other (in all cases considering the translational and rotational parts as a whole), (ii) how to compose poses with poses and poses with points in each representation and (iii) how the uncertainty of the poses (when modeled as Gaussian distributions) is affected by these transformations and compositions. Some brief notes are also given about the Jacobians required to implement least-squares optimization on manifolds, an very promising approach in recent engineering literature. The text reflects which MRPT C++ library functions implement each of the described algorithms. All formulas and their implementation have been thoroughly validated by means of unit testing and numerical estimation of the Jacobians