1.hdl_graph_slam|后端优化|hdl_graph_slam_nodelet.cpp|源码解读(四)
2.激光雷达导航技术的定位定位优势有哪些？请说具体一点！
3.ROS2测试源码编译安装cartographer
4.ORB-SLAM3 源码剖析：IMU 预积分
5.SLAM地图评估指标及EVO评估工具安装使用教程
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后端优化是关键，涉及大量信息融合与概率图构建。系统设计清晰，扩展性强，但在性能上需改进。

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

       杭州艾豆智能激光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）悬崖传感器，跌落计算处理。

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

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

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

       ） 后轮电机、滚刷电话，边刷电机，风机堵转保护功能。

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

       ） 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进行控制，建图。shrio源码分析

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提供了一系列功能，允许开发者在运行程序时设置断点、查看变量值、追踪程序执行流程等，从而深入分析代码行为和潜在问题。

SLAM地图评估指标及EVO评估工具安装使用教程

       在评估使用ORB-SLAM3构建的地图精度时，首先需要解决地图数据与GPS真值坐标尺度不一致的问题，进行地图数据对齐处理。之后，通过使用EVO工具进行评估，以获取更清晰的评估结果。整理后，以下是关于SLAM地图评估指标和EVO评估工具安装使用教程的详细内容。

       一、SLAM评价指标

       在评估SLAM/VO算法时，需要从时耗、复杂度、精度等多个角度进行。其中，精度评价是最重要的考量。视觉SLAM通常使用绝对位姿误差（APE）、均方根误差（RMSE）和标准差（STD）等指标来评估运动轨迹的精度，这些指标衡量的是算法估计位姿与真实位姿之间的误差。APE首先对齐真实值和估计值，计算每个值之间的偏差，用于评估轨迹的全局一致性。RMSE衡量整体估计值与真实值的偏差程度，偏差越大，RMSE也越大。

       二、EVO安装教程及命令概览

       EVO是一个常用的评估工具，用于评估SLAM和VO算法的性能。要安装EVO，请访问其官方源码地址。使用命令行一键安装，可能遇到安装错误，参考相关解决方法以解决可能的问题。EVO提供了多种评估指标和命令，包括评估绝对位姿误差（evo_ape）和相对位姿误差（evo_rpe）等功能。

       三、运行EVO

       1. evo_traj：主要用于绘制轨迹、输出轨迹文件和转换数据格式等功能。EVO自带示例包括使用自带数据进行轨迹对比和轨迹对齐。对于存在尺度不确定性的单目相机，EVO支持使用-s参数进行Sim(3)上的对齐（旋转、平移与尺度缩放）。

       2. 轨迹评估：EVO可以评估两条轨迹的误差，主要命令包括计算绝对位姿误差（evo_ape）和相对位姿误差（evo_rpe）。这些指令支持轨迹对齐和尺度缩放功能，并提供详细的参数说明以指导使用。通过这些命令，可以直观地评估轨迹的全局一致性与局部准确性。

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

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

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

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

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

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

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

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

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

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

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

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

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

非线性优化（三）：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系统中的优化机制。

从零开始一起学习SLAM | SLAM有什么用？

       SLAM，即Simultaneous Localization And Mapping，是一个在移动设备上同时进行定位和地图构建的过程。我们可以通过一个日常例子来理解它，比如家用的智能扫地机器人。

       早期的扫地机器人在工作时，只能简单地避开障碍物，随机游走清扫，导致效率低下且清扫不彻底。然而，随着SLAM技术的引入，现在的扫地机器人能够通过传感器扫描环境，建立地图，并根据自身的定位进行高效的清扫路径规划，包括自动回充、断点续扫等高级功能。这要求机器人具备三大能力：定位（Localization）、建图（Mapping）和路径规划（Route Planning）。

       具体来说，定位是机器人需要知道自己在房间的准确位置；建图是机器人需要构建出对周围环境的详细地图；路径规划是机器人需要找到从当前位置到指定目标的最短路径。这些能力相辅相成，使得扫地机器人能够智能地完成清扫任务。

       SLAM技术不仅在扫地机器人中得到应用，还在自动驾驶、无人机、AR、智能机器人等领域发挥了重要作用。传感器主要分为激光雷达和视觉两大类。激光雷达在早期SLAM研究中较为常用，因其高精度和成熟的解决方案，但价格高、体积大、信息较少的缺点也明显。而视觉SLAM则使用摄像头作为主传感器，广泛应用于AR、自动驾驶等前沿领域。

       定位相关应用中，SLAM技术在自动驾驶中主要用于更精确地确定汽车的位置。在室外导航方面，尽管地图类App已经做得很好，但它们在车道识别、GPS失效区域的定位等方面仍有局限性。通过SLAM技术，可以实现更精准的室内定位，如在电商仓库的AGV机器人、移动机器人等场景。

       建图相关应用中，SLAM可以用于生成物体的三维模型或对较大场景进行三维重建。这在室内场景的三维重建、增强现实游戏、三维漫游等方面有广泛应用。

       关于SFM（结构从运动）和SLAM的区别，它们讨论的是相似的问题，但起源和应用领域不同。SFM强调实时性，通常离线处理，而SLAM更注重实时定位与地图构建。SFM处理的图像通常为同一场景在不同时间、不同相机拍摄的，而SLAM一般要求同一相机的序列图像或连续视频。SFM使用相机作为传感器，而SLAM除了相机外，还会集成惯导、激光雷达等传感器。

       对于快速对自由女神像进行3D重建，考虑到没有特殊硬件的情况下，选择SLAM可能是一个更合适的方式，因为它可以实时处理动态环境，而SFM通常更适合静态场景的重建。

       为了深入学习SLAM，可以参考以下资源：

       基于LiDAR的多传感器融合SLAM系列教程：LOAM、LeGO-LOAM、LIO-SAM

       系统全面的相机标定课程：单目/鱼眼/双目/阵列相机标定：原理与实战

       视觉SLAM必备基础课程：视觉SLAM必学基础：ORB-SLAM2源码详解

       深度学习三维重建课程：基于深度学习的三维重建学习路线

       激光定位+建图课程：激光SLAM怎么学？手把手教你Cartographer从入门到精通！

       视觉+IMU定位课程：视觉惯性里程计讲教程全部上线！IMU预积分/残差雅克比推导、边缘化约束、滑窗BA！

       手把手图像三维重建课程：系统学三维重建讲全部上线！掌握稠密匹配、点云融合、网格重建、纹理贴图！

       通过这些课程和资源，你可以快速入门SLAM，并在实践中掌握相关技能。

视觉SLAMORB-SLAM：让人Orz的SLAM

       ORB-SLAM，在视觉SLAM领域享有盛誉，其源码在GitHub上已有4.4K+Star，充分彰显了西班牙小哥的出色贡献。近期深入研究其论文并结合源码，体验了一番酸爽，发现它在SLAM领域确实有着独特的魅力与贡献。

       ORB-SLAM的核心框架由三个并发进程组成：跟踪、局部建图和回环检测，系统结构清晰，功能分明。跟踪是主进程，负责定位和跟踪相机运动，通过特征匹配实现定位与优化。局部建图则负责关键帧与地图点的插入、删除及局部优化。回环检测则通过搜索回环关键帧，实现位姿图优化，确保系统鲁棒性。

       特征提取是ORB-SLAM的关键之一，它采用ORB特征，兼顾性能与效率。与SURF、SIFT等相比，ORB提取速度快，每张仅需.3ms，适用于实时操作。ORB在FAST角点基础上加入方向信息，使BRIEF描述子旋转不变，同时利用图像金字塔和灰度质心法提取特征，实现尺度不变性。此外，通过网格划分与四叉树结构，ORB-SLAM确保特征点分布均匀，即使特征点不足，也可通过调整FAST阈值增加。

       单目初始化是ORB-SLAM的另一大亮点，它通过特征点匹配计算基础矩阵和单应矩阵，自动判断场景类型，无需人工设定模型。共视图与本质图结构则加强了关键帧间的联系，实现高效回环检测。相机重定位策略确保了系统在跟踪失败时能快速恢复，关键帧与地图点的删增策略则优化了系统性能。

       ORB-SLAM提供多样化的Bundle Adjustment方式，包括初始化阶段的全BA、跟踪过程的运动BA及局部建图阶段的局部BA，适应不同场景需求。整个系统庞大复杂，通过总结主要特性，虽有其精髓，但仍需深入研究，以充分理解其工作原理与优化策略。

       总之，ORB-SLAM在视觉SLAM领域展现出了其独特魅力与贡献，从其高效的特征提取到灵活的系统框架，再到多样化的优化策略，无不体现其在SLAM技术中的卓越地位。向所有SLAM领域的先驱者致以崇高的敬意。