宁波环保营销型网站建设,企业公司网页设计方案,网站建设评估,网络营销常用工具有哪些?回环检测是指检测传感器的两次测量#xff08;如图像、激光雷达扫描#xff09;是否发生在同一场景#xff0c;它是对于SLAM问题至关重要。基于激光雷达的回环检测应该满足如下要求#xff1a;
无论视点如何变化#xff0c;回环检测方法应该实现旋转和平移不变性#xf…回环检测是指检测传感器的两次测量如图像、激光雷达扫描是否发生在同一场景它是对于SLAM问题至关重要。基于激光雷达的回环检测应该满足如下要求
无论视点如何变化回环检测方法应该实现旋转和平移不变性回环检测方法对不同的点云密度和环境应具有鲁棒性因为激光扫描点云的稀疏性随距离、场景和激光雷达类型而变化回环检测算法能够更好的提供两帧激光扫描之间的相对位姿一个好的初始值能让后续的配准算法收敛的更快更准确。
STD-LCD可以高效地检测闭环并提供可靠的闭环几何变换。此外它可以处理小重叠回环检测的挑战情况。与其他描述符中使用的多边形相比三角形更稳定因为三角形的形状是给定边长或夹角唯一确定的。此外三角形的形状对于刚性变换是完全不变的。与关键点附近的的局部描述符相比三角形的形状是旋转和平移不变的。基于这一性质STD设计了一种算法从三维点云中高效地提取局部关键点并将这些关键点编码为三角形描述符。然后通过匹配点云之间描述符的边长和一些其他信息来实现回环检测。从描述符匹配得到的点对关系可以进一步用于几何验证这大大提高了回环检测的准确率。
关键点提取和描述符构建
当接收到累积子图后STD-LCD对子图进行体素化对体素中的点 p i ( i 1 , … , N ) \mathbf{p}_i(i1,\ldots,N) pi(i1,…,N)求协方差矩 Σ \bm \Sigma Σ。 p ˉ 1 N ∑ i 1 N p i ; Σ 1 N ( p i − p ˉ ) ( p i − p ˉ ) T \bar{\mathbf{p}}\frac{1}{N}\sum_{i1}^{N}\mathbf{p}_i;\quad\boldsymbol{\Sigma}\frac{1}{N}(\mathbf{p}_i-\bar{\mathbf{p}})(\mathbf{p}_i-\bar{\mathbf{p}})^T pˉN1i1∑Npi;ΣN1(pi−pˉ)(pi−pˉ)T
令 λ k \lambda_k λk为 Σ \bm{\Sigma} Σ第 k k k大特征值平面判定准则为 λ 3 σ 1 , λ 2 σ 2 \lambda_3\sigma_1,\lambda_2\sigma_2 λ3σ1,λ2σ2其中 σ 1 \sigma_1 σ1和 σ 2 \sigma_2 σ2是预设超参数一般 σ 1 \sigma_1 σ1为一个较小的值 σ 2 \sigma_2 σ2为一个较大的值。将满足平面判定准则的体素定义为平面体素否则为非平面体素。
通过区域生长的方法将平面体素合并为更大的平面即从一个随机的平面体素开始如果其相邻体素有相同的平面法向量且平面距离小于阈值则将其加入到当前正在生长的平面中。否则如果相邻体素并不在同一平面上则将其加入到当前正在生长的平面的边界体素表中。重复以上操作直到所有添加进来的体素都已经被扩展或者达到边界体素。
以生长得到的平面为图像平面将边界体素的点投影到相邻平面上以边界体素中的点到平面的最大距离为像素值。对于图像将具有局部最大点到平面距离的像素中距离最大的点设置为关键点。将提取到的关键点构建成一个kd树并对每一个关键点搜索其最近邻的20个关键点构建三角描述符。进行空间非极大值抑制将具有相同边长的描述符去重以增强提取的非重复性。
每个三角形描述符 Δ \bm{\Delta} Δ包含的元素如下
三个顶点 p 1 , p 2 , p 3 \mathbf{p}_1,\mathbf{p}_2,\mathbf{p}_3 p1,p2,p3三个对应投影平面的法向量 n 1 , n 2 , n 3 \mathbf{n}_1,\mathbf{n}_2,\mathbf{n}_3 n1,n2,n3三个边长 l 12 , l 23 , l 13 l_{12},l_{23},l_{13} l12,l23,l13且 l 12 ≤ l 23 ≤ l 13 l_{12}\leq l_{23}\leq l_{13} l12≤l23≤l13三角形的中心 q \mathbf{q} q描述符所属的关键帧索引 k k k 除了描述符我们还将保存从这个子图中提取的所有 n n n个平面 Π ( π 1 , π 2 , … , π n ) \bm \Pi (\bm \pi_1,\bm \pi_2,\ldots,\bm \pi_n) Π(π1,π2,…,πn)用于之后的几何验证步骤。
回环检测与验证
回环检测与验证包括三部分粗回环检测、细回环检测和几何验证。粗循环检测通过在描述符数据库的哈希表中搜索匹配的历史描述符可以快速地提取出N个可能的候选。精细回环检测通过计算配对三角形之间的所有变换矩阵并对变换进行聚类将包含变换数最多的聚类簇的中心作为优秀候选。最后计算当前帧到优秀候选者的平面重合度筛选误检选择点到平面距离最小的候选作为最佳候选。由于精细回环检测提供了一个较好的初始猜测使得之后的ICP配准更快收敛。
由于一个子图可以提取上百个描述符为了快速查询和匹配描述符我们使用哈希表存储所有描述符。我们使用描述子中具有旋转和平移不变性的6个属性来计算哈希密钥分别为边长 l 12 , l 23 , l 13 l_{12},l_{23},l_{13} l12,l23,l13和投影法向量的点积 n 1 ⋅ n 2 , n 2 ⋅ n 3 , n 1 ⋅ n 3 \mathbf{n}_1\cdot\mathbf{n}_2,\mathbf{n}_2\cdot\mathbf{n}_3,\mathbf{n}_1\cdot\mathbf{n}_3 n1⋅n2,n2⋅n3,n1⋅n3。具有6个相似属性的描述符将具有相同的哈希密钥因此存储在同一个容器中。对于一个查询帧我们提取其所有描述符。对于每个描述符 Δ i \bm{\Delta}_i Δi计算其哈希密钥将其定位到哈希表中对应的容器中并对该容器中存在的所有描述符的关键帧进行投票。当查询帧中的所有描述符 Δ i \bm \Delta_i Δi都被处理后匹配过程结束。投票数前十多的关键帧将被选为描述符的候选关键帧用于回环检测。
当给定一个候选回环关键帧时我们进行几何验证以消除由于描述符匹配不正确而导致的误检。由于边长确定后三角形的形状是唯一确定的一旦 Δ a \bm\Delta_a Δa匹配到 Δ b \bm\Delta_b Δb它们的顶点 ( p a 1 , p a 2 , p a 3 ) (\mathbf{p}_{a1},\mathbf{p}_{a2},\mathbf{p}_{a3}) (pa1,pa2,pa3)和 ( p b 1 , p b 2 , p b 3 ) (\mathbf{p}_{b1},\mathbf{p}_{b2},\mathbf{p}_{b3}) (pb1,pb2,pb3)将自然匹配。然后利用这个对应关系根据\ref{pt2ptICP}节通过奇异值分解Singular Value DecompositionSVD很容易计算出这两个关键帧之间的相对变换 T ( R , t ) \boldsymbol{T}(\boldsymbol{R},\boldsymbol{t}) T(R,t) W ∑ i 1 3 ( p a i − q a ) ( p b i − q b ) [ U , S , V ] S V D ( W ) R V U T , t − R ∗ q a q b . \begin{aligned} \mathbf{W}\sum_{i1}^3(\mathbf{p}_{a_i}-\mathbf{q}_a)(\mathbf{p}_{b_i}-\mathbf{q}_b) \\ [\mathbf{U},\mathbf{S},\mathbf{V}]\mathbf{SVD}(\mathbf{W}) \\ \bm{R}\mathbf{V}\mathbf{U}^T,\bm{t}-\bm{R}*\mathbf{q}_a\mathbf{q}_b. \end{aligned} Wi1∑3(pai−qa)(pbi−qb)[U,S,V]SVD(W)RVUT,t−R∗qaqb.
为了增加鲁棒性使用RANSAC方法来寻找最大化正确匹配描述符数量的刚体变换。
基于此刚体变换我们计算当前帧与候选帧之间的平面重叠度来进行几何验证。 用中心点 q \mathbf{q} q和法向量 u \mathbf{u} u 表示的平面 π \bm\pi π。记当前帧的平面集为 B Π [ ( B q 1 , B u 1 ) , … ( B q n , B u n ) ] ^{B}\Pi [(^{B}\mathbf{q}_1,{}^{B}\mathbf{u}_1),\ldots(^{B}\mathbf{q}_n,{}^{B}\mathbf{u}_n)] BΠ[(Bq1,Bu1),…(Bqn,Bun)]候选帧的平面集为 C Π [ ( C q 1 , C u 1 ) , … ( C q n , C u n ) ] ^{C}\Pi [(^{C}\mathbf{q}_1,{}^{C}\mathbf{u}_1),\ldots(^{C}\mathbf{q}_n,{}^{C}\mathbf{u}_n)] CΠ[(Cq1,Cu1),…(Cqn,Cun)]刚体变换为 C T B ( C R B , C t B ) ∈ S E ( 3 ) {}^{C}\bm{T}_B ( {}^C\bm{R}_B,{}^C\bm{t}_B)\in SE(3) CTB(CRB,CtB)∈SE(3)其中 n n n为当前帧的平面数 m m m为候选框的平面数。我们以 C Π {}^C\Pi CΠ的中心点 ( C q 1 , C q 2 , … , C q m ) ({}^C\mathbf{q}_1,{}^C\mathbf{q}_2,\ldots,{}^C\mathbf{q}_m) (Cq1,Cq2,…,Cqm)构建一个kd树k3。则对于每个平面中心点 B q i ( i 1 , 2 , … , n ) ∈ B Π {}^B\mathbf{q}_i(i1,2,\ldots,n)\in{}^B\Pi Bqi(i1,2,…,n)∈BΠ首先用 C T B {}^C\mathbf{T}_B CTB对 B q i {}^B\mathbf{q}_i Bqi进行变换然后在kd树中搜索一个最近的点 C q j {}^C\mathbf{q}_j Cqj通过法向量差和点到平面的距离判断两个平面是否重合 ∥ C R B B u i − C u j ∥ 2 σ n C u j T ( C T B B q i − C q j ) σ d \begin{aligned} \|{}^C\bm{R}_B{}^B\mathbf{u}_i-{}^C\mathbf{u}_j\|_2\sigma_n \\[0.3em] {}^C\mathbf{u}_j^T(^C\bm{T}_B{}^B\mathbf{q}_i-{}^C\mathbf{q}_j)\sigma_d \end{aligned} ∥CRBBui−Cuj∥2σnCujT(CTBBqi−Cqj)σd 其中 σ n \sigma_n σn和 σ d \sigma_d σd是用来判断平面重合的预设参数。
计算当前帧满足式\ref{plane_overlap}的平面占所有平面的比例 N c N coincide N sum × 100 % , N_c\frac{N_{\textsf{coincide}}}{N_{\textsf{sum}}}\times100\%, NcNsumNcoincide×100%, 其中 N coincide N_{\textsf{coincide}} Ncoincide为当前帧所有满足约束的平面数 N sum N_{\textsf{sum}} Nsum为当前帧所有平面的数量。
如果 N c N_c Nc超过阈值 σ p c \sigma_{pc} σpc我们就认为这是一个可靠的回环检测。由于平面的数量远远少于点云的数量基于平面的几何验证比基于ICP的验证方法效率更高。按照式\ref{STD-ICP}优化点面距离和法向量差异可以得到回环的精确刚体变换。 min C T B ∥ C R B B u i − C u j ∥ 2 2 ( C u j T ( C T B B q i − C q j ) ) 2 \min_{^C\bm{T}_B}\|{}^C\bm{R}_B{}^B\mathbf{u}_i-{}^C\mathbf{u}_j\|_2^2\left({}^C\mathbf{u}_j^T(^C\bm{T}_B{}^B\mathbf{q}_i-{}^C\mathbf{q}_j)\right)^2 CTBmin∥CRBBui−Cuj∥22(CujT(CTBBqi−Cqj))2
STD-LCD回环检测通过求解如式所示的优化问题可以直接得到查询帧 i i i与回环帧 j j j之间的相对位姿约束 Δ T i j \Delta \mathbf{T}_{ij} ΔTij构造回环因子并加入到因子图中 F L ∑ ( i , j ) ∈ L ∥ L o g ( Δ T i j − 1 ( X i − 1 X j ) ) ∥ Σ L i j 2 F_{\mathcal{L}} \sum_{{(i,j)}\in\mathcal{L}}\left\|\mathrm{Log}\left(\Delta \mathbf{T}_{ij}^{-1} \left({\bm{X}_{i}^{-1} }\bm{X}_{j}\right)\right)\right\|^2_{\bm \Sigma_{\mathcal{L}_{ij}}} FL(i,j)∈L∑ Log(ΔTij−1(Xi−1Xj)) ΣLij2 其中 X i \bm{X}_{i} Xi为查询帧全局位姿变量 X j \bm{X}_{j} Xj为回环帧全局位姿变量 Σ L i j \Sigma_{\mathcal{L}_{ij}} ΣLij为对应的信息矩阵 L \mathcal{L} L为回环约束帧索引集合。
