Voxelized GICP for Fast and Accurate 3D Point Cloud Registration
Kenji Koide1 , Masashi Yokozuka1 , Shuji Oishi1 , and Atsuhiko Banno1

Abstract— This paper proposes the voxelized generalized
iterative closest point (VGICP) algorithm for fast and accurate three-dimensional point cloud registration. The proposed
approach extends the generalized iterative closest point (GICP)
approach with voxelization to avoid costly nearest neighborsearch while retaining its accuracy. In contrast to the normal
distributions transform (NDT), which calculates voxel distributions from point positions, we estimate voxel distributions
by aggregating the distribution of each point in the voxel.
The voxelization approach allows us to efficiently process the
optimization in parallel, and the proposed algorithm can run
at 30 Hz on a CPU and 120 Hz on a GPU. Through evaluations
in simulated and real environments, we confirmed that theaccuracy of the proposed algorithm is comparable to GICP, but
is substantially faster than existing methods. This will enable thedevelopment of real-time 3D LIDAR applications that require
extremely fast evaluations of the relative poses between LIDAR
frames.

I. I NTRODUCTION
Registration of three-dimensional (3D) point clouds is a
crucial task for many 3D LIDAR applications, such as calibration, localization, mapping, and environment recognition.
There are two popular point cloud registration methods for
3D LIDARs: Generalized Iterative Closest Point (GICP) [1]and the Normal Distributions Transform (NDT) [2], [3].
Whereas GICP extends the classical ICP algorithm [4] in a
distribution-to-distribution comparison mannar for accurate
registration, the NDT takes advantage of the voxelization
approach to avoid costly nearest neighbor search and improve
processing speed. However, both methods have their own
weaknesses. Because GICP and other ICP-variants highly depend on nearest neighbor search, it is sometimes hard to run
them in realtime on a computer with limited computational
power if the number of points is large. In contrast, the NDT is
typically very sensitive to the choice of the voxel resolution.
The best voxel resolution depends on the environment andsensor properties, and if we do not choose an appropriate
resolution, the registration accuracy of the NDT drastically
drops.
In this paper, we propose the Voxelized GICP (VGICP)
algorithm for fast and accurate 3D point cloud registration.
The voxelization approach enables the proposed algorithm
to efficiently run in parallel, and our VGICP implementation
can process a point cloud containing 15,000 points at 30 Hz
on a CPU and 120 Hz on a GPU. By aggregating the distribution of all points in a voxel (multi-point distributions toa single-voxel distribution), we estimate the distributions of
1 All the authors are with the Department of Information Technology
and Human Factors, the National Institute of Advanced Industrial Science and Technology, Umezono 1-1-1, Tsukuba, 3050061, Ibaraki, Japan,

k.koide@aist.go.jp

the voxels robustly. In contrast to the NDT, which estimatesvoxel distributions from point positions, this approach yields
valid voxel distributions even when there are few points ina voxel, yielding an algorithm that is robust to changes invoxel resolution. The evaluation results obtained in simulated
and real environments illustrate that the proposed VGICP
algorithm shows registration accuracy that is comparable tothat of GICP and outperforms other methods in terms ofprocessing speed.
The contribution of this paper is three-fold. First, we
propose a multi-point distribution aggregation approach torobustly estimate the distribution of a voxel from a smaller
number of points. Second, we propose the VGICP algorithm,
which is as accurate as GICP but substantially faster thanexisting methods. Third, the implementations are available
from a public repository 1 . The repository contains the implementation of the proposed VGICP as well as a parallelized
implementation of GICP.
II. R ELATED W ORK
A. GICP
There are many variants of the classic ICP algorithm,
such as Trimmed ICP [5] and Normal ICP [6]. GICP [1]is one of the most popular ICP variants. GICP extends
the classical ICP algorithm in a distribution-to-distribution
matching fashion. Although it is known for its good accuracy, this algorithm (and other ICP-variants) highly dependon nearest neighbor search to associate the closest points.
Although an efficient KD-tree-based search is typically used,
the nearest neighbor search often becomes a bottleneck sothat the algorithm cannot run in realtime when the numberof points is large. Furthermore, a method based on a nearest
neighbor search is not suitable for optimization on a GPU
because it makes heavy use of conditional branches, which
decreases the performance of the GPU significantly.
B. NDT
The NDT [2] takes a voxel-based association approachinstead of an exact nearest neighbor search. This algorithm
first splits an input point cloud into a set of voxels and fitsa normal distribution to the points in each voxel. Then, italigns another point cloud to the voxelized one by finding
the transformation that maximizes the likelihood of the input
points under the distributions of the voxels. Because the
NDT avoids the costly nearest neighbor association, it istypically much faster than ICP-variant algorithms. D2D-NDT(Distribution-to-Distribution NDT) [7] is an extension of
1 https://github.com/koide3/fast_gicp

the NDT that voxelizes both the source and target point
clouds and calculate the distance between the distributionsof the source and target voxels. Its comparison scheme issimilar to that of GICP, and [8] suggests that D2D-NDT issuperior to the classic NDT in terms of accuracy. However,
the accuracy of the NDT and its variants depends on the
choice of the voxel size. To obtain the best performance with
the NDT, we need to carefully choose an appropriate voxel
size depending on the sensor and environment properties.
Some studies have proposed methods to make the NDTrobust to hyper-parameter changes (e.g., multi-resolution [9]and trilinear voxel smoothing [3]). However, those extensions
have a negative influence on the processing speed of the
NDT.
C. Feature-based registration
Feature-based registration methods first extract a number of representative features from input point clouds andthen estimate the transformation from the feature correspondences. Many features have been proposed for point cloud
registration, such as basic plane and edge features [10], [4],Fast Point Feature Histogram (FPFH) [11], and Signatureof Histograms of OrienTations (SHOT) [12]. Because thesefeatures enable correspondences between point clouds to berobustly found, feature-based methods are typically robust
to initial pose errors (some do not even require an initial
guess). However, because feature-based methods use only alimited number of features (which is usually much smaller
than the number of input points), their accuracy is worse
than that of point-based methods. Therefore, in a typicaluse case, a point-based fine registration is performed after
a feature-based method. Feature-based and point-based registration methods are orthogonal, and they should be used tocomplement each other.

In this section, we first explain the GICP algorithm andthen extend it in a one-to-multiple distribution correspondence fashion to derive our VGICP algorithm.
A. GICP algorithm
We consider the estimation of the transformation T, whichaligns a set of points A = {a0 , · · · , aN } (source point cloud)with respect to another set of points B = {b0 , · · · , bN }(target point cloud). Following the classic ICP algorithm, we
assume that the correspondences between A and B are givenby nearest neighbor search: bi = Tai . The GICP algorithm[1] models the surface from which a point was sampled asa Gaussian distribution: ai ∼ N (âi , CiA ), bi ∼ N (b̂i , CiB ).
Then, we define the transformation error as follows:(1)

The distribution of di is given by the reproductive propertyof the Gaussian distribution asdi ∼ N (b̂i − Tâi , CiB + TCiA TT ),= N (0, CiB + TCiA TT ).

T= arg min

(2)
(3)

i
X

T

dTi (CiB + TCiA TT )−1 di .

(5)i

The covariance matrix of each point is typically estimatedfrom its k neighbors (e.g., k = 20). Following the suggestionin [1], each covariance matrix is regularized by replacing
its eigenvalue with (1, 1, ). This regularization makes GICP
work as a plane-to-plane ICP.
B. Voxelized GICP algorithm
To derive the voxelized GICP algorithm, we first extend
Eq. (1) so that it calculates the distances between ai and its
neighbor points {bj |kai − bj k < r} as follows:Xdˆ0i =b̂j − Tâi .
(6)jThis equation can be interpreted as smoothing the target point
distributions. Then, similar to Eq. (3), the distribution of d0i
is given bydˆ0i ∼ (µdi , C di ),Xµdi =(b̂j − Tâi ) = 0,

(7)
(8)

j

C di =X(CjB + TCiA TT ).

(9)j

We estimate the transformation T that maximizes the log
likelihood of Eq. (7) as follows:X
T = arg mind˜Ti C̃i−1 d˜i ,
(10)
T

d˜i =

III. P ROPOSED M ETHOD

dˆi = b̂i − Tâi .

The GICP algorithm finds the transformation T that maximizes the log likelihood of Eq. (3) as follows:
X
T = arg max
log(p(di )),
(4)

X

i

(bj − T ai ) ,(11)CjB + T CiA T T .

(12)

j

C̃i =

X
j

To efficiently calculate the above equation, we modify it toX
T = arg min
Ni d˜Ti C̃i−1 d˜i ,
(13)
T

d˜i =

P

j bj

i

− T ai ,
N
P i B
j Cj
C̃i =
+ T CiA T T .
Ni

(14)
(15)

where Ni is the number of neighbor points. Eq. (13) suggeststhat we can efficiently compute the objective function by
substituting the mean of the distributions of the points (bjand C j ) around ai for bi and CiB in Eq. (5) and weighting
the function by Ni .
We can naturally adapt
this equation P
to voxel-based calcuPCBblation by storing b0i = Ni j and Ci0 = Nij in each voxel.
Fig. 1 illustrates the correspondence models used in
GICP, the NDT, and our VGICP. GICP employs the nearest

Algorithm 1 VGICP algorithm
1: Point clouds : A = {a0 , · · · , aN }, B = {b0 , · · · , bM }A
2: Covariances : C A= {C0A , · · · , CN}, C B =B{C0B , · · · , CM}

3: Initial guess: T̃(a) GICP(b) NDT(c) VGICP

Fig. 1: Correspondence models for distance calculation in(a) GICP, (b) the NDT, and (c) VGICP. The red circles indicate a source point and the blue circles indicate target points. (GICP: nearest distribution-to-distribution,
NDT: voxel-based point-to-distribution, VGICP: voxel-based
distribution-to-multi-distribution.) The VGICP model yields
a valid distribution even when a voxel contains only a few
points.

distribution-to-distribution correspondence model, which isreasonable, but relies on costly nearest neighbor search. For
fast registration, the NDT uses the point-to-voxel-distribution
correspondence model. However, we need at least four points(more than ten in practice) to calculate a 3D covariancematrix. If the number of points in the voxel is low, thecovariance matrix becomes corrupted. Our VGICP exploits
single-to-multiple distributions in the voxel correspondences
to deal with the case in which only a few points fall withina voxel. Because it calculates a voxel distribution from point
distributions, it yields a proper covariance matrix even whenthe voxel contains only one point.
C. Implementation
Algorithm 1 describes the registration procedure of VGICP in detail. As stated above, the VGICP algorithm does not
require costly nearest neighbor search during optimization,and it can hence leverage CPU and GPU parallel processing.
For pose optimization, we choose the Gauss–Newton optimizer because it converges quickly and requires no hyperparameters, unlike quasi-Newton methods.
We implemented three versions of the VGICP algorithm:
single-threaded, multi-threaded, and GPU processing. All
versions first estimate the covariance matrix of each point
using a KD-tree-based nearest neighbor search [13]. This
covariance estimation is parallelized in the multi-threaded
and GPU processing versions. We also implemented a GPUbased brute force nearest neighbor search. As the baseline,
we implemented the GICP algorithm in addition to VGICP.
The GICP implementation is also parallelized by CPU multithreading, is but not implemented on the GPU because it
relies on KD-tree nearest neighbor search, which is notsuitable for the GPU.
IV. E XPERIMENT
A. Simulated environment
We evaluated the proposed VGICP algorithm in a simulated environment. For this evaluation, we developed arealistic LIDAR data simulator. To generate realistic LIDAR

4: procedure VGICP(A, B, C A , C B , T̃)5:T ← T̃
6:
V ← VOXELIZATION(B, C B )7:
while T is not converged do
8:
e = [], J = []9:for i ∈ {0, · · · , N } dovoxel index ← floor(ai /voxel resolution)
if voxel index ∈/ V then . Not fall in a voxel
continue
13:
ei , Ji ← Cost(T, ai , CiA , v.µ, v.C, v.N )14:e ← e ∪ ei , J ← J ∪ J i
15:
δT ← −(J T J)−1 J T e . Gauss-Newton update
16:
T ← T  δT
return T
17: procedure VOXELIZATION (B, C B )18:
V ← []
19:
for j ∈ {0, · · · , M } do
20:
voxel index = cast to int (bj /voxel resolution)
21:
if voxel index ∈/ V then22:
V[voxel index] ← (µ = 0, C = 0, N = 0)23:
V[voxel index].µ ← V[voxel index].µ + bj
24:
V[voxel index].C ← V[voxel index].C + CjB
25:
V[voxel index].N ← V[voxel index].N + 1
26:
for v ∈ V do
27:v.µ ← v.µ/v.N
28:
v.C ← v.C/v.N
return V
10:
11:
12:

data, the simulator first renders an omnidirectional depth
image and then performs ray casting to generate Velodynelike rotating LIDAR data. Fig. 2 shows examples of point
clouds generated by our simulator and Microsoft AirSim[14]. Whereas our simulator generates realistic point clouds,
Airsim and other popular simulators generate LIDAR datafrom collision models, and thus their point clouds are notrealistic.
With this simulator, we generated a sequence of LIDAR
point clouds (P0 , · · · , PN ) and corresponding sensor poses(R̂0 , · · · , R̂N and p̂0 , · · · , p̂N ). We used the parameters of
Velodyne’s HDL-32e sensor to simulate a real sensor model.
We applied a scan matching method between consecutive
frames and estimated the sensor pose (Rt , pt ) at time t byaccumulating the scan matching results (i.e., the scan matching odometry). Following [15], we calculated the absolute
trajectory error (ATE) and relative error (RE) to evaluate theaccuracy of the scan matching methods.
The ATE is defined as follows:

(a) Our LIDAR simulator.

(b) Microsoft Airsim [14].

Fig. 2: Examples of point clouds generated by our simulator and Microsoft AirSim. AirSim generates point clouds fromcollision models, and thus objects in the LIDAR data have shapes that are too simplified (see the points on the trees, cars,
and buildings), and objects without collision models (pedestrians) do not appear in the point clouds. Our simulator performs
raycasting on an omnidirectional depth image to generate realistic point clouds.

ATErot =ATEpos =1 X
k∠ (∆Ri ) k2
N i
! 12
1 X
2
,
k∆pi k
N i! 12

TABLE I: Absolute trajectory errors.
,

(16)

(17)

where ∆Ri = Ri (R̂i )−1 and ∆pi = pi − ∆Ri p̂i . Note thatthe estimated sensor trajectories are aligned with respect tothe ground truth using [16]. The RE is defined as follows:RErot = ∠ (δRk ) = ∠ Re R̂eT ,(18)
REpos = kpe − δRk p̂e k2 ,(19)

where Re = Rt−1 Rt+N and pe = pt − pt+N . Parameters t
and t + N define the size of the window used to evaluate theRE. We set the window size to 1, 5, and 25 m (sensor travel
distance) and evaluated the RE using a sliding window.
Fig. 3 shows the REs of different registration methods,
and the absolute trajectory errors are shown in Table I.
We evaluated our VGICP and GICP implementations aswell as the ICP, GICP, and NDT implementations in Point
Cloud Library (PCL). For VGICP and the NDT, we tested
several voxel resolutions to evaluate the stability of theirperformance with respect to changes in hyper-parametervalues.
The accuracy of the NDT heavily relies on the choice ofvoxel resolution. We can see that the registration accuracyof the NDT largely drops for voxel resolutions of 1, 2, and 6
m compared to the best one. In this evaluation, we observed
the best accuracy with the NDT when the voxel resolution
is 4.0 m. This value is, however, too large to capture thefine details of the environment, and thus the accuracy of the
NDT is worse than that of GICP.
The GICP-based algorithms show superior accuracy compared to the classic ICP algorithm. Our VGICP and GICP
implementations yield slightly better accuracy than does the
PCL implementation of GICP. This might be due to the
choice of the optimizer (our implementations use Gauss–
Newton, which can be faster and more accurate than the[m][°]
GICP(ours)
0.562
1.696
VGICP(1.0m)
0.624
2.777
GICP(PCL)
0.954
1.724
75.34932.171ICP
NDT(4.0m)
7.952
23.971
Bold indicates the top three results.

Broyden–Fletcher–Goldfarb–Shanno (BFGS) algorithm usedin the PCL version of GICP). We note that our VGICP
algorithm shows consistent results over a wide range of voxel
resolutions thanks to the proposed voxelization approach,which yields a valid distribution even when the numberof points in a voxel is low. This result illustrates that the
proposed VGICP algorithm shows comparable accuracy withrespect to GICP and is robust to hyper-parameter changes.
Fig. 4 shows the average processing times of the registration methods. All methods were run on an Intel Corei9-9900K and NVIDIA Geforce RTX2080Ti. For our GICP
and VGICP implementations, we evaluated both the single
and multi-threaded versions. We also evaluated the GPU
version of our VGICP algorithm. The results show that oursingle-thread implementations (GICP: 189 msec, VGICP:
156 msec) are faster than the reference GICP implementationin PCL (201 msec). Because the preprocessing part (theestimation of the covariance matrix of each point) is thesame for both GICP and VGICP, they spend the same amountof time for preprocessing. The following pose optimization
part is greatly sped up by the voxelization approach. Thissuggests that in scenarios in which the target point cloudis not frequently changed (e.g., keyframe-based odometry
estimation), we can obtain more benefits from the highefficiency of the VGICP algorithm. With multi-threading, ourimplementations of GICP and VGICP are further sped up,
taking only 68 msec and 50 msec, respectively. Our GPUimplementation of VGICP is extremely fast and takes only
6 msec to optimize. It is worth mentioning that the GPU-

Rotation error (deg / m)Translation error (%)

20

10

0
1

5
Distance traveled (m)

25

1

5
Distance traveled (m)

25

1.0

0.5

0.0

Fig. 3: REs of different registration methods.

Fig. 4: Average processing time per scan.

based brute-force nearest neighbor search is slower than the
CPU-based parallel KD-tree in this evaluation. However, it
would be a reasonable choice for computers with a lowspecification CPU and a high-performance GPU such as the
NVIDIA Jetson.
B. Real environment
We recorded eight LIDAR sequences in the environmentshown in Fig. 5 using a Velodyne HDL-32e sensor. The
length of each trajectory is about 120m, and the numberof points in each frame is about 15,000. Similar to theevaluation in Sec. IV-A, we estimated the sensor trajectory by
applying registration algorithms between consecutive frames.
For our implementations, we reused the calculated covariance matrices of an input point cloud in the registration of
the next frame to reduce the preprocessing cost. For eachsequence, we aligned the last frame with respect to the first
frame using GICP and used the result as ground truth.
Table II shows the pose estimation errors at the last frames(i.e., the accumulated registration errors). ICP and the NDT
obtain worse results than the GICP-based algorithms. The
NDT obtains the best accuracy (2.717m and 0.164°error)
when the voxel resolution is set to 1.0 m. However, theaccuracy drastically drops when the voxel resolution is too
small or too large (7.119m and 0.443°for 0.5m resolution;
5.590m and 0.149°for 4.0m resolution).
The PCL-based GICP obtains a better result than do ICP
and the NDT (1.316m and 0.051°). However, because it
heavily relies on costly nearest neighbor search, it is theslowest of the evaluated methods (5.2 fps).
Our GICP implementation is substantially sped up by
multi-threading and the fast Gauss–Newton optimizer (20.5to develop an IMU-LIDAR joint optimization-based localization method with VGICP, which requires an extremely fastevaluation of the relative poses between keyframes.
ACKNOWLEDGEMENT
This work was partly supported by a project commissionedby the New Energy and Industrial Technology Development
Organization (NEDO).
R EFERENCES

START/END

Fig. 5: Experimental environment.
TABLE II: Processing speed and transformation errors
MethodVGICP (0.5m)
VGICP (1.0m)
GICP (ours)
GICP (PCL)
NDT (0.5m)
NDT (1.0m)
NDT (2.0m)
NDT (4.0m)
ICP

FPS (CPU/GPU)
Translation[m]28.7/116.5
0.852 ± 0.289
30.4/120.1
1.177 ± 0.456
20.5
0.893 ± 0.210
5.2
1.316 ± 0.310
8.3
7.119 ± 5.640
10.3
2.717 ± 2.645
10.4
3.290 ± 1.420
9.3
5.590 ± 2.177
11.0
10.192 ± 4.365
Bold indicates the top three results.

Rotation[°]0.049 ± 0.013
0.048 ± 0.026
0.045 ± 0.023
0.051 ± 0.022
0.443 ± 0.331
0.164 ± 0.088
0.116 ± 0.066
0.149 ± 0.077
0.371 ± 0.106

fps). It also obtains a higher accuracy than the PCLbased GICP (0.893m and 0.045°). The VGICP algorithm
has almost the same accuracy as our GICP implementation(0.852m and 0.049°) when the voxel resolution is set to 0.5
m. Although the accuracy of VGICP slightly drops (1.177m
and 0.048°) when the voxel resolution is changed to 1.0 m,
the result is still comparable to that of GICP. Our VGICP
algorithm leverages CPU multi-threading, which results inthe best processing speed (about 30 fps). Furthermore, it
becomes extremely fast on the GPU (about 120 fps).
V. C ONCLUSION AND F UTURE W ORK
In this study, we proposed the voxelized GICP algorithm.
The proposed VGICP is as accurate as GICP because it
utilizes the voxel-based association approach. The evaluation
results in the simulated and real environments show that the
proposed method shows a superior processing speed (30fps
on a CPU and 120 fps on a GPU) and is robust to voxel
resolution changes.
We plan to evaluate and improve the convergence of the
propose VGICP algorithm because it employs the voxelization approach which may affect the registration result when
the initial guess is not close to the true pose. We also plan

[1] A. Segal, D. Haehnel, and S. Thrun, “Generalized-ICP,” in Robotics:
Science and Systems. Robotics: Science and Systems Foundation,
jun 2009.
[2] P. Biber and W. Strasser, “The normal distributions transform: a
new approach to laser scan matching,” in IEEE/RSJ International
Conference on Intelligent Robots and Systems. IEEE, Oct. 2003.
[3] M. Magnusson, A. Nuchter, C. Lorken, A. Lilienthal, and J. Hertzberg,“Evaluation of 3d registration reliability and speed - a comparison of
ICP and NDT,” in IEEE International Conference on Robotics andAutomation. IEEE, may 2009.
[4] J. Zhang and S. Singh, “LOAM: Lidar odometry and mapping inreal-time,” in Robotics: Science and Systems. Robotics: Science andSystems Foundation, jul 2014.
[5] D. Chetverikov, D. Svirko, D. Stepanov, and P. Krsek, “The trimmed
iterative closest point algorithm,” in Object recognition supported byuser interaction for service robots. IEEE, Aug. 2002.
[6] J. Serafin and G. Grisetti, “NICP: Dense normal based point cloud
registration,” in IEEE/RSJ International Conference on Intelligent
Robots and Systems. IEEE, sep 2015.
[7] T. Stoyanov, M. Magnusson, H. Andreasson, and A. J. Lilienthal, “Fastand accurate scan registration through minimization of the distance
between compact 3d NDT representations,” International Journal of
Robotics Research, vol. 31, no. 12, pp. 1377–1393, sep 2012.
[8] M. Magnusson, N. Vaskevicius, T. Stoyanov, K. Pathak, and A. Birk,“Beyond points: Evaluating recent 3d scan-matching algorithms,” inIEEE International Conference on Robotics and Automation. IEEE,may 2015.
[9] E. Takeuchi and T. Tsubouchi, “A 3-d scan matching using improved
3-d normal distributions transform for mobile robotic mapping,” inIEEE/RSJ International Conference on Intelligent Robots and Systems.
IEEE, oct 2006.
[10] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and groundoptimized lidar odometry and mapping on variable terrain,” inIEEE/RSJ International Conference on Intelligent Robots and Systems.
IEEE, oct 2018.
[11] R. B. Rusu, N. Blodow, and M. Beetz, “Fast point feature histograms(FPFH) for 3d registration,” in IEEE International Conference on
Robotics and Automation. IEEE, may 2009.
[12] S. Salti, F. Tombari, and L. D. Stefano, “SHOT: Unique signatures ofhistograms for surface and texture description,” vol. 125. Elsevier,aug 2014, pp. 251–264.
[13] J. L. Bentley, “Multidimensional binary search trees used for associative searching,” vol. 18, no. 9. ACM, sep 1975, pp. 509–517.
[14] S. Shah, D. Dey, C. Lovett, and A. Kapoor, “AirSim: High-fidelity
visual and physical simulation for autonomous vehicles,” in Field andService Robotics. Springer, nov 2017, pp. 621–635.
[15] Z. Zhang and D. Scaramuzza, “A tutorial on quantitative trajectory
evaluation for visual(-inertial) odometry,” in IEEE/RSJ International
Conference on Intelligent Robots and Systems. IEEE, oct 2018.
[16] S. Umeyama, “Least-squares estimation of transformation parameters
between two point patterns,” vol. 13, no. 4. IEEE, apr 1991, pp.
376–380.