1 Introduction
The problem of point registration or matching plays a crucial role in many engineering applications and scientific disciplines from robot navigation, odometry and autonomous vehicles to graphics, object modeling, and medical imaging. In all of these applications, it is important to acquire a very accurate point registration, despite the sensors errors and limitations. The goal of point registration is to use 3D dataset observation and try to find the best rigid transformation hypothesis that maps one frame to the next. The rigid transformation consists of a rotation matrix and a translation vector ; and a 3D point can be accordingly transformed rigidly as .
Recently, the point registration algorithms have received extensive focus from academia and industries in autonomous driving, since the rapidly developed 3D sensing technology endows the intelligent vehicle the capacity of accurate mapping and localization. Superiority of 3D LiDAR which can reliably employed during night, bad weather (e.g. rain, snow), and in terms of computing complexity, makes it a suitable choice of 3D perception for intelligent vehicles. However, the limited sensor’s coverage, intrinsic sensor errors, and outliers caused by occlusion or unpredictable traffic participators in the real traffic scenarios are formidable challenges which require to be addressed when trying to deploy the point registration algorithm in intelligent vehicles.
A standard approach for point registration problem is the Iterative Closest Point (ICP) algorithm (Besl and McKay (1992)) which solves the problem in two iterative steps: finding the correspondence points and then minimizing the sum of squared residuals to find the best transformation. The ICP method is sensitive to the outliers and the solution is heavily influenced by the initialization. There are several variants of ICP method that try to improve the original method in different regards: ICPp2pl (Low (2004)) ICPpl2pl (Serafin and Grisetti (2015)), GICP (Segal et al. (2009)). In all of these methods both rotation matrix and translation vector are found together iteratively. In addition, the correspondence phase is hardassignment or onetoone.
Our proposed method solves the problem of point registration in a probabilistic fashion by incorporating the orientational information from the pointcloud. The method is mainly motivated by the fact that it can robustify the registration to noise and outliers by incorporating surface normals. In particular, the surface normals are assumed to be drawn from a directional distribution called Kent distribution. Directional statistic is a suitable way to represent pointcloud features like surface normals, since they represent a directional pattern in the pointcloud and also they are invariant with respect to translation. In this approach the estimation of rigid transformation can be decoupled into two steps: first, the rotation matrix between two frames can be estimated using the surface normals, and second, the translation vector can be found using the positional information of the pointcloud. The decoupling of rotation matrix from translation vector is also addressed in Thomas et al. (2019).
Many previous works use the probabilistic approach (i.e. softassignment) for the correspondence phase (Rangarajan et al. (1996); Luo and Hancock (2001); Myronenko and Song (2010)
). In contrast to hardassignment or onetoone correspondence in classical ICP methods, in this method the points are associated to each other in two frames according to a probability distribution.
Myronenko and Song (2010)formulated the point registration problem as a maximum likelihood (ML) estimation problem by using Gaussian mixture model (GMM).
Evangelidis and Horaud (2018) also use GMM to describe and register multiple pointclouds jointly that are assumed to be drawn from an underlying distribution. There are other variants of GMM method with different noise and outlier conditions (Jian and Vemuri (2011); Horaud et al. (2011)).Recently, Min et al. (2019) and Billings and Taylor (2015) proposed point registration with Von Mises–Fisher distribution which is also a directional distribution defined on a unit sphere for 3D points. The method is a hybrid method that combines both directional and positional information into a hybrid of Gaussian and Von Mises distributions. Although the method improves the registration with regard to outliers, its isotropic assumption is not realistic in the real world applications. In contrast, Kent distribution accounts for anisotropic (i.e. the ovalness of surface normals on a unit sphere) dataset as seen in Fig. 1. This will further address the problem of outliers in dataset. Based on the proposed statistical framework, the iterations of finding correspondences and updating transformations are now considered as a type of ExpectationMaximization (EM) procedure.
The rest of this paper is organized as follows; first in Section 2 we start by preliminaries and introducing nomenclatures, including the definition of Kent distribution. Then we introduce the mixture model that describes the probabilistic relationship between points in two frames. In Section 3 we detail the steps in the EM algorithm in the proposed method. In Section 4 we present the results from the experiments on ETH dataset, and finally in Section 5 we make the concluding remarks and discuss the future directions.
2 Methods
2.1 Preliminaries
Throughout the paper we use the following notations for pointclouds:

, – number of points in the model and observed pointclouds, respectively,

– the model pointcloud,

– the observed pointcloud,

– the surface normals of model data,

– the surface normals of observed data,

the positional mean of the model pointcloud,

the positional mean of the observed pointcloud,

,
, the hidden random variable in the mixture model.
Kent distribution is the spherical analogous of the bivariate Gaussian distribution which was introduced by
Kent (1982). Since the distribution is defined over the set of unit vector on a sphere, it is suitable for representing surface normals of data points in a pointcloud. The distribution can be represented with 5 parameters () as^{1}^{1}1In the original paper the distribution was called FisherBingham with parameters, therefore we use the same notation in this paper as well.,where is the concentration, describes ovalness, is the mean direction, and are the major axis and minor axis of the orientation of the distribution, respectively. Together,
creates an orthogonal matrix representing the total orientation of data on the sphere.
is the normalization term which can be expressed, in general, with an infinite sequence of the Modified Bessel functions, but it can be simplified under the assumption of and large as .2.2 Problem Formulation
The point registration problem can be modeled as a mixture of Kent distributions. The distribution which describes the probability of an observed data point corresponding to a model data point has the form (Billings and Taylor (2015)),
(1) 
where are substituted as the mean directions, and consequently the marginal distribution of an observed data point can be computed as,
(2) 
In general, the membership probabilities are part of parameter estimation in mixture models, but here they are assumed to be constant and identical. We also add a uniform distribution which accounts for the noise/outlier in the data points as
. can be chosen empirically based on the approximate amount of noise/outlier in the dataset. Fig. 2 shows an example of two urban scenes and the estimated surface normals which represents a mixture of Kent distributions. Normals rotate on the sphere in consecutive frames and are invariant to the translation along the trajectory of the vehicle.By assuming that all surface normals of the observed data points are independent, the likelihood function is
(3) 
and the loglikelihood function can be computed as
(4)  
where the parameters are . The matrix rotates the surface normals of the observed pointcloud to the surface normals of the model pointcloud , and has been expressed implicitly in (2.2) as . Rotating surface normals does not change other parameters in the distribution as stated in the following lemma.
Lemma 1
If R is an orthogonal matrix, and , then, when .
From the definition of Kent distribution we know that we always have when . Then if , therefore, , which means . This is also intuitive that the orientation parameter does not change the compactness of the Kent distribution, since the rotation matrix is orthogonal, therefore and are preserved. This helps us to estimate the change of the orientation of surface normals without considering any changes in the compactness and ovalness.
Loglikelihood function in (4) assumes a prior knowledge of point correspondence (i.e. the correspondence between points in two pointclouds). However, in practice we don’t know those correspondences and we refer to the observed data as incomplete. Since the correspondence between surface normals in observed and model data is missing, the hidden variable is introduced in EM algorithm to represent their probabilistic assignment. We describe an EMlike approach in the next section in order to iteratively solve for incompletedata version of the maximum likelihood (ML) problem.
3 Point Registration Algorithm
In this section we describe the proposed approach for 3D point registration using Kent distribution. The method relies on decoupling of rotation from translation by estimating rotation based on surface normals and translation from the positional data points. Algorithms 1 and 2 outline the steps of the method.
3.1 Surface Normals Estimation
Several methods exist to estimate the surface normals associated with each point in the pointcloud (Klasing et al. (2009)
). In this paper we compute the normals using principal component analysis (PCA) method. To improve the performance of the proposed method, we preprocess the surface normals with: a) orientation consistency and b) outliers removal. The orientations of surface normals computed by PCA are ambiguous (i.e. it is impossible to solve for the sign of normal vectors). To ensure we have consistent surface normals in the algorithm, we use a defined viewpoint
to make their orientation consistent (Rusu (2013)). All normals of points with the same directions should satisfy . We also use a modified version of outlier removal method for the surface normals. We utilize cosine similarity in order to remove normals that are far away from their mean direction. The method is detailed in Zhang (1994) and Rusu et al. (2008).3.2 Clustering with Spherical means
In the most of urban scene pointclouds, the majority of surface normals are estimated based on prominent surfaces such as buildings which generate the pattern of surface normals concentrated in specific areas on the sphere (for example see Fig. 2). Therefore we use spherical means approach (Hornik et al. (2012)) to cluster surface normals and use those normals from the same cluster in both observed and model data points. This helps to run the algorithm in parallel for the smaller size of data points in each cluster and finally take an average to find the overall rotation matrix in each step. It is worth noting that the spherical clustering is closely related to ML estimate of VonMises distribution which is the same as Kent distribution when (Banerjee et al. (2005)). Fig. 3 depicts steps in order to prepare surface normals for the EM algorithm which will be explained in the following subsection.
3.3 EMlike Algorithm
A variant of EM approach is adopted here to estimate the parameter . As stated in Section 2, since the point correspondence is not available (i.e. the observed data is viewed as being incomplete), loglikelihood function in (4) cannot be optimized directly. The completedata loglikelihood function is given by
(5)  
where is the posterior correspondence probability that links the observed data to the realization of hidden variables. The parameters will be found by minimizing the negative of the conditional expectation of iteratively in the following two steps.
3.4 Estep
The hidden correspondencelabel will be handled by the Estep, which is computing the posterior probability distribution . This posterior distribution plays the role of softassignment between the model normals and the observed normals , and can be computed as
(6) 
for , , and where the denominator is defined in (2). The posterior probability of assigning observed normal to an outlier is . By replacing in (5) we have the function that needs to be maximized in the Mstep,
(7)  
3.5 Mstep
This step requires to find the parameter of the distribution by maximizing the function
(8) 
Although the optimization problem in (8) has closed form solution for some types of probability distributions, unfortunately due to the orthogonality constraints on Kent distribution parameters and rotation matrix, this constrained nonlinear problem is hard to solve. Therefore we solve the problem in two substeps: finding rotation matrix and then updating Kent distribution parameters.
3.5.1 Rotation Matrix Update:
Substituting in (7) we have
(9) 
The optimal rotation matrix can be found by maximizing function with respect to the rotation matrix which belongs to the set of special orthogonal matrices defined as
(10) 
After removing constant terms and also minimizing negative of function, the constraint optimization problem can be written as,
(11) 
Using matrix trace and its cyclic property, the first term above can be rewritten as
The second term in (3.5.1) also can be simplified the same way,
By substituting the following matrices
and rearranging the terms, we can rewrite (3.5.1) in a compact matrix form as
(12) 
Since the constraint in (10) is the set of special orthogonal group or the set of rotation matrices which are smooth, it is convenient to use Riemannian manifold optimization. We use the manifold optimization solver from the package in Boumal et al. (2014).
3.5.2 Distribution Parameters Update:
After updating the rotation matrix, we need to update distribution parameters as well. Recalling equation (3.5.1), we just retain terms that depend on the parameters and also substitute the optimal rotation matrix from previous step,
(13) 
There are some attempts to solve variant of this problem (in the context of mixture of Kent distributions) iteratively using BFGS quasiNewton method with reparametrization of variables (Billings and Taylor (2015)) or BSLM approach (Nguyen (2017)), but there is no guarantee to find a solution considering the nonconvex nature of the problem. A more convenient method to solve for parameters, as described in Peel et al. (2001), is the moment estimation method. The method should be adapted for the mixture of Kent distribution, as it is only described before for the unimodal Kent distributions. Here we compute the weighted sample moments as follows,
where and . Using the above weighted moments, the rest of the method has been explained in Kent (1982), and we exclude the details here for brevity.
Using the moment estimate instead of ML estimate makes the EM algorithm to lose the theoretical convergence guarantees, however as stated in Kent (1982), the moment estimation is very close to ML estimate in case of small eccentricity or large , which is a usual case in practice.
3.5.3 Rotation averaging and Translation:
Finally we need to compute average of the rotations from different clusters and estimate the translation. Rotation averaging in Euclidean sense can be computed as,
(14) 
where , , and . Also if is positive and otherwise (Moakher (2002)). are the optimal rotation matrices found by the algorithm for each cluster in the dataset. The translation vector also can easily be computed based on the positional mean of rotated observed data and model data,
(15) 
This is valid, since it is the optimal translation in the sense of Euclidean norm which has been used in ICPbased methods as well.
4 Experimental Results
In this section the proposed method is tested and verified via 3D pointcloud registration on real indoor scanner pointclouds (Pomerleau et al. (2012)). We preprocessed the pointcloud frames before applying the algorithm. In the first stage, frames were down sampled to of the original size (about points) which was empirically chosen. In addition, all the processes on the surface normals described in Section 3.1 was performed. The proposed method was implemented in MATLAB. The algorithm was initialized with identity rotation matrix and zero translation vector. The distribution parameters was initialized in Estep based on the current frame. We empirically found best results with neighborhood points for surface normals estimation. We adopt the error metric for comparison of estimated and true rotation matrices from Wang et al. (2018),
where and are estimated and true rotation matrix, respectively. And for the translation error we have norm of the difference of estimated and true translation .
4.1 Outlier Robustness
We performed a set of point registration experiments comparing the robustness of the proposed method comparing with ICP and NDT methods. Fig. 4 shows an example of frame with outlier points in blue. It can be seen that how the positional outliers impact and spread their associated surface normals on the unit sphere. The bottom plots illustrate the average of rotation and translation errors with different level of outliers injected into the pointclouds over trials. As seen, ICP and NDT methods have inconsistent and higher error with regard to outliers which makes them unreliable in practice. In contrast, the proposed method has relatively lower errors and consistent with the level of outliers.
4.2 Performance Evaluation
The results from comparison of different point registration methods are tabulated in table 1. The results show the average rotation and translation error on the sequence of same frames from the dataset. For NDT (Stoyanov et al. (2012)), the grid size is set as meters with aligns the pointclouds in a finercoarsertofiner manner, and for CICP (Du et al. (2018)) the scale factor is meter with a decay rate of 0.96. Fig. 5 represents an example of the point matching of two consecutive frames. The green is the pointcloud that is transformed back into the model pointcloud based on the estimated rigid transformation found by the proposed method.
Trials  ICP  ICPp2pl  CICP  GICP  NDT  Ours 

Avg.  1.27  0.57  1.162  0.23  0.52  0.66 
Avg.  0.403  0.164  0.260  0.025  0.070  0.018 
As seen in the table 1, the proposed algorithm outperforms other methods with regard to translation error and generates satisfying rotation errors compared with the stateoftheart NDT and GICP. It is reasonable to infer that the more information involved, the more accurate of the point registration algorithm. For instance, the ICP and CICP based on pointtopoint distance produce the worse matching results. Likewise, GICP and NDT utlize more geometrical information like curvatures and more abstracted covariance matrices, which guarantees the better results.
The proposed featurebased point registration method has two advantages over other methods: firstly only the directional information is utilized for rotation estimation and secondly the computationally correspondence phase in ICP or NDTbased methods is avoided, but the registration results are still competitive even with NDT or GICP. On the other hand, since the indoor pointcloud is free of most of missing data comparing with outdoor scenes, we expect our method would perform better in the presence of outliers, occluded, and missing data as presented in the previous experiment case.
Finally as stated in Section 3, although using moment estimate method makes the EM algorithm without convergence guarantee, since the algorithm utilizes the highly concentrated surface normals on the unit sphere (i.e. large ) the moment estimate is closely related to ML estimates. In fact, the empirical results from our experiment show that (7) the function is not decreasing after each iteration of the algorithm.
5 Conclusion
In this paper, our proposed featurebased method for 3D point registration was presented and evaluated with examples of indoor pointclouds. The algorithm incorporates surface normals in the scene as directional information to be used within our cohesive and probabilistic framework which improves the registration accuracy comparing to ICPbased methods. The method utilizes the fact that the translationinvariant property of surface normals decouples the estimation of rotation from translation. Additionally, the proposed method provides a robust mechanism that rejects outliers in the correspondence phase in a probabilistic fashion. The computation time comparison is not analyzed in this paper and one possible future direction can be reducing the computation time of the proposed algorithm.
References
 Clustering on the unit hypersphere using Von MisesFisher distributions. J. Mach. Learn. Res. 6, pp. 1345–1382. External Links: ISSN 15324435 Cited by: §3.2.
 A method for registration of 3D shapes. IEEE Trans. Pattern Anal. Mach. Intell. 14 (2), pp. 239–256. External Links: ISSN 01628828 Cited by: §1.
 Generalized iterative most likely orientedpoint (GIMLOP) registration. International Journal of Computer Assisted Radiology and Surgery 10 (8), pp. 1213–1226. External Links: ISSN 18616429 Cited by: §1, §2.2, §3.5.2.

Manopt, a Matlab toolbox for optimization on manifolds.
Journal of Machine Learning Research
15, pp. 1455–1459. Cited by: §3.5.1.  Robust rigid registration algorithm based on pointwise correspondence and correntropy. Pattern Recognition Letters. External Links: ISSN 01678655 Cited by: §4.2.
 Joint alignment of multiple point sets with batch and incremental expectationmaximization. IEEE Trans. Pattern Anal. Mach. Intell. 40 (6), pp. 1397–1410. Cited by: §1.
 Vision meets robotics: the KITTI dataset. International Journal of Robotics Research (IJRR). Cited by: Figure 2.
 Rigid and articulated point registration with expectation conditional maximization. IEEE Transactions on Pattern Analysis and Machine Intelligence 33 (3), pp. 587–602. Cited by: §1.

Spherical kmeans clustering
. Journal of Statistical Software 50, pp. 1–22. Cited by: §3.2.  Robust point set registration using Gaussian mixture models. Pattern Analysis and Machine Intelligence, IEEE Transactions on 33, pp. 1633 – 1645. Cited by: §1.
 The FisherBingham distribution on the sphere. Journal of the Royal Statistical Society. Series B (Methodological) 44 (1), pp. 71–80. External Links: ISSN 00359246 Cited by: §2.1, §3.5.2, §3.5.2.
 Comparison of surface normal estimation methods for range sensing applications. Proceedings  IEEE International Conference on Robotics and Automation, pp. 3206–3211. Cited by: §3.1.
 Linear leastsquares optimization for pointtoplane ICP surface registration. Cited by: §1.

Structural graph matching using the EM algorithm and singular value decomposition
. IEEE Trans. Pattern Anal. Mach. Intell. 23 (10), pp. 1120–1136. External Links: ISSN 01628828 Cited by: §1.  Robust generalized point cloud registration with orientational data based on expectation maximization. IEEE Transactions on Automation Science and Engineering (), pp. 1–15. External Links: ISSN 15455955 Cited by: §1.
 Means and averaging in the group of rotations. SIAM J. Matrix Anal. Appl. 24 (1), pp. 1–16. External Links: ISSN 08954798 Cited by: §3.5.3.
 Point set registration: coherent point drift. IEEE transactions on pattern analysis and machine intelligence 32, pp. 2262–75. Cited by: §1.
 A novel algorithm for clustering of data on the unit sphere via mixture models. Cited by: §3.5.2.
 Fitting mixtures of Kent distributions to aid in joint set identification. Journal of the American Statistical Association 96 (453), pp. 56–63. Cited by: §3.5.2.
 Challenging data sets for point cloud registration algorithms. The International Journal of Robotics Research 31 (14), pp. 1705–1711. Cited by: §4, Figure 5.
 A robust point matching algorithm for autoradiograph alignment. In Visualization in Biomedical Computing, pp. 277–286. External Links: ISBN 9783540707394 Cited by: §1.
 Towards 3D point cloud based object maps for household environments. Robot. Auton. Syst. 56 (11), pp. 927–941. External Links: ISSN 09218890 Cited by: §3.1.
 Semantic 3d object maps for everyday robot manipulation. Springer Publishing Company, Incorporated. External Links: ISBN 9783642354786 Cited by: §3.1.
 GeneralizedICP. Proc. of Robotics: Science and Systems, pp. . Cited by: §1.
 NICP: dense normal based point cloud registration. Proceedings of the … IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 742–749. Cited by: §1.
 Point set registration through minimization of the l2 distance between 3dndt models. In 2012 IEEE International Conference on Robotics and Automation, Vol. , pp. 5196–5201. External Links: ISSN Cited by: §4.2.
 DeLiO: decoupled LiDAR odometry. CoRR abs/1904.12667. External Links: 1904.12667 Cited by: §1.
 Accurate mixnormbased scan matching. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1665–1671. Cited by: §4.

Iterative point matching for registration of freeform curves and surfaces.
International journal of computer vision
13, pp. 119–152. Cited by: §3.1.