视觉SLAM十四讲学习5 位姿估计（6）ICP

前言

本篇继续学习3D-3D的方法，ICP

SVD方法

基于RGBD相机或者Lidar的SLAM前端常用ICP。

已知匹配点在上一帧的世界坐标$P^{\prime }$，以及在当前帧的相机坐标$P$，则可以把问题转化为最小二乘：
$$\begin{gathered} e=P^{\prime }-(RP+t) \\ \underset{R,t}{\mathrm{argmin}}\sum _{i=1}^n\frac{1}{2}||P^{\prime }-(RP+t)|{|}_2^2 \end{gathered}$$
首先计算每帧所有匹配点的中心坐标：
$$\begin{gathered} P_c=\frac{1}{n}\sum _{i=1}^n{P}_i \\ {P}_c^{\prime }=\frac{1}{n}\sum _{i=1}^n{P}_i^{\prime } \end{gathered}$$

接着代入最小二乘公式：
$$\begin{gathered} \frac{1}{2}\sum _{i=1}^n||P^{\prime }-(RP+t)|{|}_2^2 \\ =\frac{1}{2}||P^{\prime }-(RP+t)-P_c^{\prime }+R{P}_c+P_c^{\prime }-R{P}_c|{|}^2 \\ =\frac{1}{2}||P^{\prime }-P_c^{\prime }-R(P-P_c)+P_c^{\prime }-R{P}_c-t|{|}^2 \\ =\frac{1}{2}(||P^{\prime }-P_c^{\prime }-R(P-P_c)|{|}^2+||P_c^{\prime }-R{P}_c-t|{|}^2 \\ +(P^{\prime }-P_c^{\prime }-R(P-P_c){)}^T(P_c^{\prime }-R{P}_c-t)+(P_c^{\prime }-R{P}_c-t{)}^T(P^{\prime }-P_c^{\prime }-R(P-P_c))) \end{gathered}$$

由于：
$$\begin{gathered} n{P}_c^{\prime }-nR{P}_c-nt=\sum _{i=1}^n{P}_i^{\prime }-\sum _{i=1}^n(R{P}_i+t)=0 \\ n(P_c^{\prime }-R{P}_c-t)=0 \end{gathered}$$
因此：
$$\begin{gathered} \underset{R,t}{\mathrm{argmin}}\frac{1}{2}\sum _{i=1}^n||P^{\prime }-(RP+t)|{|}_2^2 \\ =\underset{R,t}{\mathrm{argmin}}\frac{1}{2}(||P^{\prime }-P_c^{\prime }-R(P-P_c)|{|}^2+||P_c^{\prime }-R{P}_c-t|{|}^2) \end{gathered}$$
上面的式子中，左边的式子与R有关，右边的式子与R,t有关，也就是说求出一个R后，可以通过改变t使得右式为0，故最终的最小二乘为：
$$\begin{gathered} \underset{R,t}{\mathrm{argmin}}||P^{\prime }-P_c^{\prime }-R(P-P_c)|{|}^2 \\ t=P_c^{\prime }-R{P}_c \end{gathered}$$

$$\begin{gathered} set{p}_i=P_i-P_c,p_i^{\prime }=P_i^{\prime }-P_c^{\prime } \\ setW=\sum _{i=1}^n{p}_i^{\prime }{p}_i^T \\ thenW=U\Sigma V^T \\ R=U{V}^T \end{gathered}$$

BA方法

与PnP的BA方法基本相同：
$$\underset{\delta }{\mathrm{argmin}}\frac{1}{2}\sum _{i=1}^n||P^{\prime }-e^{{\delta }^{\wedge }}P|{|}_2^2$$
然后通过高斯牛顿法优化迭代即可。