1.hdl_graph_slam|后端优化|hdl_graph_slam_nodelet.cpp|源码解读(四)
2.[ORB-SLAM2] ORB-SLAM中的源码阈值ORB特征（提取）
3.[LIDAR-SLAM] Iterative Closest Point (ICP)简单实现
4.[ORB-SLAM2] 回环&DBoW视觉词袋
5.Cartographer调参
6.ORB-SLAM2源码系列--局部建图线程（MapPointCulling和KeyFrameCulling）

hdl_graph_slam|后端优化|hdl_graph_slam_nodelet.cpp|源码解读(四)

       hdl_graph_slam源码解读(八)：后端优化

       后端概率图构建核心：hdl_graph_slam_nodelet.cpp

       整体介绍

       这是整个系统建图的核心，综合所有信息进行优化。源码阈值所有的源码阈值信息都会发送到这个节点并加入概率图中。

       包含信息

       1）前端里程计传入的源码阈值位姿和点云

       2）gps信息

       3）Imu信息

       4）平面拟合的参数信息

       处理信息步骤

       1）在对应的callback函数中接收信息，并放入相应的源码阈值队列

       2）根据时间戳对队列中的信息进行顺序处理，加入概率图

       其他内容

       1）执行图优化，源码阈值模拟微信菜单源码这是源码阈值一个定时执行的函数，闭环检测也在这个函数里

       2）生成全局地图并定时发送，源码阈值即把所有关键帧拼一起，源码阈值得到全局点云地图，源码阈值然后在一个定时函数里发送到rviz上去

       3）在rviz中显示顶点和边，源码阈值如果运行程序，源码阈值会看到rviz中把概率图可视化了

       关键帧同步与优化

       cloud_callback

       cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg,源码阈值const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)

       该函数主要是odom信息与cloud信息的同步，同步之后检查关键帧是源码阈值否更新。

       关键帧判断：这里主要看关键帧设置的源码阈值这两个阈值keyframe_delta_trans、keyframe_delta_angle

       变成关键帧的要求就是：/hdl_graph_slam/include/hdl_graph_slam/keyframe_updater.hpp

       优化函数

       optimization_timer_callback(const ros::TimerEvent& event)

       函数功能：将所有的位姿放在posegraph中开始优化

       loop detection 函数：主要就是将当前帧和历史帧遍历，寻找loop。

       闭环匹配与信息矩阵计算

       匹配与闭环检测

       潜在闭环完成匹配（matching 函数）

       不同loop的信息矩阵计算（hdl_graph_slam/information_matrix_calculator.cpp）

       gps对应的信息矩阵

       hdl_graph_slam/graph_slam.cpp

       添加地面约束

       使用add_se3_plane_edge函数的代码

       执行图优化

       优化函数optimization_timer_callback

       执行图优化，闭环检测检测闭环并加到了概率图中，优化前

       生成简化版关键帧，KeyFrameSnapshot用于地图拼接

       生成地图并定时发送

       生成地图：简化版关键帧拼接

       定时发送：src/hdl_graph_slam_nodelet.cpp文件中

       系统性能与扩展性

       hdl_graph_slam性能问题在于帧间匹配和闭环检测精度不足，系统代码设计好，模块化强，易于扩展多传感器数据融合。

       总结

       hdl_graph_slam后端优化是敢死队问题源码关键，涉及大量信息融合与概率图构建。系统设计清晰，扩展性强，但在性能上需改进。

[ORB-SLAM2] ORB-SLAM中的ORB特征（提取）

       ORB-SLAM中的特征构建以其使用统一的ORB（Oriented FAST and Rotated BRIEF）为核心创新点，使得系统构建更为简洁稳定。本文旨在深入探讨ORB特征的提取流程及对ORB特征的优化改良，以提供直观解答。

       提取流程概览如下：

       第一步：构造金字塔。

       金字塔的构建是理解关键点分布的基础，它通过不同尺度的图像层次化表示，以便在多尺度上寻找特征。金字塔的层级数量与特征的分配直接相关，每层图像面积的减小导致特征点数量的减少，分配策略需确保各层特征点的均衡。

       第二步：提取FAST角点。

       FAST（Features from Accelerated Segment Test）算法基于中心像素与周围像素的灰度对比，高效快速地识别关键点。通过设定阈值，判断像素是否为角点，FAST算法在每个像素点上执行，产生大量角点，再经过非极大值抑制处理以去除冗余点。

       第三步：计算角度。火凤凰翻牌机源码

       通过灰度质心与圆心的向量角度，ORB特征不仅提取了角点，还计算出了每个角点相对于坐标系的角度，这有助于确保每次描述子计算的方向一致性，实现了角度不变性。

       第四步：计算旋转感知的BRIEF描述子。

       BRIEF（Binary Robust Independent Elementary Features）描述子是一个二进制描述子，以其高速匹配速度著称。通过选定的点对对块内的像素进行比较，形成描述子序列。Steered BRIEF根据角点方向旋转描述子坐标点对位置，实现了旋转不变性。

       最后一步：提升抗噪能力。

       ORB在计算描述子时使用周围5×5的patch灰度信息，进行滤波处理，提高了描述子的抗噪性。

       ORB-SLAM的改进主要集中在FAST角点提取步骤。该系统通过动态调整阈值、利用四叉树划分图像等方法，提高了特征分布的均匀性，有效避免了特征扎堆现象。此策略有助于提升SLAM精度、闭环识别性能，并确保全图特征量满足需求，突破新高幅图源码避免了丢失追踪问题。

[LIDAR-SLAM] Iterative Closest Point (ICP)简单实现

       迭代最近点算法（ICP）在激光SLAM领域中用于点云配准，旨在求解两组点云的相对位姿。本文简要介绍ICP算法的基本原理及其在点云配准中的应用，并集成简化版的Odometry实现。

       ICP算法主要解决以下问题：给定两组点云，求解它们之间的相对位姿，而两组点云之间的匹配关系未知，且点的数量可能不同。算法通过迭代求解匹配关系并优化位姿。

       对于已知匹配且点数相同的点云，可通过最小化特定公式求解位姿。首先，去除两组点云的质心，然后计算旋转矩阵和平移量，从而获得解析解。

       在实际应用中，两组点云之间的匹配关系通常未知，因此ICP采用迭代方法。迭代流程包括：选择匹配点对、求解位姿、判断迭代收敛条件。通过遍历点云对，使用已知匹配求解方法计算位姿，租房/公寓管理系统源码不断迭代直至满足收敛条件。

       构建匹配点的关键是确定点对之间的欧式距离，选择最近邻点作为匹配。为避免错误匹配，通常设置距离阈值。通过与上一次迭代结果比较，剔除超出阈值的匹配点。

       ICP算法实现通常基于特定库，例如PCL库。利用TUM提供的RGB-D数据进行测试，仅使用Depth生成点云，通过ICP计算相邻帧位姿变换，形成Odometry。以此方式，可生成相邻帧间轨迹。

       ICP算法自x年提出以来，已发展出多种变体，主要区别在于匹配点选择策略、误差度量、以及优化算法的改进。

[ORB-SLAM2] 回环&DBoW视觉词袋

       回环检测在SLAM系统中至关重要，它能有效降低全局误差，构建一致的地图。在跟踪丢失时，回环检测还能用于重定位。检测回环的方法主要有三种：image-to-image、map-to-map和image-to-map，其中，基于外观的image-to-image方法在大场景适应性上表现更佳。目前，常用的方法是基于视觉词袋的方法，特别是针对BRIEF特征的DBoW2方法，以及其改进版DBoW3。以下是对DBoW及其在ORB-SLAM中应用细节的总结。

       ### 回环检测的评价指标

       在测试回环检测算法时，我们通常会关注四种可能的结果。通过在大量数据集上测试，我们能得到两个统计量：准确率（Precision）和召回率（Recall）。准确率描述的是被检测为回环的结果中真正属于回环的比例，这对于SLAM系统至关重要，我们希望其值尽可能高，避免误报。召回率则是实际回环中被检测出的比例，该值越高越好，但通常与准确率存在矛盾。

       为了评估一个回环算法的性能，我们可以在不同参数设置下进行测试，并绘制准确率-召回率曲线。理想情况下，曲线的右上角应尽可能接近右上角，形成尽可能方正的形状。

       ### DBoW2视觉词袋原理

       我们的目标是计算两张图像之间的相似度，以此判断是否出现回环。一种直接的方法是匹配特征点并统计匹配数量，但这过程耗时。为提高效率，我们可将特征抽象为单词，例如“车”、“猫”、“人”。通过比较图像中是否存在这些单词来判断图像相似性。这种方法仅关注单词的存在或不存在（0或1）。

       在DBoW2中，BRIEF特征被聚类成预设数量的单词。一幅图像的特征点可以转换为单词表示，进而形成一个向量，用于比较两幅图像的相似性。计算相似度时，我们使用诸如汉明距离的分数计算方法。需要注意的是，实际应用中，单词的权重也考虑在内，且相似度计算方式可能有所不同。

       ### DBoW2单词生成

       DBoW2通过在大量图像中提取特征，并利用K-Means算法聚类，生成预设数量的单词。为加速判断特征属于哪个单词，使用K叉树方法，实现快速查找。此外，DBoW2还构建了Direct Index和Inverse Index，分别用于存储与特定单词相关的特征索引，以及所有包含该单词的图像索引和权重信息。这些结构加速了图像相似度计算过程。

       ### DBoW2相似度计算

       在构建词典后，DBoW2通过计算单词的TF-IDF（Term Frequency-Inverse Document Frequency）权重，来调整单词的相对重要性。这有助于区分在环境中出现频率不同的单词，提高算法的区分能力。

       ### ORB-SLAM中的回环处理

       ORB-SLAM维护一个数据库，用于存储每个单词对应的观察关键帧。在检测回环时，关键帧会从数据库中进行搜索。这一过程类似于DBoW中的Inverse Index应用，但由ORB-SLAM自行管理。

       在搜索闭环候选帧时，ORB-SLAM首先计算当前帧与其共视图帧之间的相似分数，选择最小值作为基准。然后，从数据库中查找具有共同单词的关键帧。通过一系列筛选条件，包括相似度阈值、共同单词数量、共视图关系等，最终确定闭环候选帧。

       ### 几何校验与闭环融合

       闭环候选帧通过Sim3变换进行几何校验，确保变换的一致性。成功验证后，关键帧及其局部地图与闭环帧对齐，并更新共视图关系。这一过程整合了当前帧和闭环帧的地图信息，保持定位连续性。

       ### 优化过程

       最后，基于Essential graph进行局部优化，随后进行全局BA（Bundle Adjustment），以进一步提高定位和地图构建的准确性。

       总结而言，回环检测在SLAM系统中扮演着关键角色，通过高效地识别回环，可以有效降低系统误差，构建一致的地图。DBoW2及其在ORB-SLAM中的应用展示了如何在特征匹配、单词聚类、相似度计算、闭环检测和地图融合等方面优化SLAM系统的性能。

Cartographer调参

        技术标签： Cartographer

        根据Cartographer_ros文档翻译

        Cartographer是一个复杂的系统，调整它需要很好地理解其内部工作。此页面试图直观地概述Cartographer使用的不同子系统及其配置值。如果您对Cartographer的介绍不仅仅感兴趣，还应参考Cartographer论文。它仅描述了2D SLAM，但它严格定义了此处描述的大多数概念。这些概念通常也适用于3D。

        W. Hess, D. Kohler, H. Rapp, and D. Andor, Real-Time Loop Closure in 2D LIDAR SLAM , in Robotics and Automation (ICRA), IEEE International Conference on . IEEE, . pp. –.

        Cartographer可以看作是两个独立但相关的子系统。第一个是LocalSLAM（有时也称为前端或局部轨迹构建器）。它的工作是建立一系列子图。每个子图都是本地一致的，但我们接受LocalSLAM随着时间的推移而漂移。大多数地方SLAM选项中可以找到 install_isolated/share/cartographer/configuration_files/trajectory_builder_2d.lua 为2D和 install_isolated/share/cartographer/configuration_files/trajectory_builder_3d.lua 为3D。（对于本页的其余部分，我们将参考TRAJECTORY_BUILDER_nD作为常用选项）

        另一个子系统是全局SLAM（有时称为后端）。它在后台线程中运行，其主要工作是找到回环约束。它通过对子图的扫描匹配来实现。它还结合了其他传感器数据，以获得更高级别的视图，并确定最一致的全局解决方案。在3D中，它还试图找到重力方向。它的大多数选项都可以在 install_isolated / share / cartographer / configuration_files / pose_graph.lua中找到。

        在更高的抽象上，LocalSLAM的工作是生成良好的子图，而全局SLAM的工作是将它们最一致地结合在一起。

        测距传感器（例如：LIDAR）提供多个方向的深度信息。但是，有些测量与SLAM无关。如果传感器部分被灰尘覆盖或者如果它被引向机器人的一部分，则一些测量距离可被视为SLAM的噪声。另一方面，一些最远的测量也可能来自不需要的源（反射，传感器噪声），并且也与SLAM无关。为解决这些问题，Cartographer首先应用带通滤波器，并仅将范围值保持在某个最小和最大范围之间。应根据机器人和传感器的规格选择最小值和最大值。

        注意

        在2D中，Cartographer将比max_range更换范围 TRAJECTORY_BUILDER_2D.missing_data_ray_length 。它还提供了将3D点云过滤为2D切割的值 max_z 和 min_z 值。

        注意

        在Cartographer配置文件中，每个距离都以米为单位定义

        距离是在一段时间内测量的，而机器人实际上正在移动。但是，距离是由大型ROS消息中的“批量”传感器提供的。Cartographer可以独立考虑每个消息的时间戳，以考虑机器人运动引起的畸变。Cartographer进行测量频率越高，测量结果组合成一个可以立即捕获的单个相干扫描就越好。因此，强烈建议通过扫描提供尽可能多的 rangedata （ROS消息）。

        Rangedata通常从机器人上的单个点测量，但是以多个角度测量。这意味着靠近的表面（例如道路）经常被击中并提供许多点。相反，远处的物体不常被击中并且提供较少的点数。为了减少点处理的计算权重，我们通常需要对点云进行下采样。然而，简单的随机抽样将从我们已经具有低密度测量的区域移除点，并且高密度区域仍将具有比所需更多的点。为了解决这个密度问题，我们可以使用一个体素滤波，将原始点下采样为一个恒定大小的立方体，并只保留每个立方体的质心。

        较小的立方体大小将导致更密集的数据表示，从而导致更多计算。较大的立方体大小会导致数据丢失，但会更快。

        在应用了固定尺寸的体素滤镜后，Cartographer还应用了自适应体素滤镜。此过滤器尝试确定最佳体素大小（在最大长度下）以实现目标点数。在3D中，两个自适应体素滤波器用于生成高分辨率和低分辨率点云，它们的使用将在 LocalSLAM中 阐明。

        惯性测量单元可以是SLAM的有用信息源，因为它提供精确的重力方向（因此，地面）和机器人旋转的嘈杂但良好的整体指示。为了过滤IMU噪声，在一定时间内观察到重力。如果您使用2D SLAM，则可以实时处理范围数据而无需额外的信息来源，因此您可以选择是否要让Cartographer使用IMU。使用3D SLAM，您需要提供IMU，因为它用作扫描方向的初始猜测，大大降低了扫描匹配的复杂性。

        注意

        在Cartographer配置文件中，每次定义值都以秒为单位

        一旦扫描组装并从多个范围数据中过滤，就可以为LocalSLAM算法做好准备。LocalSLAM 使用来自位姿估计器的初始估计通过扫描匹配将新扫描插入其当前子图构造中。位姿估计器背后的想法是使用除测距仪之外的其他传感器的传感器数据来预测下一次扫描应该插入子图的位置。

        有两种扫描匹配策略：

        无论哪种方式， CeresScanMatcher 都可以配置为给每个输入一定的权重。权重是衡量对数据的信任度，可以将其视为静态协方差。重量参数的单位是无量纲的数量，不能在彼此之间进行比较。数据源的权重越大，Cartographer在进行扫描匹配时就会越强调这个数据源。数据来源包括占用空间（扫描点），位姿外推器（或 RealTimeCorrelativeScanMatcher ）的平移和旋转

        注意

        在3D中， occupied_space_weight_0 和 occupied_space_weight_1 参数分别与高分辨率和低分辨率滤波点云相关。

        在 CeresScanMatcher 从得名 Ceres Solver ，以谷歌为解决非线性最小二乘问题的库。扫描匹配问题被建模为这样的问题的最小化，其中两个扫描之间的运动（变换矩阵）是要确定的参数。Ceres使用下降算法针对给定的迭代次数优化运动。Ceres可以配置为根据您自己的需要调整收敛速度。

        该 RealTimeCorrelativeScanMatcher 可以根据您在传感器的信任进行切换。它的工作原理是在搜索窗口中搜索类似的扫描，搜索窗口由最大距离半径和最大角度半径定义。当使用此窗口中的扫描执行扫描匹配时，可以为平移和旋转组件选择不同的权重。例如，如果您知道机器人不会旋转很多，您可以使用这些权重。

        为避免每个子图插入太多扫描，一旦扫描匹配器找到两次扫描之间的运动，它就会通过运动滤波器。如果导致它的运动不够重要，则扫描将被删除。仅当扫描的运动高于特定距离，角度或时间阈值时，才会将扫描插入到当前子图中。

        当LocalSLAM已经接收到给定量的范围数据时，认为子图构建完成。LocalSLAM会随着时间漂移，GlobalSLAM用于解决这种漂移问题。子图必须足够小，以使其内部的漂移低于分辨率，以便它们在局部是正确。另一方面，子图应该足够大以使环路闭合能够正常工作。

        子图可以将它们的范围数据存储在几个不同的数据结构中：最广泛使用的表示称为概率网格。但是，在2D中，还可以选择使用截断的有符号距离场（TSDF）。

        概率网格将空间划分为2D或3D表格，其中每个单元格具有固定大小并包含被障碍物占有的概率。根据“ 命中 ”（测量范围数据）和“ 未命中 ”（传感器和测量点之间的自由空间）更新Odds。 命中 和 未命中 可以在占用概率计算不同的权重，赋予更多或更少的信任。

        在2D中，每个子图仅存储一个概率网格。在3D中，出于扫描匹配性能的原因，使用两个 混合 概率网格。（术语“混合”仅指内部树状数据表示并被抽象给用户）

        扫描匹配首先将低分辨率点云的远点与低分辨率混合网格对齐，然后通过将高分辨率点与高分辨率混合网格对齐来细化位姿。

        注意

        CartographerROS提供了一个可视化子图的RViz插件。您可以从其编号中选择要查看的子图。在3D中，RViz仅显示3D混合概率网格的2D投影（灰度）。RViz左侧窗格中提供了选项，可在低分辨率和高分辨率混合网格可视化之间切换。

        TODO： 记录TSDF配置

        当LocalSLAM生成其连续的子图时，全局优化（通常称为“ 优化问题 ”或“ 稀疏位姿调整 ”）任务在后台运行。它的作用是重新安排彼此之间的子图，以便它们形成一个连贯的全局地图。例如，该优化负责改变当前构建的轨迹以正确地对准关于环闭合的子图。

        一旦插入了一定数量的轨迹节点，就会批量运行优化。根据您运行它的频率，您可以调整这些批次的大小。

        注意

        将 POSE_GRAPH.optimize_every_n_nodes 设置为 0 是禁用全局SLAM并专注于LocalSLAM行为的便捷方法。这通常是调整Cartographer的第一件事。

        全局SLAM是一种“ GraphSLAM ”，它本质上是一种位姿图优化，它通过在节点和子图之间构建约束然后优化得到的约束图来工作。可以直观地将约束视为将所有节点捆绑在一起的小绳索。稀疏位姿调整完全固定这些绳索。生成的网称为“ 位姿图 ”。

        注意

        约束可以在RViz中可视化，调整全局SLAM非常方便。还可以切换 POSE_GRAPH.constraint_builder.log_matches 以获得格式化为直方图的约束构建器的常规报告。

        注意

        实际上，全局约束不仅可以在单个轨迹上查找循环闭包。它们还可以对齐由多个机器人记录的不同轨迹，但我们将保留此用法以及与“全局本地化”相关的参数超出本文档的范围。

        为了限制约束（和计算）的数量，Cartographer仅考虑构建约束的所有关闭节点的子采样集。这由采样率常数控制。采样太少的节点可能导致错过约束和无效的循环闭包。对太多节点进行采样会降低全局SLAM的速度并阻止实时循环关闭。

        当考虑节点和子图建立约束时，它们会通过名为的第一个扫描匹配器 FastCorrelativeScanMatcher 。该扫描匹配器专为Cartographer设计，可实现实时循环闭合扫描匹配。在 FastCorrelativeScanMatcher 依靠“ 分支定界 ”机制在不同的格点分辨率的工作，有效地消除不正确匹配数。这种机制在本文件前面介绍的制图文章中有广泛的介绍。它适用于可以控制深度的探索树。

        一旦 FastCorrelativeScanMatcher 有足够好的建议（高于最低匹配分数），然后将其输入Ceres扫描匹配器以改进位姿。

        当Cartographer运行 优化问题时 ，Ceres用于根据多个 残差 重新排列子图。残差是使用加权损失函数计算的。全局优化具有成本函数以考虑大量数据源：全局（循环闭包）约束，非全局（匹配器）约束，IMU加速和旋转测量，局部SLAM粗略姿态估计，测距源或固定框架（如GPS系统）。可以按照 LocalSLAM 部分中的说明配置权重和Ceres选项。

        注意

        通过切换，可以找到有关优化问题中使用的残差的有用信息 POSE_GRAPH.max_num_final_iterations

        作为其IMU残差的一部分，优化问题为IMU姿态提供了一些灵活性，默认情况下，Ceres可以自由地优化IMU和跟踪帧之间的外部校准。如果您不信任您的IMU位姿，可以记录Ceres全局优化的结果并用于改进您的外部校准。如果Ceres没有正确优化您的IMU位姿并且您完全相信您的外在校准，则可以使此位姿保持不变。

        在残差中，异常值的影响由配置有某个Huber量表的Huber损失函数处理。Huber量表 越大 ，（潜在）异常值 的影响越大 。

        一旦轨迹完成，Cartographer就会运行一个新的全局优化，通常比以前的全局优化要多得多。这样做是为了完善Cartographer的最终结果，通常不需要是实时的，所以大量的迭代通常是正确的选择。

ORB-SLAM2源码系列--局部建图线程（MapPointCulling和KeyFrameCulling）

       ORB-SLAM2源码系列--局部建图线程详解

       MapPointCulling模块负责筛选新加入的地图点，确保地图质量。在ProcessNewKeyFrame函数中，新点被暂存于mlpRecentAddedMapPoints。筛选过程包括：

       根据相机类型设定不同的观测阈值

       遍历新点，若点已标记为坏点则直接从队列中移除

       若点的观察帧数少于预期值的%，或者观察相机数量少于阈值cnThObs，即使过了两个关键帧也会被删除

       只有经过三个关键帧且未被剔除的点，才会被认定为高质量点，仅从队列移除

       另一方面，KeyFrameCulling则针对共视图中的关键帧进行冗余检测。步骤如下：

       提取当前关键帧的共视关键帧，并遍历它们

       对于每个共视关键帧，检查其地图点：若至少有3个其他关键帧观测到，被认为是冗余点

       对于双目或RGB-D，仅考虑近距离且深度值大于零的地图点

       若关键帧%以上的有效地图点被判断为冗余，该关键帧将被标记为冗余并删除

       这样的筛选机制确保了地图数据的准确性和效率。