宜都网站建设,如何自己设计一个网页,wordpress inaction,网站开发计划书网站技术解决方案0. 简介
对于激光SLAM来说#xff0c;现在越来越多的算法不仅仅局限于点线等简答特征的场景了#xff0c;文章《PLC-LiSLAM: LiDAR SLAM With Planes, Lines,and Cylinders》说到#xff0c;平面、线段与圆柱体广泛存在于人造环境中。为此作者提出了一个使用这些landmark的…0. 简介
对于激光SLAM来说现在越来越多的算法不仅仅局限于点线等简答特征的场景了文章《PLC-LiSLAM: LiDAR SLAM With Planes, Lines,and Cylinders》说到平面、线段与圆柱体广泛存在于人造环境中。为此作者提出了一个使用这些landmark的激光雷达SLAM系统。目前已有工作做了类似于BA的平面adjustment(即.PA)但是这类工作在缺少平面的场景效果很差为了增强系统的鲁棒性引入更多的特征是有必要的因此作者在本文中引入了额外的线和圆柱体特征并且实现了平面-线-圆柱体的adjustment即(PLCA)PLCA不依赖于从三维特征点捕获的点数这使得高效求解大规模PLCA问题成为可能。定位部分通过将局部平面、直线和柱面与全局平面、直线和柱面来进行配准从而进行实时姿态估计这种被称为平面-直线-柱面配准(PLCR, plane-line-cylinder registration)。在检测landmark并完成数据关联的过程中很可能会引入误检。本文通过检查后端的成本cost函数来纠正这些错误这个错误纠正机制是那些基于配准的方法。例如LOAM和ICP由于它们并不维护数据间的关联关系所以很难纠正这些错误。这里由于该论文没有预印本所以作者以附录的形式展示了本文的内容。由于本文的内容较多所以作者主要来讲解特征提取这块的内容。其余的用泡泡机器人的内容作为补充。
1. 文章贡献
在之前的工作[1][2]中我们探索了激光雷达SLAM的平面并引入平面调整(PAplane adjustment)来纠正漂移这是视觉SLAM中的bundle adjustment(BA)的对应。由于只考虑平面如果没有足够的平面进行姿态估计这些方法将失败。本文扩展了我们[2]工作的内容并加入了线条和圆柱体。具体来说该算法包括局部映射、全局映射和定位三个部分。在局部和全局映射中通过位姿联合优化平面、直线和圆柱体。我们将由此产生的最小二乘问题分别称为局部和全局平面-柱面调整(PLCA)。在定位中通过使用平面、直线和柱面进行扫描到模型的连续时间配准来估计新的激光雷达扫描的姿态称为连续时间平面-直线-柱面配准(PLCR)。本文的贡献如下: 本文证明通过一些预处理PLCA问题的求解与面、线以及圆柱体里的三维点的数量无关。因为这些landmark的表示是无边界的许多三维点都会被包含在这些landmark里面。对于求解大尺度的PLCA问题我们的方法行而有效。 本文提供了一个高效的PLCR问题求解方法。我们使用一阶泰勒展开来近似旋转考虑到两帧之间的旋转一般是足够小的。如果比较大的旋转发生在两帧之间我们进行迭代近似。 本文的算法能够容忍足够的检测误检测。因为遮挡以及激光雷达点云的稀疏性检测这些landmark的过程容易引入误检测。这个错误纠正机制是那些基于配准的方法例如LOAM以及ICP很难做到的因为他们并不维护数据关联。我们的算法通过检验后端的cost来纠正这些误检测。
2. 详细内容
2.1 前后端概念 位姿这个公式表示通过转换矩阵X[Rt01]∈SE(3)X \left[ \begin{matrix} Rt\\01\end{matrix}\right]∈SE(3)X[R0t1]∈SE(3)将局部LiDAR坐标系中的点坐标转换到全局坐标系中的一个姿态。其中∈SO(3)∈SO(3)∈SO(3)为旋转矩阵t∈R3t∈\mathbb{R}^3t∈R3为平移向量。在PLCR和PLCA中xxx参数为x[ω;t]x [ω;t]x[ω;t]其中ωωω代表了RRR的角轴(angle-axis)表示形式: 其中[ω]×[ω]_×[ω]×表示ωωω的斜对称矩阵∣∣ω∣∣2||ω||_2∣∣ω∣∣2表示ωωω的L2L^2L2范数。 平面本文中的平面通过一个四维向量表示。前三维表示该平面的法向量模长为1最后一维为坐标系原点到该平面的距离。我们将平面表示为π[n;d]π [\mathbf{n};d]π[n;d]其中n\mathbf{n}n为∣∣n∣∣21||\mathbf{n}||_2 1∣∣n∣∣21的平面法线∣d∣|d|∣d∣为原点到该平面的距离。在PLCA中πππ用最近点参数化ηdnη d\mathbf{n}ηdn表示[27]。 线本文中的直线通过一个六维向量表示。前三维表示该直线的方向后三维表示从原点出发的垂直于该直线的向量并且其模长为原点到直线的距离。文中用普吕克坐标Plücker坐标[28]表示一条三维空间中的直线它是一个六维向量l[d;m]l [d;m]l[d;m]其中ddd是lll的方向∣∣d∣∣21||d||_2 1∣∣d∣∣21, mmm垂直于由原点和lll确定的平面∣∣m∣∣2||m||_2∣∣m∣∣2等于从原点到lll的距离lll有四个自由度。在PLCA中我们采用[29]中引入的方法对lll进行参数化。具体来说给定lll我们首先构造一个旋转矩阵Rl[d,m∣∣m∣∣2,d×m∣∣m∣∣2]R^l [d, \frac{m}{||m||_2}, d × \frac{m}{||m||_2}]Rl[d,∣∣m∣∣2m,d×∣∣m∣∣2m]其中×××为叉积。假设ωlω^lωl是(1)中RlR^lRl的角轴表示我们可以用一个四维向量将lll参数化ζ[ωl;∣∣m∣∣2]ζ \left[ω^l; | |m| |_ 2 \right]ζ[ωl;∣∣m∣∣2]。 圆柱体本文中的圆柱体通过一个七维向量表示前六维为该圆柱体所在的直线表示同上最后一维为该圆柱体的半径。文中将圆柱体表示为c[lc;r]c [l^c;r]c[lc;r]其中lcl^clc为ccc中线的Plücker坐标rrr为其半径。在PLCA中我们通过一个五维向量ν[ζc;r]ν \left[ζ^c;r\right]ν[ζc;r]来完成参数化;其中ζζζ是上面定义的lcl^clc的参数化。 观察:用mmm表示平面、直线或圆柱体即m∈{π,l,c}m∈\{π, l, c\}m∈{π,l,c}。其中mjm_jmj表示为第jjj个landmark特征点信息。并且认为mjm_jmj在XiX_iXi处是可观测的mjm_jmj在XiX_iXi处的观测值形成一组NijmN^m_{ij}Nijm点表示为Pijm{pijkm}k1Nijm\mathbb{P}^m_{ij} \{p^m_{ijk}\}^{N^m_{ij}} _{k1}Pijm{pijkm}k1Nijm。我们将pijkmp^m_{ijk}pijkm的齐次坐标表示为p~ijkm[pijkm;1]\tilde{p}^m_{ijk} [p^m_{ijk}; 1]p~ijkm[pijkm;1]。将Pijm\mathbb{P}^m_{ij}Pijm中各点的同质坐标堆叠起来我们就会得到一个Pijm×4\mathbb{P}^m_{ij}× 4Pijm×4的矩阵 由于平面、直线和圆柱体是无界物体NijmN^m_{ij}Nijm可能非常大这导致了大规模的最小二乘问题。 优化:Levenberg-MarquardtLM[30]算法通常用于解决最小二乘问题。给定一个最小二乘问题 χ^argmin∣∣(χ)∣∣22\hat{χ} \arg\min||(χ)||^2_2χ^argmin∣∣(χ)∣∣22LM算法在第kkk次迭代时通过χk1χkΔχ_{k1} χ_{k} Δχk1χkΔ更新解决方案。这里的ΔΔΔ是以下线性方程组的解 其中JεJ_\varepsilonJε是在χkχ_kχk处的雅各布矩阵λλλ是LMalgorithm的参数在每次迭代时都要调整以确保∣∣(χ)∣∣22||(χ)||^2_2∣∣(χ)∣∣22的值减少。 为了应用LM算法我们通常先计算JεJ_\varepsilonJε和ε\varepsilonε然后将其代入(3)来计算ΔΔΔ。 因为 N^m_{ij}可能非常大这种直接的方法可能是计算上的要求。另一方面显而易见的是 只需要JεTJεJ_\varepsilon^TJ_\varepsilonJεTJε和JεTεJ_\varepsilon^T \varepsilonJεTε就可以得到varDeltavarDeltavarDelta。我们表明 PLCA的JεTJεJ_\varepsilon^TJ_\varepsilonJεTJε和JεTεJ_\varepsilon^T \varepsilonJεTε有特殊的结构可以用来加速计算。用来加速计算。 二次方形式:一个对称矩阵A[aij]∈Rn×nA[a_{ij}]∈\mathbb{R}^{n×n}A[aij]∈Rn×n决定了一个二次形式qAyTAyq_Ay^TAyqAyTAy其中y[y1;...;yn]y[y1; ...; yn]y[y1;...;yn]。我们可以把qAq_AqA写成 其中χ[−−−;yiyj;−−](i≤j)α[−−−;cijaij;−−]χ [ - - - ; y_iy_j ; - - ] (i ≤ j)α [ - - - ; c_{ij}a_{ij} ; - - ]χ[−−−;yiyj;−−](i≤j)α[−−−;cijaij;−−] (如果iji jijcij1c_{ij} 1cij1并且(ij)cij2(i j)cij 2(ij)cij2)。 矩阵计算:假设z[z1;...;zm]z [z1; ...; zm]z[z1;...;zm]zzz是上面定义的yyy的向量函数。假设P∈Rk×mP∈\mathbb{R}^{k×m}P∈Rk×m是一个恒定的k×mk×mk×m矩阵。让我们定义fPzf P_zfPz并将fff的雅各布矩阵表示为JfJ_fJf。根据矩阵计算规则JfJ_fJf的形式为 这里∂z∂y\frac{∂_z}{∂_y}∂y∂z是一个m×nm×nm×n的矩阵∂zj∂yi\frac{∂_{z_j}}{∂_{y_i}}∂yi∂zj是∂z∂y\frac{∂_z}{∂_y}∂y∂z的第iii行和第jjj列的条目。
2.2 系统总览
本文提出的系统由前端与后端组成前端检测面、线与圆柱体并建立局部到全局的数据关联从而进行后续的位姿估计。前端也决定了什么时候一个新的关键帧应当被创建。后端包括局部与全局的PLCA。局部的PLCA通过滑窗优化位姿与landmark。全局的PLCA优化所有的PLCA以及除了第一帧外的所有位姿。当有Landmark被再次观测到时全局的PLCA便会被触发。
2.3 前端 Landmark检测对激光雷达每一条线的数据进行切分类似于LOAM计算每个三维点的曲率并将属于edge点的部分提取出来组成集合E\mathbb{E}E。以每条线上每两个edge点作为两端可以把这条线分为许多段这些段里的点为平坦点组成集合F\mathbb{F}F。 平面与圆柱体检测对于每个线上的每段平坦点寻找与其相近的但不属于同一条线上的平坦点如果用RANSAC法拟合平面的平均误差比较小则利用平面模型来拟合否则用圆柱体来拟合这些点。 直线检测对每个段里的edge点找到所有与其相近的但属于另一条线的edge点ppp。利用这两个点可以求得直线方程。对ppp点再寻找一个在其他线的最近点p2p_2p2。如果p2p_2p2到该直线的距离足够小则利用这三个点来拟合一条直线。重复这个过程直到没有点能够被加入到直线中。 Landmark关联 当新一帧点云获得后首先计算属于该帧的集合E\mathbb{E}E与集合F\mathbb{F}F。并对这两个集合分别构建KD-tree。对上一帧里的每个点ppp寻找其在当前帧的nnn个最近点。如果ppp属于面或者圆柱体则这nnn个最近点都被保留如果ppp属于直线则保留这n个点的曲率最大的一个点。随后利用RANSAC算法找到符合模型的内殿。通过这些内点我们之后会使用PLCR方法来进行位姿估计。 关键帧创建 受到ORB-SLAM的启发当以下情况发生时新的关键帧会被创建 当前帧20%以上的点不能被追踪与关联。当前帧与上一个关键帧的旋转角度大于5°。当前帧与上一帧的距离大于设定的阈值。 对于新的关键帧我们在没被关联到的三维点上检测面、线与圆柱体。对于新检测出来的landmark首先会寻找与已有的landmark的均方根之差如果大于设定的阈值则新增一个landmark。
2.4 面-线-圆柱体 联合优化 (PLCA) 全局PLCA 在全局的PLCA中对所有点计算点到模型的损失从而构建优化问题来同时优化位姿与landmark的参数。 作者详细给出了各个模型的LM优化时的参数的形式有兴趣的同学可以阅读原文这里不再赘述。 作者证明构建优化问题Axb时A的规模仅与Landmark的数量有关而与landmark里的点的数量无关。
…详情请参照古月居
