1.hdl_graph_slam|后端优化|hdl_graph_slam_nodelet.cpp|源码解读(四)
2.ORB-SLAM2源码系列--局部建图线程（MapPointCulling和KeyFrameCulling）
3.不可错过的激光激光gmapping算法使用与详细解释
4.激光雷达导航技术的优势有哪些？请说具体一点！
5.非线性优化（三）：g2o源代码
6.ROS2测试源码编译安装cartographer

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

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

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

       整体介绍

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

       包含信息

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

       2）gps信息

       3）Imu信息

       4）平面拟合的参数信息

       处理信息步骤

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

       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源码系列--局部建图线程（MapPointCulling和KeyFrameCulling）

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

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

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

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

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

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

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

       提取当前关键帧的e语言源码损坏共视关键帧，并遍历它们

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

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

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

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

不可错过的gmapping算法使用与详细解释

       了解移动机器人构建地图的必备条件、算法流程及原理，对gmapping算法的应用与解释进行深入探讨。gmapping是一个基于2D激光雷达使用RBPF算法完成二维栅格地图构建的SLAM算法，具有实时构建室内环境地图、计算量小、地图精度高、对激光雷达扫描频率要求低等优点。然而，随着环境增大，构建地图所需的内存和计算量增大，不适用于大场景构图。

       gmapping算法的使用步骤包括了解算法、安装算法、更改参数、执行算法和保存地图。首先，需要理解gmapping基于RBPF算法的工作原理和其在小场景中的优势。接下来，通过ROS提供的功能包，以二进制方式安装gmapping算法，确保机器人具备运行所需的话题和服务，如/tf、/odom和/scan。在更改参数后，启动gmapping算法，其TF树应满足特定的配置。最后，通过命令保存地图至map.pgm和map.yaml文件。

       深入探讨gmapping的前世今生，SLAM问题通过概率描述和分解为机器人定位和地图构建两个问题进行理解。FastSLAM算法采用RBPF方法，将问题分解为估计机器人轨迹和已知机器人位姿进行地图构建。在gmapping中，为解决内存爆炸和粒子耗散问题，提出降低粒子数量和选择性重采样的平衡车电机源码方法。通过极大似然估计和激光雷达观测模型优化粒子数量，同时通过权重离散程度控制重采样操作，保证算法的有效性。

       gmapping算法流程清晰，从SLAM问题的概率描述到算法分解，再到机器人位姿估计的迭代转换，直至gmapping算法的伪代码，每一步都有其明确的目的和作用。深入理解gmapping源码、已知位姿构建地图算法以及贝叶斯滤波、粒子滤波等主题的相关文章，将在未来陆续发布。

       了解gmapping论文、带中文注释的源码以及相关课件，可关注公众号获取。

       粒子滤波概念通过一个趣味解释进行了形象描述，从理论层面阐述了粒子滤波在机器人定位问题中的应用，如何根据机器人的感受逐步缩小搜索范围，最终得到准确的定位结果。

       总结gmapping算法进行地图构建的流程，从SLAM问题的全面理解到gmapping算法的具体实现，每一步都清晰明了。深入分析算法原理和流程，有助于掌握移动机器人构建地图的关键技术和方法。

激光雷达导航技术的优势有哪些？请说具体一点！

       杭州艾豆智能激光SLAM 智能扫地机器人开源系统

       一、说明

       杭州艾豆智能科技有限公司，专注于机器人的室内定位与导航、自主运动，智能避障和视觉技术的研究。在SLAM算法，扫地机的运动控制，视觉等技术领域有六年多技术积累。

       我们致力于为高性能消费级机器人提供室内定位导航及视觉解决方案，主要的产品有：°扫描激光雷达SLAM定位导航套件，固定式激光雷达定位导航套件，及深度摄像头定位导航套件，陀螺仪惯导套件、智能扫地机器人控制主板、通用型激光SLAM 机器人底盘，智能消毒机器人。蓝牙支付模块源码

       我们积累了丰富的智能机器人室内导航定位系统，产品广泛应用与智能扫地机器人，智能全自动消毒机器人。

       杭州艾豆智能，基于激光SLAM的智能扫地机人源码是一套完整的量产的源码。基于STM和linux下C语言开发，基于本代码可以创建完整的商业级激光SLAM智能扫地机器人。

       二、功能简介

       1．功能

       杭州艾豆智能科技有限公司的激光SLAM智能扫地机人源码，具备完整的智能扫地机器人功能，功能包括。

       1）LDS激光雷达度全方位扫描，赫兹自适应扫描频率。

       2）不低于8米的测距范围，测量量程1%的解析。

       3） Class 1 激光安全标准。

       4）激光SLAM定位，建图，导航功能。

       5）快速全屋扫描地图算法，快速生成室内二维地图。

       6）弧形掉头，工字清扫功能。

       7）沿边清扫功能，支持激光沿边和红外沿边。

       8）红外碰撞，碰撞块处理机制。

       9）悬崖传感器，跌落计算处理。

       ） 以房间为单位划分区域，智能清扫策略，路径规划功能。

       ） 计算导航路径，导航算法。

       ） 后轮电机、滚刷电机、边刷电机，风机的驱动和调速功能。

       ） 后轮电机、滚刷电话，边刷电机，javaweb打开源码风机堵转保护功能。

       ） 自动回充对接充电桩功能。

       ） APP地图显示、控制功能。

       ） 遥控器功能

       ） 手动遥控功能

       ） 智能避障

       ） 预约功能

       ） 虚拟墙功能

       ） 禁区功能

       ） 指哪去哪功能。

       ） 区域清扫功能

       ） 断点续扫功能。

       ） 智能语音功能。

       ） OTA远程升级功能

       2．技术特色

       杭州艾豆智能科技有限公司激光SLAM智能扫地机器人采用自持专利算法，有别其他开源项目。

       1) 完全自主知识产权激光SLAM算法。

       2) 不采用传统开源SLAM算法，无需操作系统支持，支持裸奔，支持嵌入式linux，抛弃臃肿的ubuntu和ROS系统。

       3) 快速识别门和房间，全屋快速扫描，生成地图算法。适用于室内自动扫描建图系统。

       4) 快速重定位功能，只要建好图，机器人随便扔，都能快速拟合重定位。

       三、系统结构

       1．系统结构

       本激光SLAM导航智能扫地机器人系统由以下单元组成：

       1) 主控系统

       2) 激光SLAM算法板

       3) 传感器板

       4) 电机驱动器

       系统结构图如下：

       2．主控系统

       主控系统采用STM或GD系统，用于电机运动控制和清扫策略计算，以及各个传感器的数据采集，分析。

       3．激光SLAM算法板

       激光SLAM算法板实现SLAM算法，房屋识别，门识别，分区算法。

       4．传感器模块

       传感器模块用于采集各种外围传感器数据。

       电机控制模块

       电机控制模块用于控制左行动轮，右行动轮，边刷电机，滚刷电机，风机，并通过编码器反馈形成电机闭环控制。

       反馈监视电机电流，以便主控系统计算电机堵转。

       四、快速入门

       1．快速使用

       从艾豆智能科技有限公司获取到激光SLAM智能扫地机器人源码。源码分三部分：

       1) 主控板源码，采用Keil uVision编译。

       2) SLAM算法源码，基于linux编译。

       3) APP代码，基于linux编译。

       第一步：打开主控板源码。

       使用Keil uVision V5..1.0打开“RE\USER”目录下的irobot.uvprojx 文件，编译，生成hex或bin文件，烧录hex或bin文件。

       本代码使用Source Insight编辑，建议使用者也采用SI编辑，如使用Keil编辑，可能会存在代码不对齐的情况。

       第二步：打开SLAM源码：

       进入slam 的build目录，执行make命令，生成slam文件，将slam

       文件通过网络上传到linux算法板的update目录下。

       第三步：打开APP源码。

       在linux下，进入app的build目录，执行make命令，生成app文件，将app上传到linux算法板的update目录下。

       重启系统，听到“系统载入中”的语音，稍后，开始按键，配网，使用APP进行控制，建图。

非线性优化（三）：g2o源代码

       新年伊始，让我们探讨一下g2o（通用图优化）在SLAM（Simultaneous Localization and Mapping）中的后端优化库应用。在《十四讲》中，我们对g2o有了初步的了解，并总结了其在SLAM中的使用情况。与ceres相比，g2o的文档较为简略，主要依赖于两篇论文进行参考。本文将深入探讨g2o的源代码，特别是核心文件夹中的部分，以揭示这个在SLAM领域广为人知的后端优化库的内在机理。

       首先，让我们通过一张类关系图来直观理解g2o的架构。整个g2o系统分为三层：HyperGraph、OptimizableGraph、以及SparseOptimizer。HyperGraph作为最高层，提供了一个高度抽象的框架，其内部通过内类的方式实现了Vertex和Edge的结构。Vertex和Edge相互关联，Vertex存储与节点相关联的边的集合，而Edge则记录了与之链接的节点信息。HyperGraph提供了基本的节点和边的操作，如获取、设置等，同时也包含了更复杂的功能，如节点和边的合并、删除等。

       OptimizableGraph继承自HyperGraph，进一步丰富了Vertex和Edge的实现，为图优化提供了更具体的接口。OptimizableGraph引入了海塞矩阵和b向量的概念，以及与之相关的操作，如获取海塞矩阵元素、设置参数位置等。此外，它还支持通过栈操作（pop、push）来管理节点信息。

       在OptimizableGraph之上，SparseOptimizer作为优化操作的对象，实现了优化的接口，并提供了初始化、辅助函数以及优化的核心函数。SparseOptimizer通过内部类实现了Vertex和Edge的实例化，为具体的优化算法提供了操作图的接口。

       在实现细节方面，BaseVertex和BaseEdge类继承了OptimizableGraph中的相应类，实现了节点和边的基本功能。BaseVertex类负责记录节点的海塞矩阵、b向量和估计值，并提供了数值求导的备份和恢复功能。BaseEdge类则负责处理测量信息和信息矩阵的计算，包括计算误差、构造二次形式等。此外，不同类型的边（BaseUnaryEdge、BaseBinaryEdge、BaseMultiEdge）通过继承BaseEdge类，实现了不同链接节点数量的边的特殊操作。

       鲁棒核函数的实现是g2o优化框架中一个关键部分，它在处理非线性优化问题时提供了鲁棒性，确保了优化过程的稳定性。g2o通过RobustKernel虚基类提供了设置和获取核函数参数的接口，并在具体实现中使用了简化版本的计算公式，以保证信息矩阵的正定性。

       最后，OptimizationAlgorithm类定义了优化器的一系列接口，如初始化、计算边际值和求解等。g2o的优化算法包括GN、LM和dog-leg，它们分别实现了不同的求解策略，而具体的矩阵求解任务则通过Solver类及其派生类（如BlockSolver）完成。BlockSolver类提供了一个通用框架，允许用户自定义线性求解器，如直接求解、迭代求解等。

       综上所述，g2o通过层次化的类结构，提供了从抽象到具体、从基础到进阶的图优化解决方案，其设计旨在高效、鲁棒地解决SLAM中的后端优化问题。深入理解g2o的源代码，对于开发者和研究者来说，不仅能够提高优化算法的实现效率，还能深刻理解SLAM系统中的优化机制。

ROS2测试源码编译安装cartographer

       Cartographer是一个跨平台、传感器配置提供实时同步定位和绘图(SLAM)的系统，具有回环检测优势，资源占用适中。

       选择源码编译安装方式，以适应后期项目修改和移植需求。首先，使用Ubuntu虚拟机测试验证。

       若国内访问github受限，可选择Gitee上的备份仓库进行下载。尝试多个版本，确认在Ubuntu humble版本下能够成功下载和安装。

       在安装过程中，需要下载依赖项。在Ubuntu上，首先安装libabsl-dev、libceres-dev以及liblua5.3-dev等包。对于ceres-solver，需确保CUDA、显卡加速和TBB指令集优化选项已配置。

       在开发板上，通过源码编译安装三方依赖。确保所有依赖包均正确安装，包括protobuf版本为v3.4.1分支。

       完成所有依赖安装后，开始编译Cartographer源码。首先下载官方数据集，注意ROS2格式的rosbag转换，使用rosbags工具进行转换。

       介绍ROSbag格式，ROS1的.rosbag文件为二进制存储格式，而ROS2使用SQLite数据库格式，支持跨平台和扩展性。两种格式转换方法，推荐使用rosbags工具，无需依赖ROS环境。

       测试Cartographer时，使用ros2命令启动示例launch文件，输入特定的bag文件名以加载数据集。测试3D数据集时，使用相应的launch文件和bag文件名。

       资源占用情况分析将后续进行。

ORB-SLAM3 源码剖析：IMU 预积分

       IMU的数据结构在ORB-SLAM3中用于表示机体坐标系中的测量值。在特定时刻，加速度计测量线加速度和陀螺仪测量角速度。假设这些测量值包含高斯白噪声，且偏置建模为随机游走，其导数也是高斯白噪声。将重力转换到机体坐标系后，得到连续视觉帧间的IMU预积分结果。这些预积分包括旋转、速度和位置测量，以及整个测量向量的协方差矩阵。

       在ORB-SLAM3中，每帧的IMU预积分在tracking线程中计算，具体由Tracking::PreintegrateIMU()函数执行。每帧间的IMU测量通过src/ImuTypes.cc中的Preintegrated::IntegrateNewMeasurement()进行积分。主要步骤如下：首先进行偏置校正，然后计算位置、速度的增量，接着计算旋转的增量。旋转变化量以李代数中的旋转向量表示，并通过指数映射转换为旋转矩阵。旋转矩阵按旋转顺序右乘。最后，更新协方差矩阵，并调整与偏置修正相关的位置、速度和旋转雅可比。

       IMU的偏置校正、测量、标定和预积分类定义在include/ImuTypes.h文件中。

       值得注意的是，对于初学者，了解GDB调试方法是提高ORB-SLAM3源码理解效率的重要步骤。GDB提供了一系列功能，允许开发者在运行程序时设置断点、查看变量值、追踪程序执行流程等，从而深入分析代码行为和潜在问题。