基于高斯牛顿的点-点，点-线，点到面ICP过程推导

基于高斯牛顿的点-点，点-线，点到面ICP过程推导

简介

最近在学习深蓝学院的<<多传感器融合定位>>这一课程，本身也是个半路出家的SLAM小白，也挺感兴趣的，就尝试一下了，目前做了两章作业，其中第一章有道题需要自己手写ICP或者NDT的匹配，因为PCL_ICP默认是使用SVD分解的，所以这里尝试使用高斯牛顿的迭代法来求解点-点的ICP匹配，同时结合之前看的LIVOX-LOAM里面的匹配，有个用点到面，点到线的匹配，在这里做个总结。
自己也是个小白，如果这里有什么地方错误的还请大家指教，请轻喷，我及时改正！再有就是要感谢高翔博士，贺一家博士的VIO课程，对后端优化讲解的很清楚，再就是<<多传感器融合定位>>课程的任乾任大佬的开源分享。
更新不易，如需转载，请著名出处，谢谢。

1 回顾

1.1 优化问题

对于一个多维的最小二乘优化问题，我们希望寻找一个$x\text{x}x$，能够使得目标函数达到最小，即：
$$\underset{x\text{x}x}{\mathrm{argmin}}F\text{F}F(x\text{x}x)=\underset{x\text{x}x}{\mathrm{argmin}}({\Sigma }_{i=1}^n\frac{1}{2}||f_i(x\text{x}x)|{|}^2)\text{\hspace{1em}(1)}$$
传统的求解最小化问题就是对(1)其求导，将其泰勒展开可以得到：
$$F(x+\Delta x)=F(x)+\frac{\partial F(x)}{\partial x}\Delta x+\Delta x^T\frac{\partial F^2(x)}{2!\partial x^2}\Delta x+O((\Delta x{)}^3)\text{\hspace{1em}(2)}$$
将(2)求关于$\Delta x$的导数令其为0，忽略高阶项，可以得到：
$$g(x)+H(x)\Delta x=0，其中g(x)=(\frac{\partial F(x)}{\partial x}{)}^T，H(x)=\frac{\partial F^2(x)}{\partial x^2}\text{\hspace{1em}(3)}$$
根据公式(3)可以得到迭代值，如此循环迭代，直到达到收敛条件(收敛次数，收敛精度)就退出
$$\Delta x=-H^{-1}(x)\ast g(x)\text{\hspace{1em}(4)}$$

1.2 高斯牛顿法

由于海森矩阵$H$的求解不方便，有些函数很难去求解二次导数，所以，可以用一种新的迭代方式求解$\Delta x$，即对子函数$f(x)$进行一阶泰勒展开，原函数转化为：
$$\begin{gathered} \underset{x\text{x}x}{\mathrm{argmin}}({\Sigma }_{i=1}^n\frac{1}{2}||f_i(x\text{x}x+\Delta x)|{|}^2) \\ =\underset{x\text{x}x}{\mathrm{argmin}}{\Sigma }_{i=1}^n(f_i(x)+J_i(x)\Delta x){)}^2 \\ =\underset{x\text{x}x}{\mathrm{argmin}}{\Sigma }_{i=1}^n(f_i^T(x)\ast f(x)+2\ast f_i^T(x)J(x)\Delta x+\Delta x^T{J}^T(x)\ast J(x)\Delta x) \\ 设G(\Delta x)={\Sigma }_{i=1}^n(f_i^T(x)\ast f(x)+2\ast f_i^T(x)J(x)\Delta x+\Delta x^T{J}^T(x)\ast J(x)\Delta x) \\ {g}_i(\Delta x)=f_i^T(x)\ast f(x)+2\ast f_i^T(x)J(x)\Delta x+\Delta x^T{J}^T(x)\ast J(x)\Delta x \\ \text{\hspace{1em}(5)} \end{gathered}$$
求$g_i(\Delta x)$关于$\Delta x$的导数令其为0：
$$\begin{gathered} \frac{\partial g_i(x)}{\partial \Delta x}=2\ast J^T(x)f(x)+2\ast J^T(x)J(x)\ast \Delta x=0 \\ 所以有:\Delta x=-(J^T(x)J(x){)}^{-1}\ast (J^T(x)\ast f(x))\text{\hspace{1em}(6)} \end{gathered}$$

1.3 点云匹配问题

​ 点云匹配是为了找到两组点云 P = { p 1 , p 2 , . . . , p n } P = {\{p_1,p_2,...,p_n\}} P={ p1​,p2​,...,pn​}和 Q = { q 1 , q 2 , . . . , q m } Q={\{q_1,q_2,...,q_m\}} Q={ q1​,q2​,...,qm​}之间的关系，一种是对应点之间的匹配，即在$Q$里的每一个点都已知和$P$中的某个点匹配，在视觉里面，根据特征点可以这样匹配，还有一种是不对应点的匹配，就是不知道$P$和$Q$的对应关系，这里讨论不知道对应关系的情况，因为在雷达中，都不知道点与点的对应关系。
匹配的目的是为了最小化点与点，点与线，点与面的距离：

1.3.1 点到点的模型：

arg min ⁡ R , t = 1 2 Σ i = 1 n ∣ ∣ R ∗ q i + t − p c l o s e t ∣ ∣ 2 (7) {\underset {R,\pmb t}{\operatorname {arg\,min} }} = \frac{1}{2}\Sigma_{i=1}^n||R*\pmb q_i+\pmb t - \pmb p_{closet}||^2 \quad \quad \tag{7} R,tttargmin​=21​Σi=1n​∣∣R∗q​q​​qi​+t