博客

介绍 在技术博客的写作中，如何优雅地展示代码、数学公式和复杂的逻辑流程，直接决定了文章的阅读体验。传统的博客往往受限于静态生成器的默认支持，导致代码高亮生硬、公式解析错误或无法原生支持流程图。 经过一系列的折腾与优化，我总结了一套在 Hugo (PaperMod 主题) 下近乎完美的解决方案： 代码可视化：摒弃传统的 Chroma/Highlight.js，引入 Shiki，实现与本地 VS Code 完全一致的语法解析与 Dark+ 主题高亮。 数学公式可视化：集成 KaTeX，实现轻量级、极速的 LaTeX 公式渲染。 流程图可视化：原生集成 Mermaid.js，用纯文本绘制可交互的架构图与流程图。 本文将详细记录这些特性的实现效果与使用经验。 代码可视化：VS Code 原生体验 (Shiki) 以前的代码高亮要么颜色不够精确，要么对现代语言（如 C++ 的复杂模板）支持不佳。我们引入了 Shiki，它基于 VS Code 原生的 TextMate 语法解析器，能够在网页上 1:1 还原你在编辑器里看到的绝美高亮。 核心亮点 精准解析：依靠真实的 AST 语法树和正则，完美区分函数、变量、宏定义。 暗黑沉浸：采用了纯正的 dark-plus 主题，加上现代编程等宽字体（JetBrains Mono / Fira Code）。 零前端负担：在线上通过 GitHub Actions 在 Node.js 环境下静态预渲染，浏览器完全不需要加载庞大的 JS 库；而在本地 hugo server 模式下，则通过 CDN 动态实时加载 Shiki 引擎，实现所见即所得。 演示效果 真正的 C++ 算法代码： 1 2 3 4 5 6 7 8 9 10 // 计算两个描述子的相似度 double binary_similarity(const BinaryDescriptor &b1, const BinaryDescriptor &b2) { int same_num = 0; for (int i = 0; i < b1.occupy_array_.size(); ++i) { if (b1.occupy_array_[i] == b2.occupy_array_[i]) { same_num++; } } return (double)same_num / b1.occupy_array_.size(); } 注意：如果是带有大量中文解释的伪代码，请务必使用 text 语言标识，以免触发 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 列非零。 ...

介绍 在三维点云处理与空间几何分析中，平面拟合是一项基础且关键的任务。给定一组三维点坐标 $\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 倍以上的加速。 ...

介绍 ROS 服务（Service）是一种请求-响应模式的通信机制，适合一次性查询或控制操作（区别于持续发布的 Topic）。 1 2 3 4 5 客户端 (Client) 服务端 (Server) | | |--- 请求 (Request) ------------->| | | 处理逻辑 |<-- 响应 (Response) -------------| 原理 通信模型 ROS 服务基于 TCP/IP 的同步 RPC（远程过程调用）机制： 服务端在 ROS Master 注册服务名和地址 客户端通过 ROS Master 查找服务端地址 客户端与服务端建立 TCP 直连 客户端发送请求，阻塞等待响应 服务端处理完成后返回响应，客户端解除阻塞 graph LR A[ROS Master<br>注册中心] -->|① 注册服务| B[Server<br>服务端] A -->|② 查找服务| C[Client<br>客户端] B <-->|③ TCP 直连| C 与 Topic 的对比 特性 Topic（话题） Service（服务） 通信模式 发布-订阅（异步） 请求-响应（同步） 是否阻塞 否，持续发布 是，等待响应 数据流向 单向（发布者→订阅者） 双向（请求+响应） 适用场景 持续数据流（传感器、位姿） 一次性操作（查询、控制） 典型例子 点云、里程计、地图 获取变换、保存文件、重置状态 底层实现 服务定义文件 .srv 编译后自动生成 C++/Python 的请求和响应类 通信协议：roscpp 使用 TCPROS，rospy 使用 XML-RPC + TCPROS 服务端回调函数在 ROS spinner 线程中执行，多个请求会排队处理 作用 适用场景 状态查询：获取某个变量的当前值（如本项目的变换矩阵查询） 配置修改：运行时修改参数（如切换模式、调整阈值） 触发动作：执行一次性操作（如保存地图、重置系统） 同步确认：需要确认操作结果（如配准是否成功） 本项目中的应用 sequenceDiagram participant Other as 其他节点 participant MRLO as MRLO 节点 Other->>MRLO: get_transform(robot_id=1) MRLO->>MRLO: 从 drone_gicp_poses_ 读取 MRLO-->>Other: transform (Pose) Other->>Other: 使用变换矩阵转换坐标系 外部节点（如导航、规划模块）需要知道各机器人相对于 robot0 的坐标变换，通过服务按需获取，避免不必要的持续订阅。 ...

介绍 在 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); } }; 这是一个经典的空间哈希函数，三个大质数分别与坐标异或后取模，将三维整数坐标映射到一维哈希值。选择大质数可以减少哈希冲突。 ...

介绍 对于通过解压即可使用的 Linux 应用程序（如 Antigravity IDE），系统默认不会为其创建桌面入口。本指南介绍如何通过 .desktop 文件将它们注册到系统应用菜单、任务栏，并配置独立桌面图标。 一键快速配置（推荐模板） 只需在系统存放快捷方式的目录下创建一个 .desktop 配置文件。 1. 创建配置文件 打开终端，执行以下命令： 1 nano ~/.local/share/applications/antigravity-ide.desktop 2. 粘贴并修改配置内容 在打开的编辑器中，粘贴以下标准配置。请务必将路径中的 yj 替换为系统实际用户名，并确保路径与软件存放位置一致： 1 2 3 4 5 6 7 8 9 [Desktop Entry] Version=1.0 Type=Application Name=Antigravity IDE Comment=My Antigravity IDE Exec=/home/yj/Antigravity_IDE/antigravity-ide Icon=/home/yj/Antigravity_IDE/resources/app/resources/linux/code.png Terminal=false Categories=Development;IDE; 配置字段说明 字段 说明 示例 Exec 可执行程序的绝对路径 /home/yj/Antigravity_IDE/antigravity-ide Icon 应用图标的绝对路径 /home/yj/Antigravity_IDE/resources/icon.png Categories 应用分类（分号分隔） Development;IDE; Terminal 是否在终端中运行 false 避坑关键点 禁止使用波浪号（~）：Exec 和 Icon 必须使用绝对路径（如 /home/yj/...），系统无法解析 ~/ 处理路径空格：若文件夹名包含空格（如 Antigravity IDE），请给路径加上英文双引号，例如：Exec="/home/yj/下载/Antigravity IDE/antigravity-ide" 路径中的用户名：将 yj 替换为实际系统用户名 3. 保存并退出 在 nano 编辑器中，按下 Ctrl + O，然后按 Enter 确认保存，最后按 Ctrl + X 退出。 ...

介绍 多机器人系统初始化时，各机器人的局部坐标系往往不一致，需要通过点云配准将它们统一到同一个世界坐标系。本文介绍一种三阶段配准策略：重力对齐消除姿态差异、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）帧点云并融合，提高配准鲁棒性。单帧点云的特征信息有限，多帧融合后点云密度更高，几何特征更丰富，配准成功率显著提升。 ...