SLAM不是单函数可实现的系统，而是感知-建模-优化闭环；需基于ORB-SLAM2等成熟框架集成，C++用于调用、扩展与调度，而非从零造轮子。
SLAM 不是“用 C++ 实现一个函数就能跑起来”的东西，它是一整套感知-建模-优化闭环，直接手写完整系统既不现实，也容易陷入数值不稳定、帧间漂移累积、回环失败等深坑。实际工程中，你应该基于成熟开源框架做集成和定制，C++ 是用来调用、扩展和部署的工具，不是从零造轮子的语言选择。
为什么别从零写 ORB-SLAM2 或 LIO-SAM 的核心逻辑
这些算法依赖大量精密设计：特征提取的亚像素精度、SE(3) 李代数下的位姿图优化、g2o 或 ceres 的稀疏雅可比构造、关键帧管理策略、地图点共视关系维护……哪怕一个 cv::undistortPoints 调用参数错（比如没传入正确的 distortion_coefficients），都会导致特征匹配失效；而 ceres::Problem::AddResidualBlock 中 residual functor 的导数写错半行，整个 BA 就发散。
- 真实传感器数据（尤其是激光雷达）存在运动畸变，必须在
LidarOdometry前做时间戳对齐和插值，这不是靠std::vector::push_back能解决的 -
ORB-SLAM2的Tracking::TrackWithMotionModel()依赖上一帧的velocity估计，但该 velocity 实际来自两帧间SE3变换的李代数平均——初学者常误当成简单欧氏速度向量来用 - ROS 环境下
tf2坐标系广播频率和 SLAM 输出频率不一致时，tf2::Buffer::lookupTransform("map", "base_link", ...)会抛出tf2::ExtrapolationException，而不是静默失败
怎么用 C++ 正确接入 ORB-SLAM2 或 FAST-LIO2
核心动作是：加载配置、喂数据、取结果、处理异常。C++ 在这里负责粘合与调度，不是数学推导。
- 读配置必须用
cv::FileStorage（ORB-SLAM2）或YAML::Node（FAST-LIO2），不能硬编码内参；"Camera.fx"和"camera.fx"是两个不同 key - 图像输入要确保格式严格为
CV_8UC1（灰度），传入cv::Mat前检查img.type() == CV_8UC1，否则mpTracker->GrabImageMonocular()内部会静默跳过特征提取 - 激光雷达点云需转成
sensor_msgs::PointCloud2→pcl::PointCloud<:pointxyzi></:pointxyzi>→ 再按帧拆成std::vector<:vector3d></:vector3d>输入到FAST-LIO2的livox_ros_driver回调中；中间任何一步resize()或reserve()失误，都会触发std::bad_alloc - 获取位姿时，
ORB-SLAM2返回的是cv::Mat形式的Tcw（世界到相机），而下游导航常需geometry_msgs::PoseStamped，必须用cv::Rodrigues()提取旋转向量再转四元数，不能直接拷贝矩阵元素
g2o 自定义顶点/边时最容易崩的三个地方
这是少数需要你真正在 C++ 层写新类的地方，但每处都卡在模板和内存模型上。
立即学习“C++免费学习笔记（深入）”；
- 自定义顶点继承
g2o::BaseVertex>后，oplusImpl()必须用李代数加法（se3::exp(v)× 当前位姿），不是estimate_ += v—— 后者会导致优化过程严重偏离流形 - 边的
linearizeOplus()返回的雅可比维度必须严格匹配：若误差维度是 6（SE3error），则雅可比应为6×6（顶点）+6×6（另一顶点），少一维就会触发g2o::OptimizationAlgorithmLevenberg::solve()内部断言失败 - 所有自定义类型必须在 main 之前注册：
g2o::Factory::instance()->registerType("VERTEX_SE3:QUAT", new g2o::VertexSE3WriteG2o();，漏掉这句，g2o::SparseOptimizer::load()会静默返回 false
真正花时间的从来不是“怎么写”，而是“怎么让数据对得上、让坐标系不打架、让优化不爆炸”。SLAM 的 C++ 工作流里，gdb 下断点看 vertex->estimate() 是否 NaN，比翻论文推公式更常发生。
