3D激光里程计其一:ICP

这里写目录标题

  • 1. ICP 整体流程
  • 2. ICP 的数学表示
  • 3. 基于 SVD 的 ICP
    • 3.1 旋转部分求解
    • 3.2 平移部分求解
  • 4. 基于优化的 ICP
  • 5. ICP 系列汇总

Reference:

  1. 深蓝学院-多传感器融合

1. ICP 整体流程

3D激光里程计其一:ICP_第1张图片
目的:有两个相似的刚体点云,它们之间没有做好配准,现在想将它们之间的旋转和平移找出来。

3D激光里程计其一:ICP_第2张图片

解决思路:如上图所示,从蓝色的点云里面选一些点,根据这些点,在红色的点云里面找离它最近的一些点,根据这些匹配点,可以将 R, t 计算出来。

存在问题:在找配准点的时候,找的点是最近的,但是找的这个点不见得是真正在刚体里面匹配的点。这个时候点找错了,那么根据这个点计算出来的 R, t,肯定是不精确的。就会出现中间图片的样子------两个刚体点云并没有完全重合。但是经过一次匹配后,俩点云至少更接近了,这时可以再做一次上面的流程。循环几次之后,可能就完全重合了。

2. ICP 的数学表示

  • 点集:
    X = { x 1 , x 2 , ⋯   , x N x } Y = { y 1 , y 2 , ⋯   , y N y } \begin{aligned} & X=\left\{x_1, x_2, \cdots, x_{N_x}\right\} \\ & Y=\left\{y_1, y_2, \cdots, y_{N_y}\right\} \end{aligned} X={x1,x2,,xNx}Y={y1,y2,,yNy}

    其中 X X X Y Y Y原始点云的子集,选取的是两个点集中能够互相关联的那些点,即 N x = N y N_x=N_y Nx=Ny

  • 目标:找到一组(R, t),使误差的二范数最小化
    min ⁡ E ( R , t ) = min ⁡ 1 N y ∑ i = 1 N y ∥ x i − R y i − t ∥ 2 \min E(R, t)=\min \frac{1}{N_y} \sum_{i=1}^{N_y}\left\|x_i-R y_i-t\right\|^2 minE(R,t)=minNy1i=1NyxiRyit2

3. 基于 SVD 的 ICP

SVD 的好处在于一步求解。完成了点的关联后,就可以通过一步运算将 R 和 t 求出来。
现在来看看公式上怎样解决这个问题:
E ( R , t ) = 1 N y ∑ i = 1 N y ∥ x i − R y i − t − u x + R u y + u x − R u y ∥ 2 = 1 N y ∑ i = 1 N y ( ∥ x i − u x − R ( y i − u y ) + ( u x − R u y − t ) ∥ 2 ) = 1 N y ∑ i = 1 N y ( ∥ x i − u x − R ( y i − u y ) ∥ 2 + ∥ u x − R u y − t ∥ 2 + 2 ( x i − u x − R ( y i − u y ) ) T ( u x − R u y − t ) ) = 1 N y ∑ i = 1 N y ( ∥ x i − u x − R ( y i − u y ) ∥ 2 + ∥ u x − R u y − t ∥ 2 ) \begin{aligned} E(R, t)&=\frac{1}{N_y} \sum_{i=1}^{N_y}\left\|x_i-R y_i-t-u_x+R u_y+u_x-R u_y\right\|^2 \\ & =\frac{1}{N_y} \sum_{i=1}^{N_y}\left(\left\|x_i-u_x-R\left(y_i-u_y\right)+\left(u_x-R u_y-t\right)\right\|^2\right) \\ & =\frac{1}{N_y} \sum_{i=1}^{N_y}\left(\left\|x_i-u_x-R\left(y_i-u_y\right)\right\|^2+\left\|u_x-R u_y-t\right\|^2+2\left(x_i-u_x-R\left(y_i-u_y\right)\right)^T\left(u_x-R u_y-t\right)\right) \\ & =\frac{1}{N_y} \sum_{i=1}^{N_y}\left(\left\|x_i-u_x-R\left(y_i-u_y\right)\right\|^2+\left\|u_x-R u_y-t\right\|^2\right) \\ & \end{aligned} E(R,t)=Ny1i=1NyxiRyitux+Ruy+uxRuy2=Ny1i=1Ny(xiuxR(yiuy)+(uxRuyt)2)=Ny1i=1Ny(xiuxR(yiuy)2+uxRuyt2+2(xiuxR(yiuy))T(uxRuyt))=Ny1i=1Ny(xiuxR(yiuy)2+uxRuyt2)

其中 u x u_x ux u y u_y uy 分别是点集 X X X Y Y Y 的质心,即:
u x = 1 N x ∑ i = 1 N x x i u y = 1 N y ∑ i = 1 N y y i u_x=\frac{1}{N_x} \sum_{i=1}^{N_x} x_i \quad u_y=\frac{1}{N_y} \sum_{i=1}^{N_y} y_i ux=Nx1i=1Nxxiuy=Ny1i=1Nyyi

  1. 在第一步加了几项进公式中: − u x + R u y + u x − R u y -u_x+R u_y+u_x-R u_y ux+Ruy+uxRuy,用来化简方程;
  2. 公式移项,就变成了 ∣ ∣ a + b ∣ ∣ 2 ||a+b||^2 ∣∣a+b2这样了;
  3. 根据 ∣ ∣ a + b ∣ ∣ 2 = ∣ ∣ a ∣ ∣ 2 + ∣ ∣ b ∣ ∣ 2 + 2 a T b ||a+b||^2=||a||^2+||b||^2+2a^Tb ∣∣a+b2=∣∣a2+∣∣b2+2aTb,将公式拆开,可以发现 2 ( x i − u x − R ( y i − u y ) ) T ( u x − R u y − t ) 2\left(x_i-u_x-R\left(y_i-u_y\right)\right)^T\left(u_x-R u_y-t\right) 2(xiuxR(yiuy))T(uxRuyt)累加和为零。
    ∑ i = 1 N y ( x i − u x − R ( y i − u y ) ) \sum_{i=1}^{N_y} \left(x_i-u_x-R\left(y_i-u_y\right)\right) i=1Ny(xiuxR(yiuy)),其中 ∑ i = 1 N y ( x i − u x ) = 0 \sum_{i=1}^{N_y} (x_i-u_x)=0 i=1Ny(xiux)=0 ∑ i = 1 N y R ( y i − u y ) = 0 \sum_{i=1}^{N_y} R\left(y_i-u_y\right)=0 i=1NyR(yiuy)=0

下面继续化简:
E ( R , t ) = 1 N y ∑ i = 1 N y ( ∥ x i − u x − R ( y i − u y ) ∥ 2 + ∥ u x − R u y − t ∥ 2 ) = E 1 ( R , t ) + E 2 ( R , t ) \begin{aligned} E(R, t)&=\frac{1}{N_y} \sum_{i=1}^{N_y}\left(\left\|x_i-u_x-R\left(y_i-u_y\right)\right\|^2+\left\|u_x-R u_y-t\right\|^2\right) \\ &=E_1(R, t)+E_2(R, t) \end{aligned} E(R,t)=Ny1i=1Ny(xiuxR(yiuy)2+uxRuyt2)=E1(R,t)+E2(R,t)

其中: E 1 ( R , t ) = 1 N y ∑ i = 1 N y ∥ x i − u x − R ( y i − u y ) ∥ 2 (只与旋转有关)  E 2 ( R , t ) = ∥ u x − R u y − t ∥ 2 (用于求平移部分)  \begin{aligned} &E_1(R, t)=\frac{1}{N_y} \sum_{i=1}^{N_y}\left\|x_i-u_x-R\left(y_i-u_y\right)\right\|^2 \quad \text {(只与旋转有关) } \\ & E_2(R, t)=\left\|u_x-R u_y-t\right\|^2 \quad\text {(用于求平移部分) } \end{aligned} E1(R,t)=Ny1i=1NyxiuxR(yiuy)2(只与旋转有关E2(R,t)=uxRuyt2(用于求平移部分

可以发现的是,在 E 1 E_1 E1里面只有 R R R没有 t t t了。我们的目标是让 E 1 + E 2 E_1+E_2 E1+E2最小。在 E 1 E_1 E1里面,可以找到一个 R R R,使结果最小。在 E 2 E_2 E2里面不管 R R R是什么,都可以让 t = u x − R u y t=u_x-Ru_y t=uxRuy,即这个值总是零。

因此,可以先根据 E 1 ( R , t ) E_1(R, t) E1(R,t)求旋转,再根据 E 2 ( R , t ) E_2(R,t) E2(R,t)求平移。

3.1 旋转部分求解

接下来就是看怎么把这个旋转求出来:
E 1 ( R , t ) = 1 N y ∑ i = 1 N y ∥ x i − u x − R ( y i − u y ) ∥ 2 = 1 N y ∑ i = 1 N y ∥ x i ′ − R y i ′ ∥ 2 = 1 N y ∑ i = 1 N y ( x i ′ T x i ′ + y i ′ R T R y i ′ − 2 x i ′ T R y i ′ ) \begin{aligned} E_1(R, t) & =\frac{1}{N_y} \sum_{i=1}^{N_y}\left\|x_i-u_x-R\left(y_i-u_y\right)\right\|^2 \\ & =\frac{1}{N_y} \sum_{i=1}^{N_y}\left\|x_i^{\prime}-R y_i^{\prime}\right\|^2 \\ & =\frac{1}{N_y} \sum_{i=1}^{N_y}( x_i^{\prime T} x_i^{\prime}+y_i^{\prime} R^T R y_i^{\prime}-2 x_i^{\prime T} R y_i^{\prime}) \end{aligned} E1(R,t)=Ny1i=1NyxiuxR(yiuy)2=Ny1i=1NyxiRyi2=Ny1i=1Ny(xiTxi+yiRTRyi2xiTRyi)

x i ′ x_i' xi 表示 x i − u x x_i-u_x xiux y i ′ y_i' yi 表示 y i − u y y_i-u_y yiuy,公式展开。其中 x ′ i T x ′ i {x'}_i^T{x'}_i xiTxi R R R 无关,而 R T R R^TR RTR 是单位阵也没什么关系(旋转矩阵都是单位正交阵)。这时需要考虑的就只有 − 2 x i ′ T R y i ′ -2 x_i^{\prime T} R y_i^{\prime} 2xiTRyi 这一项了。令 :
E 1 ′ ( R , t ) = ∑ i = 1 N y x i ′ T R y i ′ E_1^{\prime}(R, t)=\sum_{i=1}^{N_y} x_i^{\prime T} R y_i^{\prime} E1(R,t)=i=1NyxiTRyi

则:
arg ⁡ min ⁡ R E 1 ( R , t ) = arg ⁡ max ⁡ R E 1 ′ ( R , t ) = arg ⁡ max ⁡ R ∑ i = 1 N y x i ′ T R y i ′ \begin{aligned} & \arg \min _R E_1(R, t) \\ = & \arg \max _R E_1^{\prime}(R, t) \\ = & \arg \max _R \sum_{i=1}^{N_y} x_i^{\prime T} R y_i^{\prime}\end{aligned} ==argRminE1(R,t)argRmaxE1(R,t)argRmaxi=1NyxiTRyi

即求最小的 E 1 E_1 E1 也就是求最大的 x i ′ T R y i ′ x_i^{\prime T} R y_i^{\prime} xiTRyi(因为该项带负号)。
E 1 ′ ( R , t ) = ∑ i = 1 N y x i ′ T R y i ′ ( 1 ) = ∑ i = 1 N y Trace ⁡ ( x i ′ T R y i ′ ) ( 2 ) = ∑ i = 1 N y Trace ⁡ ( R y i ′ x i ′ T ) = Trace ⁡ ( ∑ i = 1 N y R y i ′ x i ′ ) ( 3 ) = Trace ⁡ ( R H ) E_1^{\prime}(R, t)=\sum_{i=1}^{N_y} x_i^{\prime T} R y_i^{\prime}{}^{(1)}=\sum_{i=1}^{N_y} \operatorname{Trace}\left(x_i^{\prime T} R y_i^{\prime}\right)^{(2)}=\sum_{i=1}^{N_y} \operatorname{Trace}\left(R y_i^{\prime} x_i^{\prime T}\right)=\operatorname{Trace}\left(\sum_{i=1}^{N_y} R y_i^{\prime} x_i^{\prime}\right)^{(3)}=\operatorname{Trace}(R H) E1(R,t)=i=1NyxiTRyi(1)=i=1NyTrace(xiTRyi)(2)=i=1NyTrace(RyixiT)=Trace i=1NyRyixi (3)=Trace(RH)

  • 等式(1):常数的迹等于它自身,因此上式是相等的
  • 等式(2): tr ⁡ ( A B ) = ∑ i = 1 m ∑ j = 1 n a i j b j i = ∑ i = 1 m ∑ j = 1 n b j i a i j = tr ⁡ ( B A ) \operatorname{tr}(A B)=\sum_{i=1}^m \sum_{j=1}^n a_{i j} b_{j i}=\sum_{i=1}^m \sum_{j=1}^n b_{j i} a_{i j}=\operatorname{tr}(B A) tr(AB)=i=1mj=1naijbji=i=1mj=1nbjiaij=tr(BA)
  • 等式(3): H = ∑ i = 1 N y y i ′ x i ′ T H=\sum_{i=1}^{N_y} y_i^{\prime} x_i^{\prime T} H=i=1NyyixiT

问题转化为,找到合适的 R R R,使 Trace ⁡ ( R H ) \operatorname{Trace}(R H) Trace(RH) 达到最大值。

  • 定理:若有正定矩阵 A A T A A^T AAT ,则对于任意正交矩阵 B B B ,有 Trace ⁡ ( A A T ) ≥ Trace ⁡ ( B A A T ) \operatorname{Trace} (A A^T) \geq \operatorname{Trace}\left(B A A^T\right) Trace(AAT)Trace(BAAT)
    意义:若能找到 R R R,把 Trace ⁡ ( R H ) \operatorname{Trace}(R H) Trace(RH) 转换成 Trace ( A A T ) \left(A A^T\right) (AAT) 的形式,则该 R R R 就是我们要找的旋转矩阵证明(只要变成这种形式,再把 R R R变成任意的旋转矩阵,都不可能比这个要大了。而我们的目标就是让这个值达到最大。):
    tr ⁡ ( B A A T ) = tr ⁡ ( A T B A ) = ∑ i a i T ( B a i ) \operatorname{tr}\left(B A A^T\right)=\operatorname{tr}\left(A^T B A\right)=\sum_i a_i^T\left(B a_i\right) tr(BAAT)=tr(ATBA)=iaiT(Bai)

    其中 a i \mathrm{a}_{\mathrm{i}} ai A A A 的列向量。根据柯西-施瓦茨不等式(有两向量 x 、 y x、y xy x T y ≤ ( x T x ) ( y T y ) x^Ty\leq\sqrt{(x^Tx)(y^Ty)} xTy(xTx)(yTy) ),有
    a i T ( B a i ) ≤ ( a i T a i ) ( a i T B T B a i ) = a i T a i a_i^T\left(B a_i\right) \leq \sqrt{\left(a_i^T a_i\right)\left(a_i^T B^T B a_i\right)}=a_i^T a_i aiT(Bai)(aiTai)(aiTBTBai) =aiTai

    因此,
    tr ⁡ ( B A A T ) = ∑ i a i T ( B a i ) ≤ ∑ i a i T a i = tr ⁡ ( A A T ) \operatorname{tr}\left(B A A^T\right)=\sum_i a_i^T\left(B a_i\right) \leq \sum_i a_i^T a_i=\operatorname{tr}\left(A A^T\right) tr(BAAT)=iaiT(Bai)iaiTai=tr(AAT)

  • 目的:找到 R R R,把 Trace ⁡ ( R H ) \operatorname{Trace}(R H) Trace(RH) 转换成 Trace ⁡ ( A A T ) \operatorname{Trace}\left(A A^T\right) Trace(AAT) 的形式
    方法:
    H H H 进行SVD分解: H = U Σ V T H=U \Sigma V^T H=UΣVT
    R = V U T R=V U^T R=VUT
    则有: R H = V U T U Σ V T = V Σ V T = V Σ 1 2 Σ 1 2 V T = V Σ 1 2 ( V Σ 1 2 ) T R H=V U^T U \Sigma V^T=V \Sigma V^T=V \Sigma^{\frac{1}{2}} \Sigma^{\frac{1}{2}} V^T=V \Sigma^{\frac{1}{2}}\left(V \Sigma^{\frac{1}{2}}\right)^T RH=VUTUΣVT=VΣVT=VΣ21Σ21VT=VΣ21(VΣ21)T
    其中 U T U U^TU UTU 能消掉是奇异值分解中的性质。也就是说,找到了 R = V U T R=V U^T R=VUT,那就是要找的东西。

3.2 平移部分求解

旋转 R R R 确定之后,可根据
E 2 ( R , t ) = ∥ u x − R u y − t ∥ 2 E_2(R, t)=\left\|u_x-R u_y-t\right\|^2 E2(R,t)=uxRuyt2

直接得到平移量为
t = u x − R u y t=u_x-R u_y t=uxRuy

4. 基于优化的 ICP

优化的思想在于让点逐渐收敛,不想做 SVD 分解,就给一初值,让这个初值按照一定策略逐渐搜索,直到搜索到要的 R 和 t 满足一定条件,就认为得到了最终确定的结果。

非线性优化要求雅可比,而基于优化的ICP对应的雅可比推导在李代数模式下会更加简洁。ICP求解问题,可以重新表示为:
min ⁡ T = 1 2 ∑ i = 1 n ∥ ( x i − T y i ) ∥ 2 2 \min _T=\frac{1}{2} \sum_{i=1}^n\left\|\left(x_i-T y_i\right)\right\|_2^2 Tmin=21i=1n(xiTyi)22

在李代数模式下,又可以重新表示为:
min ⁡ ξ = 1 2 ∑ i = 1 n ∥ ( x i − exp ⁡ ( ξ ∧ ) y i ) ∥ 2 2 \min _{\xi}=\frac{1}{2} \sum_{i=1}^n\left\|\left(x_i-\exp \left(\xi^{\wedge}\right) y_i\right)\right\|_2^2 ξmin=21i=1n(xiexp(ξ)yi)22

其中 ξ \xi ξ T T T 对应的李代数。
残差对应的雅可比为:
J = ∂ e ∂ δ ξ = − ( exp ⁡ ( ξ ∧ ) y i ) ⊙ J=\frac{\partial e}{\partial \delta \xi}=-\left(\exp \left(\xi^{\wedge}\right) y_i\right)^{\odot} J=δξe=(exp(ξ)yi)
随后,便可根据优化的固定步骤求解位姿。

5. ICP 系列汇总

3D激光里程计其一:ICP_第3张图片

你可能感兴趣的:(激光雷达,SLAM,3d,算法)