基于高斯牛顿的点-点，点-线，点到面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 点到点的模型：

$$\underset{R,t\text{t}t}{\mathrm{argmin}}=\frac{1}{2}{\Sigma }_{i=1}^n||R\ast {q\text{q}q}_i+t\text{t}t-{p\text{p}p}_{closet}|{|}^2\text{\hspace{1em}(7)}$$

1.3.2 点到线的匹配：

$$\underset{R,t\text{t}t}{\mathrm{argmin}}=\frac{1}{2}{\Sigma }_{i=1}^n||n\text{n}n(R\ast {q\text{q}q}_i+t\text{t}t-{p\text{p}p}_{in\_line})|{|}^2，\text{\hspace{1em}(8)}$$
$n\text{n}n$ 为线的法向量

1.3.3 点到面的匹配：

$$\underset{R,t\text{t}t}{\mathrm{argmin}}=\frac{1}{2}{\Sigma }_{i=1}^n||n\text{n}n(R\ast {q\text{q}q}_i+t\text{t}t-{p\text{p}p}_{in\_planner})|{|}^2，\text{\hspace{1em}(9)}$$
$n\text{n}n$ 为面的法向量

2 优化方法的使用

​ 在ICP中，待求解的变量为$R$和$t\text{t}t$，其中$R$表示姿态，可以用四元数$q$或者姿态角$(roll,pitch,yaw)$来表示，$t\text{t}t$表示平移。

2.1 点-点

1. 初始化Hessian<6,6>=Zero,B<6,1> = Zero，首先，将待匹配点云中的每个点$p_i^{{}^{\prime }}\text{pi′}{p}_i^{{}^{\prime }}$利用初始的旋转矩阵$R$和平移向量$t\text{t}t$，转换到目标点云中，即$p_i^{{}^{\prime }}\text{pi′}{p}_i^{{}^{\prime }}=R\ast p_i+t\text{R∗pi+t}R\ast p_i+t$，利用KD树的算法寻找到最近的一个点$q_i^{{}^{\prime }}\text{qi′}{q}_i^{{}^{\prime }}$，
2. 计算距离误差$f=p_i^{{}^{\prime }}-q_i^{{}^{\prime }}\text{f=pi′−qi′}f=p_i^{{}^{\prime }}-q_i^{{}^{\prime }}$，如果$||f\text{f}f||>threshold$，舍弃这组点对，否则执行下面的步骤。
3. 对找到的每一组点对，计算Jacobian<3,6> ， Hessian_i<6,6>， B_i<6,1>.
   $$\begin{gathered} 对平移求导\frac{\partial f\text{f}f}{\partial t\text{t}t}=I\text{I}I \\ 对旋转求导，采用右导数\frac{\partial f\text{f}f}{\partial R\text{R}R}=-R\ast p_i^{\Lambda }\text{−R∗piΛ}-R\ast p_i^{\Lambda } \\ Jacobian=[\frac{\partial f\text{f}f}{\partial t\text{t}t},\frac{\partial f\text{f}f}{\partial R\text{R}R}] \\ Hessia{n}_i=Jacobia{n}^{T}Jacobian \\ {B}_i=-Jacobia{n}^{T}f\text{f}f \end{gathered}$$
4. 对所有点对计算出来的Hessian_i和B_i累加，计算增量$\delta x\text{δx}\delta x$
   $$\begin{gathered} Hessian=\Sigma Hessia{n}_i \\ B=\Sigma B_i \\ \delta x\text{δx}\delta x=Hessian.inverse()\ast B \end{gathered}$$
5. 利用增量$\delta x\text{δx}\delta x$计算新的$R,t$，返回第一步，直到达到收敛条件或者迭代次数。

2.2 点-线

对于点-线的情况：
​ 首先，将待匹配点云中的每个点利用初始的旋转矩阵和平移向量转换到目标点云中，利用KD树的算法寻找到最近的几个点，根据这几个点计算出协方差，将协方差进行奇异值分解，如果最大的奇异值比第二大的奇异值要大得多，则认为找到的这几个点是在同一条直线上。


目标函数为点到直线的距离，即$||l_{dc}|{|}^2$，对于向量有，这里为了方便，把${l\text{l}l}_{ab}$进行了归一化：
$$\begin{gathered} {l\text{l}l}_{dc}={l\text{l}l}_{bc}-{l\text{l}l}_{bd}={l\text{l}l}_{bc}-{l\text{l}l}_{ab}\ast ({l\text{l}l}_{ab}^T\ast {l\text{l}l}_{bc}) \\ {l\text{l}l}_{bc}={p\text{p}p}_c-{p\text{p}p}_b=Rq\text{q}q+t\text{t}t-{p\text{p}p}_b\text{\hspace{1em}(10)} \end{gathered}$$
将(10)对状态量$R$和$t$求偏导，因为目标函数是${l\text{l}l}_{bc}$是一个3维的变量，而状态量是一个6维的变量，所以$\frac{\partial f}{\partial x}=Matri{x}_{3\ast 6}$

2.2.1 公式推导：

求偏导数是为了求R和t相关的变量，所以无关的可以直接忽略，这里对第$i$个点进行分析：
先求关于$t\text{t}t$的偏导：
$$\begin{gathered} \frac{\partial {l\text{l}l}_{dc}}{\partial t\text{t}t}=\frac{\partial {l\text{l}l}_{bc}}{\partial t\text{t}t}-\frac{\partial {l\text{l}l}_{bd}}{\partial t\text{t}t} \\ =I\text{I}I-{l\text{l}l}_{ab}\ast {l\text{l}l}_{ab}^T\text{\hspace{1em}(11)} \end{gathered}$$
再求关于$R$的偏导：
$$\begin{gathered} \frac{\partial {l\text{l}l}_{dc}}{\partial R}=\frac{\partial {l\text{l}l}_{bc}}{\partial R}-\frac{\partial {l\text{l}l}_{bd}}{\partial R} \\ =\frac{\partial Rq\text{q}q}{\partial R}-{l\text{l}l}_{ab}\ast \frac{\partial ({l\text{l}l}_{ab}^T\ast Rq\text{q}q)}{\partial R} \\ =\underset{\Delta \varphi \to 0}{\lim }\frac{Rexp(\Delta {\varphi }^{\times })q\text{q}q-Rq\text{q}q}{\Delta \varphi }-{l\text{l}l}_{ab}\ast \underset{\Delta \varphi \to 0}{\lim }\frac{{l\text{l}l}_{ab}^T\ast Rexp(\Delta {\varphi }^{\times })q\text{q}q-{l\text{l}l}_{ab}^T\ast Rq\text{q}q}{\Delta \varphi } \\ =-R{q\text{q}q}^{\times }+{l\text{l}l}_{ab}\ast ({l\text{l}l}_{ab}^T\ast R{q\text{q}q}^{\times })\text{\hspace{1em}(12)} \end{gathered}$$
上式中${q\text{q}q}^{\times }$表示$q\text{q}q$的反对称矩阵：
$${q\text{q}q}^{\times }=\left[\begin{array}{ccc}0& -q_2& q_1\\ q_2& 0& -q_0\\ -q_1& q_0& 0\end{array}\right]$$
所以雅克比矩阵可写为：
$$\begin{gathered} J_i=\left(\begin{array}{cc}\frac{\partial {l\text{l}l}_{dc}}{\partial t\text{t}t}& \frac{\partial {l\text{l}l}_{dc}}{\partial R}\end{array}\right) \\ =\left(\begin{array}{cc}I\text{I}I-{l\text{l}l}_{ab}\ast {l\text{l}l}_{ab}^T& -R{q\text{q}q}^{\times }+{l\text{l}l}_{ab}\ast ({l\text{l}l}_{ab}^T\ast R{q}^{\times })\end{array}\right)\text{\hspace{1em}(13)} \end{gathered}$$

所以Hessian矩阵可以表示为 $H_i=J_i^T{J}_i$，残差项 ${b\text{b}b}_i=-J_i^T\ast f=-J_i^T\ast {l\text{l}l}_{dc}$

从而根据(6)得出：
$${\Sigma }_{i=1}^n{H}_i\Delta x={\Sigma }_{i=1}^n{b\text{b}b}_i\text{\hspace{1em}(14)}$$

2.2.2 位姿更新

到上面计算出了$\Delta x=(\Delta t\Delta \varphi {)}^T$，将$\Delta x$更新上一次的估计值，完成一次迭代：
需要注意的是平移向量$\Delta t$是可以直接相加的，但是旋转向量则需要利用四元数/李代数的乘法：
$$\begin{gathered} t_1=t_0+\Delta t \\ {q}_1=q_0\ast \left[\begin{array}{c}1\\ \frac{1}{2}\Delta \varphi \end{array}\right]或者R_1=R_{0}exp(\Delta {\varphi }^{\times })\text{\hspace{1em}(15)} \end{gathered}$$

2.3 点-面

对于点-面的情况：
​ 首先，将待匹配点云中的每个点利用初始的旋转矩阵和平移向量转换到目标点云中，利用KD树的算法寻找到最近的几个点，根据这几个点计算出协方差，将协方差进行奇异值分解，如果最小的奇异值比第二小的奇异值要小得多，则认为找到的这几个点是在同一个平面上。

目标函数为点到面的距离，即$||l_{od}|{|}^2$，先求出法向量并进行归一化得到$n\text{n}n=({l\text{l}l}_{ac}\times {l\text{l}l}_{ab}).normlize()$：
$$\begin{gathered} {l\text{l}l}_{od}=n\text{n}n\ast ({n\text{n}n}^T\ast {l\text{l}l}_{cd}) \\ {l\text{l}l}_{cd}=R\ast q\text{q}q+t-{p\text{p}p}_c\text{\hspace{1em}(16)} \end{gathered}$$
将(16)对状态量$R$和$t$求偏导，因为目标函数是${l\text{l}l}_{od}$是一个3维的变量，而状态量是一个6维的变量，所以$\frac{\partial f}{\partial x}=Matri{x}_{3\ast 6}$

2.3.1 公式推导：

和点-线的求导方式一样，求偏导数是为了求R和t相关的变量，所以无关的可以直接忽略，这里对第$i$个点进行分析：
先求关于$t\text{t}t$的偏导：
$$\begin{gathered} \frac{\partial {l\text{l}l}_{od}}{\partial t\text{t}t}=\frac{\partial n\text{n}n\ast ({n\text{n}n}^T\ast (R\ast q\text{q}q+t))}{\partial t\text{t}t} \\ =n\text{n}n\ast {n\text{n}n}^T\text{\hspace{1em}(17)} \end{gathered}$$
再求关于$R$的偏导：
$$\begin{gathered} \frac{\partial {l\text{l}l}_{od}}{\partial R}=\frac{\partial n\text{n}n\ast ({n\text{n}n}^T\ast (R\ast q\text{q}q+t))}{\partial R} \\ =n\text{n}n\ast \underset{\Delta \varphi \to 0}{\lim }\frac{{n\text{n}n}^T\ast Rexp(\Delta {\varphi }^{\times })q\text{q}q-{n\text{n}n}^{\ast }TRq\text{q}q}{\Delta \varphi } \\ =-n\text{n}n\ast ({n\text{n}n}^T\ast R{q\text{q}q}^{\times })\text{\hspace{1em}(18)} \end{gathered}$$
所以雅克比矩阵可写为：
$$\begin{gathered} J_i=\left(\begin{array}{cc}\frac{\partial {l\text{l}l}_{od}}{\partial t\text{t}t}& \frac{\partial {l\text{l}l}_{od}}{\partial R}\end{array}\right) \\ =\left(\begin{array}{cc}n\text{n}n\ast {n\text{n}n}^T& -n\text{n}n\ast ({n\text{n}n}^T\ast R{q\text{q}q}^{\times })\end{array}\right)\text{\hspace{1em}(19)} \end{gathered}$$
所以Hessian矩阵可以表示为 $H_i=J_i^T{J}_i$，残差项 ${b\text{b}b}_i=J_i^T\ast f=J_i^T\ast {l\text{l}l}_{dc}$
从而根据(6)得出：
$${\Sigma }_{i=1}^n{H}_i\Delta x={\Sigma }_{i=1}^n{b\text{b}b}_i\text{\hspace{1em}(20)}$$

结果

这里放一张作业的结果图，因为点-点的ICP是基于RTK给的初始值，所以结果还算理想，红色的是激光里程计的，黄色是GPS的

参考

[1] 深蓝学院SLAM/VIO课程
[2] 深蓝学院SLAM/多传感器融合定位 课程
[3] 从零开始做激光SLAM
[3] Livox-Loam