ICP的具体原理和步骤：
https://blog.csdn.net/qq_35102059/article/details/121693863?spm=1001.2014.3001.5501
构建误差函数：
首先针对两个点云，进行点云的数据关联（一般是投影到同一帧，使用kdtree找最临近），做一个相减构造误差函数，最小化误差函数求旋转R和平移t。
最小化误差函数过程的注意事项：
1.分别取两块点云的质心（一般是求均值，为已知），在误差函数的二范数中添加项，并构造。这一步去质心的目的是为了将R和t分解开来。
2.展开二范数，由质心的定义，可消去交叉相乘项，即可解耦旋转和平移。
3.第一项中只有R，第二项中有R和t。由第二项可知：对于任意的R，都存在一个t，使得第二项为0，即最小。所以，问题就可转化为用第一项先求R最小，再令第二项为0求t。
4.将第一项的二范数再展开，根据矩阵二范数的性质，且R为正交矩阵->RTR为单位矩阵，可消去多余的R，仅留交叉项中有R，问题即可转化为求交叉项最大。
5.对第一项展开式的交叉项做变换。由于x，y为3*1的向量，可知交叉项相乘为1*1的矩阵，即为一个常数，所以如下图（1），可直接由矩阵转换为矩阵的迹。
6.由矩阵乘法本质可知，两个矩阵交换相乘，迹不变，可得下图（2）过程。此步骤目的是将R提出来，将两个点云的坐标x，y合并，方便后续求解。?
7.所以如下图，问题转化为求矩阵迹的最大值。
?8.求矩阵最大值，有如下定理（推导过程如注释）。即在R为任意值的情况下，RH都小于一个正定矩阵（此正定矩阵就为最大值）。则求一个R，可以使得RH为正定矩阵（最大情况），此时的R即为要求的、使得第一项展开交叉项最大的R。?
9.假设RH最大的时候，为一个正定矩阵。凑此时的R和H，将H做SVD分解，取R=VUT，可凑出RH=AAT的形式。此时的A为如下图形式。
10.则R为H分解得到的VUT，t也可求得。
?完。。。