对点云匹配算法ICP、PL-ICP、NICP和IMLS-ICP的理解
点云匹配算法是为了匹配两帧点云数据,从而得到传感器(激光雷达或摄像头)前后的位姿差,即里程数据。匹配算法已经从最初的ICP方法发展出了多种改进的算法。他们分别从配准点的寻找,误差方程等等方面进行了优化。下面分别介绍:
ICP
ICP的基本思想是:
给定两个点云集合
X={x1,x2,⋯,xNx}X=\left\{x_{1}, x_{2}, \cdots, x_{N_{x}}\right\}X={x1,x2,⋯,xNx}
P={p1,p2,⋯,pNp}P=\left\{p_{1}, p_{2}, \cdots, p_{N_{p}}\right\}P={p1,p2,⋯,pNp}
其中 xix_{i}xi和pip_{i}pi表示点云坐标,NxN_{x}Nx和NpN_{p}Np表示点云的数量。
求解旋转矩阵R和平移向量t使得下式结果最小。
E(R,t)=1Np∑i=1Np∥xi−Rpi−t∥2E(R, t)=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}-R p_{i}-t\right\|^{2}E(R,t)=Np1∑i=1Np∥xi−Rpi−t∥2
在实际工程中不可能知道两个点云的点是如何配对的。只能通过迭代求解的方法一步步缩小误差,最终得到使误差方程最小的旋转矩阵R和平移矩阵t。
算法流程:
1)寻找对应点
通常使用编码盘的里程计数据得到位姿差,即当前机器人在上次机器人坐标系中的位姿。将此R和t作为ICP算法的first guess,帮助算法寻找点云对应点。这里需注意,如果激光传感器没有安装在机器人坐标系中心,则存在里程计得到的位姿到激光传感器位姿的坐标转换关系。如下图中lll所示:
2)根据对应点计算R和t。
这一步就是根据找好的对应点构建误差方程。普通的ICP 是使用点到点的距离作为误差的。误差方程如下:
E(R,t)=1Np∑i=1Np∥xi−Rpi−t∥2E(R, t)=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}-R p_{i}-t\right\|^{2}E(R,t)=Np1∑i=1Np∥xi−Rpi−t∥2
然后求解出R和t。具体的求解推导如下:
E(R,t)=1Np∑i=1Np∥xi−Rpi−t∥2E(R,t)=1Np∑i=1Np∥xi−Rpi−t−ux+Rup+ux−Rup∥2=1Np∑i=1Np∥xi−ux−R(pi−up)+(ux−Rup−t)∥2=1Np∑i=1Np∥xi−ux−R(pi−up)∥2+∥ux−Rup−t∥2+2(xi−ux−R(pi−up))T(ux−Rup−t)=1Np∑i=1Np∥xi−ux−R(pi−up)∥2+∥ux−Rup−t∥2\begin{aligned} E(R, t) &=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}-R p_{i}-t\right\|^{2} \\ E(R, t) &=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}-R p_{i}-t-u_{x}+R u_{p}+u_{x}-R u_{p}\right\|^{2} \\=& \frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}-u_{x}-R\left(p_{i}-u_{p}\right)+\left(u_{x}-R u_{p}-t\right)\right\|^{2} \\=& \frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}-u_{x}-R\left(p_{i}-u_{p}\right)\right\|^{2}+\left\|u_{x}-R u_{p}-t\right\|^{2} \\ &+2\left(x_{i}-u_{x}-R\left(p_{i}-u_{p}\right)\right)^{T}\left(u_{x}-R u_{p}-t\right) \\&=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}-u_{x}-R\left(p_{i}-u_{p}\right)\right\|^{2}+\left\|u_{x}-R u_{p}-t\right\|^{2} \end{aligned}E(R,t)E(R,t)===Np1i=1∑Np∥xi−Rpi−t∥2=Np1i=1∑Np∥xi−Rpi−t−ux+Rup+ux−Rup∥2Np1i=1∑Np∥xi−ux−R(pi−up)+(ux−Rup−t)∥2Np1i=1∑Np∥xi−ux−R(pi−up)∥2+∥ux−Rup−t∥2+2(xi−ux−R(pi−up))T(ux−Rup−t)=Np1i=1∑Np∥xi−ux−R(pi−up)∥2+∥ux−Rup−t∥2
其中
ux=1Np∑i=1Npxiup=1Np∑i=1Nppiu_{x}=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}} x_{i} \quad u_{p}=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}} p_{i}ux=Np1∑i=1Npxiup=Np1∑i=1Nppi
而(xi−ux−R(pi−up))\left(x_{i}-u_{x}-R\left(p_{i}-u_{p}\right)\right)(xi−ux−R(pi−up))项累加NpN_{p}Np次后会等于0。
最终得到的等式中只有∥xi−ux−R(pi−up)∥2\left\|x_{i}-u_{x}-R\left(p_{i}-u_{p}\right)\right\|^{2}∥xi−ux−R(pi−up)∥2项与RRR有关。
可以先令∥ux−Rup−t∥2\left\|u_{x}-R u_{p}-t\right\|^{2}∥ux−Rup−t∥2等于0,则只需保证∥xi−ux−R(pi−up)∥2\left\|x_{i}-u_{x}-R\left(p_{i}- u_{p}\right)\right\|^{2}∥xi−ux−R(pi−up)∥2项最小即可。求出RRR后带入∥ux−Rup−t∥2=0\left\|u_{x}-R u_{p}-t\right\|^{2}=0∥ux−Rup−t∥2=0求解出t。所以误差方程可以简化成下式:
minE(R,t)=1Np∑i=1Np∥xi−ux−R(pi−up)∥2\min E(R, t)=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}-u_{x}-R\left(p_{i}-u_{p}\right)\right\|^{2}minE(R,t)=Np1∑i=1Np∥xi−ux−R(pi−up)∥2
=1Np∑i=1Np∥xi′−Rpi′∥2=1Np∑i=1NpxiTxi′+piTRTRpi′−2xiTRpi′=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}}\left\|x_{i}^{\prime}-R p_{i}^{\prime}\right\|^{2}=\frac{1}{N_{p}} \sum_{i=1}^{N_{p}} x_{i}^{T} x_{i}^{\prime}+p_{i}^{T} R^{T} R p_{i}^{\prime}-2 x_{i}^{T} R p_{i}^{\prime}=Np1∑i=1Np∥xi′−Rpi′∥2=Np1∑i=1NpxiTxi′+piTRTRpi′−2xiTRpi′
=∑i=1Np−2xi′TRpi′=\sum_{i=1}^{N_{p}}-2 x_{i}^{\prime T} R p_{i}^{\prime}=∑i=1Np−2xi′TRpi′
因为xiTxi′x_{i}^{T} x_{i}^{\prime}xiTxi′项与RRR矩阵没有关系,所以可以不考虑。由于RRR为正交矩阵,则RTR=IR^TR=IRTR=I,进而piTRTRpi′=piTpi′p_{i}^{T} R^{T} R p_{i}^{\prime}=p_{i}^{T} p_{i}^{\prime}piTRTRpi′=piTpi′。可以看到该项与RRR也没有什么关系,同样不考虑它。最后就只剩下一项与RRR相关了。
要求最小即最大化下面的式子:
max∑i=1Npxi′TRpi′=∑i=1NpTrace(Rxi′piT)=Trace(RH)\max \sum_{i=1}^{N_{p}} x_{i}^{\prime T}{{R} p_{i}^{\prime}}=\sum_{i=1}^{N_{p}} \operatorname{Trace}\left(R x_{i}^{\prime} p_{i}^{T}\right)=\operatorname{Trace}(R H)max∑i=1Npxi′TRpi′=∑i=1NpTrace(Rxi′piT)=Trace(RH)
其中H=∑i=1Npxi′pi′TH=\sum_{i=1}^{N_{p}} x_{i}^{\prime} p_{i}^{\prime T}H=∑i=1Npxi′pi′T
利用定理,假设矩阵A为正定对称矩阵,则对于任意的正交矩阵B,都有
Trace(A)≥Trace(BA)\operatorname{Trace}(A) \geq \operatorname{Trace}(B A)Trace(A)≥Trace(BA)
对HHH进行SVD分解H=UΛVTH=U \Lambda V^{T}H=UΛVT
令X=VUTX=V U^{T}X=VUT,则XXX为正交矩阵。
XH=VUTUΛVT=VΛVTX H=V U^{T} U \Lambda V^{T}=V \Lambda V^{T}XH=VUTUΛVT=VΛVT得到正定对称矩阵。
则:
Trace(XH)≥Trace(BXH)\operatorname{Tr} \operatorname{ace}(X H) \geq \operatorname{Tr} \operatorname{ace}(B X H)Trace(XH)≥Trace(BXH)
B是任意的正交矩阵,X也是一个正交矩阵,因此BX可以取遍所有的正交矩阵。所以BX也包括了需要求解的旋转矩阵T,因此
Trace(RH)≤Trace(XH)\operatorname{Trace}(R H) \leq \operatorname{Trace}(X H)Trace(RH)≤Trace(XH)
当R=X时,等式成立。因此:
R=X=VUTR=X=V U^{T}R=X=VUT
则t=ux−Rup\mathrm{t}=u_{x}-R u_{p}t=ux−Rup
3)将计算得到的R和t用于下一次迭代计算,直到误差值小于设定的阈值。
这里指出ICP的一个明显缺陷:
两帧激光点云数据中的点不可能表示的是空间中相同的位置。所以用点到点的距离作为误差方程势必会引入随机误差。
参考论文:Least-Squares Fitting of Two 3-D Point Sets
PL-ICP
PL-ICP相对于PP-ICP最大的区别是其改进了误差方程。PP-ICP是点对点的距离作为误差而PL-ICP是采用点到其最近两个点连线的距离。下图展示了误差方程的差异。
从上方的(a)中可以看出,激光点是对实际环境中曲面的离散采样。最好的误差尺度是激光点到实际曲面的距离。而PL-ICP采用分段线性的方法对实际曲面进行近似,用激光点到最近两点连线的距离来模拟实际激光点到曲面的距离。可以看出PL-ICP的误差方程更贴近实际情况。
算法流程:
先贴一张论文里的算法步骤
1)给定一个初始的转换矩阵q0q_{0}q0,将当前激光帧的数据转换到参考帧坐标系下。初始的转换矩阵q0q_{0}q0一般通过里程计来获得。后面迭代计算所需的qkq_{k}qk由上一次算法迭代计算得到。
2)为当前激光帧中的每一个点,找到其最近的两个点j1和j2。
3)去除误差过大的点。
4)构建最小化误差方程。
5)求解出位姿转换矩阵qk+1\boldsymbol{q}_{k+1}qk+1。然后将其用于下次迭代计算。
总结PL-ICP与ICP的主要区别:
1)误差函数的形式不同,ICP对点对点的距离作为误差,PL-ICP为点到线的距离作为误差;
PL-ICP的误差形式更符合实际情况。
2)收敛速度不同,ICP为一阶收敛,PL-ICP为二阶收敛。
∥qk−q∞∥<c∥qk−1−q∞∥∥qk−q∞∥2<c∥qk−1−q∞∥2\left\|\boldsymbol{q}_{k}-\boldsymbol{q}_{\infty}\right\|<c\left\|\boldsymbol{q}_{k-1}-\boldsymbol{q}_{\infty}\right\| \quad\left\|\boldsymbol{q}_{k}-\boldsymbol{q}_{\infty}\right\|^{2}<c\left\|\boldsymbol{q}_{k-1}-\boldsymbol{q}_{\infty}\right\|^{2}∥qk−q∞∥<c∥qk−1−q∞∥∥qk−q∞∥2<c∥qk−1−q∞∥2
3)PL-ICP的求解精度高于ICP,特别是在结构化环境中。
4)PL-ICP对初始值更敏感。不单独使用。其容易陷入局部循环,如下图(a)所示。通常用里程计得到一个初始转换矩阵q0q_{0}q0给到PL-ICP算法。
csm源码包分析:
ros的csm包实现了ICP和PL-ICP算法。源码包和论文下载地址如下:
https://censi.science/software/csm/
csm包的作者给出了一个该功能包的操作说明文件(csm_manual.pdf)。里面详细描述了各项配置参数的含义。
其中sm/app文件夹中的sm0.c sm1.c sm2.c sm3.c 相当于是几个使用示例。
主要的算法实现是在csm/icp文件夹中的几个文件里。论文中的所有算法步骤完整的体现在了icp_loop.c文件中的icp_loop函数里。
开源代码
参考论文:An ICP variant using a point-to-line metric
NICP
NICP方法与ICP方法的主要流程和思想是一致的。但是它在Trim outlier和误差项里考虑了更多的因素。这也是它效果更好的原因。它充分利用实际曲面的特征来对错误点进行了滤除,主要使用的特征为法向量和曲率。在误差项里除了考虑了点到对应点切面的距离,还考虑了对应点法向量的角度差。目前NICP方法开源的代码主要是针对3D点云的,其调用了Eigen库和OpenCV库。源码中显示的部分调用了QT5。既然NICP方法考虑了法向量和曲率,那么就涉及到了如何求解点的法向量和曲率。下面简述论文中的方法:
1)高斯拟合。找到点pip_ipi周围半径RRR范围内的所有点ViV_iVi。求解均值和协方差。
μis=1∣Vi∣∑pj∈VipiΣis=1∣Vi∣∑pj∈Vi(pi−μi)T(pi−μi)\begin{aligned} \mu_{i}^{s} &=\frac{1}{\left|\mathcal{V}_{i}\right|} \sum_{\mathbf{p}_{j} \in \mathcal{V}_{i}} \mathbf{p}_{i} \\ \boldsymbol{\Sigma}_{i}^{s} &=\frac{1}{\left|\mathcal{V}_{i}\right|} \sum_{\mathbf{p}_{j} \in \mathcal{V}_{i}}\left(\mathbf{p}_{i}-\mu_{i}\right)^{T}\left(\mathbf{p}_{i}-\mu_{i}\right) \end{aligned}μisΣis=∣Vi∣1pj∈Vi∑pi=∣Vi∣1pj∈Vi∑(pi−μi)T(pi−μi)
2)对协方差矩阵进行SVD分解,得到按从小到大顺序的特征值λ1:3\lambda_{1: 3}λ1:3。
Σis=R(λ1000λ2000λ3)RT\mathbf{\Sigma}_{i}^{\mathrm{s}}=\mathbf{R}\left(\begin{array}{ccc}{\lambda_{1}} & {0} & {0} \\ {0} & {\lambda_{2}} & {0} \\ {0} & {0} & {\lambda_{3}}\end{array}\right) \mathbf{R}^{T}Σis=R⎝⎛λ1000λ2000λ3⎠⎞RT
论文中曲率的定义为:
σi=λ1λ1+λ2\sigma_{i}=\frac{\lambda_{1}}{\lambda_{1}+\lambda_{2}}σi=λ1+λ2λ1
法向量的定义:最小特征值对应的特征向量。
Trim outlier中的改进:
1)如果没有well define的法向量,则拒绝。即选择比较结构化的点,如果对应点周围过于杂乱就丢弃该点。
2)两点间的距离大于阈值,则拒绝。
∥pic−T⊕pjr∥>ϵd\left\|\mathbf{p}_{i}^{\mathrm{c}}-\mathbf{T} \oplus \mathbf{p}_{j}^{\mathrm{r}}\right\|>\epsilon_{d}∥∥pic−T⊕pjr∥∥>ϵd
3)两点的曲率之差距大于阈值,则拒绝。
∣logσic−logσjr∣>ϵσ\left|\log \sigma_{i}^{\mathrm{c}}-\log \sigma_{j}^{\mathrm{r}}\right|>\epsilon_{\sigma}∣∣logσic−logσjr∣∣>ϵσ
4)两点的法向量角度之差大于阈值,则拒绝。
nic⋅T⊕njr<ϵn\mathbf{n}_{i}^{\mathrm{c}} \cdot \mathbf{T} \oplus \mathbf{n}_{j}^{\mathrm{r}}<\epsilon_{n}nic⋅T⊕njr<ϵn
误差项中的改进:
1)点到对应点切面的距离作为其中的一个误差项。
2)法向量的夹角作为另一个误差项。考虑该项误差增加了旋转矩阵RRR的求解精度。
最后得到的误差定义为:
eij(T)=(p~ic−T⊕p~jr)\mathbf{e}_{i j}(\mathbf{T})=\left(\tilde{\mathbf{p}}_{i}^{\mathrm{c}}-\mathbf{T} \oplus \tilde{\mathbf{p}}_{j}^{\mathrm{r}}\right)eij(T)=(p~ic−T⊕p~jr)
其中
T⊕p~i=(Rpi+tRni)T \oplus \widetilde{\mathrm{p}}_{i}=\left(\begin{array}{c}{R p_{i}+t} \\ {R n_{i}}\end{array}\right)T⊕pi=(Rpi+tRni)
即同时考虑了欧式距离和法向量的夹角。
目标函数的求解:
1)目标函数的定义为:
∑ceij(T)TΩ~ijeij(T)\sum_{c} \mathbf{e}_{i j}(\mathbf{T})^{T} \tilde{\Omega}_{i j} \mathbf{e}_{i j}(\mathbf{T})∑ceij(T)TΩ~ijeij(T)
2)非线性最小二乘问题,通过LM方法进行求解。
(H+λI)ΔT=bH=∑JiJiTJi=∂eij(T)∂TT←ΔT⊕T\begin{aligned}(\mathbf{H}+\lambda \mathbf{I}) \Delta \mathbf{T} &=\mathbf{b} \\ H &=\sum_{J_{i} J_{i}}^{T} \\ J_{i} &=\frac{\partial e_{i j}(T)}{\partial T} \quad \mathbf{T} \leftarrow \Delta \mathbf{T} \oplus \mathbf{T} \end{aligned}(H+λI)ΔTHJi=b=JiJi∑T=∂T∂eij(T)T←ΔT⊕T
对NICP的总结:
1)由于在寻找点匹配的过程中,考虑了环境 曲面的法向量和曲率,因此可以提前排除 一些明显是错误的匹配。这样就减少了计算量并且提高了计算结果的精度。
2)在误差定义中,除了考虑欧式距离之外,还考虑了法向量之间的夹角,因此具有更加准确的求解角度。
3)用LM方法进行迭代求解目标误差方程,迭代收敛即可得到两帧激光数据之间的相对位姿。
源码地址
参考网址:http://jacoposerafin.com/nicp/
IMLS-ICP
IMLS-SLAM是一个仅依赖点云数据的低漂移(low-drift)SLAM算法。其依赖于一个scan-to-model的匹配框架。这里的model可以认为是对点云进行的局部曲面建模。
基本思想:
1)选择具有代表性的激光点来进行匹配,既能减少计算量同时又能减少激光点分布不均匀导致的计算结果出现偏移。
2)点云中隐藏着真实的曲面,最好的做法是能从参考帧点云中把曲面重建出来。
3)曲面重建的越准确,对真实世界描述越准确,匹配的精度就越高。
具体的改进措施:
1)scan egomotion
egomotion的定义如下:
进行激光数据的运动畸变去除。在雷达扫描一圈的过程中,车子是在运动的。这会造成一帧激光的时间内,每束激光测量时车子实际的位置是不同的。论文里假设两次连续的激光扫描间隔,车子的egomotion是相似的。所以采用上一次的相对位移来计算当前的实际的egomotion。
2)去除动态障碍物
在匹配scan和model之前,我们要去除scan中所有移动物体。我们采用去除所有可能会动的小物体的方法:首先,删去地面点云,聚类,并抛弃所有(bounding box的)长小于14m,宽小于14m,高小于4m的物体。剩下的结构足够我们匹配。最后添上去掉的地面点云。
3)特征点的选取
选取思路:
具有丰富特征的点,即为结构化的点:具有良好的曲率和法向量的定义。
曲率越小的点越好,因为曲率为0代表着直线,代表着最结构化的点,也代表着具有非常好的法向量定义,能够提供足够的约束。
选点的时候需要注意选取的激光点的均衡以保证可观性,因为是平面匹配,不存在角度不可观的情况。只需要考虑X方向和Y方向的可观性。要保证两者的约束基本上是一致的, 才能让结果不出现偏移。
大概率上,随着角度的偏转,观测的点云是不一样的。所以角度一般是可观的。对于二维SLAM,只需保证X方向和Y方向上选取的点云数量接近就可以。而对于三维则需要考虑6个维度(X,Y,Z,roll,pitch,yaw)。
4)曲面重建
- 已知点云集合PkP_kPk中每一个点pip_ipi的法向量nin_ini ,则PkP_kPk中隐藏的曲面为:
IPk(x)=∑pi∈PkWi(x)((x−pi)⋅ni→)∑pj∈PkWj(x)Wi(x)=e−∥x−pi∥2/h2\begin{aligned} I^{P_{k}}(x) &=\frac{\sum_{p_{i} \in P_{k}} W_{i}(x)\left(\left(x-p_{i}\right) \cdot \overrightarrow{n_{i}}\right)}{\sum_{p_{j} \in P_{k}} W_{j}(x)} \\ W_{i}(x) &=e^{-\left\|x-p_{i}\right\|^{2} / h^{2}} \end{aligned}IPk(x)Wi(x)=∑pj∈PkWj(x)∑pi∈PkWi(x)((x−pi)⋅ni)=e−∥x−pi∥2/h2
xxx是空间中的一个点,pipipi是点云中的一个点。
上图中的Height=(x−pi)⋅niHeight=(x-p_i)\cdot n_iHeight=(x−pi)⋅ni,表示点到曲面的距离。
我们认为激光点云是分布在真实曲面的附近,并可以用高斯分布描述。如下图
所以可以用Wi(x)W_i(x)Wi(x)表示点xxx到点云pip_ipi距离的权重。当点xxx到点云pip_ipi距离很远时,权重会接近0。该算法会选取点xxx附近的一部分点使用上面的公式重建曲面。公式中的∑pj∈PkWj(x){\sum_{p_{j} \in P_{k}} W_{j}(x)}∑pj∈PkWj(x)为所有这些权重的累加。
公式的实现代码
double mh2 = m_h * m_h;for(int i = 0; i < nearPoints.size(); ++i){Eigen::Vector2d delta_p = x - nearPoints[i];double weight = std::exp(-delta_p.squaredNorm()/mh2);projSum += weight * delta_p.dot(nearNormals[i]);weightSum += weight;}height = projSum / (weightSum + 0.000001);//加一个很小的数避免除0
- IPk(x)=0I^{P_{\mathrm{k}}}(x)=0IPk(x)=0对应的集合表示曲面。
- 空间中点到曲面的距离,直接代入上述方程即可。
5)匹配求解
当前帧中一点xix_ixi 到曲面的距离为IPk(xi)I^{P_k}(x_i)IPk(xi)
则点xix_ixi在曲面上的投影yiy_iyi为:yi=xi−IPk(xi)⋅ni\mathrm{y}_{i}=x_{i}-I^{P_{k}}\left(x_{i}\right) \cdot n_{i}yi=xi−IPk(xi)⋅ni
PkP_kPk中离点xix_ixi最近的点的法向量为nin_ini。利用这个公式寻找xix_ixi对应的投影点yiy_iyi。点xix_ixi和点yiy_iyi为对应的匹配点:∑((Rxi+t−yi)⋅ni)2\sum\left(\left(\mathrm{R} \mathrm{x}_{\mathrm{i}}+\mathrm{t}-\mathrm{y}_{\mathrm{i}}\right) \cdot \mathrm{n}_{\mathrm{i}}\right)^{2}∑((Rxi+t−yi)⋅ni)2
这里的法向量nin_ini是yiy_iyi点的法向量。该公式描述的是转换后的xix_ixi点到投影点yiy_iyi上的距离(注意是法向量上的距离)。
总结:
IMLS-ICP使用高斯拟合和最小二乘重建出一个隐含的曲面。找到空间点在隐含曲面的投影点。使用点到该曲面上投影点间的距离构建误差方程。
参考网址:
IMLS-SLAM 论文学习
IMLS-SLAM: scan-to-model matching based on 3D data,Jean-Emmanuel Deschaud
相关论文放到我的github里了:
相关论文
ICP方法初始值的设定方法
一个好的初始值可以减少icp迭代的次数,提高效率和效果。初始值的设定可以从下面三个角度来考虑:
1.、假设机器人静止,设置帧间位移为0。
2、假设机器人匀速运动,利用上一时刻机器人的速度和时间,计算当前时刻机器人位移间隔,将此作为初值传入icp
3、利用传感器如里程计、IMU等,计算前后两时刻的初始位移。实际使用时要注意坐标系的转换。因为icp方法实在激光坐标系下进行的。
提升ICP方法精度和速度的一些考虑
精度上:
1)使用其他传感器或方法(如里程计、IMU)提供初值(coarse to fine)。
2)Ransac框架,去除outlier。
效率上:
1)选取合适的采样点。IMLS-ICP的论文里就提到了一些去除误差点的方法。
2)使用合适的数据结构,提高程序的运行效率。
关注公众号《首飞》回复“机器人”获取精心推荐的C/C++,Python,Docker,Qt,ROS1/2,机器人学等机器人行业常用技术资料。
对点云匹配算法ICP、PL-ICP、NICP和IMLS-ICP的理解相关推荐
- 工信部域名icp查询 在哪查询域名icp信息
根据现有流程,用户在提交网站ICP信息后,不能自行修改信息,需由其接入服务商代为修改,其目的在于强化接入服务商的代ICP责任,减轻网站主办者自行ICP负担. 最近入职了一家SEO公司,接手管理了50多 ...
- icp光谱仪的工作原理_ICP2060T ICP光谱仪
产品说明.技术参数及配置1.主要配置及附件: 以光电倍增管PMT检测器为基础的单道扫描电感耦合等离子体发射光谱仪,高频发生器,自动调谐,提供具有中英文操作界面的软件: 专用自动温控循环冷却水系统: 氩 ...
- icp光谱仪的工作原理_ICP2060T ICP光谱仪工作原理
产品说明.技术参数及配置1.主要配置及附件: 以光电倍增管PMT检测器为基础的单道扫描电感耦合等离子体发射光谱仪,高频发生器,自动调谐,提供具有中英文操作界面的软件: 专用自动温控循环冷却水系统: 氩 ...
- ICP许可证是什么证?ICP备案和ICP许可证的区别?
1.ICP许可证是什么证 ICP许可证是增值电信业务经营许可证的一个种类,全称是信息服务业务(仅限互联网信息服务). 审批部门:各地通信管理局 年检:每年1-3月 有效期:五年 2.哪些企业需要办理I ...
- java day65【视图[应用] 、索引[应用] 、pl/sql 基本语法[了解] 、存储过程[理解] 、 存储函数[理解]、 触发器[理解]、Java 程序调用存储过程[应用]】...
转载于:https://www.cnblogs.com/xuweng/p/11341232.html
- 【云原生】k8s(Kubernetes)中yaml文件快速阅读理解
简言: k8s yaml中文件内容一般有kind类型之分,每种类型有不同的功能(一般用---符号隔开) 常见的kind类型: (1)Endpoints:Endpoints可以把外部的链 ...
- mysql是怎样运行的 从根儿 百度云_MySQL 是怎样运行的:从根儿上理解 MySQL|完结|百度云下载...
-代码开始- /MySQL 是怎样运行的:从根儿上理解 MySQL [13.8M] ┣━━[天下无鱼][shikey.com]0万里长征第一步(非常重要) -- 如何愉快的阅读本小册(01).md [ ...
- 自动驾驶系统设计及应用--清华大学出版--部分笔记
作者:余贵珍.周彬.王阳.周亦威.白宇 目录 第一章 自动驾驶系统概述 1.1 自动驾驶系统架构 1.1.1 自动驾驶系统的三个层级 1.1.2 自动驾驶系统的基本技术架构 1.2 自动驾驶技术国内外 ...
- 对ICP、PL-ICP、NICP、IMLS-ICP匹配算法的解析
1.ICP(Iterative Closest Point) 通过迭代最近点来实现数据的匹配,ICP方法是用来求解两个点云集合转换关系的最通用的方法 数学描述: 如下图所示,我们需要求解出R,t,使得 ...
最新文章
- 一个非常好用的 Python 魔法库
- JAVA 创建线程池
- 百度开源的 71 个项目,看看你用过几个?
- TF-IDF + K-Means 中文聚类例子 - scala
- vue动态监听窗口高度 - 全背景banner
- 独家=垄断?从抖音快手电影宣发签独家谈起
- Compose 1.0 即将发布,你准备好了吗?
- DBCC CHECKIDENT 和SET IDENTITY_INSERT table OFF
- java 离线版语音转文字
- Zune账号注册教程
- 【Python_绘图】堆积柱形图
- 计算机科学之父——图灵
- mysql 分库备份_如何分表分库备份及批量恢复?MySQL
- python 二进制的数为啥带0b?,怎样去掉0b?
- Centos 7分辨率调整成适应虚拟机屏幕大小
- 集线器,交换机与路由器
- 30系 显卡显存被占用又找不到进程的解决办法
- 勇者斗恶龙服务器没有响应,PC版勇者斗恶龙英雄打不开怎么办?
- 记一次linux挖矿木马的处置
- 报录比超过13:1的计算机专硕?211暨南大学公布报考人数!
热门文章
- 笔记本外接显示屏模糊解决办法,调缩放比没用?
- php html5 cms,建站教程|CMS教程|PHP教程|html5教程 - 站长图库
- 全国高等学校计算机等级考试大纲,全国高等学校计算机等级考试大纲.doc
- 计算机Excel怎么弄迷你图,教大家excel2016怎么添加迷你图
- 基于STM32的脉搏心率检测仪(OLED可以实时显示脉冲波形)
- 成人肛门镜的全球与中国市场2022-2028年:技术、参与者、趋势、市场规模及占有率研究报告
- 两位图灵奖得主万字长文:新计算机架构,黄金十年爆发!
- Hyper-V 2012 R2 故障转移群集
- 如何根据系谱计算近交系数并保存
- python安装需要什么电脑配置_自学python用什么配置电脑?