精华内容
下载资源
问答
  • 【PL-SLAM原理部分

    2020-06-02 11:30:35
    简述PL-SLAM的原理部分

    1. Overview

    PL-SLAM的原理如下图所示,主要包括三部分:视觉里程计VO,局部建图,闭环。
    在这里插入图片描述

    A. map

    map 包括以下,

    • a set of KFs(关键帧)
    • the detected 3-D landmarks (both keypoints and line segments)
    • a covisibility graph
    • a spanning tree

    其中,

    • 关键帧包含了,观测到的立体特征和其描述子(利用字典计算出的左图描述子)、相机3D位姿信息。
    • 路标包含了,一系列观测、每个被观测到的路标最具代表性的描述子、路标点的3D位置、路标线段的方向和其端点的坐标。
    • 共视图建立了共视信息的模型,节点表示关键帧,边表示关键帧之间共视路标(文中设定共视路标的数目≥20)。如下图所示,用绿边表示的共视关系。
    • 为了快速闭环优化,采用了本征图(essential graph),比共视图稀疏些,因其控制共视路标的数目≥100。
    • 生成树表示了包含关键帧的图的最小连接表示。与本征图一起用于闭环。
      在这里插入图片描述

    B. Feature Tracking

    注意,此处 feature 包括 point 和 line segment。

    从一系列双目立体帧中跟踪特征,并计算它们的3D位置和其关联的协方差矩阵表示的不确定性。接着,3D路标被投影到新的相机位姿,并最小化投影误差以获取相机位姿增量和其估计的协方差。在每一帧重复这个过程,简单执行 frame-to-frame VO,直到新的关键帧插入地图map。

    一旦关键帧插入地图,局部建图和闭环检测便同时进行(并行但不同线程)。

    C. Local Mapping

    局部建图,在共视图中,在新关键帧、上一关键帧和连接上一关键帧的其余关键帧之间,寻找新的特征对应关系。这样,构建了局部地图,其中包括与当前关键帧共视至少20个路标的所有关键帧,以及它们所观测到的所有路标。最后对局部地图执行优化。

    D. Loop Closure

    与局部建图并行,闭环检测采用基于BoW方法。在相机运动期间,捕获的 KFs 的所有视觉描述子都存储在数据库中,该数据库随之用于查找与当前帧相似的帧。仅当围绕此 KF 的局部序列也相似时,最佳匹配才被视为闭环候选。 最后,估算当前 KF 与闭环候选者之间的相对 *SE(3)*转换,以便在找到正确的估计的情况下,通过位姿图优化(PGO)过程校正闭环中涉及的所有 KFs 姿态 。

    notice
    这的局部建图和闭环,与ORB-SLAM中的方法一样,其目的是为了减小BA优化的计算量。
    在BA优化时,采用近年流行的 relative techniques,而非消耗更大的 global approaches。

    2. Feature Tracking

    简而言之,沿着一连串的立体帧跟踪点和线段,然后通过最小化投影误差来计算相机的3D运动及其不确定性。 注意,此过程仅执行相机位姿的优化,而不优化被跟踪特征的3D位置,后者在后续的 LBA 过程中优化。

    由于实验中使用的双目立体相机是经过预先标定的,因此可以通过直接使用其外部参数来确定观察到的关键点和直线的3D位置,从而在单个立体镜头中对这些特征进行初始化。

    A. Point Features

    采用ORB特征,关键点检测性能好,描述子匹配快速。为了减少匹配的离群点,我们仅考虑满足以下条件的测量:左图像中的最佳匹配对应于右图像中的最佳匹配,即它们是互为最佳的匹配。此外,还过滤掉出,在描述子空间中,那些 “ 第二最佳匹配的距离 ” 小于 “ 最佳匹配的距离 ” 的两倍的匹配,以确保对应关系足够有意义。(更鲁棒吧)

    B. Line Segment Features(LSD线段特征+LBD描述子)

    LSD方法已用于提取线段,从而提供了高精度和可重复性。对于立体匹配和帧到帧跟踪,使用 LBD 方法 (line band descriptor)提供的二进制描述子来增强线段,这能够根据线的局部外观找到线之间的对应关系。 与点的情况类似,我们检查两个候选特征是否互为最佳匹配,并且特征是否足够有意义。 最后,我们利用线段提供的有用的几何信息来滤除那些具有不同方向和长度的线匹配,以及那些在端点视差上有很大差异的线匹配。 注意,此过滤器有助于系统保留大量的结构线,从而允许基于点和线形成更一致的地图[见下图]。
    在这里插入图片描述

    C. Motion Estimation

    将关键点和线段从第一帧反向投影到下一帧。 然后,采用鲁棒的高斯-牛顿法,最小化线和关键点投影误差来迭代估计相机的运动。为了处理离群值,我们采用了 Pseudo-Huberr 损失函数并执行两步最小化。最后,获得两个连续帧之间的增量运动估计,可以通过以下正态分布来建模:
    ξt,t+1N(ξt,t+1,Σξt,t+1)1\boldsymbol{\xi}_{t, t+1} \sim \mathcal{N}\left(\boldsymbol{\xi}_{t, t+1}^{*}, \boldsymbol{\Sigma}_{\boldsymbol{\xi}_{t, t+1}^{*}}\right)(1)
    其中 ξt,t+1se(3)\boldsymbol{\xi}_{t, t+1}^{*} \in \mathfrak{s e}(3) 是在 ttt+1t+1之间的相机运动的6维向量 , 而 Σξt,t+1\boldsymbol{\Sigma}_{\boldsymbol{\xi}_{t, t+1}^{*}} 估计的运动的协方差。

    D. Keyframe Selection

    为了确定何时在地图中插入新的KF,采用了 relative motion estimation 的不确定性。 因此,根据(1),通过以下表达式将不确定性从协方差矩阵转化为标量,称为熵:
    h(ξ)=3(1+log(2π))+0.5log(Σξ)2h(\boldsymbol{\xi})=3(1+\log (2 \pi))+0.5 \log \left(\left|\boldsymbol{\Sigma}_{\xi}\right|\right)(2)
    然后,对于给定的 KF i,我们检查 “ KF i与当前帧 i + u 之间的运动估计熵 ” 与 “ KF i与其首个连续帧 i + 1之间的熵 ” 之间的比率,即
    α=h(ξi,i+u)h(ξi,i+1)3\alpha=\frac{h\left(\boldsymbol{\xi}_{i, i+u}\right)}{h\left(\boldsymbol{\xi}_{i, i+1}\right)}(3)
    如果 α 值低于某个预先确定的阈值(在实验中已将其设置为0.9),则将帧 i + u 作为新的 KF 插入到系统中。注意,要计算(2)中的表达式,需要非连续帧之间的位姿增量的不确定性。 由于(1)仅估计连续帧之间的增量运动,因此通过一阶高斯传播技术来组合一系列此类估计,以获得两个非连续 KF 之间的协方差。

    3. Local mapping

    本节描述了在插入新的 KF 时系统的行为,该行为主要包括执行局部地图的 BA 优化,该局部地图就是,“ 通过共视图与当前 KF 连接的那些 KFs ” + “ 由这些局部 KFs 所观察到的路标 ”。

    A. Keyframe Insertion

    每次 VO 选择一个KF,都会将其插入 SLAM 系统并优化局部地图。首先,我们优化当前和先前KF之间的相对位姿变化的估计,因为由 VO 提供的位姿是通过合成中间帧之间的相对运动来估计的。为此,我们考虑了第2节中描述的几何约束,并在 KFs 之间执行了数据关联,并获得了在其中观察到的一致的共同特征集。 然后,执行与第2-C节中介绍的优化类似的优化,为此,我们将VO 线程提供的位姿用作高斯-牛顿最小化的初始估计。计算完 KFs 之间的相对位姿变化后,将当前的 KF 插入系统,包括以下内容。

    • 1)关键帧的索引。
    • 2)其 3D 位姿的信息,包括绝对位姿、与前一个 KF 的相对位姿及其相关的不确定性。
    • 3)新的 3D 路标,通过存储其 2D 图像坐标和其描述子进行初始化。已存在的路标的新观测值也将添加到地图中。

    最后,还在当前帧的未匹配特征观测值与局部地图中的地标之间寻找新的对应关系。

    B. Local Bundle Adjustment

    插入 KF\mathrm{KF} 之后, 接下来执行局部地图的 BA 优化。定义一个向量 ψ\psi ,其包含了待优化的变量, 它们是 每个关键帧 KF 的 se(3)\operatorname{se}(3) 位姿ξiw\boldsymbol{\xi}_{i w}, 每个 3D3\mathrm{D} 点的位置 Xwj\mathbf{X}_{w j}, 和每个线段的端点的3D坐标: {Pwk,Qwk}.\left\{\mathbf{P}_{w k}, \mathbf{Q}_{w k}\right\} . 然后,将观测值和投影到观测帧的路标之间的投影误差最小化
    ψ=argminψiKl[jPleijΣeij1eij+kLleikΣeik1eik]4\boldsymbol{\psi}^{*}=\underset{\boldsymbol{\psi}}{\operatorname{argmin}} \sum_{i \in \mathcal{K}_{l}}\left[\sum_{j \in \mathcal{P}_{l}} \mathbf{e}_{i j}^{\top} \boldsymbol{\Sigma}_{\mathbf{e}_{i j}}^{-1} \mathbf{e}_{i j}+\sum_{k \in \mathcal{L}_{l}} \mathbf{e}_{i k}^{\top} \boldsymbol{\Sigma}_{\mathbf{e}_{i k}}^{-1} \mathbf{e}_{i k}\right](4)

    其中 Kl,Pl,\mathcal{K}_{l}, \mathcal{P}_{l},Ll\mathcal{L}_{l} 分别指的是局部 KFs 集合 ,点集, 和线段集。

    在(4)中,点的重投影误差eij\mathbf{e}_{i j},按如下计算,
    eij=xijπ(ξiw,Xwj)5\mathbf{e}_{i j}=\mathbf{x}_{i j}-\boldsymbol{\pi}\left(\boldsymbol{\xi}_{i w}, \mathbf{X}_{w j}\right)(5)
    其中函数 π:se(3)×R3R2\boldsymbol{\pi}: \mathfrak{s e}(3) \times \mathbb{R}^{3} \mapsto \mathbb{R}^{2} 表示重投影,先将世界坐标系下第 jj 个 3D 点 Xwj\mathbf{X}_{w j} 放入第iiKF\mathrm{KF}的局部参考坐标系下,即, Xij,\mathbf{X}_{i j}, 再把这个点投射到图像中。

    关于线段,将3D线段的投影端点与其在图像平面中对应的无限线之间的距离作为误差函数。在第 i 帧观测到的第 k 条线段的重投影误差eik\mathbf{e}_{i k},有
    eik=[likπ(ξiw,Pwk)likπ(ξiw,Qwk)]6\mathbf{e}_{i k}=\left[\begin{array}{l} \mathbf{l}_{i k} \cdot \boldsymbol{\pi}\left(\boldsymbol{\xi}_{i w}, \mathbf{P}_{w k}\right) \\ \mathbf{l}_{i k} \cdot \boldsymbol{\pi}\left(\boldsymbol{\xi}_{i w}, \mathbf{Q}_{w k}\right) \end{array}\right](6)
    其中 Pwk\mathbf{P}_{w k}Qwk\mathbf{Q}_{w k} 指的是世界坐标下线段端点的 3D 坐标,lik\mathbf{l}_{i k}是与第 i 个KF 中的第 k 个线段相对应的无穷线的方程式,可以用齐次坐标中的线段的二维端点之间的叉积获得,即:lik=pik×qik\mathbf{l}_{i k}=\mathbf{p}_{i k} \times \mathbf{q}_{i k}

    利用Levenberg-Marquardt优化方法可以迭代地解决(4)中的问题,为此我们需要估计Jacobian矩阵和Hessian矩阵,
    Δψ=[H+λdiag(H)]1JWe7\Delta \boldsymbol{\psi}=[\mathbf{H}+\lambda \operatorname{diag}(\mathbf{H})]^{-1} \mathbf{J}^{\top} \mathbf{W} \mathbf{e}(7)
    其中误差向量e\mathbf{e}包含了点和线段的重投影误差。(7)和更新步骤(8),
    ψ=ψΔψ8\boldsymbol{\psi}^{\prime}=\boldsymbol{\psi} \oplus \Delta \boldsymbol{\psi}(8)
    可以递归应用,直到收敛为止,从而得到最佳ψ\boldsymbol{\psi},从中可以更新局部 KF 和路标的位置。 注意,鉴于ψ\boldsymbol{\psi} 中变量的不同性质,更新方程无法直接应用于整个矢量。

    应强调的是,在(4)中,点和线中的误差影响分别由 Σeij1\boldsymbol{\Sigma}_{\mathbf{e}_{i j}}^{-1}Σeik1\boldsymbol{\Sigma}_{\mathbf{e}_{i k}}^{-1} 加权,代表与每个投影误差不确定性相关的协方差矩阵的逆 。但实际上,将此类协方差设置为单位矩阵并遵循与2-C节中所述方法类似的方法更为有效,因为它引入了稳健的权重,并且还处理了离群值的存在。

    最后,移除地图中观测少于三次的路标点。

    4. Loop closure

    采用基于所提取的关键点和线段的二进制描述子的 BoW 方法,以鲁棒地应对数据关联和闭环检测。

    简而言之,BoW技术将从图像中提取的所有信息(关键点和线段的描述子)汇总到一个单词向量中,为此,它使用了从不同图像数据集离线构建的字典表。 然后,随着相机的移动,从捕捉的图像中计算出单词并存储在数据库中,从而随后将用于搜索与当前关键帧最相似的图像。

    A. Loop-Closure Detection

    闭环检测既要找到类似于当前正在处理的图像,又要估算它们之间的相对位姿变化,如下所述。

    1)Visual Place Recognition:

    针对点和线段,建立两个特定的字典和数据库。这样,在每个时间步,并行检索关键点和线段数据库中最相似的图像,以查找闭环。双重搜索的动机是,某些场景可能用线条而不是关键点来描述,反之亦然。 因此,同时使用这两种方法并合并它们的结果使我们能够精确数据库查询的输出,从而减少计算量。

    为了说明这一点,首先定义一个相似度矩阵,该矩阵在每一行中都包含相似度值,其值的范围为[0,1],于是计算相似度矩阵。

    具体地,如下图4(a) 中的矩阵仅使用ORB关键点来计算,以建立字典和数据库,而图4(b)中的矩阵仅依赖于线段。调色板从蓝色(分数= 0)到红色(分数= 1)。注意到,根据关键点,图像看起来相似的地方,在第一个矩阵中会出现一些淡黄色区域。这表明存在潜在的闭环,但实际上它们只是假阳闭环。但是,第二个矩阵没有这种现象。另一方面,基于关键点的检测,由于不相似图像的分数差异通常更大,因此第一个矩阵具有更大的区别性。因此,当检测潜在的闭环时,可以将组合查询两个特征数据库而产生的图像相似度,以提高鲁棒性。

    在这里插入图片描述
    作者制定了一套点线特征的加权求和的方式(sks_{k}表示关键点分数,sls_{l}表示线段分数),来计算图像之间的相似度得分,根据的两个标准,分别是 strength 和 dispersion。 前者根据图像中检测到的一组特征中关键点或线条的特征数量,按比例对相似性评分加权,而后者则考虑特征在图像中的离散程度( 分散度越高,权重越大)。得到鲁棒的相似度评分如下:
    st=0.5(nk/(nk+nl)+dk/(dk+dl))sk+0.5(nl/(nk+nl)+dl/(dk+dl))sl10\begin{aligned} s_{t}=& 0.5\left(n_{k} /\left(n_{k}+n_{l}\right)+d_{k} /\left(d_{k}+d_{l}\right)\right) s_{k} \\ &+0.5\left(n_{l} /\left(n_{k}+n_{l}\right)+d_{l} /\left(d_{k}+d_{l}\right)\right) s_{l} \end{aligned}(10)
    其中,nkn_{k}nln_{l}为提取的关键点和线段的数量,dkd_{k}dld_{l}表示离散值——分别计算检测到特征的 x 、y 坐标的方差之和的平方根。针对线段特征,x 和 y 为线段中点。

    此公式给予两种特征都相同的重要性(因此采用0.5系数)。

    将上述策略与其余四种方法作比较,采用四种数据集测试。策略3即对应(10),表现最好。

    在这里插入图片描述
    2)估计相对运动

    一旦有了闭环候选,需要滤掉假阳的闭环。 恢复形成闭环的两个KF之间的相对姿态(我们把其中一个命名为当前帧,另一个为旧帧),来实现筛除假阳。首先,先在这两个关键帧寻找特征匹配,接着在当前帧与旧帧关联的局部地图之间寻找新对应关系。接着估计两个KF之间的刚性变换 ξ^ijse(3)\hat{\boldsymbol{\xi}}_{i j} \in \mathfrak{s e}(3)。最后,我们再检查闭环候选的一致性,检测方法如下:

    1. 协方差矩阵 Σξ~ij\Sigma_{\tilde{\xi}_{i j}} 的最大特征值小于0.01
    2. 获得的平移和旋转不能分别超过0.50米和3度。
    3. 估计计算时,内点所占比例大于50%;

    关于第一个标准,不确定性矩阵的特征值的较大值(参见(1))通常指出了病态Hessian矩阵,这很可能是由于特征匹配集中存在大量离群值 。确保协方差矩阵的最大特征值低于某个阈值,能够检测到可能不正确的闭环候选并将其丢弃。

    关于第二个标准,为估计的姿态设置了最大平移和旋转限制,因为基于BoW的方法通常会提供外观和位姿非常相似的匹配,因此若两帧之间的位姿有很大变化,通常为错误的闭环检测。

    关于第三个标准,由于运动估计会受到离群值以及视觉场景识别的错误关联的强烈影响,因此在优化之后设置检测到的离群值的最小比率。

    B. Loop Correction

    在估计了轨迹中所有连续的闭环之后,融合闭环的两侧,校正沿环分布的误差。将问题作为PGO(位姿图优化?)来解决,其中节点为关键帧,边由本征图和生成树给定。定义以下误差函数为se(3)\mathfrak{s e}(3)差:(不太懂)
    rij(ξiw,ξjw)=log(exp(ξ^ij)exp(ξjw)exp(ξiw)1)11\mathbf{r}_{i j}\left(\boldsymbol{\xi}_{i w}, \boldsymbol{\xi}_{j w}\right)=\log \left(\exp \left(\hat{\boldsymbol{\xi}}_{i j}\right) \cdot \exp \left(\boldsymbol{\xi}_{j w}\right) \cdot \exp \left(\boldsymbol{\xi}_{i w}\right)^{-1}\right)(11)
    其中,log 映射表示 SE(3)se(3)SE(3) \mapsto \mathfrak{s e}(3),exp 映射表示 se(3)\mathfrak{s e}(3) \mapsto SE(3)S E(3)

    用 g2o 库来解决这个G2O问题,从而生成关键帧的最优位姿。我们更新KFs的位姿,以及它们所观测到的地标的位姿,并且,还要将回环两边的局部地图进行融合,方法是首先融合在估计它们的相对位姿时所匹配的地标点,然后再在未匹配的地标之间寻找新的匹配关系。

    参考
    https://zhuanlan.zhihu.com/p/41371645

    展开全文
  • SLAM系统原理推导

    2020-09-08 11:40:11
    此博文主要讲解SLAM系统的原理推导

    1.SLAM 基础知识

      SLAM 系统框架如图1 所示。包括两个主要传感器:激光雷达与轮式里程计。其中激光雷达用来获取环境信息,通过激光扫描可以测量周围 360°环境的障碍物信息。轮式里程计则通过移动底盘的电机编码器持续获取机器人的里程信息。

      激光雷达的作用主要有两个方面:一方面可以为构图算法提供点云数据。当构图算法获取足够的点云数据后,即可构建出以机器人为中心,以雷达射程为半径的局部地图。另一方面可以通过系统观测模型修正贝叶斯滤波器的预测位姿,提高滤波估计的机器人位姿精度。轮式里程计主要作用则是为 SLAM 系统提供里程信息。通过轮式里程计运动学模型,可以将里程信息转化为机器人位姿变化信息,送入贝叶斯滤波器初步计算出预测位姿。 预测精度完全依赖于轮式里程计提供的数据精度。而轮式里程计的测量精度对于获取精确的机器人位姿来讲远远不够。因此,需要用到系统观测模型对预测结果进行修正。经过修正并加权求和得到的机器人位姿即为当前估计的最佳位姿。在已知最佳位姿的条件下,局部地图即可更新至全局地图中。全局地图进一步进行高斯模糊,构建新的观测模型。至此整个 SLAM系统形成完整的闭环系统。
    在这里插入图片描述

    1.1 SLAM 系统概率模型

      在 SLAM 系统框架中, 采用贝叶斯滤波器为 SLAM 系统的后端优化算法提供概率模型。 贝叶斯滤波的原理是基于已经发生的事件对未发生事件进行估计,求解未发生事件的最可能概率分布。贝叶斯滤波的推导如图 2 所示。
    在这里插入图片描述
      在贝叶斯滤波器构建的 SLAM 概率模型基础上, 发展出了两大分支: 基于卡尔曼滤波的 SLAM 算法, 主要代表有 EKF-SLAM, UKF-SLAM 等; 以及基于粒子滤波的 SLAM 算法, 主要代表有 RBPF-SLAM, FastSLAM 等。 其中基于卡尔曼滤波的 SLAM 算法要求噪声必须服从高斯分布且系统非线性不能太大;相反基于粒子滤波的 SLAM 算法适用于非线性较大的系统, 但受限于粒子退化与粒子耗散这一对耦合问题。

    1.2 里程计运动模型

      机器人底盘分为两轮差速底盘和三轮全向底盘。两轮差速底盘其结构简单,造价较低,并且建立的模型相对简单,因此被应用于各种类型的机器人生产中。两轮差速底盘的运动学模型如图 3 所示。
    在这里插入图片描述
    该模型将机器人的任意移动视为某一圆心做圆弧运动,其中 d 为两轮距离的二分之一, r 为底盘中心做圆弧运动的半径。设左轮速度为 vLv_{L},右轮速度为vRv_{R}, 底盘中心的角速度为 ω, 若已知 vLv_{L},vRv_{R},可以由式(1)计算出底盘运动中心的角速度 ω 和线速度 v。
    在这里插入图片描述
      假设机器人在里程计数据更新的两时刻之间的极短时间 Δt\Delta t内作匀速运动,则相邻两时刻之间机器人位移由式(2)给出。
    在这里插入图片描述
    式中: dx ,dy ,dθd\theta 分别为机器人坐标系下沿 x 轴方向位移,沿 y 轴方向位移,以及以 z 轴为旋转轴的逆时针旋转角度。式(2)给出的是在机器人坐标系下的位移。机器人坐标系与世界坐标系的关系是由机器人位姿联系起来的,如图 2-4 所示。 图中(x,y,θ)\left ( x,y,\theta \right )为机器人在世界坐标系下的位姿表示。
    在这里插入图片描述
      可以通过机器人位姿 (x,y,θ)\left ( x,y,\theta \right )将机器人在机器人坐标系的位移转换到世界坐标系中。最终计算的机器人运动模型由式(3)给出:
    在这里插入图片描述

    1.3 激光雷达测距模型

      激光雷达的测距模型主要有以下两种:三角测距与 TOF( Time of Flight)测距。其中基于三角测距模型的激光雷达实物图如图 5 a 所示。图中左边小孔为激光发射器,右边稍大的孔为接受激光的光学摄像头。 三角测距模型主要原理为三角形的角边角定理,激光发射器位置与摄像头光心以及扫描点构成的三角形如图 5 b 所示。
    在这里插入图片描述
      图 5b 中 O1O_{1}点为激光发射器, O2O_{2}点为摄像头光心轴,光心轴旁边的三角形代表用来捕捉反射光斑的相机模型。激光发射器发射的激光由红色虚线表示,相机接收的反射激光由蓝色虚线表示。 A 点表示激光击中的障碍物位置,障碍物A 在成像平面上的位置用 A1A_{1} 表示。其中线段O1O_{1} O2O_{2}长度已知;由于激光垂直于直线 O1O_{1} O2O_{2} 发射,AO1O2\angle AO_{1}O_{2} 为 90°;O1O2A\angle O_{1}O_{2}A 可以通过成像点 A1A_{1} 的位置获取。在三角形 O1O2AO_{1}O_{2}A中已知两内角与一条边长,即可通过正弦定理求出三角形另外一边 O1AO_{1}A的长度,见式(4)。 线段 O1AO_{1}A 即为障碍物 A 到雷达的距离。

    在这里插入图片描述

    单线激光雷达通过电机驱动雷达测距核心进行顺时针旋转,最高可达每秒10Hz。旋转过程中不断发射激光束,基于三角测距模型获取 360°方向的障碍物距离信息,完成激光扫描数据的封装。

    1.4 系统观测模型

      激光雷达的观测模型包括光束模型和似然场模型。其中似然场模型的基本原理是对地图进行高斯模糊(如图6 所示),利用 SLAM算法输出的环境地图, 对于地图上存在障碍物的位置, 以该点为中心, 以地图上其他点到该点的欧氏距离为变量来构造高斯分布函数,设 d 表示激光测量终点坐标(xzt,yzt)\left ( x_{zt},y_{zt} \right )与地图 m 上最近的障碍物之间的欧式距离,那么激光雷达测量的概率分布可以由一个以 0 为中心,方差为 d 的高斯函数ε(d)\varepsilon (d)给出,见式(2-5):
    在这里插入图片描述
      依据地图提供的障碍物信息可以获取地图上全部点对应的高斯分布函数值,并将该值作为地图上的似然评分,评分越高则代表扫描点与障碍物点越接近,其测量结果也就越准确。
      地图经高斯模糊后, 环境ztz_{t}的期望值相对于位姿来讲都是连续的,相比于光束模型,似然场模型不会因为环境形状变化不规律导致观测期望突变,因此似然场模型适合结构化与非结构化的环境,增加了该模型的适用场景。此外,对于似然场每一点的概率都可以预先计算,在实际 SLAM 算法执行过程中计算式仅需要对预先计算的似然场概率分布进行查表即可,计算量可以忽略不计,提高了算法的实时性。
    在这里插入图片描述

    2 SLAM实例

    2.1 粒子滤波算法原理

      粒子滤波算法使用贝叶斯估计的思想, 用粒子分布去近似机器人位姿的概率分布,如图7 所示。在这里插入图片描述
      首先将一定数量的粒子均匀撒在可能分布的空间中,作为粒子滤波的初始状态x0x_{0} 的概率分布。按照贝叶斯估计的流程,可以通过里程计运动模型求得t时刻的机器人位姿 xtx_{t} ,作为预测粒子集。 再根据激光雷达观测模型对预测粒子集中每一个粒子赋予权值, 粒子权值越高, 则代表该粒子与实际位置的误差越小。最终对所有粒子根据其权值进行加权平均,即可得到滤波后的位姿估计结果。
      SLAM 中主要任务是机器人的自主定位与环境地图的建立。 其中在已知机器人的位姿信息的条件下计算环境地图存在闭式解。 因此 SLAM 问题的主要难点在于如何获取机器人位姿信息。通过粒子滤波得到的机器人位姿x1:tx_{1:t},与激光雷达的历史观测信息z1:tz_{1:t}可以直接计算得到环境的栅格地图,下面主要介绍基于粒子滤波器获取机器人位姿的算法步骤。
      粒子滤波算法的基本流程如下:

    1. 用粒子集表示机器人的位姿信息: Xt={(xti,wti)i=1,...,n}X_{t}=\left \{ \left (x_{t}^i,w_{t}^i{} \right )|i=1,...,n\right \}, 其中每一个粒子包含两个参数: 机器人位姿 xtix_{t}^i ,粒子权重 wtiw_{t}^i
    2. 将总数为 N 的粒子集随机且均匀地撒在所有可能的位姿空间内,作为粒子滤波算法的初始状态 。
    3. 已知前一时刻的粒子集状态 Xt1={(xt1i,wt1i)i=1,...,n}X_{t-1}=\left \{ \left (x_{t-1}^i,w_{t-1}^i{} \right )|i=1,...,n\right \}, 每一个粒子通过建议分布 xtip(xtut,xt1i)x_{t}^{i}\sim p(x_{t}|u_{t},x_{t-1}^{i}) 计算当前时刻的位姿信息。 这里建议分布采用里程计运动模型。其中 Δx, Δy, Δθ 为里程计信息 utu_{t}NxN_{x}, NyN_{y}, NθN_{\theta }为里程计的测量噪声,噪声服从均值为 0 的高斯分布。最后粒子在建议分布进行随机采样,见式(6)。

    在这里插入图片描述
    4. 评估每一个粒子的权重:
      机器人实际位姿分布未知,粒子集内每一个位姿都可能是机器人的实际位姿。定义粒子权重wtiw_{t}^{i},用来描述粒子的预测分布与实际分布的匹配程度,粒子权重定义见式(7),权重越大,表明粒子的预测分布实际分布匹配程度越高。
    在这里插入图片描述
      经贝叶斯公式推导,可以得到权重的计算式,见式(8):
    在这里插入图片描述
      由于粒子权重与机器人在xtx_{t}位姿下获取观测数据ztz_{t}的置信度成正比,其系数η\eta为归一化因子。因此首先将每个粒子对应的p(ztxti)p(z_{t}|x_{t}^{i}) 求出,计算出归一化因子η\eta (见式)即可通过式()求出归一化后的粒子权重。
    在这里插入图片描述

    1. 计算出机器人位姿估计结果,见式(10)。

    在这里插入图片描述
    6. 将得到的当前时刻粒子集Xt{X_{t }} 替换前一时刻粒子集Xt1{X_{t - 1}} ,返回步骤(3)进行迭代计算。
    7. 通过上述步骤可以迭代计算出机器人在每一时刻的位姿分布情况,并通过式(11)计算出每一时刻下的机器人位姿估计结果。通过此结果可以进一步计算出环境地图,完成即时定位与地图构建的任务。

    展开全文
  • 1、hector-slam代码框架概述下载源码:git clone ...原理解读参照:https://blog.csdn.net/weixin_40047925/article/details/80679496其中包含了许多用于仿真文件,hector slam算法主要集中在hector ma...

    1、hector-slam代码框架概述

    下载源码:git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git

    原理解读参照:https://blog.csdn.net/weixin_40047925/article/details/80679496

    其中包含了许多用于仿真的文件,hector slam算法主要集中在hector mapping包中,同时核心的位姿估计和点云建立栅格图操作主要在文件HectorSlamProcessor.h的函数update中。如下:

    函数newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov))根据输入的激光点云数据和现有的栅格图进行对齐,这部分是整个代码的核心,主要思想是使用高斯牛顿优化代价函数,求取让代价函数达到最优的位姿偏量,从而求出当前机器人的位姿态。

    当然,如果已经知道机器人的准确位姿不需要进行上面的位姿解算,直接用函数mapRep->updateByScan(dataContainer, newPoseEstimateWorld)更新栅格地图。这部分算法原理比较简单,主要两个方面:一是,用对数表达每一个子栅格的概率值;二是,用Bresenham 画线算法来更新子栅格的概率值。

    为了说明上述算法原理,先介绍一部分的预备知识,然后再切入正题。

    2、hector-slam预备知识

    2-1 占用栅格地图

    这里直接使用上述参考文章进行说明,假设机器人的位姿已知,那么经过每一次激光数据扫描,真实障碍物在栅格地图上的占用概率不断提升最终无限接近1,而没有障碍物的地方则趋近于0。整个栅格地图被分成了一定分辨率的子栅格,最终地图的概率分布如下:

    p(m|z_{1:t},x_{1:t})=\prod p(m_{i}|z_{1:t},x_{1:t})

    整个栅格图构建需要计算每一个子栅格的占用概率值。实际在表达栅格占用概率使用对数形式表达,好处是避免在0和1附近概率值不稳定的问题。具体如下:

    l_{t,i}=log\frac{p(m_{i}|z_{1:t},x_{1:t})}{1-p(m_{i}|z_{1:t},x_{1:t})}                         p(m_{i}|z_{1:t},x_{1:t})=1-\frac{1}{1+e^{l_{t,i}}}

    在实际使用栅格地图进行导航或者路径规划时,每一个栅格仅有三个状态:占用、空闲和未知,该三种状态仅需对p(m_{i}|z_{1:t},x_{1:t})或者l_{t,i}设定范围进行判断即可,比如hector slam中是l_{t,i}值大于0为占用,小于0为空闲,等于0为未知。

    2-2 非线性优化——高斯牛顿法

    在处理优化问题时经常会求去某个指标的最值,比如最小二乘、最大似然、最小方差等,在hector slam求解位姿时也构造了一个最小二乘问题,但是使用的模型是关于位姿的非线性函数,所以需要使用非线性优化的方法,hector slam使用的是高斯牛顿法。这里对高斯牛顿法做个简单说明。

    对于非线性最小二乘问题,可以用如下数学表达式来描述:

    x = argminF(x)=argmin\frac{1}{2}\left \|f(x))\right \|_{2}^{2}

    如果f(x)数学表达式比较简单,可以直接对F(x)求导,并领其等于0,可以求出极值。对于无法求导情况,一般用迭代求解\Delta x,求解的迭代值不断的趋使F(x)趋近最小值,直到\Delta x变化很小或者达到最大迭代次数。那么关键问题在于求取\Delta x,这里有很多方法,如:梯度下降法、牛顿法、牛顿高斯法和LM法。简单比较一下:梯度下降法——对代价函数F(x)进行泰勒展开,仅保留一阶导数部分,按照梯度反方向进行迭代,即\Delta x=-J(x),但是这种方向并不一定是最合适的方向,有时可能迭代过头,反而会绕路;牛顿法——其实是上面F(x)泰勒展开,保留到二阶,增加了海森矩阵,因此其迭代的路径一般是比较短的,但是需要计算F(x)的二阶导数,这个是很难求得的,因此实际应用受限;牛顿高斯法——为了避免牛顿法求解海塞矩阵难的问题,牛顿高斯法先对f(x)进行一节泰勒展开,然后得到F(x)近似表达式,然后另其导数为0,求取F(x)局部最小时的\Delta x,然后不断迭代,直至达到收敛条件;LM法是在牛顿高斯法上进行了修改,避免了牛顿高斯法求解正规方程时可能出现的奇异解,被广泛使用。

    话不多说,直接介绍高斯牛顿法。

    f(x+\Delta x) \approx f(x) + J(x)\Delta x \Rightarrow

    F(x) = \frac{1}{2}( f(x) + J(x)\Delta x )^{T}( f(x) + J(x)\Delta x ) \Rightarrow

    F{}'(x) = J^{T}(x)( f(x) + J(x)\Delta x ) =0 \Rightarrow

    J^{T}(x)J(x)\Delta x = - J^{T}f(x)\overset{H = J^{T}(x)J(x)}{\rightarrow}\Delta x=-H^{-1}J^{T}f(x)

    和牛顿法比较实际上是用雅克比矩阵二次来代替了海塞矩阵,省去了计算海塞矩阵。

    3、hector-slam位姿解算

    当我们传感器第一次接收到数据,我们可以人为设定当前位置,以此来第一次建立栅格地图m,可以认为是初始化操作。之后,我们在根据前一时刻建立的地图求解的位姿都是相对初始化时刻的位姿而言的,这一点需要明白。那么这个问题可以用贝叶斯估计描述为:

    p(x_{t}|m_{t-1},z_t)=\beta p(m_{t-1}|x_{t},z_{t})*p(x_{t}|x_{t-1})

    这里假设状态x符合马尔科夫过程,但是对于先验p(x_{t}|x_{t-1})概率很多情况下是不知道的,如果有这部分的先验的话可以添加这部分代码。在没有这部分信息情况下,hector-slam是让似然概率p(m_{t-1}|x_{t},z_{t})达到最大的最大似然估计。hector-slam通过最小二乘的方式来表达x_{t}=argmaxP(m_{t-1}|x_{t},z_{t})最大似然问题(最小二乘实际上就是最大似然估计的一种):

    x=argmin\sum [1-M(S_{i}(x))]^{2}

    未完待续。。。。

    展开全文
  • SLAM——hector-slam算法原理解析

    千次阅读 2019-04-29 15:59:19
    1、hector-slam代码框架概述 下载源码:git clone ... 原理解读参照:https://blog.csdn.net/weixin_40047925/article/details/80679496 其中包含了许多用于仿真文件,he...

    1、hector-slam代码框架概述

          下载源码:git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git

          原理解读参照:https://blog.csdn.net/weixin_40047925/article/details/80679496

           其中包含了许多用于仿真的文件,hector slam算法主要集中在hector mapping包中,同时核心的位姿估计和点云建立栅格图操作主要在文件HectorSlamProcessor.h的函数update中。如下:

           函数newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov))根据输入的激光点云数据和现有的栅格图进行对齐,这部分是整个代码的核心,主要思想是使用高斯牛顿优化代价函数,求取让代价函数达到最优的位姿偏量,从而求出当前机器人的位姿态。

           当然,如果已经知道机器人的准确位姿不需要进行上面的位姿解算,直接用函数mapRep->updateByScan(dataContainer, newPoseEstimateWorld)更新栅格地图。这部分算法原理比较简单,主要两个方面:一是,用对数表达每一个子栅格的概率值;二是,用Bresenham 画线算法来更新子栅格的概率值。

           为了说明上述算法原理,先介绍一部分的预备知识,然后再切入正题。

    2、hector-slam预备知识

          2-1 占用栅格地图

           这里直接使用上述参考文章进行说明,假设机器人的位姿已知,那么经过每一次激光数据扫描,真实障碍物在栅格地图上的占用概率不断提升最终无限接近1,而没有障碍物的地方则趋近于0。整个栅格地图被分成了一定分辨率的子栅格,最终地图的概率分布如下:

         p(m|z_{1:t},x_{1:t})=\prod p(m_{i}|z_{1:t},x_{1:t})

           整个栅格图构建需要计算每一个子栅格的占用概率值。实际在表达栅格占用概率使用对数形式表达,好处是避免在0和1附近概率值不稳定的问题。具体如下:

         l_{t,i}=log\frac{p(m_{i}|z_{1:t},x_{1:t})}{1-p(m_{i}|z_{1:t},x_{1:t})}                         p(m_{i}|z_{1:t},x_{1:t})=1-\frac{1}{1+e^{l_{t,i}}}

            在实际使用栅格地图进行导航或者路径规划时,每一个栅格仅有三个状态:占用、空闲和未知,该三种状态仅需对p(m_{i}|z_{1:t},x_{1:t})或者l_{t,i}设定范围进行判断即可,比如hector slam中是l_{t,i}值大于0为占用,小于0为空闲,等于0为未知。

               2-2 非线性优化——高斯牛顿法

            在处理优化问题时经常会求去某个指标的最值,比如最小二乘、最大似然、最小方差等,在hector slam求解位姿时也构造了一个最小二乘问题,但是使用的模型是关于位姿的非线性函数,所以需要使用非线性优化的方法,hector slam使用的是高斯牛顿法。这里对高斯牛顿法做个简单说明。

             对于非线性最小二乘问题,可以用如下数学表达式来描述:

             x = argminF(x)=argmin\frac{1}{2}\left \|f(x))\right \|_{2}^{2} 

             如果f(x)数学表达式比较简单,可以直接对F(x)求导,并领其等于0,可以求出极值。对于无法求导情况,一般用迭代求解\Delta x,求解的迭代值不断的趋使F(x)趋近最小值,直到\Delta x变化很小或者达到最大迭代次数。那么关键问题在于求取\Delta x,这里有很多方法,如:梯度下降法、牛顿法、牛顿高斯法和LM法。简单比较一下:梯度下降法——对代价函数F(x)进行泰勒展开,仅保留一阶导数部分,按照梯度反方向进行迭代,即\Delta x=-J(x),但是这种方向并不一定是最合适的方向,有时可能迭代过头,反而会绕路;牛顿法——其实是上面F(x)泰勒展开,保留到二阶,增加了海森矩阵,因此其迭代的路径一般是比较短的,但是需要计算F(x)的二阶导数,这个是很难求得的,因此实际应用受限;牛顿高斯法——为了避免牛顿法求解海塞矩阵难的问题,牛顿高斯法先对f(x)进行一节泰勒展开,然后得到F(x)近似表达式,然后另其导数为0,求取F(x)局部最小时的\Delta x,然后不断迭代,直至达到收敛条件;LM法是在牛顿高斯法上进行了修改,避免了牛顿高斯法求解正规方程时可能出现的奇异解,被广泛使用。

            话不多说,直接介绍高斯牛顿法。

            f(x+\Delta x) \approx f(x) + J(x)\Delta x \Rightarrow       

            F(x) = \frac{1}{2}( f(x) + J(x)\Delta x )^{T}( f(x) + J(x)\Delta x ) \Rightarrow

            F{}'(x) = J^{T}(x)( f(x) + J(x)\Delta x ) =0 \Rightarrow

            J^{T}(x)J(x)\Delta x = - J^{T}f(x)\overset{H = J^{T}(x)J(x)}{\rightarrow}\Delta x=-H^{-1}J^{T}f(x)

            和牛顿法比较实际上是用雅克比矩阵二次来代替了海塞矩阵,省去了计算海塞矩阵。

    3、hector-slam位姿解算

           当我们传感器第一次接收到数据,我们可以人为设定当前位置,以此来第一次建立栅格地图m,可以认为是初始化操作。之后,我们在根据前一时刻建立的地图求解的位姿都是相对初始化时刻的位姿而言的,这一点需要明白。那么这个问题可以用贝叶斯估计描述为:

                                       p(x_{t}|m_{t-1},z_t)=\beta p(m_{t-1}|x_{t},z_{t})*p(x_{t}|x_{t-1})

            这里假设状态x符合马尔科夫过程,但是对于先验p(x_{t}|x_{t-1})概率很多情况下是不知道的,如果有这部分的先验的话可以添加这部分代码。在没有这部分信息情况下,hector-slam是让似然概率p(m_{t-1}|x_{t},z_{t})达到最大的最大似然估计。hector-slam通过最小二乘的方式来表达x_{t}=argmaxP(m_{t-1}|x_{t},z_{t})最大似然问题(最小二乘实际上就是最大似然估计的一种):

                                         x=argmin\sum [1-M(S_{i}(x))]^{2}

       未完待续。。。。

     

                    

     

     

       

     

     

     

     

     

     

     

     

    展开全文
  • slam的简单操作以及原理

    千次阅读 2018-11-22 10:26:01
    slam的原理以及应用## 1 新建工作文件夹 使用mkdir命令新建文件夹 slambook和ch1文件夹,并利用cd命令进入ch1文件下,截图如 2 编写helloslam小程序 使用vim命令创建helloslam.cpp文件,并填写如下代码 按...
  • 【ORB-SLAM原理部分

    2020-04-28 17:19:42
    本文简要说明说明ORB-SLAM的工作原理,若要理解每个技术细节,应结合源代码亲自实验。
  • 激光SLAM原理

    千次阅读 2019-09-23 15:13:31
    在机器人定位导航中,目前主要涉及到激光SLAM与视觉SLAM,激光SLAM在理论、技术和产品落地上都较为成熟,因而成为现下最为主流定位导航方式,在家用扫地机器人及商用送餐机器人等服务机器人中普遍采用了...
  • SLAM算法简明原理

    2021-02-07 09:13:00
    Simultaneous Localization and Mapping (SLAM)处理方法主要分为滤波和图优化两类。 基于滤波器的SLAM 其中,黑色为预测(经验),红色为观测 图例 观测 预测(经验) 概率 图一 未观测到特征 初始...
  • hector slam原理详解

    千次阅读 2018-10-18 21:52:30
    hector slam 原理详解 个人理解,不当之处请指出,谢谢! 首先是进行scan matching过程 激光自身坐标系与全局坐标系(可以理解为栅格地图坐标系)原点已知且重合。即激光在地图中初始位置已知,此时激光第一帧...
  • SLAM总结(一)- SLAM原理概述与简介

    千次阅读 2021-02-12 23:27:30
    SLAM总结(一)SLAM原理概述与简介 SLAM原理概述与简介 SLAM(Simultaneous Localization and Mapping):同时定位和建图,定位是定位机体在世界坐标系下位姿(pose、transformation)。单传感器机体一般指相机光...
  • 本文是关于slam建图过程中使用的栅格地图,解释了栅格地图的工作原理,即概率累积的原理。是我自己整理的好多博客的内容。
  • 本文前言 ———————————————— 版权声明:本文为CSDN博主「梦凝小筑」原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上...本人研究方向为激光SLAM,因此对于Google Cartographer 经典算法十分...
  • slam原理流程详解:http://www.360doc.com/content/10/0928/16/3482759_57075454.shtml ...)理顺SLAM的整个流程,加深对SLAM 的理解 http://wenku.baidu.com/link?url=D9uGc1u_yDvI9l6p4EENoIXJCuWP
  • SLAM导航技术原理

    万次阅读 2018-06-07 22:48:17
    经过这几年快速发展,AGV、叉车AGV等已经是自动化行业众所周知了。从2015年受宠磁条导航、磁钉导航、惯性导航... 反射板导航原理相对简单一些。在叉车AGV行驶路线周围一定距离间隔位置布置反射板,叉车...
  • SLAM 原理推论和详细解析

    千次阅读 2017-04-19 14:41:14
    SLAM 基本原理和累积误差消除 http://www.360doc.com/content/10/0928/16/3482759_57075454.shtml SLAM 基础概念 http://blog.csdn.net/xdgs_2005/article/details/52137052 SLAM for dummies 翻译版本。基础...
  • Cartographer的原理探究——GraphSLAM理论基础
  • 官方代码链接 1 system.cc文件代码 // Output welcome message //Check settings file //Load ORB Vocabulary ...疑问:词袋模型文件初始时从哪来? 参考https://www.jianshu.com/p/cfcdf12a3bb6
  • SLAMEKF,UKF,PF原理简介

    千次阅读 2018-07-05 20:04:13
     原问题:能否简单并且易懂地介绍一下多个基于滤波方法的SLAM算法原理?目前SLAM后端都开始用优化方法来做,题主想要了解一下之前基于滤波方法,希望有大神能够总结一下各个原理(EKF,U...
  • 本文介绍了Autolabor Pro1 导航机器人平台激光SLAM建图导航实现原理。 Autolabor Pro1 导航机器人平台是什么? 平台由 Autolabor Pro1 底盘 与 Autolabor Pro1 导航套件组成,目前已实现了室内外自动导航、自动...
  • SLAM的UKF、EFK、PF 的基本原理

    千次阅读 2018-01-22 20:02:15
    任何传感器,激光也好,视觉也好,整个SLAM系统也好,要解决问题只有一个:如何通过数据来估计自身状态。每种传感器测量模型不一样,它们精度也不一样。换句话说,状态估计问题,也就是“如何最好地使用传感器...
  • 激光slam4:基于滤波的激光SLAM方法----Grid-based课程内容贝叶斯滤波粒子滤波FastSLAM的原理及优化gmapping 课程内容 基于滤波的激光SLAM方法----Grid-based 栅格地图 基于激光 贝叶斯滤波 1.概念 2.特性 3....
  • 文章目录一、词袋模型引出二、原理三、实现步骤3.1 生成词袋3.2 用词表示图像四、vocabulary tree(字典)4.1 生成字典4.2 使用字典 一、词袋模型引出 最初Bag of words,也叫做“词袋”,在信息检索中,Bag of ...
  • LOAM SLAM LOAM细节分析 比较详细分析代码流程 https://zhuanlan.zhihu.com/p/57351961 除了LOAM还有易读版本A-LOAM http://tb.huitaofuwu.com/2020/03/14/%E8%AE%BA%E6%96%87%E5%9B%9E%E9%A1%BE%E4%B9%8BLOAM/ ...
  • 给定两组对应点,此函数计算定义转换tr缩放、旋转和平移,以最小化tr(x)与其在y中对应点之间平方误差之和。此例程需要O(n k^3)-时间。 Inputs: X - a k x n matrix whose columns are points Y - a k...
  • 本人研究方向为激光SLAM,因此对于Google Cartographer 经典算法十分感兴趣,但是苦于该算法论文是英文写作,且该论文有着公式多,解释少特点。因此在看了原论文和网上各种论文解读,都没有能够完全把...
  • Hector SLAM算法扫描匹配原理解析Hector SLAM算法扫描匹配原理解析1. 相关定义2. 公式推导3. 代算求解4. 总结和延伸5. 结语 Hector SLAM算法扫描匹配原理解析 扫描匹配就是使用当前帧与已有地图数据构建误差函数,...
  • ORB-SLAM2工作原理总结

    2019-07-18 11:26:37
    ORB-SLAM2是如何完成工作? 第一步:系统会读取摄像头参数和ORB字典数据,接着初始化整个系统。 第二步:利用ORB算法初始化相机位姿,在完成第一帧图像特征点提取后,紧接着可以进行两相 邻帧图像特征点...
  • ORB_SLAM2 特征法SLAM 单目 双目 RGBD * ORB主要借鉴了PTAM思想,借鉴工作主要有 * RubbleORB特征点; * DBow2place recognition用于闭环检测; * Strasdat闭环矫正和covisibility graph思想; * 以及...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 575
精华内容 230
关键字:

slam的原理