slam 3D-3D ICP （六）

目录

一、介绍ICP（Iterative Closest Point）

二、求解ICP

1、使用线性代数SVD求解

2、线性SVD求解方法总结

2.1、计算两幅图像匹配好的3D点 的质心坐标,然后计算去质心坐标：

2.2、计算旋转矩阵

2.3计算t

2、非线性优化方法

---

一、介绍ICP（Iterative Closest Point）

      前面两节内容我们已经介绍过2D-2D,2D-3D，这节我们来介绍3D-3D的算法。3D-3D就是两张深度图像来生成点云。使用深度相机进行拍照，与前面两种方法相比深度相机的成本更高。

二、求解ICP

     假设两张深度图像的三维坐标,

     欧式变换R,T使得

                                                                                                                                                   (2.1)

R——相机姿态变化的旋转向量

t——相机姿态变化的平移向量

注：仅考虑两组3D点之间的变换时和相机并没有关系

1、使用线性代数SVD求解

1.1奇异值介绍

奇异值分解（singular value decomposition，SVD）是一种矩阵因子分解的方法。

         如上图所示，一个由7×8个像素组成的图像，在中间有个3×3空心的正方形。对该图像进行奇异值分解，可以纵向分解成三个部分，一部分是用红色圈出来全部都是白色的小方块，一部分是用绿色圈出来其中有三个黑方块的部分，一部分是用紫色圈出来的具有两个黑方块的部分。

1.2奇异值计算

                                                                                                 (2.2)

例：

1.3 定义误差项              

1.4 最小二乘法的问题，使误差平方和达到极小的R,T

                                                                                                                             (2.3)

1.5 定义两组点的质心

                                                                                           

                          

2、线性SVD求解方法总结

2.1、计算两幅图像匹配好的3D点 的质心坐标,然后计算去质心坐标：

                                                                                  

2.2、计算旋转矩阵

                                                                  

2.3计算t

                                                                                

a、R的计算

b、通过SVD解出最优的R

                                                                                

 当W满秩时                  

2、非线性优化方法