1.EPNP学习笔记
2.实操指南：ORB-SLAM3的闭环闭环编译运行
3.视觉SLAM综述
4.LVI-SAM：紧耦合的激光视觉惯导SLAM系统（Tixiao Shan新作，已开源）
5.这家提供数据闭环完整链路的源码企业，已拿下多家头部主机厂定点
6.hdl_graph_slam|后端优化|hdl_graph_slam_nodelet.cpp|源码解读(四)

EPNP学习笔记

       在视觉SLAM中，检测追踪、闭环闭环闭环检测以及重定位经常遇到Perspective-n-Point (PnP)问题。源码PnP问题的检测洛奇源码已知条件包括：在世界坐标系中的n个3D特征点坐标，与这些点在图像上的闭环闭环对应2D参考点坐标，以及摄像头的源码内参。解决PnP问题的检测目的在于确定摄像头的位姿。

       在EPnP算法中，闭环闭环我们引入了控制点的源码概念。对于三维坐标系内的检测任意一个3D点，可以通过4个不共面的闭环闭环控制点进行线性表示。EPnP算法首先在世界坐标系内寻找四个控制点，源码这四个点满足特定条件，检测使得任意一个世界坐标系内的特征点与相应的控制点之间存在线性关系。具体来说，对于世界坐标系内的任意特征点，存在对应的控制点，使得特征点的坐标可以通过控制点的坐标进行线性组合。

       控制点的选择可以是任意的四个点，理论上需要满足特定的条件以确保C矩阵可逆。实践中，EPnP算法选择一个质心坐标和三个主方向向量作为四个控制点，以获得更好的稳定性。

       选择3D参考点的质心为第一个控制点，并构建相应的矩阵。接着，通过计算特征值和特征向量来确定剩余的三个控制点。在世界坐标系下，我们已经获得了四个控制点，接下来可以通过这些控制点和3D物方点云，利用EPnP算法中的公式（5）计算每个点对应的4个齐次重心坐标。

       为了计算系数，我们需要将控制点转换到相机坐标系下，并通过相机的内参矩阵进行投影。通过相机的xv6源码下载成像原理，我们知道从世界坐标系到相机坐标系存在转换关系。EPnP算法中，通过控制点和3D物方点云，以及相机坐标系下的2D像点坐标，应用公式（）的矩阵方程进行计算。

       公式（）是一个个未知数构成的向量方程，描述了控制点在相机坐标系下的坐标和相应的投影参数。利用矩阵的奇异值分解（SVD）方法，我们可以求解出控制点在相机坐标系下的坐标。具体过程包括求解特征值和特征向量，确定零特征值对应的特征向量，进而计算出控制点的坐标。

       EPnP算法在求解过程中，不需要对2D投影坐标进行归一化处理，这与直接线性变换（DLT）方法不同。在求解中，我们关注的是控制点的相对位置关系，引入相对位置约束条件，例如公式（）和（），来进一步确定控制点的坐标。

       通过以上步骤，我们能够利用EPnP算法解决PnP问题，确定摄像头相对于世界坐标系的位姿。整个过程不仅在理论层面详细阐述了EPnP算法的实现，而且通过具体的数学公式和步骤，展示了如何从特征点坐标到摄像头位姿的转换过程。通过分析控制点的选择、控制点坐标求解以及最终的相机位姿计算，EPnP算法为视觉SLAM中的追踪、闭环检测以及重定位提供了有效的方法。

实操指南：ORB-SLAM3的编译运行

       ORB-SLAM3，作为视觉SLAM领域的明星技术，因其强大的多相机支持和卓越的性能，是机器人建图与定位的必备技能。为了帮助大家深入了解和实践，我们将分享一系列深度技术内容，淘宝客平台源码程序从理论到实践，包括算法流程、关键模块解析以及编译与运行指导。

       首先，我们会一步步引导您进行ORB-SLAM3的编译和运行，解决常见问题。例如，您可能遇到的环境配置问题，如安装GLEW库和处理找不到libpango_image.so的错误。指南中详细介绍了下载代码的步骤，以及针对不同传感器的运行方法，如使用EuROC和TUM VI数据集进行测试。

       在非ROS环境中，我们提供了详细的操作指南，包括使用微型飞行器采集的EuROC数据集和TUM VI数据集的运行命令。对于ROS环境，也给出了对应的数据包播放命令，让您能在ROS环境中无缝运行ORB-SLAM3。

       尽管本文仅是系列课程的开端，但它为初学者提供了一份实用的编译运行指南。后续还将深入探讨ORB-SLAM3的各个核心模块，如跟踪线程、局部建图线程和闭环检测。所以，如果您对视觉SLAM感兴趣，这将是一个很好的起点。

视觉SLAM综述

       视觉SLAM：前沿技术概览

       SLAM，即同步定位与建图，是机器人领域中的核心技术，目标是实时构建环境地图并定位。其中，视觉SLAM（Visual Simultaneous Localization and Mapping, V-SLAM）仅依赖相机，其核心组件包括视觉里程计、后端优化、闭环检测和建图四个关键模块。

视觉里程计：主要负责姿态估计，分时日内源码通过实时追踪特征点，如 MonoSLAM 使用单目EKF实现，但受制于场景限制。

后端优化：如PTAM引入关键帧优化，通过非线性优化提升轨迹和地图一致性，但对场景大小有一定的要求。

闭环检测：识别轨迹重叠，如ORB-SLAM利用ORB特征进行回环检测，显著增强鲁棒性。

建图：根据轨迹构建环境地图，如ORB-SLAM2采用三个并行线程处理，提供高精度和立体效果，但对CPU性能有一定需求。

       SLAM技术可分为间接法和直接法。间接法，如MonoSLAM和PTAM，通过预处理测量数据优化几何误差；而直接法，如DTAM和LSD-SLAM，更注重密集处理和实时性能，但计算量较大。深度学习在SLAM中扮演重要角色，如CNN-SLAM通过深度网络增强特征学习，LIFT则提升特征匹配精度。

       最新进展

       CNN-SLAM利用Spatial Transformers和soft argmax函数，通过孪生训练架构实现端到端可微性，显著优于传统方法如SIFT[]。

       UnDeepVO采用无监督深度学习，实现了单目相机的位姿估计和深度恢复，特别在大规模数据集如KITTI上表现出色[]。

       UnDeepVO的位姿估计和深度预测分别采用VGG CNN和encoder-decoder结构，提供高效而精确的解决方案[]。

       未来SLAM的发展趋势包括主动SLAM、语义SLAM和多传感器融合，以应对复杂环境和高精度定位需求[]。随着技术的不断进步，视觉SLAM将在增强现实、拼团系统源码开发机器人导航和自主驾驶等领域发挥更大作用。

       参考文献

       [1] Durrant-Whyte, H., Bailey, T.

       [2] Fuentes-Pacheco, J., et al.

       [3] 陈常, 朱华, 由韶泽.

       [4] Nister, D., et al. "Visual odometry." CVPR .

       [5] 高翔. 《视觉SLAM十四讲》[M]. 电子工业出版社, .

       [6] 赵洋等. "基于深度学习的视觉SLAM综述." 机器人 .6():-.

       ...

       注：以上内容概述了视觉SLAM的原理、关键技术和最新进展，展示了这项技术如何在不断演进中提升性能和应用范围。未来，视觉SLAM将继续向着更高精度和更广泛的应用场景迈进。

LVI-SAM：紧耦合的激光视觉惯导SLAM系统（Tixiao Shan新作，已开源）

       激光视觉惯导融合系统LVI-SAM设计旨在提供实时、高精度、鲁棒的状态估计与地图构建。该系统架构于因子图之上，由视觉惯导子系统和激光惯导子系统组成。通过紧耦合方法，激光惯导系统为视觉惯导提供初始化，而视觉惯导利用激光雷达测量增加深度信息，提高定位精度。同时，激光惯导系统采用视觉惯导估计作为帧匹配的初始值，实现闭环检测优化。LVI-SAM系统在数据集上表现优异，尤其在纹理较少或特征稀缺场景中展现出强大鲁棒性。

       在基于雷达的SLAM系统中，获取远距离环境细节的同时，容易在缺少结构信息场景中失效。相比之下，视觉系统在纹理丰富的场景中表现良好，但对光照变化、快速移动和初始化敏感。激光与视觉融合，结合惯导，可以显著提升精度与鲁棒性。惯导可去除点云运动畸变，在特征稀少环境中提供一定时间的支撑，并恢复尺度信息。

       LVI-SAM系统集成了雷达视觉惯导里程计（LIS系统）与视觉惯导系统（VIS系统），两者可独立运行或耦合使用。VIS系统通过雷达帧恢复深度信息，执行视觉特征跟踪，并利用优化后的视觉重投影误差和IMU测量误差进行视觉里程计初始化。激光里程计通过检测线面特征与特征图对齐实现定位。闭环检测通过视觉词袋模型识别匹配帧，优化系统状态。

       LVI-SAM系统的独特之处在于其融合了VIO和LIO领域最新技术，实现了高精度激光视觉惯导系统。系统设计旨在减少数据交换，提高效率，为激光惯导视觉融合系统的发展奠定基础。

       完整激光视觉惯导SLAM系统设计如下：

       系统接收3D激光点云、单目图像与IMU信息作为输入。VIS系统处理图像与IMU信息，雷达点云作为可选输入。视觉里程计通过最小化IMU与视觉测量残差实现。激光里程计则通过最小化检测的线面特征至特征图距离得到。实时特征图保存在滑窗中，状态估计问题公式化为MAP问题。使用iSAM2优化因子图中的约束，包括IMU预积分、视觉里程计、激光里程计与闭环约束。

       视觉惯导系统中，初始化与特征深度估计是关键步骤。通过优化VIO系统可能因非线性导致的初始化问题，利用LIS系统提供的状态X与IMU偏置b作为初始值，显著提高初始化速度与鲁棒性。深度特征关联与失败检测机制确保系统在不同场景下稳定运行。

       雷达惯导系统基于LIO-SAM，通过因子图进行全局位姿优化。系统整合四种约束，通过闭环检测优化，提供鲁棒性与实时性能。初始值估计与失败检测策略增强系统在复杂场景下的表现。

       实验结果显示，LVI-SAM系统在多个数据集上表现出色，尤其在纹理缺乏与特征稀少场景中，展现了其强大鲁棒性与高精度。

这家提供数据闭环完整链路的企业，已拿下多家头部主机厂定点

       在自动驾驶迈向数据驱动的浪潮中，数据闭环的重要性日益凸显。小鹏、理想等头部汽车品牌正在加速城市NOA的升级，这需要高效算力、精准算法和大量有效数据的闭环支持。然而，实现量产车数据闭环的完整链路在国内尚面临诸多挑战，觉非科技凭借其独特的解决方案脱颖而出。

       觉非科技作为少数能提供完整量产车数据闭环解决方案的企业，近期已成功获得多家头部主机厂的定点，计划于年进行量产。其成功的关键在于，他们理解数据闭环对智能驾驶迭代的决定性影响，以及如何解决数据采集、标注、处理和传输等环节的复杂工程问题。例如，他们利用Transformer算法特性，实现动态目标和环境感知数据的闭环，通过实时BEV感知与高精度地图的互补，提升驾驶体验和安全性。

       觉非科技的核心竞争优势在于其与电子导航地图企业的战略合作，确保数据合规采集；丰富的自动驾驶数据集和工程化实践；高效的自动化数据处理平台；以及独特的SLAM建图能力。他们的方案以“重感知、轻地图”为核心，通过实时数据更新生成动态高精度地图，降低制图成本，满足智能驾驶快速上车的需求。

       总的来说，觉非科技通过整合数据闭环与高阶感知技术，构建了覆盖高快道路、城市道路及停车场的全方位解决方案，为自动驾驶的量产落地提供了有力支持，成为了在这个领域的佼佼者。

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技术，全称Simultaneous Localization And Mapping，是机器人领域中不可或缺的基石，它致力于解决路径规划、定位误差累积以及提供全局参考。这个技术可细分为激光SLAM和视觉SLAM两大类，其中Cartographer框架以其独特流程展现了其核心：数据预处理（同步传感器、处理数据），前端的LIO（利用pose-extrapolator进行姿态预测，点云注册确保精确性），后端Refinement通过G2o或Ceres等优化库进行位姿图优化，闭环检测（LoopClosure）则用于地图的自我校准，而地图构建与显示则是整个过程的最终输出。

       构建SLAM地图的方法多种多样，包括语义地图（理解环境内容的抽象表示）、概率更新的黑白灰地图（实时反映不确定性）、三维点云，以及基于TSDF（Truncated Signed Distance Field）的栅格地图，它们各有优势，适应不同的应用场景和精度需求。

       2D栅格地图的生成与维护，涉及概率更新算法，如Bresenham算法实现精确的线绘制，这种简洁而高效的策略在地图构建中占有重要地位。

       激光SLAM技术，如Gmapping（采用RBpf粒子滤波），尽管精度高，但处理高分辨率数据时有挑战，其开源实现由Ros Perception提供；Hector-SLAM则基于优化方法，无需里程计，但对雷达频率和快速转向敏感，其GitHub仓库tu-darmstadt-ros-pkg中提供了详细资料。Karto则作为图优化SLAM的代表，适用于大面积建图，特别强调回环检测的correlative scan matcher技术。

       在回环检测和地图构建过程中，如Cartographer，通过localMap的生成，利用候选匹配并根据score确定闭环，这一过程在Cartographer和相关论文（Hess et al. ）中有详尽探讨。

       LOAM和LeGO-LOAM作为经典SLAM方法，前者在Kitti odometry中广泛应用，而LeGO-LOAM在其基础上增加了回环功能，特别适合低功耗设备。此外，LIO-SAM在速度和精度上都有显著提升，通过不同模块的组合，如LIO-odom, LIO-GPS, 和 LIO-SAM，提供了更灵活的解决方案，GitHub仓库链接为LIO-SAM。

       在算法层面，hdl_graph_slam实现了GPS融合的图优化SLAM，而SegMap则通过3D点云分割和语义特征提取，为建图提供了新的视角。这些技术的不断迭代，得益于学术界如SuMa、SuMa++和SegMap等项目的创新，以及相关论文的发表，如Efficient Surfel-Based SLAM和IROS 的SuMa++。

       总的来说，SLAM技术在不断进化中，既要处理传感器融合、精度提升，又需关注实时性和鲁棒性，而这些技术的发展与优化，为自动驾驶、机器人导航等领域的进步奠定了坚实基础。关注机器人和自动驾驶的Sky Shaw，将持续分享前沿动态和实用经验。