The registration problem at hand consists of finding the transformation that best fits a set of noisy observations of geometric primitives taken from the frames of references and . That is, we are interested in finding the relative pose that minimizes a particular cost function.
Fundamentally different problems arise next depending on whether the correspondences between the two sets are known or unknown. If they are known, an optimal transformation algorithm, which usually has a closed-form solution, must be employed; otherwise, the usual approach is to apply a version of the Iterative Closest Point (ICP) ([Besl and McKay(1992), Pomerleau et al.(2015)Pomerleau, Colas, Siegwart, et al.]) to iterate between establishing correspondences and finding the corresponding optimal transformation (using one of the algorithms of the former group) until convergence is achieved.
Existing ICP algorithms have been reviewed extensively in the literature, for example, in [Pomerleau et al.(2015)Pomerleau, Colas, Siegwart, et al.]. Most proposed versions deal only with point-to-point correspondences, or propose point-to-plane correspondences, as in [Segal et al.(2009)Segal, Haehnel, and Thrun]. Closed-form solution for plane to plane pairings was already proposed in the seminar work [Faugeras and Hebert(1986)], although line to line correspondences lacked a closed-form solution and a nonlinear iterative solution was proposed in that case.
The present approach aims at integrating planes and lines into existing optimal attitude solvers, an idea that seems not to be proposed in related works. Note that a fundamental limitation of our proposal in its present form is that planes and lines only contribute information about the relative orientation between two frames of references, thus at least one point to point pairing is required to completely solve the full SE(3) relative pose.
The goal of this work is, therefore, to: (i) provide a way to integrate different geometric primitives into the existing Horn’s and OLAE solvers, and (ii) perform an experimental evaluation of their performance.
The rest of this paper is organized as follows. Section 2 reviews the foundations of the related methods. Then, the proposed method, together with a throughout discussion of how to reliably implement OLAE is provided in Section 3. Experimental results are then exposed in the next section and we finally provide some discussion and conclusions in Section 5.
The algorithm in [Horn(1987)] to find the optimal transformation between pairs of points, relative to their corresponding point cloud centroids, is very well-known in the robotics and computer vision communities, so it will be not further described here.
On the other hand, we have a large body of research focused on finding the optimal relative orientation, i.e. a SO(3) transformation, between a set of unit vector observations. Note that this is in contrast to relative point observations, which may have arbitrary norms, leading to a similar, but different problem. The vector observation problem arises naturally in spacecraft and satellite localization, hence it has been of the maximum interest to the aerospace community since the original Wahba’s 1965 formulation (see [Whaba(1965)]):
where is the sought optimal rotation matrix of with respect to , and are the unit vector observations, taken in frames of reference and , respectively, and
are the relative scalar weights for each observation. If weights are identified as the inverse of each observation variance, the problem becomes a maximum likelihood estimator.
The present work focuses on a the Optimal Linear Attitude Estimator (OLAE), as introduced in [Mortari et al.(2007)Mortari, Markley, and Singla]. OLAE allows the global estimation (noniterative, without any initial guess) of the optimal relative attitude between a set of unit vector observations. In a sense, it is similar to Wahba’s problem (becoming equivalent only in the limit case of zero noise), but with a different target cost function and using a Caley-Gibbs-Rodrigues rotation vector parameterization ([Bauchau and Trainelli(2003)]). This formulation leads to the formation of a linear system, with a strictly quadratic cost function:
Recently, [Lourakis and Terzakis(2018)] proposed to use OLAE as an alternative to Horn’s method for determining the orientation inside the general loop of ICP, but that work did not include any way to also handle planes or lines in the matching process.
3 Proposed approach
In the following we provide details on how to build an ICP system, capable of handling point, line, and plane correspondences. At the core of the ICP algorithm we are free to choose any optimal transformation algorithm, hence we will describe three of them first: the well-known Horn’s optimal quaternion solution in §3.1, the OLAE in §3.2, and an iterative, Gauss-Newton method in §3.3 as baseline.
3.1 Unifying primitives for Horn’s optimal solution
Let and be the sets of geometric primitives as observed from frames and , respectively. At this point we will assume that correspondences have been already being established between the two sets, hence the element of is believed to correspond to the element of , so we let be the total number of features for simplicity of notation.
Horn’s method accepts as input two sets of points, hence it is not directly suitable to handle other kind of geometric primitives. However, a key observation is that [Horn(1987)] actually estimates the transformation in three steps: first the rotation, then the scale, and finally the translation. Rotation is decoupled by means of redefining points in local coordinates with respect to their corresponding point cloud centroid. Therefore, the core of the Horn’s method actually takes as input two sets of vectors, and , which are always assumed to represent local coordinates of points in a point cloud.
However, nothing prevents us to include additional vectors into these sets, with any other geometric meaning. In particular, we propose to build the sets of vectorsand from the set of geometric primitives and as follows:
If the -th pair corresponds to a pair of points, we follow the standard procedure ([Horn(1987)]) and define:
where are the weighted centroids of all point features in and
. Due to the importance of the centroid in this method, we propose to remove those pairs that can be clearly classified as outliers, based on a scale mismatch detector. Elaborating on this later idea, please note that, if all pairings were inliers, and under the assumption of zero-mean additive random noise, it should be fulfilled that, with the mathematical expectation. Equivalently, we could write this condition as , leading to the following test for early classification of pairings as outliers:
being the scale outlier detection threshold. Reasonable values for this threshold are in the range. Smaller values tend to discard good pairings, while larger ones will only filter the most severe and obvious outliers. If the noise distribution of points and outliers is known, its value can be precisely determined to fulfill with a predetermined level of confidence. A remark on how to make the detection of outliers more robust is given in the discussion of future works in §5.
If the -th pair corresponds to a pair of lines, we define and as the unit director vectors of the corresponding lines. To solve the direction ambiguity of the director vector (i.e. any point and both and define the same line), a criterion must be taken regarding the relative orientation of lines when observed from the sensor. For example, we could arbitrarily impose that director vectors should make an angle smaller than radians with respect to the unit vector from the sensor to the closest point of the line with respect to the sensor.
If the -th pair corresponds to a pair of planes, we define and as the unit normal vectors of the corresponding planes. Again, ambiguity should be addressed to enforce that the same plane, when observed from two different view points, has a vector that points in the same direction. A natural choice in this case is to select the outwards-pointing direction of measured surfaces.
Once we have all the vectors stacked into the lists and , including their pairwise relative weights , we follow the standard method described in [Horn(1987)] to recover the optimal SO(3) transformation, then use the point cloud centroids (excluding outliers) to solve for optimal translation.
3.2 Unifying primitives for OLAE
Just like in the former section, we start with the input sets and . Conversely to Horn’s method, vector-based attitude estimator as OLAE or any other solution to the Wahba’s problem, accepts two sets of unit vectors, and , as input. Again, we propose to include other entities, like points, by converting them appropriately. We can build the sets of unit vectors and from the set of geometric primitives and as follows:
For pairs of points , we start following the same procedure than described for the Horn’s method above, then we normalize the local coordinates of the points with respect to the centroids , that is:
Note that this simple transformation enables attitude estimation algorithms to also cope with point correspondences. Outliers can be also detected in this case using Eq. (5) without modifications.
For line or plane correspondences, the unit director and normal vectors, respectively, already are unit vectors (the natural input to OLAE), so no further actions are required.
Once we have all observations stacked into the lists of unit vectors and , and we are given a vector of relative weights , we can find the attitude profile matrix and the vector in Eq. (2) as:
from which can be approximate in Eq. (2) as (which is more convenient for reasons that will be clear when dealing with the sequential rotation method), and where is (see [Mortari et al.(2007)Mortari, Markley, and Singla]):
where it has been used:
leading to the linear system of equations:
from which the optimal rotation can be solved for as the Gibbs vector . In order to recover a quaternion from , we can use:
The only edge case that remains to be dealt with is the singularity of the Gibbs vector representation when representing a rotation of 180 degrees. In fact, the accuracy of the solution of the linear system in Eq. (14) may in theory be compromised when working near the singularity. In practice, for reasonable noise levels in the input data, the solution is robust even with rotations of 179 degrees.
Nevertheless, in order to work near the optimal conditions of OLAE, we propose to evaluate four different solutions: one for the unmodified linear system in Eq. (14), and three for systems that have undergone a rotation around each one of the axes (x,y,z). The approximation reveals useful here, since the rotated matrices can be evaluated in closed form from , avoiding the need to rotate all the input vectors and going through the summation again. This method, originally described in [Shuster and Oh(1981)], can be found in Eqs.(35)-(37) in [Mortari et al.(2007)Mortari, Markley, and Singla]
We propose to evaluate the determinant of the matrix of coefficients of the linear system (i.e. for the unrotated case, for the rotated systems) and use the system with the largest absolute value, since it will ensure that the system has a full rank of 3.
3.3 Gauss-Newton iterative solver
In order to validate the implementation of the multi-primitive optimal transformation algorithms based on the Horn’s method and OLAE, we also implemented a baseline method based on a non-linear, iterative solver. It support the same kind of pairings than the aforementioned methods, plus additional ones, e.g. point-to-plane. The interested reader is referred to the source code for details on the cost functions. Closed-form Jacobians have been found for each kind of pairing by applying the chain rule to the corresponding target function and making use of the expressions in[Blanco(2010)] to solve for SE(3) on-manifold increments at each iteration.
3.4 Complete ICP algorithm
Once the optimal transformation estimators have been defined, we can build a complete multi-primitive alignment algorithm by iterating between selecting closest correspondences between the two maps, and finding the optimal transformation that raises from the selected pairings. We use the nearest neighbor criterion for pairing points, using KD-trees for efficiency. Planes are paired by looking for the nearest centroids in the reference map whose normal vector makes an angle below a certain threshold in the map to be aligned. A robust kernel is optionally applied by multiplying the relative weights of each pair with a robust loss function, i.e. . The interested reader is referred to the source code for further details.
Errors for the optimal transformation estimated by three different methods (OLAE, Horn’s optimal quaternion, Gauss-Newton) for 100 point-to-point correspondences. The horizontal axis represents the standard deviation of the points additive isotropic Gaussian noise.
4 Experimental results
Next we describe different numerical simulations aimed at evaluating the algorithms proposed in former sections. The implementation used to obtain these results has been released as open source for the sake of reproducibility.
4.1 Sensitivity to noise
To benchmark the performance of the implemented methods against noise, sets of points and plane centroids are randomly drawn following an uniformly distributed in the cube (0,0,0)-(50,50,50), and then transformed following a random SE(3) pose which is then estimated from the three methods described in the text above. Points in the transformed map are corrupted with additive Gaussian noise. Plane normals are also rotated following random SO(3) rotations whose rotation angle also follows a Gaussian distribution. Errors are evaluated in two parts: rotation error with respect to the ground truth is measured as the norm of the matrix logarithm of the rotation error, while translation is measured using Euclidean distances. All the experiments have been repeated 1000 times to obtain significant statistics.
4.2 Absolute outlier rejection
Next we evaluate the outlier rejection method based on the scale mismatch threshold, as described by Eq. (5). The outlier detector has been integrated into both, OLAE and Horn’s solution, and the error achieved after removing outliers (those that were successfully detected) is shown in Figure 4. For comparison, we added the result of the Gauss-Newton and the original Horn’s method, both of them without any outlier filter. Here we used the threshold .
4.3 Robust loss cost
When a guess for the solution is available, e.g. in the final refining stages of ICP, when the solution is near convergence, we can enable the additional robust loss weight factor mentioned in §3.4 to further mitigate the effect of outliers. Figure 5 shows a comparison of OLAE with the robust loss function and the classic Horn’s solution (without loss function).
4.4 Complete ICP system
In order to test the complete ICP system described in §3.4, we used 3D pointcloud models publicly available in the Stanford’s dataset ([Curless and Levoy(1996)]). In particular, in this section we used the Bunny dataset, downsampled to 1000 points, as a reference pointcloud. Then, a transformed pointcloud is generated by translating and rotating the model using a random SE(3) pose with translation in X,Y, and Z, uniformly drawn in the range , with the maximum length of the model bounding box, and with random rotations built from values of yaw, pitch, and roll drawn from a uniform distribution in the range . Note that, while the optimal transformation methods (OLAE, Horn’s) are able to find a global optimal transformation without iterating, the association stage required in ICP limits the convergence volume of the state space; that explains the relatively small translations and rotations used in this test. The whole process is repeated 10 times, and we measured the SO(3) rmse, the translation rmse, and CPU time, of the final estimation of the ICP algorithm when using each of the three different algorithms discussed above at its core while solving for optimal transformations. The statistical results can be seen in Figure 6.
5 Discussion and conclusions
We have presented a methodology to allow point, line, and plane features to be integrated into Horn’s method and into OLAE, which originally only supported point and vector observations, respectively.
It is worth mentioning that some information is lost due to the vector normalization stage in Eq. (6) when using OLAE (but not in Horn’s method), and this has an impact in the solution accuracy: when most correspondences are points, attitude-base methods (i.e. OLAE) performed slightly worse than Horn’s method, which in turn considers the full scale of relative point coordinates. From the statistical results, it can be concluded that OLAE performs identical to Horn’s method when most features are vector-like (i.e. planes or lines). OLAE is faster than Horn’s method only when working with a reduced number of correspondences (roughly less than 10 primitive pairings), thus it should be the preferred choice only for applications that require the fast evaluation of small sets, e.g. inside a RANSAC loop.
Future works include methods to avoid relying on a centroid. This has revealed as a weak point of the Horn’s classic solution, which may be mitigated by using relative vectors for different graph topologies, as recently proposed in [Yang and Carlone(2019)].
- [Besl and McKay(1992)] Paul J Besl and Neil D McKay. Method for registration of 3-d shapes. In Sensor Fusion IV: Control Paradigms and Data Structures, volume 1611, pages 586–607. International Society for Optics and Photonics, 1992.
- [Pomerleau et al.(2015)Pomerleau, Colas, Siegwart, et al.] François Pomerleau, Francis Colas, Roland Siegwart, et al. A review of point cloud registration algorithms for mobile robotics. Foundations and Trends® in Robotics, 4(1):1–104, 2015.
- [Segal et al.(2009)Segal, Haehnel, and Thrun] Aleksandr Segal, Dirk Haehnel, and Sebastian Thrun. Generalized-icp. In Robotics: science and systems, volume 2, page 435, 2009.
- [Faugeras and Hebert(1986)] Olivier D Faugeras and Martial Hebert. The representation, recognition, and locating of 3-d objects. The international journal of robotics research, 5(3):27–52, 1986.
- [Horn(1987)] Berthold KP Horn. Closed-form solution of absolute orientation using unit quaternions. Josa a, 4(4):629–642, 1987.
- [Whaba(1965)] G Whaba. A least squares estimate of spacecraft attitude. SIAM Review, 7(3):409, 1965.
- [Markley and Mortari(1999)] F Landis Markley and Daniele Mortari. How to estimate attitude from vector observations. In AAS/AIAA Astrodynamics Specialist Conference, 1999.
- [Markley and Mortari(2000)] F Landis Markley and Daniele Mortari. Quaternion attitude estimation using vector observations. Journal of the Astronautical Sciences, 48(2):359–380, 2000.
- [Mortari et al.(2007)Mortari, Markley, and Singla] Daniele Mortari, F Landis Markley, and Puneet Singla. Optimal linear attitude estimator. Journal of Guidance, Control, and Dynamics, 30(6):1619–1627, 2007.
- [Bauchau and Trainelli(2003)] Olivier A Bauchau and Lorenzo Trainelli. The vectorial parameterization of rotation. Nonlinear dynamics, 32(1):71–92, 2003.
- [Lourakis and Terzakis(2018)] Manolis Lourakis and George Terzakis. Efficient absolute orientation revisited. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 5813–5818. IEEE, 2018. URL https://ieeexplore.ieee.org/abstract/document/8594296/.
- [Shuster and Oh(1981)] M.D. Shuster and S.D. Oh. Three-axis attitude determination from vector observations. Journal of Guidance, Control, and Dynamics, 4(1):70–77, 1981.
- [Blanco(2010)] Jose-Luis Blanco. A tutorial on se(3) transformation parameterizations and on-manifold optimization. University of Malaga, Tech. Rep, 3, 2010.
- [Curless and Levoy(1996)] Brian Curless and Marc Levoy. A volumetric method for building complex models from range images. 1996.
- [Yang and Carlone(2019)] Heng Yang and Luca Carlone. A polynomial-time solution for robust registration with extreme outlier rates. arXiv preprint arXiv:1903.08588, 2019.