C++

介绍 在 LIO 的 IESKF 点面观测中，每行雅可比只有前 6 列非零，因此 $H^\top H$ 与 $H^\top Z$ 的信息可压缩为 $6 \times 6$ 与 $6 \times 1$ 的汇总量。本文记录并推导由此产生的两种等价计算路径： 路径 简述 路径 A 累加 HTVH/HTVr → Cholesky 还原为 6 行合成观测 $H_s, Z_s$ → 走标准更新公式 路径 B 累加 HTVH/HTVr → 在更新阶段直接消费汇总量，不显式构造 $H$、$Z$、$K$ 两者观测模型相同，在保留迭代误差（IE）left + right 形式时，与逐点构建 $V$ 行 $H$ 的基准写法代数等价。下文以典型 18 维 LIO 状态为例，逐步展开推导。 1. 背景：18 维状态 vs 6 维观测 考虑 18 维误差状态： 下标 状态 LiDAR 点面观测 0–2 旋转 $\delta\theta$ 有 3–5 位置 $\delta p$ 有 6–8 速度 $\delta v$ 无 9–11 陀螺零偏 $b_g$ 无 12–14 加计零偏 $b_a$ 无 15–17 重力 $g$ 无 点到平面残差只依赖 $R, p$，每行雅可比 $h_i \in \mathbb{R}^{1 \times 18}$ 只有前 6 列非零。 ...

介绍 在将 VoxelSLAM 的 BTC（Binary Triangle Context）描述子模块移植到 ZLIO 项目时，我们经历了从"检测不出回环（STD 数量为 0）“到"性能极慢（searchLoop 单次 60+ ms）“再到"极速运行（单次 0.5 ms 以下）“的完整过程。本文档记录了这期间的所有致命 Bug、性能瓶颈以及最终的极致性能优化方案。 算法逻辑修复（Bug Fixes） YAML-cpp 节点引用失效 现象：所有 btc/ 前缀的参数读取都失败，使用了硬编码的旧默认值。 原因：ModuleBase 构造函数中直接用子节点覆盖了根节点 config_node_，由于 YAML-cpp 的 Node 是引用语义，原根节点析构导致子节点变为 Undefined。 修复：保留根节点，用单独的 prefix_node_ 存储子节点。 点到平面距离公式符号错误（致命） 现象：所有点的 dis 值异常大，全部被过滤掉。 原因：重构时凭直觉把点到平面距离错写成了 n·p + n·c，而正确数学推导应为 n·p - n·c。 教训：涉及数学公式时，必须逐项对照参考实现验证，绝不可凭直觉重写。 proj_image_high_inc 默认值过大 现象：summary_min_thre = 10，要求 10 层必须全部被占据。 原因：参数读取失败后采用了 0.5 的默认值，导致总层数缩减为 10，完全无法满足阈值。 教训：代码里的"兜底默认值"必须与原版保持一致，否则读取失败会引发隐性逻辑断层。 缺少分段局部最大值选择 现象：提取出的 binary 描述子数量远少于参考代码。 原因：重构时为了简化，把原作者的"分段局部找最大值"的网格操作，简化成了"全局固定阈值过滤”。 修复：还原 5x5 网格局部最大值选择的机制。 法向量过滤条件逻辑错误 现象：大量投影平面被错误跳过。 原因：把原版的 || 错写成了 &&。if (normal_diff.norm() < 0.3 && normal_add.norm() > 0.3) 导致条件几近无法触发。 修复：将 && 更正为 ||。 兜底逻辑缺失 现象：当没有检测到平面时，后续流程完全中断并崩溃。 原因：原作在没找到平面时会强行插入一个默认法向量的伪平面作为兜底，重构时丢失了这一行。 教训：异常处理和边缘情况兜底往往是原作者踩过坑加上的，不能轻易删除。 C++ 工程与性能优化（Performance Optimizations） 在算法逻辑打通后，我们发现代码耗时奇高，随后进行了深度底层优化，取得了 20 倍以上的加速。 ...

介绍 本文主要介绍 LIO SLAM 系统中的可视化工具 Pangolin 和日志工具 glog 的使用方法。 主要依赖 Pangolin：3D 可视化库 glog：Google 日志库 可视化 可视化功能在 visualize.cpp 中独立实现，创建单独的显示线程，使用原子布尔进行线程控制： 1 std::atomic<bool> exit_flag_{false}; 头文件 1 2 #include <pangolin/pangolin.h> #include <GL/gl.h> 原子布尔 原子布尔需要进行读写操作： 1 2 exit_flag_.store(true); // 写入 exit_flag_.load(); // 读取 原子布尔有锁机制，会强制同步线程的缓存值到主内存。 可视化窗口设置 1 pangolin::CreateWindowAndBind("ZLIO Visualize", 1920, 1080); 投影参数配置： 1 2 3 pangolin::OpenGlRenderState s_cam( pangolin::ProjectionMatrix(1920, 1080, 5000, 5000, 960, 540, 1, 1000), pangolin::ModelViewLookAt(0, -10, 20, 0, 0, 0, pangolin::AxisY)); 参数 值 说明 ProjectionMatrix 第1/2个参数 1920, 1080 屏幕分辨率 ProjectionMatrix 第3/4个参数 5000, 5000 fx, fy 焦距 ProjectionMatrix 第5/6个参数 960, 540 cx, cy 主点（屏幕中心） ProjectionMatrix 第7/8个参数 1, 1000 near, far 显示深度 ModelViewLookAt 第1/2/3个参数 0, -10, 20 相机位置 ModelViewLookAt 第4/5/6个参数 0, 0, 0 相机观察目标点 ModelViewLookAt 第7个参数 pangolin::AxisY Y 轴向上 按钮设置 创建左侧菜单面板： ...

介绍 打算实现读书时应该完成的工作，写一个自己的 SLAM，来对 SLAM 的实现有一个更深层次的了解，项目参考了 FAST_LIO2、Voxel-SLAM、lightning-lm 数据结构定义和数据缓存 首先是对数据结构体进行创建，点、点云、IMU、同步后的数据体，为了结构清楚，我单独写一个头文件common.h方便查看和管理 点和点云通过PCL注册，hku是通过PointXYZNomal的曲率存储时间戳，lm是通过重新注册一个 IMU 包含时间戳、加速度计、陀螺仪 同步后的数据measuregroup 包含点云 两帧点云之间的所有IMU 之所以单独定义结构体，更多的是增强独立性，这样其他如ros或者雷达调用接口就可调用该里程计 数据缓存 imu_buffer_.push_back(imu) lidar_buffer_.push_back(cloud) 点云创建和注册 1 2 3 4 5 6 7 8 9 10 11 // PCL兼容点类型 (必须在全局或pcl命名空间定义) struct PointXYZIT { PCL_ADD_POINT4D PCL_ADD_INTENSITY double timestamp; PointXYZIT() {} EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; POINT_CLOUD_REGISTER_POINT_STRUCT( PointXYZIT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)) 注意 每个数据在存储时会用到 std::lock_guard<std::mutex> lock(mtx_buffer_); 因为回调函数出发存储，避免和其他位置同时操作缓存导致错误 说明：std::lock_guard 能自动解锁，在作用域结束、return、异常时自动解锁，作用域是从声明开始到所在的大括号结束 ROS接口 创建ROS接口实现，算法模块与ROS系统解耦，虽然现在基本是在ROS系统中运行，但是解耦后更方便移植在ROS1和ROS2中，以及万一之后在其他嵌入式上运行 主要是对应的话题回调函数中调用slam借口实现 RosApp::CloudCallback RosApp::IMUCallback 开始我自己创建的点云结构体，没有用PCL，所以不是很理解time_buffer的作用，三个参考项目都加了time_buffer，time_buffer中存储的是话题的时间戳，在时间同步时作为点云的扫描开始时间，那为什么不直接在点云存储时间戳，我就省略了time_buffer 我知道了，因为他们创建的点云结构体没有时间戳，我是自己定义的点云，考虑到点云只在同步时才需要时间，确实没必要多一个成员 fastlio对livox雷达会进行IMU和雷达的时间软同步处理，不是很知道区别对待的原图，其他的算法似乎没有 参考算法似乎对不同雷达还会有不同的处理，但是最后还是只用到了XYZI，以及用curvature记录不同雷达的点云时间偏移，lm则是用的动态平均值处理的点云时间偏移，计划先用lm的方法，后面再两个都加上吧 lm在IMU回调中还额外EKF处理了，估计是为了高频输出，后续再进行添加吧 回调函数中还通过跳点逻辑进行了降采样，我之前都是用的体素滤波，非常耗时 ...

点云配准方法介绍 常见的有 ICP、GICP、NDT 等，快捷实现基本基于 PCL 库中自带的点云处理实现。 GICP 原理 Generalized Iterative Closest Point (GICP) 是一种改进的 ICP 算法，通过协方差矩阵优化对应点匹配，提升配准精度。 头文件 1 2 3 4 5 6 7 #include <pcl/point_types.h> // 点类型定义 #include <pcl/point_cloud.h> // 点云容器 #include <pcl/registration/gicp.h> // GICP 算法核心 #include <pcl/registration/icp.h> // ICP 基类（部分功能依赖） #include <pcl/features/normal_3d.h> // 法向量估计（GICP 需要） #include <pcl/filters/voxel_grid.h> // 下采样（可选） #include <pcl/io/pcd_io.h> // PCD 文件读写 参数配置 参数 值 说明 setMaximumIterations 500 最大迭代次数，达到后停止配准 setTransformationEpsilon 1e-10 变换矩阵变化的收敛阈值，小于此值认为已收敛 setEuclideanFitnessEpsilon 0.0001 点到点欧氏距离误差的收敛阈值 setMaxCorrespondenceDistance 50.0 对应点搜索半径上限，超过则不考虑 setCorrespondenceRandomness 20 每次迭代随机采样对应点数量，影响计算效率和精度 setRANSACOutlierRejectionThreshold 0.05 RANSAC 离群点阈值，超过此距离的点被标记为异常值 setRANSACIterations 1000 RANSAC 迭代次数 setTransformationRotationEpsilon 1e-10 旋转矩阵变化的收敛阈值 setUseReciprocalCorrespondences false 是否使用互对应关系（source-target双向匹配） 测试结果 通过将目标点云变换作为原始点云进行测试，实际变换为 T=[10, 10, 10]m, R=[90, 0, 0]deg。 ...

介绍Gtsam Gtsam（General and Tunable Structure-from-Motion）是一个用于结构化从运动学（SFM）的优化库。Gtsam提供了一种基于图优化的算法，用于解决各种类型的结构化从运动学问题。Gtsam支持多种优化算法，如Levenberg-Marquardt、Gauss-Newton、Dogleg、Gauss-Newton-Dogleg等，以及多种优化目标，如最小二乘、最小 Trust Region等。Gtsam提供了多种数据结构，如变量、因子、 优化问题等，以及多种工具函数，如线性方程求解、矩阵运算、向量运算等。（AI生成） Gtsam库 https://github.com/borglab/gtsam Gtsam的使用 快速使用 引入头文件 1 2 3 4 5 6 #include <gtsam/slam/PriorFactor.h> // 先验因子，为Pose提供绝对约束（如首帧初始位姿） #include <gtsam/nonlinear/ISAM2.h> // 增量平滑与建图优化器，支持增量式求解因子图 #include <gtsam/nonlinear/Values.h> // 优化变量值的容器，存储键值对形式 #include <gtsam/slam/BetweenFactor.h> // 里程计/回环约束因子，表示两Pose间相对变换 #include <gtsam/inference/Symbol.h> // GTSAM符号系统，用于生成因子图变量唯一键 ，当直接用帧创建key时无需引入 #include <gtsam/geometry/Pose3.h> // 3D位姿（Rot3旋转+Point3位置），表示机器人姿态 创建变量（Values） 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 // 使用 Symbol 创建唯一键：'x' + 0 = 键 "x0" gtsam::Key key1 = gtsam::symbol('x', 0); gtsam::Key key2 = gtsam::symbol('x', 1); //直接用帧数也可以 gtsam::Key key1 = 0; // 直接用整数 // 创建 Pose3 位姿（旋转 + 平移） gtsam::Rot3 rotation = gtsam::Rot3::Ypr(0.0, 0.0, 0.0); // yaw, pitch, roll gtsam::Point3 translation(0.0, 0.0, 0.0); // x, y, z gtsam::Pose3 pose1(rotation, translation); // 存入 Values 容器 gtsam::Values initial_values; initial_values.insert(key1, pose1); // 初始值 initial_values.insert(key2, pose1); // 第二帧初始值 创建噪声模型（NoiseModel） 1 2 3 4 5 6 7 8 9 // 高斯噪声模型（6维：旋转3维 + 平移3维） gtsam::Vector6 variance = (gtsam::Vector6() << 0.01, 0.01, 0.01, 0.1, 0.1, 0.1).finished(); gtsam::SharedNoiseModel noise = gtsam::noiseModel::Gaussian::Covariance( gtsam::Matrix6::Identity() * variance.asDiagonal()); // 或使用对角噪声模型 auto odometryNoise = gtsam::noiseModel::Diagonal::Sigmas( (gtsam::Vector6() << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); auto priorNoise = gtsam::noiseModel::Isotropic::Sigma(6, 0.01); 创建因子（Factor） 1 2 3 4 5 6 7 8 gtsam::NonlinearFactorGraph graph; // 先验因子（为第一帧提供绝对约束） graph.add(gtsam::PriorFactor<gtsam::Pose3>(key1, pose1, priorNoise)); // 里程计因子（两帧之间的相对变换） gtsam::Pose3 odom_delta = pose1.between(pose2); // 计算相对变换 graph.add(gtsam::BetweenFactor<gtsam::Pose3>(key1, key2, odom_delta, odometryNoise)); 创建 ISAM2 优化器 1 2 3 4 5 gtsam::ISAM2Params params; params.relinearizeThreshold = 0.01; // 重新线性化阈值 params.relinearizeSkip = 10; // 跳帧数 gtsam::ISAM2 isam(params); isam.update(graph, initial_values); // 首次更新 增量优化 1 2 3 4 5 6 7 8 9 10 11 12 13 // 添加新帧时 gtsam::Key key3 = gtsam::symbol('x', 2); initial_values.clear(); initial_values.insert(key3, pose_guess); // 新帧初始值 // 更新优化器（增量式，只优化新节点） isam.update(graph, initial_values); // 获取优化结果 gtsam::Values result = isam.calculateEstimate(); // 读取特定节点 gtsam::Pose3 optimized_pose = result.at<gtsam::Pose3>(key3); 完整示例 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 #include <gtsam/slam/PriorFactor.h> #include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/Values.h> #include <gtsam/slam/BetweenFactor.h> #include <gtsam/inference/Symbol.h> #include <gtsam/geometry/Pose3.h> int main() { // 1. 创建变量 gtsam::Key key1 = gtsam::symbol('x', 0); gtsam::Key key2 = gtsam::symbol('x', 1); gtsam::Pose3 pose1; // 单位位姿 gtsam::Pose3 pose2(gtsam::Rot3::Ypr(0.1, 0, 0), gtsam::Point3(1, 0, 0)); // 2. 创建噪声模型 auto priorNoise = gtsam::noiseModel::Isotropic::Sigma(6, 0.01); auto odomNoise = gtsam::noiseModel::Isotropic::Sigma(6, 0.05); // 3. 创建因子图 gtsam::NonlinearFactorGraph graph; graph.add(gtsam::PriorFactor<gtsam::Pose3>(key1, pose1, priorNoise)); // 先验 graph.add(gtsam::BetweenFactor<gtsam::Pose3>(key1, key2, pose1.between(pose2), odomNoise)); // 里程计 // 4. 创建初始值 gtsam::Values initial_values; initial_values.insert(key1, pose1); initial_values.insert(key2, pose2); // 5. ISAM2 优化 gtsam::ISAM2 isam; isam.update(graph, initial_values); gtsam::Values result = isam.calculateEstimate(); // 6. 读取结果 gtsam::Pose3 optimized_pose2 = result.at<gtsam::Pose3>(key2); std::cout << "Optimized pose: " << optimized_pose2 << std::endl; return 0; } 注意事项 每一个Key必须有初始值 一个图必须创建先验约束 不能有孤立节点，必须与其他节点建立约束 优化效果 原始x轴位移 ...