NDT registration principle

summary ：NDT It is a point cloud registration method of probability model . be relative to icp for ： It has a wide range of convergence , Insensitive to initial value . Speed and other advantages . But the precision is not icp high .

NDT Preliminary knowledge

Normal distribution

n Dimensional normal random process , The probability density function is ：
$$p(\vec{x})=\frac{1}{(2\pi {)}^{D/2}\sqrt{|\mathbf{\Sigma }|}}\exp \left(-\frac{(\vec{x}-\vec{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\vec{x}-\vec{\mu })}{2}\right)\text{\hspace{1em}(1)}$$
among $\vec{\mu },\Sigma$ Means mean , And covariance matrix , The calculation is as follows ：
$$\vec{\mu }=\frac{1}{m}\sum _{k=1}^m{\vec{y}}_k\text{\hspace{1em}(2)}$$

$$\Sigma =\frac{1}{m-1}\sum _{k=1}^m\left({\vec{y}}_k-\vec{\mu }\right){\left({\vec{y}}_k-\vec{\mu }\right)}^{\mathrm{T}}\text{\hspace{1em}(3)}$$

Gauss Newton method for solving nonlinear least squares

I will write a blog afterwards

NDT principle

(1) Objective function

NDT Full name ： normal distributions transform, Normal distribution transformation , It can be described as a kind of compact （ concise ） A method of representing a surface .
This transform maps the point cloud to a smooth surface representation , Described as a set of local probability density functions （PDF）, Each of these functions describes the shape of a partial surface . The local probability density function is used to represent the local point cloud .
When using NDT When performing scanning registration , By maximizing the position of the current point to be registered in the reference （ The benchmark ） Possibility of scanning the surface , So as to find the current posture of the point cloud to be registered . That is to maximize the following likelihood function ：
$$\Psi =\prod _{k=1}^{n}p\left(T\left(\vec{p},{\vec{x}}_k\right)\right)\text{\hspace{1em}(4)}$$
$\vec{p},{\vec{x}}_k,p(\vec{x})$ Namely ： Transformation matrix corresponding to attitude , Point cloud to be registered , The probability density function of the reference point cloud corresponding to the point cloud to be registered .PDF The probability density function is not necessarily a normal distribution , Able to describe local surfaces , And the probability density functions that are robust to outliers are all OK .
The maximization formula （4） Equivalent to minimization formula （5）：
$$-\log \Psi =-\sum _{k=1}^n\log \left(p\left(T\left(\vec{p},{\vec{x}}_k\right)\right)\right)\text{\hspace{1em}(5)}$$

(2) Simplify the objective function

left （a） It's a normal distribution （ Red ） And mixed distribution $\bar{p}(\vec{x})$（ normal 、 Uniform distribution , green ） Image , On the right side （b） For the corresponding -log logarithm . from （b） In the clear , The original normal distribution is relative to x There is no constraint , stay x The value of the function will also be greatly affected when the value of becomes larger , And the hybrid model will be right x The value of , Better robustness .（ Less affected by noise points ）

To observe the above （b） The red curve of $-log(p(x))$, Its extreme value is found to be positive infinity . To avoid extremes x Value causes y The value is infinite or infinitesimal , And avoid the influence of outliers , The normal probability density function （1） Change to the mixed distribution formula of normal distribution and uniform distribution （6）:
$$\bar{p}(\vec{x})=c_1\exp \left(-\frac{(\vec{x}-\vec{\mu }{)}^{\mathrm{T}}{\Sigma }^{-1}(\vec{x}-\vec{\mu })}{2}\right)+c_2{p}_o\text{\hspace{1em}(6)}$$

$p_o$ Is the ratio of outliers , constant c1 and c2 You can integrate the probability density function to be equal to 1 To make sure .
The probability density function formula （1） Approximate formula （6） after , The target function $-\log (\bar{p}(\vec{x}))$ The first and second derivatives of are complex , therefore , Then put the formula （6） Convert to Gaussian form for approximation ：
$$\tilde{p}\left({\vec{x}}_k\right)=-d_1\exp \left(-\frac{d_2}{2}{\left({\vec{x}}_k-{\vec{\mu }}_k\right)}^{\mathrm{T}}{\mathbf{\Sigma }}_k^{-1}\left({\vec{x}}_k-{\vec{\mu }}_k\right)\right)\text{\hspace{1em}(7)}$$
among ：
$$\begin{array}{rl}& d_3=-\log \left(c_2\right)\\ & d_1=-\log \left(c_1+c_2\right)-d_3\\ & d_2=-2\log \left(\left(-\log \left(c_1\exp (-1/2)+c_2\right)-d_3\right)/d_1\right)\end{array}\text{\hspace{1em}(8)}$$
The formula （7） omitted d3, Because it's a constant offset , The shape of its probability density function or the optimized parameters will not be changed .
Sum up , Given a series of point clouds $\mathcal{X}=\left\{{\vec{x}}_1,\dots ,{\vec{x}}_n\right\}$, Posture $\vec{p}$, Transformation matrix $T(\vec{p},\vec{x})$, the NDT score function $s(\vec{p})$ It can be expressed as ：
$$s(\vec{p})=-\sum _{k=1}^n\tilde{p}\left(T\left(\vec{p},{\vec{x}}_k\right)\right)\text{\hspace{1em}(9)}$$
score function Defined as log likelihood First order derivation of See know . That's the formula 9 It's the formula 5 First derivative of .

(3) Numerical solution

The probability density function requires the inverse of the covariance , Because for linear 、 For flat point clouds , Covariance is singular , And irreversible . In addition to 3D In the space , When the number of point clouds is less than 3, Matrices are also singular , Covariance matrix is irreversible . Therefore, the maximum eigenvalue is assumed , The next largest eigenvalue , The minimum eigenvalues are respectively ：${\lambda }_3{\lambda }_2{\lambda }_1$, When ${\lambda }_3$ Greater than 100 Times ${\lambda }_{2}orperson{\lambda }_1$ when , Make ${\lambda }_{1,2}^{\prime }={\lambda }_3/100$. New covariance matrix ${\mathbf{\Sigma }}^{\prime }=\mathbf{V}{\Lambda }^{\prime }\mathbf{V}$ Replace the original covariance matrix $\mathbf{\Sigma }$, $\mathbf{V}$ Is the matrix corresponding to the eigenvector $\mathbf{\Sigma }$ ：
$${\Lambda }^{\prime }=\left[\begin{array}{ccc}{\lambda }_1^{\prime }& 0& 0\\ 0& {\lambda }_2^{\prime }& 0\\ 0& 0& {\lambda }_3\end{array}\right]\text{\hspace{1em}(11)}$$
Using Newton's method , By optimizing the $s(\vec{p})$, Can find Attitude parameters $\vec{p}$ .
Newton’s method iteratively solves the equation $\mathrm{H}\Delta \vec{p}=-\vec{g}$, where $\mathrm{H}$ and $\vec{g}$are the Hessian matrix and gradient vector of$s(\vec{p})$. The gradient can be expressed as ：
$$g_i=\frac{\delta s}{\delta p_i}=\sum _{k=1}^n{d}_1{d}_2{\vec{x}}_k^{\prime \mathrm{T}}{\Sigma }_k^{-1}\frac{\delta {\vec{x}}_k^{\prime }}{\delta p_i}\exp \left(\frac{-d_2}{2}{\vec{x}}_k^{\prime \mathrm{T}}{\Sigma }_k^{-1}{\vec{x}}_k^{\prime }\right)\text{\hspace{1em}(11)}$$
The Hessian matrix is ：
$$\begin{gathered} H_{ij}=\frac{{\delta }^{2}s}{\delta p_i\delta p_j}=\sum _{k=1}^n{d}_1{d}_2\exp \left(\frac{-d_2}{2}{\vec{x}}_k^{\prime }{}^{\mathrm{T}}{\mathbf{\Sigma }}_k^{-1}{\vec{x}}_k^{\prime }\right)\left(-d_2\left({\vec{x}}_k^{\prime }{}^{\mathrm{T}}{\mathbf{\Sigma }}_k^{-1}\frac{\delta {\vec{x}}_k^{\prime }}{\delta p_i}\right)\left({\vec{x}}_k^{\prime }{}^{\mathrm{T}}{\mathbf{\Sigma }}_k^{-1}\frac{\delta {\vec{x}}_k^{\prime }}{\delta p_j}\right)+ \\ {\vec{x}}_k^{\mathrm{T}}{\mathbf{\Sigma }}_k^{-1}\frac{{\delta }^2{\vec{x}}_k^{\prime }}{\delta p_i\delta p_j}+\frac{\delta {\vec{x}}_k^{\prime }}{\delta p_j}{\mathbf{\Sigma }}_k^{-1}\frac{\delta {\vec{x}}_k^{\prime }}{\delta p_i}\right)\text{\hspace{1em}(12)} \end{gathered}$$

2D,3D NDT The gradient and Heissian The matrix form is the same , The difference is that the transformation matrix T（ See the original text for the formula , There is no formula posted here ）.2D 3D NDT The difference between them mainly lies in the transformation matrix , And the transformation matrix is different from the first-order and second-order matrix .

（4） Algorithm flow ：

The algorithm flow is as follows ：( Be careful Register scan ： Point cloud to be registered （source）, reference scan： Base point cloud （target）)

（5） comparison ICP The advantages of

NDT Class method ratio ICP This kind of algorithm has higher efficiency and wider convergence region , about 2D and 3D The application has a relatively mature solution , But in the absence of a good initial value , It will also fall into local optimization .

• Insensitive to initial value （ The initial value is also required ）
• No nearest neighbor search is required , Fast

NDT stay PCL application

#pragma once
/* * @Description:  Registration using normal distribution transformation  * https://www.cnblogs.com/li-yao7758258/p/6554582.html * http://robot.czxy.com/docs/pcl/chapter03/registration/#ndt * @Author: HCQ * @Company(School): UCAS * @Email: [email protected] * @Date: 2020-10-22 19:20:43 * @LastEditTime: 2020-10-22 19:26:04 * @FilePath: /pcl-learning/14registration Registration /4 Normal distribution transformation registration (NDT)/normal_distributions_transform.cpp */
 /*  The experiment of registration using normal distribution transformation  . among room_scan1.pcd room_scan2.pcd These point clouds contain the same room 360 Scanning data from different perspectives  */
#include <iostream>

#include <pcl/point_types.h>
#include <pcl/registration/ndt.h> //NDT( Normal distribution ) Register class header file 



	int ndt(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source,
		const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target,
		pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_source)
	{
    
		
		//  Initialize normal distribution (NDT) object 
		pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

		//  Set according to the scale of the input data NDT Related parameters 

		ndt.setTransformationEpsilon(0.03);   // Set the minimum conversion difference for the termination condition 

		ndt.setStepSize(0.1);    // by more-thuente Set the maximum step size for line search 

		ndt.setResolution(2.0);   // Set up NDT The resolution of the mesh structure （voxelgridcovariance）

		// Adding the maximum number of iterations limit can increase the robustness of the program and prevent it from running too long in the wrong direction 
		//ndt.setMaximumIterations(50);

		ndt.setInputSource(cloud_source);  // Source point cloud 
		// Setting point cloud to be aligned to.
		ndt.setInputTarget(cloud_target);  // Target point cloud 


		// Use the unit matrix as the initial value of the matrix 
		ndt.align( *transformed_source);

		// This place output_cloud It cannot be used as the final source cloud transformation , Because the point cloud is filtered 
		std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
			<< " score: " << ndt.getFitnessScore() << std::endl;
		std::cout << "tf matrix:\n" << ndt.getFinalTransformation() << std::endl;
		//  Transform the filtered input point cloud using the transform you created 
		pcl::transformPointCloud(*cloud_source, *transformed_source, ndt.getFinalTransformation());

		//  Save the converted source cloud as the final transformation output 
		//pcl::io::savePCDFileASCII(transformed_source_name, *output_cloud);



		return 0;
	}

NDT The source code parsing

To be changed

Reference material

3d ndt The original paper 《Scan registration for autonomous mining vehicles using 3D-NDT》
3d ndt Doctoral Dissertation （ A more detailed ）《The Three-Dimensional Normal-Distributions Transform — an Efficient Representation for Registration, Surface Analysis, and Loop Detection》
ndt And icp Compare 《 A survey of laser scanning matching methods 》
https://zhuanlan.zhihu.com/p/96908474
https://www.modb.pro/db/101593
https://www.codetd.com/en/article/13213393