## 2 ICP and NDT
### Ndt  
The basic model of the NDT algorithm is: Given source point cloud \(\mathbf{P}=\{\mathbf{p_{1}},\mathbf{p_{2}},\mathbf{p_{3}},\ldots,\mathbf{p_{m}}\}\) and target point cloud \(\mathbf{Q}=\{\mathbf{q_{1}},\mathbf{q_{2}},\mathbf{q_{3}},\ldots,\mathbf{q_{n}}\}\), \(\mathbf{P}\), \(\mathbf{Q}\subset\mathbf{R^{3}}\). Firstly, the NDT algorithm divides the space covered by the target point cloud into voxels. Then, based on the distribution of the midpoints of each voxel, the probability density function (PDF) is calculated for each voxel. Suppose that the position of the point on the target point cloud is obtained by random processing of the D-dimensional normal distribution. The probability function of the point \(\mathbf{x}\) in each voxel is:  
\[F(x)=\frac{1}{(2\pi)^{\frac{D}{2}}\sqrt{|\mathbf{\Sigma}|}}\exp\left(-\frac{(\mathbf{x }-\mathbf{\mu})^{T}\mathbf{\Sigma}^{-1}(\mathbf{x}-\mathbf{\mu})}{2}\right) \tag{3}\]where \(D\) represents the dimension, \(\mathbf{\mu}\) and \(\mathbf{\Sigma}\) represent the mean vector and covariance matrix of all points in the voxel where \(\mathbf{x}\) is located, respectively, \(\mathbf{\mu}\) and \(\mathbf{\Sigma}\) are calculated by the following formulas:  
\[\mathbf{\mu}=\frac{1}{k}\sum_{i=1}^{k}\mathbf{q}_{i} \tag{4}\]  
\[\mathbf{\Sigma}=\frac{1}{k-1}\sum_{i=1}^{k}(\mathbf{q}_{i}-\mathbf{\mu})(\mathbf{q}_{i}-\mathbf{\mu })^{T} \tag{5}\]  
\(\mathbf{q}_{i=1,...,k}\) is the position coordinates of all points contained in the voxel.  
Finally, the pose transformation \(\mathbf{T}\) is obtained, so that the overall likelihood of the transformed source point cloud in its corresponding probability density function reaches the maximum, that is  
\[\mathbf{R},\mathbf{t}\leftarrow\operatorname*{argmax}_{\mathbf{R},\mathbf{t}}\prod_{i=1}^{m}F (\mathbf{R}\mathbf{p}_{i}+\mathbf{t}) \tag{6}\]  
Using the Newton iteration method, find the optimal transformation parameters to complete the point cloud registration.  
```
1:Input: Source Point Cloud \(\mathbf{P}=\{\mathbf{p}_{1},\mathbf{p}_{2},\mathbf{p}_{3},\dots,\mathbf{p}_{m}\}\)
2:Target Point Cloud \(\mathbf{Q}=\{\mathbf{q}_{1},\mathbf{q}_{2},\mathbf{q}_{3},\dots,\mathbf{q}_{n}\}\)
3:Initial Transformation \(\mathbf{T}_{0}\)
4:Output: Correct transformation matrix \(\mathbf{T}\) matching \(\mathbf{P}\) and \(\mathbf{Q}\)
5:\(\mathbf{T}\leftarrow\mathbf{T}_{0}\)
6:Target point cloud space voxel segmentation \(V=\{v_{1},v_{2},\cdots,v_{l}\}\)
7:for\(i\gets 1\)
8:Find voxel \(v_{k}\) containing point \(\mathbf{q}_{i}\)
9:Store \(\mathbf{q}_{i}\) in \(v_{k}\)
10:end for
11:for\(j\gets 1\) to \(l\)
12:\(\mathbf{Q}^{\prime}=\{\mathbf{q}^{\prime}_{1},\mathbf{q}^{\prime}_{2},\cdots,\mathbf{q}^{ \prime}_{m^{\prime}}\}\gets v_{j}\)
13:if\(n^{\prime}\leq\mathbf{5}\) then
14:delete \(v_{j}\) // Delete grid cells with too few elements
15:else
16:\(\mathbf{\mu}_{j}\leftarrow\frac{1}{n^{\prime}}\sum_{k=1}^{n^{\prime}}\mathbf{q}^{ \prime}_{k}\)
17:\(\mathbf{\Sigma}_{j}\leftarrow\frac{1}{n^{\prime}-1}\sum_{k=1}^{n^{\prime}}\left( \mathbf{q}^{\prime}_{k}-\mathbf{\mu}_{j}\right)\left(\mathbf{q}^{\prime}_{k}-\mathbf{\mu}_{j} \right)^{T}\)
18:endif
19:end for
20:\(\mathbf{T}\leftarrow\operatorname*{argmax}_{\mathbf{T}}\prod_{i=1}^{m}F(\mathbf{T}\mathbf{p} _{i})\)
```