经典文献阅读之--VoxelMap(体素激光里程计)

这篇具有很好参考价值的文章主要介绍了经典文献阅读之--VoxelMap(体素激光里程计)。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

0. 简介

作为激光里程计,常用的方法一般是特征点法或者体素法,最近Mars实验室发表了一篇文章《Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry》,同时还开源了代码在Github上。文中为雷达里程计提出了一种高效的概率自适应体素建图方法。地图是体素的集合,每个体素包含一个平面(或边缘)特征能够实现对环境的概率表示和新的激光雷达帧的精确配准。我们进一步分析了对粗到细体素建图的需要,然后使用了新颖的使用哈希表组织体素地图和八叉树来有效地构建和更新地图。我们将所提出的体素建图应用于一个迭代扩展卡尔曼滤波器,并构造了一个姿态估计的最大后验概率问题。

1. 文章贡献

这篇文章作为一篇比较有影响力的文章,目前已经收到了国内比较多的关注,这里也是跟着Catalina大佬的博客,并结合自己阅读文章的理解进行介绍。

1.1 动机

  1. 直接用点云地图表达环境的方法很难考虑由雷达点云测量噪声带来的map的不确定性。地图的不确定性需要对点云中的特征进行显式参数化,在不同的 LiDAR 扫描帧中跟踪这些特征并估计这些特征参数及其不确定性比较困难。因为点云测量中的由于雷达分辨率、扫描类型以及空间点分布不均匀导致的,点密度随着扫描时间变化导致问题进一步变得复杂。此外,衡量map不确定性的时候还要考虑pose误差在系统中的传播。
  2. surfel方法通常建模很小的面特征,但场景中经常会有比较大的面特征。surfel的数据关联一般是将map中的surfel和测量帧surfel投影到一个图像上,这种渲染过程非常耗费时间。
  3. surfel建模为类似高斯混合模型的时候,一般通过EM算法来做数据关联,无效的surfel会和相邻的合并起来,进而自适应调节surfel尺寸,而自适应voxel的方法会更加鲁棒。
  4. ICP方法不够鲁棒,NDT方法不够精确,结合两者,先划分网格,在网格中估计平面参数,最终鲁棒性和精度都会比较好

1.2 贡献

  1. 针对激光雷达点云的稀疏性和不规则性,提出了一种大小自适应、由粗到精的体素构建方法。自适应体素图被组织在八叉树-哈希表的数据结构中,以提高体素构建、更新和查询的效率。

2、本文提出了一种精确的概率体素地图表示法,准确地考虑了点测量和激光雷达位置估计引起的点不确定性,以对地图中平面的不确定性进行建模。

3、实现了所提出的系统在真实世界中的激光测距和测绘的应用,并与最新的方法进行了比较。

2. 详细内容

2.1 概率平面表达

这部分的主要内容是基于激光测量误差和位姿估计误差计算世界坐标系下点的误差,基于平面法向量估计方式估计法向量的误差
体素voxel,经典文献,人工智能,计算机视觉

2.1.1 原始点测量噪声

点云噪声来源于2部分,一是range测距不确定性(depth),二是bearing direction方位不确定性(w),公式可从我们的之前工作《Pixel-level Extrinsic Self Calibration of High Resolution LiDAR and Camera in Targetless Environments》推导得到。高斯建模如下

δ ω i ∼ N ( 0 2 × 1 , Σ ω i ) , δ d i ∼ N ( 0 , Σ d i ) \boldsymbol{\delta}_{\boldsymbol{\omega}_i} \sim \mathcal{N}\left(\mathbf{0}_{2 \times 1}, \boldsymbol{\Sigma}_{\boldsymbol{\omega}_i}\right), \delta_{d_i} \sim \mathcal{N}\left(0, \Sigma_{d_i}\right) δωiN(02×1,Σωi),δdiN(0,Σdi)

ω i ∈ S 2 ω_i∈\mathbb{S}^2 ωiS2为测量的轴承方向, δ ω i ∼ N ( 0 2 × 1 , Σ ω i ) δ_{ω_i} \sim \mathcal{N} (0_{2×1}, Σ_{ω_i}) δωiN(02×1Σωi) ω i ω_i ωi切平面上的轴承方向噪声, d i d_i di为深度测量值, δ d i   N ( 0 , Σ d i ) δ_{d_i} ~ N (0, Σ_{d_i}) δdi N(0Σdi)为测距噪声。得到测量点 L p i ^Lp_i Lpi及其协方差 Σ L p i Σ_{L_{p_i}} ΣLpi的噪声 δ L p i δ_{L_{p_i}} δLpi

δ L p i = [ ω i − d i ⌊ ω i ⌋ ∧ N ( ω i ) ] ⏟ A i [ δ d i δ ω i ] ∼ N ( 0 , Σ L p i ) Σ L p i = A i [ Σ d i 0 1 × 2 0 2 × 1 Σ ω i ] A i T \boldsymbol{\delta}_{L_{\mathbf{p}_i}}=\underbrace{\left[\begin{array}{ll}\boldsymbol{\omega}_i & -d_i\left\lfloor\boldsymbol{\omega}_i\right\rfloor \wedge \mathbf{N}\left(\boldsymbol{\omega}_i\right)\end{array}\right]}_{\mathbf{A}_i}\left[\begin{array}{c}\delta_{d_i} \\ \boldsymbol{\delta}_{\boldsymbol{\omega}_i}\end{array}\right] \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{\Sigma}_{L_{\mathbf{p}_i}}\right) \\ \boldsymbol{\Sigma}_{L_{\mathbf{p}_i}}=\mathbf{A}_i\left[\begin{array}{cc} \Sigma_{d_i} & \mathbf{0}_{1 \times 2} \\ \mathbf{0}_{2 \times 1} & \boldsymbol{\Sigma}_{\boldsymbol{\omega}_i} \end{array}\right] \mathbf{A}_i^T δLpi=Ai [ωidiωiN(ωi)][δdiδωi]N(0,ΣLpi)ΣLpi=Ai[Σdi02×101×2Σωi]AiT

其中 N ( ω i ) = [ N 1 N 2 ] ∈ R 3 × 2 N(ω_i) = [N_1 N_2]∈\mathbb{R}^{3×2} N(ωi)=[N1N2]R3×2是ωi处切平面的标准正交基, ⌊ ⌋ ∧ \lfloor \rfloor_∧ 为映射叉积的斜对称矩阵。式(1)的详细推导可以在[27]中找到。
world系下点的坐标及其不确定性为下面的公式。 Σ R Σ_R ΣR L W R ^W_LR LWR在切平面空间的不确定性, Σ t Σ_t Σt是下式中 t t t的不确定性。

W p i = L W R L p i + L W t Σ W p i = L W R Σ L p i L W R T + L W R ⌊ L p i ⌋ ∧ Σ R L p i ⌋ ∧ L T W R T + Σ t { }^W \mathbf{p}_i={ }_L^W \mathbf{R}^L \mathbf{p}_i+{ }_L^W \mathbf{t}\\ \left.\boldsymbol{\Sigma}_{W_{\mathbf{p}_i}}={ }_L^W \mathbf{R} \boldsymbol{\Sigma}_{L_{\mathbf{p}_i L}}{ }^W \mathbf{R}^T+{ }_L^W \mathbf{R}\left\lfloor{ }^L \mathbf{p}_i\right\rfloor_{\wedge} \boldsymbol{\Sigma}_{\mathbf{R}}{ }^L \mathbf{p}_i\right\rfloor_{\wedge L}^{T W} \mathbf{R}^T+\boldsymbol{\Sigma}_{\mathbf{t}} Wpi=LWRLpi+LWtΣWpi=LWRΣLpiLWRT+LWRLpiΣRLpiLTWRT+Σt

2.1.2 平面的不确定性

设一个平面特征由一组LiDAR点 W p i ( i = 1 , … , N ) ^Wp_i (i = 1,…, N) Wpi(i=1N),由于测量噪声和位姿估计误差,每个点都有一个不确定性 W Σ p i ^WΣ_{p_i} WΣpi,如(3)所示。表示点的协方差矩阵为 A A A:

p ‾ = 1 N ∑ i = 1 N W p i ; A = 1 N ∑ i = 1 N ( W p i − p ‾ ) ( W p i − p ‾ ) T \overline{\mathbf{p}}=\frac{1}{N} \sum_{i=1}^N{ }^W \mathbf{p}_i ; \quad \mathbf{A}=\frac{1}{N} \sum_{i=1}^N\left({ }^W \mathbf{p}_i-\overline{\mathbf{p}}\right)\left({ }^W \mathbf{p}_i-\overline{\mathbf{p}}\right)^T p=N1i=1NWpi;A=N1i=1N(Wpip)(Wpip)T

然后,平面可以表示为它的法向量 n n n,它是与 A A A的最小特征值相关联的特征向量,点 q = p ˉ q = \bar{p} q=pˉ,它位于这个平面上。由于 A A A p p p都依赖于 W p i ^Wp_i Wpi,我们可以将平面参数 ( n , q ) (n, q) (n,q)表示为 W p i ^Wp_i Wpi的函数,如下所示:
[ n , q ] T = f ( W p 1 , W p 2 , … , W p N ) [\mathbf{n}, \mathbf{q}]^T=\mathbf{f}\left({ }^W \mathbf{p}_1,{ }^W \mathbf{p}_2, \ldots,{ }^W \mathbf{p}_N\right) [n,q]T=f(Wp1,Wp2,,WpN)

那么真实世界中的平面normal和位置点q可以估计为下面公式
[ n g t , q g t ] T = f ( W p 1 + δ W p 1 , W p 2 + δ W p 1 , … , W p N + δ W p N ) ≈ [ n , q ] T + ∑ i = 1 N ∂ f ∂ W p i δ W p i \begin{aligned} {\left[\mathbf{n}^{g t}, \mathbf{q}^{g t}\right]^T } &=\mathbf{f}\left({ }^W \mathbf{p}_1+\boldsymbol{\delta}_{W_{\mathbf{p}_1}},{ }^W \mathbf{p}_2+\boldsymbol{\delta}_{W_{\mathbf{p}_1}}, \ldots,{ }^W \mathbf{p}_N+\boldsymbol{\delta}_{W_{\mathbf{p}_N}}\right) \\ & \approx[\mathbf{n}, \mathbf{q}]T+\sum_{i=1}N \frac{\partial \mathbf{f}}{\partial{ }^W \mathbf{p}_i} \boldsymbol{\delta}_{W_{\mathbf{p}_i}} \end{aligned} [ngt,qgt]T=f(Wp1+δWp1,Wp2+δWp1,,WpN+δWpN)[n,q]T+i=1NWpifδWpi

∂ f ∂ W p i = [ ∂ n ∂ W p i , ∂ q ∂ W p i ] T \frac{\partial \mathbf{f}}{\partial^W \mathbf{p}_i}=\left[\frac{\partial \mathbf{n}}{\partial^W \mathbf{p}_i}, \frac{\partial \mathbf{q}}{\partial{ }^W \mathbf{p}_i}\right]^T Wpif=[Wpin,Wpiq]T

假设 A A A有特征向量矩阵 U U U、最小特征值 λ 3 λ_3 λ3和对应的特征向量 u 3 u_3 u3,参照[28],我们可以对每个点 W p i ^Wp_i Wpi n n n q q q的导数,如下所示:

∂ n ∂ W p i = U [ F 1 , 3 p i F 2 , 3 p i F 3 , 3 p i ] , F m , 3 p i = { ( W p i − q ) T N ( λ 3 − λ m ) ( u m n T + n u m T ) , m ≠ 3 0 1 × 3 , m = 3 ∂ q ∂ W p i = diag ⁡ ( 1 N , 1 N , 1 N ) \begin{aligned} &\frac{\partial \mathbf{n}}{\partial W_{\mathbf{p}_i}}=\mathbf{U}\left[\begin{array}{l} \mathbf{F}_{1,3}^{\mathbf{p}_i} \\ \mathbf{F}_{2,3}^{\mathbf{p}_i} \\ \mathbf{F}_{3,3}^{\mathbf{p}_i} \end{array}\right], \mathbf{F}_{m, 3}^{\mathbf{p}_i}=\left\{\begin{array}{cc} \frac{\left({ }{\left.W_{\mathbf{p}_i}-\mathbf{q}\right)T}\right.}{N\left(\lambda_3-\lambda_m\right)}\left(\mathbf{u}_m \mathbf{n}^T+\mathbf{n u}_m^T\right) & , m \neq 3 \\ \mathbf{0}_{1 \times 3} & , m=3 \end{array}\right.\\ &\frac{\partial \mathbf{q}}{\partial W_{\mathbf{p}_i}}=\operatorname{diag}\left(\frac{1}{N}, \frac{1}{N}, \frac{1}{N}\right) \end{aligned} Wpin=U F1,3piF2,3piF3,3pi ,Fm,3pi={N(λ3λm)(Wpiq)T(umnT+numT)01×3,m=3,m=3Wpiq=diag(N1,N1,N1)

Σ n , q = ∑ i = 1 N ∂ f ∂ W p i Σ W p i ∂ f ∂ W p i \boldsymbol{\Sigma}_{\mathbf{n}, \mathbf{q}}=\sum_{i=1}^N \frac{\partial \mathbf{f}}{\partial{ }^W \mathbf{p}_i} \boldsymbol{\Sigma}_{W_{\mathbf{p}_i}} \frac{\partial \mathbf{f}}{\partial{ }^W \mathbf{p}_i} Σn,q=i=1NWpifΣWpiWpif
体素voxel,经典文献,人工智能,计算机视觉

2.2 由粗到细的体素地图构建方法

地图由哈希表和八叉树构成,哈希表管理最上层的地图结构,八叉树每个节点中存放一个平面,如果一个节点中的点不能被表示为一个特征,拆分这个节点。
后续地图的点被送到对应的节点中,在面特征稳定后删除旧的观测,只保留最新观测,如果新的观测和旧的观测冲突,删去旧估计重写估计位姿。

  1. Motivation

激光雷达点通常是按顺序采样的,因此点云的扫描总是从稀疏到密集,特别是在点分布的室外环境中更大的空间。当点云相对稀疏时,常用的基于面元的方法通常只能获得非常少量的平面,限制了高精确的和相对较低的扫描率的激光雷达的使用(例如,10Hz),这样就可以积累足够数量的点。然而,这将需要在累积的范围内补偿运动。为了解决这个问题,我们提出了一种从粗到细的体素建图方法,该方法可以在点云稀疏时构建一个粗糙的体素地图,并在接收到更多的点时对地图进行细化。

  1. Voxel Map Construction
    体素voxel,经典文献,人工智能,计算机视觉
  2. Map Update

对于在线激光雷达里程计,新的激光雷达点云帧不断的进入并且配准,并估计了它们的姿态。然后使用估计的姿态将新点注册到全局地图中:当新点位于一个未填充的体素中时,它将构造该体素。否则,当将新点添加到现有体素时,应该更新体素中平面的参数和不确定性。
体素voxel,经典文献,人工智能,计算机视觉
上图展示了不确定度的快速收敛。其中每个点的位置携带一个均值为零和方差0.1的高斯噪声。在不确定性收敛后,我们丢弃所有的历史点,并保留估计的平面参数(n,q)和协方差Σ,q。一旦新的累计点出现,我们只保留最新的10点,并计算出由这10个点组成的新平面法向量。如果新的法向量和先前收敛的法向量继续出现相对较大的差值,我们认为地图的这个区域已经发生了变化,需要重新使用算法1计算。

2.3 点与平面匹配

基于对点和平面的精确不确定性建模,我们可以很容易地实现点对平面的匹配。给定一个在具有姿态先验的世界框架中预测的激光雷达点 W P i ^WP_i WPi,我们首先通过它的哈希键找到它所在的根体素(具有粗糙的地图分辨率)。然后,对所有包含的子体素进行轮询,以此与点匹配。具体来说,让一个子体素包含一个具有法线 n i n_i ni和中心 q i q_i qi的平面,我们计算了点-平面距离:
d i = n i T ( W p i − q i ) d_i=\mathbf{n}_i^T\left({ }^W \mathbf{p}_i-\mathbf{q}_i\right) di=niT(Wpiqi)

考虑到所有这些不确定性:

d i = ( n i g t ⊞ δ n i ) T [ ( W p i g t + δ W p i ) − q i g t − δ q i ] ≈ n i g t T ( W p i g t − q i g t ) ⏟ 0 + J n i δ n i + J q i δ q i + J W p i δ W p i ⏟ w i \begin{aligned} &d_i=\left(\mathbf{n}_i^{g t} \boxplus \boldsymbol{\delta}_{\mathbf{n}_i}\right)^T\left[\left({ }^W \mathbf{p}_i^{g t}+\boldsymbol{\delta}_{W_{\mathbf{p}_i}}\right)-\mathbf{q}_i^{g t}-\boldsymbol{\delta}_{\mathbf{q}_i}\right]\\ &\approx \underbrace{\mathbf{n}_i^{g t T}\left({ }^W \mathbf{p}_i^{g t}-\mathbf{q}_i^{g t}\right)}_0+\underbrace{\mathbf{J}_{\mathbf{n}_i} \boldsymbol{\delta}_{\mathbf{n}_i}+\mathbf{J}_{\mathbf{q}_i} \boldsymbol{\delta}_{\mathbf{q}_i}+\mathbf{J}_{W_{\mathbf{p}_i}} \boldsymbol{\delta}_{W_{\mathbf{p}_i}}}_{\mathbf{w}_i} \end{aligned} di=(nigtδni)T[(Wpigt+δWpi)qigtδqi]0 nigtT(Wpigtqigt)+wi Jniδni+Jqiδqi+JWpiδWpi

也就是说,如果该点位于候选平面上,其距离 d i d_i di应服从(10)中的分布。因此,假设分布的标准差为 σ = Σ w i σ = \sqrt{^Σw_i} σ=Σwi ,就可以检验点面距离是否在 3 σ 3σ 3σ内。如果是,则选择为有效匹配。此外,如果一个点基于 3 σ 3σ 3σ准则匹配多个平面,则匹配概率最大的平面。如果没有平面通过 3 σ 3σ 3σ检验,则丢弃该点,以消除由体素量化引起的可能的错误匹配。

2.4 状态估计(基于卡尔曼滤波的定位方法)

我们建立了一个基于类似于FAST-LIO2的迭代扩展卡尔曼滤波器的激光雷达里程计,与C节中匹配的点到点平面距离进行融合,形成最大后验(MAP)估计。具体地说,第i个有效的点到平面匹配得到观测方程:
z i = h i ( x k ) + v i z_i = h_i(x_k) + v_i zi=hi(xk)+vi
线性化可以得到
体素voxel,经典文献,人工智能,计算机视觉文章来源地址https://www.toymoban.com/news/detail-594436.html

…详情请参照古月居

到了这里,关于经典文献阅读之--VoxelMap(体素激光里程计)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 经典文献阅读之--Evaluation of Lidar-based 3D SLAM algorithms (激光SLAM性能比较)

    我们在日常使用激光SLAM算法的时候,常常会发现现有的算法只会和一些比较经典或者前作去进行比较,很多时候我们更希望对主流的激光SLAM方法进行性能比较。之前作者转载过一篇文章《常见不同3D激光SLAM方案对比》。但是对比的算法有限。现在瑞典Lule科技大学评估9种最常

    2024年02月02日
    浏览(36)
  • 《LIO-SAM阅读笔记》-为何要引入增量式里程计?

    前言: LIO-SAM在后端中同时维护着两个里程计,一个是增量式里程计,一个是优化后的里程计,其中优化后的里程计是经过imu、回环、gps因子图联合优化后的结果,是整个系统中最准确的位姿估计,那么为什么还需要维护增量式里程计呢? 以下是我的理解 ,不一定正确,如有

    2024年01月22日
    浏览(45)
  • 视觉里程计(1):什么是视觉里程计

    1.概念:什么是里程计? 在里程计问题中,我们希望测量一个运动物体的轨迹。这可以通过许多不同的手段来实现。例如,我们在汽车轮胎上安装计数码盘,就可以得到轮胎转动的距离,从而得到汽车的估计。或者,也可以测量汽车的速度、加速度,通过时间积分来计算它的

    2024年02月07日
    浏览(39)
  • 视觉里程计学习笔记

    一、相机 1、双目相机的 D、P、K、R矩阵 是相机标定时得到的一组重要参数,它们分别代表以下含义: D矩阵 : 畸变矫正参数矩阵 。它包含了 相机图像坐标系下的径向畸变和切向畸变系数,用于将畸变像素坐标转化为无畸变的像素坐标 。对每个相机而言都有一个D矩阵。 P矩

    2024年02月03日
    浏览(35)
  • 基于ROS发布里程计信息

    参考文档: navigationTutorialsRobotSetupOdom 参考博客: (1)ROS机器人里程计模型 (2)ROS里程计消息nav_msgs/Odometry的可视化方法 1 常用坐标系系统模型 世界坐标系是描述机器人全局信息的坐标系;机器人坐标系是描述机器人自身信息的坐标系;传感器坐标系是描述传感器信息的坐

    2024年02月08日
    浏览(40)
  • SLAM-VIO视觉惯性里程计

    VIO(visual-inertial odometry)即视觉惯性里程计,有时也叫视觉惯性系统(VINS,visual-inertial system),是融合相机和IMU数据实现SLAM的算法,根据融合框架的区别又分为紧耦合和松耦合,松耦合中视觉运动估计和惯导运动估计系统是两个独立的模块,将每个模块的输出结果进行融合

    2024年02月11日
    浏览(35)
  • IMU惯性里程计解算(附代码实现)

    一、系统概述 IMU是机器人常用的传感器之一,IMU对机器人的定位功能实现非常重要,其优点在于是内源传感器对外部环境变化不明显,输出频率高,缺点在于存在累积误差。本文主要记录一下在机器人定位中对IMU的使用和对惯性导航里程计的理解和实现。 本文代码主要依赖

    2023年04月15日
    浏览(37)
  • 差速驱动机器人的车轮里程计模型

            车轮测程法是指使用旋转编码器(即连接到车轮电机以测量旋转的传感器)的测程法(即估计运动和位置)。这是轮式机器人和自动驾驶汽车定位的有用技术。         在本文中,我们将通过探索差速驱动机器人的

    2024年02月15日
    浏览(45)
  • 机器人运动学——轮速里程计(SLAM)

    目录 一、机器人运动学分析 二、阿克曼小车  2.1运动学分析   想要让机器人运动, 除了提供目标速度还不够, 需要将机器人的目标速度转换每个电机实际的目标速度, 最终根据电机的目标速度对电机的控制实现对机器人的控制。 机器人的目标速度转换成电机的目标速度这

    2024年02月16日
    浏览(36)
  • 【机器人模拟-02】 模拟移动机器人设置里程计

            在本教程中,我将向您展示如何设置移动机器人的测程。本教程是“机器人模拟”指南中的第二个教程。测量位移是仿真中的重要内容,设置测程的官方教程在此页面上,但我将逐步引导您完成整个过程。         您可以在此处获取此项目的完整代码。让我们

    2024年02月16日
    浏览(45)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包