SLAM

介绍 在 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 列非零。 ...

介绍 在三维点云处理与空间几何分析中，平面拟合是一项基础且关键的任务。给定一组三维点坐标 $\mathbf{p}_i = (x_i, y_i, z_i)^\top$，本文将探讨如何估计平面参数，并重点对比两种最常用的实现方法：PCA（主成分分析）法和线性最小二乘法。 平面方程的几种形式 在讨论具体算法前，先回顾一下平面的常见数学表达： 一般式（最常用） $$ ax + by + cz + d = 0 $$记 $\mathbf{n} = (a, b, c)^\top$ 为平面的法向量。若法向量为单位向量，即 $\|\mathbf{n}\| = 1$，则 $|ax + by + cz + d|$ 就是任意一点到平面的几何距离。 点法式 $$ \mathbf{n}^\top (\mathbf{p} - \mathbf{c}) = 0 $$ $\mathbf{c}$：平面上的一点（常取点云的质心） $\mathbf{n}$：单位法向量 参数形式 $$ \mathbf{p}(u, v) = \mathbf{c} + u\mathbf{e}_1 + v\mathbf{e}_2 $$其中 $\mathbf{e}_1, \mathbf{e}_2$ 为平面内两个相互正交的方向向量。 点到平面的距离 给定一个查询点 $\mathbf{p}$ 和拟合出的平面 $(\mathbf{n}, \mathbf{c})$，该点到平面的有符号距离为： $$ r = \mathbf{n}^\top (\mathbf{p} - \mathbf{c}) $$在点云配准及位姿优化任务中，$r$ 常常被用作观测残差进行最小化。 ...

介绍 BTC 是一种用于 3D 点云位置识别（Place Recognition） 的算法，核心思想是： 将点云体素化，检测平面 将点云投影到平面上，提取 二进制描述子（BinaryDescriptor） 用二进制描述子构建 稳定三角形描述子（STD, Stable Triangle Descriptor） 通过三角形匹配实现 回环检测 整个流程由 STDescManager 类管理，分为两个核心阶段： 描述子生成（GenerateSTDescs）：从点云提取 STD 描述子 回环搜索（SearchLoop）：在历史数据库中搜索匹配帧 数据结构 ConfigSetting — 配置参数 参数组 关键参数 说明 体素化 voxel_size_, voxel_init_num_ 体素大小、最小点数阈值 平面检测 plane_detection_thre_ 最小特征值阈值，低于此值判定为平面 平面合并 plane_merge_normal_thre_, plane_merge_dis_thre_ 法向量/距离阈值 投影参数 proj_plane_num_, proj_image_resolution_, proj_dis_min/max_ 投影平面数、分辨率、距离范围 二进制描述子 summary_min_thre_, line_filter_enable_ 最小占用数、线过滤开关 STD 生成 descriptor_near_num_, descriptor_min/max_len_, std_side_resolution_ 近邻数、边长范围、量化分辨率 回环检测 skip_near_num_, candidate_num_, similarity_threshold_, icp_threshold_ 跳帧数、候选数、相似度/ICP 阈值 BinaryDescriptor — 二进制描述子 1 2 3 occupy_array_: vector<bool> // 占用数组，表示各高度层是否有点 summary_: unsigned char // 占用层数（occupy_array_ 中 true 的个数） location_: Vector3d // 描述子在3D空间中的位置 本质：将一个空间区域按高度分层，用二进制位串描述该区域的几何结构。 ...

介绍 在将 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 系统中，IMU（惯性测量单元）扮演着至关重要的角色。IMU 提供高频（通常 200-400Hz）的角速度和线加速度测量，用于： 状态初始化：确定系统的初始重力方向、零偏和缩放系数 状态预测（前向传播）：在两帧 LiDAR 之间递推位姿，为滤波器提供预测值 点云去畸变（后向传播）：将一帧扫描周期内的点云补偿到同一时刻，消除运动畸变 ZLIO 中 IMU 处理的核心流程如下： 1 IMU 初始化 → 前向传播 (递推状态 + 记录位姿序列) → 后向传播 (点云去畸变) 涉及的核心文件： 文件 作用 imu_type.h IMU 数据结构定义 imu_process.h/cpp IMU 前向/后向传播与去畸变 ieskf.h/cpp IESKF 滤波器，包含 predict 步 frontend.h/cpp 前端模块，协调 IMU 初始化与数据同步 math_utils.h 李群李代数数学工具 IMU 数据结构 IMU 数据结构定义在 imu_type.h 中： 1 2 3 4 5 6 class Imu { public: Eigen::Vector3d acceleration = Eigen::Vector3d::Zero(); // 线加速度 (m/s^2) Eigen::Vector3d gyroscope = Eigen::Vector3d::Zero(); // 角速度 (rad/s) double timestamp = 0.0; // 时间戳 (s) }; IMU 数据以测量组 (MeasureGroup) 的形式与点云同步后一起处理，定义在 measure_group_type.h： ...

介绍 体素地图是 ZLIO 系统的核心数据结构之一，用于维护局部三维环境的紧凑表示。相比传统的 KD-Tree + 暴力搜索方案，体素地图通过哈希表实现 O(1) 的体素查找，并利用点集统计量 (PointCluster) 避免存储原始点云，大幅降低内存占用。 核心设计思想： 空间离散化：将连续的三维空间划分为固定大小的立方体（体素），每个体素由整数坐标 (x, y, z) 索引 增量统计：不存储原始点，而是维护每个体素内点的一阶矩（坐标和）和二阶矩（坐标外积和），可在线计算质心和协方差 平面检测：通过协方差矩阵的特征值分解判断体素内点是否构成平面，为 IESKF 提供点到平面的观测约束 涉及的核心文件： 文件 作用 voxel_map_manager.h/cpp 体素地图数据结构与管理接口 lio_zh_voxel_model.h 基于体素的点到平面观测模型 核心数据结构 体素位置 VOXEL_LOC 1 2 3 4 5 6 7 8 9 10 struct VOXEL_LOC { int64_t x, y, z; VOXEL_LOC(int64_t _x = 0, int64_t _y = 0, int64_t _z = 0) : x(_x), y(_y), z(_z) {} bool operator==(const VOXEL_LOC &other) const { return (x == other.x && y == other.y && z == other.z); } }; 设计要点： 使用 int64_t 而非 int，支持极大的坐标范围（避免负数取整溢出） 重载 operator== 以支持哈希表的相等比较 三个整数唯一确定一个体素在空间中的位置 哈希函数 VoxelHash 1 2 3 4 5 struct VoxelHash { size_t operator()(const VOXEL_LOC &v) const { return (((v.x * 73856093) ^ (v.y * 19349663) ^ (v.z * 83492791)) % 10000000); } }; 这是一个经典的空间哈希函数，三个大质数分别与坐标异或后取模，将三维整数坐标映射到一维哈希值。选择大质数可以减少哈希冲突。 ...

介绍 多机器人系统初始化时，各机器人的局部坐标系往往不一致，需要通过点云配准将它们统一到同一个世界坐标系。本文介绍一种三阶段配准策略：重力对齐消除姿态差异、RANSAC 全局粗配准找到大致变换、GICP 精配准优化精度。三个阶段层层递进，兼顾鲁棒性和精度。 三阶段配准策略 整个配准流程分为三个阶段，每个阶段解决不同层次的问题： graph TD A["原始点云 (body frame)"] --> B["Stage 1: 重力对齐"] B --> C["Stage 2: RANSAC 全局配准"] C --> D["Stage 3: GICP 精配准"] D --> E["变换矩阵 T (src → target)"] B -.- B1["估计重力方向\n消除 Roll/Pitch 姿态差异"] C -.- C1["FPFH 特征提取\n基于特征匹配的采样一致性估计"] D -.- D1["协方差矩阵加权\n迭代最近点优化"] 阶段 目标 方法 输出 Stage 1 消除 Roll/Pitch 姿态差异 水平平面法向量统计 旋转矩阵 $T_{gravity}$ Stage 2 全局粗配准 FPFH + RANSAC 粗配准变换 $T_{ransac}$ Stage 3 高精度精配准 GICP 协方差加权 精配准变换 $T_{fine}$ 数据准备 点云融合 每个机器人采集 init_frame_num（默认 16）帧点云并融合，提高配准鲁棒性。单帧点云的特征信息有限，多帧融合后点云密度更高，几何特征更丰富，配准成功率显著提升。 ...

介绍 本文主要介绍 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。 ...