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

前言

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

SVD方法

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

已知匹配点在上一帧的世界坐标 P ′ P' P′，以及在当前帧的相机坐标 P P P，则可以把问题转化为最小二乘： e = P ′ − ( R P + t ) arg min ⁡ R , t ∑ i = 1 n 1 2 ∣ ∣ P ′ − ( R P + t ) ∣ ∣ 2 2 e=P'-(RP+t) \\ \argmin_{R,t} \sum_{i=1}^n \frac {1}{2} || P'-(RP+t) ||_2^2 e=P′−(RP+t)R,targmin​i=1∑n​21​∣∣P′−(RP+t)∣∣22​ 首先计算每帧所有匹配点的中心坐标： P c = 1 n ∑ i = 1 n P i P c ′ = 1 n ∑ i = 1 n P i ′ P_c = \frac{1}{n} \sum_{i=1}^n P_i \\ \quad \\ P'_c = \frac{1}{n} \sum_{i=1}^n P'_i \\ \quad \\ Pc​=n1​i=1∑n​Pi​Pc′​=n1​i=1∑n​Pi′​

接着代入最小二乘公式： 1 2 ∑ i = 1 n ∣ ∣ P ′ − ( R P + t ) ∣ ∣ 2 2 = 1 2 ∣ ∣ P ′ − ( R P + t ) − P c ′ + R P c + P c ′ − R P c ∣ ∣ 2 = 1 2 ∣ ∣ P ′ − P c ′ − R ( P − P c ) + P c ′ − R P c − t ∣ ∣ 2 = 1 2 ( ∣ ∣ P ′ − P c ′ − R ( P − P c ) ∣ ∣ 2 + ∣ ∣ P c ′ − R P c − t ∣ ∣ 2 + ( P ′ − P c ′ − R ( P − P c ) ) T ( P c ′ − R P c − t ) + ( P c ′ − R P c − t ) T ( P ′ − P c ′ − R ( P − P c ) ) ) \frac {1}{2} \sum_{i=1}^n || P'-(RP+t) ||_2^2 \\ \quad \\ = \frac {1}{2} || P' - (RP+t)-P'_c+RP_c+P_c' - RP_c ||^2 \\ = \frac {1}{2} || P' - P_c' - R(P-P_c) + P'_c - RP_c - t ||^2 \\ = \frac {1}{2} ( || P'-P_c' - R(P-P_c) ||^2 + || P_c' - RP_c - t ||^2 \\ + (P'-P_c'-R(P-P_c))^T(P_c' - RP_c - t)+(P_c' - RP_c - t)^T(P'-P_c'-R(P-P_c))) 21​i=1∑n​∣∣P′−(RP+t)∣∣22​=21​∣∣P′−(RP+t)−Pc′​+RPc​+Pc′​−RPc​∣∣2=21​∣∣P′−Pc′​−R(P−Pc​)+Pc′​−RPc​−t∣∣2=21​(∣∣P′−Pc′​−R(P−Pc​)∣∣2+∣∣Pc′​−RPc​−t∣∣2+(P′−Pc′​−R(P−Pc​))T(Pc′​−RPc​−t)+(Pc′​−RPc​−t)T(P′−Pc′​−R(P−Pc​)))

由于： n P c ′ − n R P c − n t = ∑ i = 1 n P i ′ − ∑ i = 1 n ( R P i + t ) = 0 n ( P c ′ − R P c − t ) = 0 nP_c' - nRP_c - nt = \sum_{i=1}^nP'_i - \sum_{i=1}^n (RP_i + t) =0 \\ n(P_c' - RP_c - t) = 0 nPc′​−nRPc​−nt=i=1∑n​Pi′​−i=1∑n​(RPi​+t)=0n(Pc′​−RPc​−t)=0 因此： arg min ⁡ R , t 1 2 ∑ i = 1 n ∣ ∣ P ′ − ( R P + t ) ∣ ∣ 2 2 = arg min ⁡ R , t 1 2 ( ∣ ∣ P ′ − P c ′ − R ( P − P c ) ∣ ∣ 2 + ∣ ∣ P c ′ − R P c − t ∣ ∣ 2 ) \argmin_{R,t} \frac {1}{2} \sum_{i=1}^n || P'-(RP+t) ||_2^2 \\ \quad \\ = \argmin_{R,t} \frac {1}{2} ( || P'-P_c' - R(P-P_c) ||^2 + || P_c' - RP_c - t ||^2) \\ R,targmin​21​i=1∑n​∣∣P′−(RP+t)∣∣22​=R,targmin​21​(∣∣P′−Pc′​−R(P−Pc​)∣∣2+∣∣Pc′​−RPc​−t∣∣2) 上面的式子中，左边的式子与R有关，右边的式子与R,t有关，也就是说求出一个R后，可以通过改变t使得右式为0，故最终的最小二乘为： arg min ⁡ R , t ∣ ∣ P ′ − P c ′ − R ( P − P c ) ∣ ∣ 2 t = P c ′ − R P c \argmin_{R,t} || P'-P_c' - R(P-P_c) ||^2 \\ t = P_c' - RP_c R,targmin​∣∣P′−Pc′​−R(P−Pc​)∣∣2t=Pc′​−RPc​

s e t p i = P i − P c , p i ′ = P i ′ − P c ′ s e t W = ∑ i = 1 n p i ′ p i T t h e n W = U Σ V T R = U V T set \quad p_i=P_i-P_c, \quad p_i'=P_i'-P_c' \\ \quad \\ set \quad W= \sum_{i=1}^n p_i'p_i^T \\ then \quad W=U\Sigma V^T \\ R=UV^T setpi​=Pi​−Pc​,pi′​=Pi′​−Pc′​setW=i=1∑n​pi′​piT​thenW=UΣVTR=UVT

BA方法

与PnP的BA方法基本相同： arg min ⁡ δ 1 2 ∑ i = 1 n ∣ ∣ P ′ − e δ ∧ P ∣ ∣ 2 2 \argmin_{\delta} \frac {1}{2} \sum_{i=1}^n || P' - e^{\delta^\land}P ||_2^2 \\ \quad \\ δargmin​21​i=1∑n​∣∣P′−eδ∧P∣∣22​ 然后通过高斯牛顿法优化迭代即可。