前端匹配:ICP, PL-ICP, N-ICP, IMLS-ICP
PL-ICP
PL-ICP的目标函数实际上表示点到曲面距离,即点到直线距离。
ICP目标函数:minq=∑i∣∣pi⊕q−∏(Sres,pi⊕q)∣∣2\min\limits_{q}=\sum\limits_{i}||p_i\oplus q - \prod(S^{res},p_i\oplus q )||^2qmin=i∑∣∣pi⊕q−∏(Sres,pi⊕q)∣∣2
q=(t,θ)q = (t,\theta)q=(t,θ)
p⊕(t,θ)≜R(θ)+tp\oplus (t,\theta) \triangleq R(\theta)+tp⊕(t,θ)≜R(θ)+t
SrefS^{ref}Sref表示参考激光帧生成的曲面
∏(Sref,pi)\prod(S^{ref},p_i)∏(Sref,pi)表示激光点pip_ipi在曲面上的投影
PL-ICP目标函数: minqk+1=∑i(niT[pi⊕qk+1−∏(Sres,pi⊕qk)])2\min\limits_{q_{k+1}}=\sum\limits_{i}(n_i^T[p_i\oplus q_{k+1} - \prod(S^{res},p_i\oplus q_k )])^2qk+1min=i∑(niT[pi⊕qk+1−∏(Sres,pi⊕qk)])2
N-ICP
替换ICP方法中的对应点匹配(point correspondences)方法。
充分利用实际曲面的特征来对错误的点匹配进行滤除,主要的特征为法向量和曲率。
误差项除了考虑对应点的欧式距离之外,同时还考虑对应点法向量的角度差。
pip_ipi表示激光点的坐标
nin_ini表示对应的法向量
σi\sigma_iσi表示对应的曲率
pi~=(pi,ni)T\tilde{p_i}=(p_i,n_i)^Tpi~=(pi,ni)T表示拓展的点
TTT表示欧式变换的矩阵
⊕\oplus⊕表示对于pi~\tilde{p_i}pi~的操作符
T⊕pi~=(RniRpi+t)T\oplus\tilde{p_i}=\left({}_{Rn_i}^{Rp_i+t}\right)T⊕pi~=(RniRpi+t)
误差函数的定义:
eij(T)=(pi~−T⊕pjr~)e_{ij}(T)=(\tilde{p_i}-T\oplus\tilde{p_j^r})eij(T)=(pi~−T⊕pjr~)
目标函数的定义:
to be continued!