[{"content":"介绍 在技术博客的写作中，如何优雅地展示代码、数学公式和复杂的逻辑流程，直接决定了文章的阅读体验。传统的博客往往受限于静态生成器的默认支持，导致代码高亮生硬、公式解析错误或无法原生支持流程图。\n经过一系列的折腾与优化，我总结了一套在 Hugo (PaperMod 主题) 下近乎完美的解决方案：\n代码可视化：摒弃传统的 Chroma/Highlight.js，引入 Shiki，实现与本地 VS Code 完全一致的语法解析与 Dark+ 主题高亮。 数学公式可视化：集成 KaTeX，实现轻量级、极速的 LaTeX 公式渲染。 流程图可视化：原生集成 Mermaid.js，用纯文本绘制可交互的架构图与流程图。 本文将详细记录这些特性的实现效果与使用经验。\n代码可视化：VS Code 原生体验 (Shiki) 以前的代码高亮要么颜色不够精确，要么对现代语言（如 C++ 的复杂模板）支持不佳。我们引入了 Shiki，它基于 VS Code 原生的 TextMate 语法解析器，能够在网页上 1:1 还原你在编辑器里看到的绝美高亮。\n核心亮点 精准解析：依靠真实的 AST 语法树和正则，完美区分函数、变量、宏定义。 暗黑沉浸：采用了纯正的 dark-plus 主题，加上现代编程等宽字体（JetBrains Mono / Fira Code）。 零前端负担：在线上通过 GitHub Actions 在 Node.js 环境下静态预渲染，浏览器完全不需要加载庞大的 JS 库；而在本地 hugo server 模式下，则通过 CDN 动态实时加载 Shiki 引擎，实现所见即所得。 演示效果 真正的 C++ 算法代码：\n1 2 3 4 5 6 7 8 9 10 // 计算两个描述子的相似度 double binary_similarity(const BinaryDescriptor \u0026amp;b1, const BinaryDescriptor \u0026amp;b2) { int same_num = 0; for (int i = 0; i \u0026lt; 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++ 语法解析错误。\n1 2 3 平面法向量: n = (A, B, C) 平面中心: c = project_center 3D位置 = py * x_axis + px * y_axis + project_center 数学公式可视化 (KaTeX) 在 SLAM 和算法推导中，数学公式是必不可少的。相比于庞大的 MathJax，KaTeX 渲染速度更快，且在移动端排版更美观。\n使用方法 只需要在 Markdown 文件的开头 Front Matter 中设置 math = true 即可启用。\n行内公式使用一对 $，例如：欧拉公式 $e^{i\\pi} + 1 = 0$ 块级公式使用一对 $$，自动居中排版： $$ \\min_{\\mathbf{T}} \\sum_{i} \\left\\| \\mathbf{n}_i^\\top (\\mathbf{T} \\mathbf{p}_i - \\mathbf{q}_i) \\right\\|^2 $$ 流程图可视化 (Mermaid) 文字难以描述复杂的系统架构和算法运行机制，这时候就需要流程图。借助于集成的 Mermaid.js，我们可以像写代码一样写图表，而且这些图表支持暗黑模式自动切换。\n演示效果 只需将代码块的语言标记为 mermaid 即可生成如下动态流程图：\nflowchart TD A[输入点云] --\u0026gt; B[GenerateSTDescs: 描述子生成] subgraph 核心算法 C[init_voxel_map] --\u0026gt; D[get_plane] D --\u0026gt; E[get_project_plane] E --\u0026gt; F[merge_plane] end B --\u0026gt; C F --\u0026gt; G[存入数据库] 由于 Mermaid 是基于纯文本生成的，这也极大地降低了我们后续修改流程图的成本，不需要再用外部软件画图、截图、上传了。\n参考 Shiki 官方文档：https://shiki.style/ KaTeX 官网：https://katex.org/ Mermaid 语法大全：https://mermaid.js.org/ ","permalink":"https://zero-kq.github.io/Pages/posts/web/hugo-advanced-visualization/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e在技术博客的写作中，如何优雅地展示代码、数学公式和复杂的逻辑流程，直接决定了文章的阅读体验。传统的博客往往受限于静态生成器的默认支持，导致代码高亮生硬、公式解析错误或无法原生支持流程图。\u003c/p\u003e\n\u003cp\u003e经过一系列的折腾与优化，我总结了一套在 Hugo (PaperMod 主题) 下近乎完美的解决方案：\u003c/p\u003e\n\u003col\u003e\n\u003cli\u003e\u003cstrong\u003e代码可视化\u003c/strong\u003e：摒弃传统的 Chroma/Highlight.js，引入 Shiki，实现与本地 VS Code 完全一致的语法解析与 Dark+ 主题高亮。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e数学公式可视化\u003c/strong\u003e：集成 KaTeX，实现轻量级、极速的 LaTeX 公式渲染。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e流程图可视化\u003c/strong\u003e：原生集成 Mermaid.js，用纯文本绘制可交互的架构图与流程图。\u003c/li\u003e\n\u003c/ol\u003e\n\u003cp\u003e本文将详细记录这些特性的实现效果与使用经验。\u003c/p\u003e\n\u003chr\u003e\n\u003ch2 id=\"代码可视化vs-code-原生体验-shiki\"\u003e代码可视化：VS Code 原生体验 (Shiki)\u003c/h2\u003e\n\u003cp\u003e以前的代码高亮要么颜色不够精确，要么对现代语言（如 C++ 的复杂模板）支持不佳。我们引入了 \u003cstrong\u003eShiki\u003c/strong\u003e，它基于 VS Code 原生的 TextMate 语法解析器，能够在网页上 1:1 还原你在编辑器里看到的绝美高亮。\u003c/p\u003e\n\u003ch3 id=\"核心亮点\"\u003e核心亮点\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003e\u003cstrong\u003e精准解析\u003c/strong\u003e：依靠真实的 AST 语法树和正则，完美区分函数、变量、宏定义。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e暗黑沉浸\u003c/strong\u003e：采用了纯正的 \u003ccode\u003edark-plus\u003c/code\u003e 主题，加上现代编程等宽字体（JetBrains Mono / Fira Code）。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e零前端负担\u003c/strong\u003e：在线上通过 GitHub Actions 在 Node.js 环境下静态预渲染，浏览器完全不需要加载庞大的 JS 库；而在本地 \u003ccode\u003ehugo server\u003c/code\u003e 模式下，则通过 CDN 动态实时加载 Shiki 引擎，实现所见即所得。\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"演示效果\"\u003e演示效果\u003c/h3\u003e\n\u003cp\u003e真正的 C++ 算法代码：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e 1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 9\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e10\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 计算两个描述子的相似度\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"kt\"\u003edouble\u003c/span\u003e \u003cspan class=\"nf\"\u003ebinary_similarity\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"k\"\u003econst\u003c/span\u003e \u003cspan class=\"n\"\u003eBinaryDescriptor\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026amp;\u003c/span\u003e\u003cspan class=\"n\"\u003eb1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"k\"\u003econst\u003c/span\u003e \u003cspan class=\"n\"\u003eBinaryDescriptor\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026amp;\u003c/span\u003e\u003cspan class=\"n\"\u003eb2\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"kt\"\u003eint\u003c/span\u003e \u003cspan class=\"n\"\u003esame_num\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"k\"\u003efor\u003c/span\u003e \u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"kt\"\u003eint\u003c/span\u003e \u003cspan class=\"n\"\u003ei\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e \u003cspan class=\"n\"\u003ei\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026lt;\u003c/span\u003e \u003cspan class=\"n\"\u003eb1\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eoccupy_array_\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003esize\u003c/span\u003e\u003cspan class=\"p\"\u003e();\u003c/span\u003e \u003cspan class=\"o\"\u003e++\u003c/span\u003e\u003cspan class=\"n\"\u003ei\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e        \u003cspan class=\"k\"\u003eif\u003c/span\u003e \u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003eb1\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eoccupy_array_\u003c/span\u003e\u003cspan class=\"p\"\u003e[\u003c/span\u003e\u003cspan class=\"n\"\u003ei\u003c/span\u003e\u003cspan class=\"p\"\u003e]\u003c/span\u003e \u003cspan class=\"o\"\u003e==\u003c/span\u003e \u003cspan class=\"n\"\u003eb2\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eoccupy_array_\u003c/span\u003e\u003cspan class=\"p\"\u003e[\u003c/span\u003e\u003cspan class=\"n\"\u003ei\u003c/span\u003e\u003cspan class=\"p\"\u003e])\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e            \u003cspan class=\"n\"\u003esame_num\u003c/span\u003e\u003cspan class=\"o\"\u003e++\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e        \u003cspan class=\"p\"\u003e}\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"p\"\u003e}\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"k\"\u003ereturn\u003c/span\u003e \u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"kt\"\u003edouble\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e\u003cspan class=\"n\"\u003esame_num\u003c/span\u003e \u003cspan class=\"o\"\u003e/\u003c/span\u003e \u003cspan class=\"n\"\u003eb1\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eoccupy_array_\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003esize\u003c/span\u003e\u003cspan class=\"p\"\u003e();\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"p\"\u003e}\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e\u003cem\u003e注意：如果是带有大量中文解释的伪代码，请务必使用 \u003ccode\u003etext\u003c/code\u003e 语言标识，以免触发 C++ 语法解析错误。\u003c/em\u003e\u003c/p\u003e","title":"Hugo 高级可视化指南：代码、公式与流程图"},{"content":"介绍 在 LIO 的 IESKF 点面观测中，每行雅可比只有前 6 列非零，因此 $H^\\top H$ 与 $H^\\top Z$ 的信息可压缩为 $6 \\times 6$ 与 $6 \\times 1$ 的汇总量。本文记录并推导由此产生的两种等价计算路径：\n路径 简述 路径 A 累加 HTVH/HTVr → Cholesky 还原为 6 行合成观测 $H_s, Z_s$ → 走标准更新公式 路径 B 累加 HTVH/HTVr → 在更新阶段直接消费汇总量，不显式构造 $H$、$Z$、$K$ 两者观测模型相同，在保留迭代误差（IE）left + right 形式时，与逐点构建 $V$ 行 $H$ 的基准写法代数等价。下文以典型 18 维 LIO 状态为例，逐步展开推导。\n1. 背景：18 维状态 vs 6 维观测 考虑 18 维误差状态：\n下标 状态 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 列非零。\n记 $J_i \\in \\mathbb{R}^6$ 为 $h_i$ 前 6 列的转置：\n$$ J_i = \\begin{bmatrix} p_{\\text{imu},i} \\times (R^\\top n_i) \\\\ n_i \\end{bmatrix} $$残差：\n$$ r_i = n_i^\\top (R \\, p_{\\text{imu},i} + p - c_i) $$2. 基准写法：逐点构建 $V$ 行 $H$ 2.1 观测矩阵 $$ H \\in \\mathbb{R}^{V \\times 18}, \\quad Z \\in \\mathbb{R}^{V \\times 1} $$$$ h_{i,\\theta} = -n_i^\\top R \\, [p_{\\text{imu},i}]_\\times, \\quad h_{i,p} = n_i^\\top $$2.2 IESKF 迭代更新 设观测噪声方差 $\\sigma^2 = 0.001$，信息形式为：\n$$ P_{\\text{inv}} = \\left(\\frac{P_{\\text{in}}}{\\sigma^2}\\right)^{-1} $$$$ M = H^\\top H + P_{\\text{inv}} \\quad (18 \\times 18) $$$$ K = M^{-1} H^\\top \\quad (18 \\times V) $$IE 形式的两项更新：\n$$ \\text{left} = -K Z $$$$ \\text{right} = -(I - KH) \\, J_{\\text{inv}} \\, \\delta $$$$ \\Delta x = \\text{left} + \\text{right} $$$$ P \\leftarrow (I - KH) \\, P_{\\text{in}} $$2.3 可压缩的 6 维汇总量 由于 $H$ 每行只有前 6 列非零，有：\n$$ \\text{HTVH} = \\sum_i J_i J_i^\\top = \\text{（} H^\\top H \\text{ 的左上角 } 6 \\times 6 \\text{ 块）} \\quad (6 \\times 6) $$$$ \\text{HTVr} = \\sum_i J_i r_i = \\text{（} H^\\top Z \\text{ 的前 6 维）} \\quad (6 \\times 1) $$公共累加逻辑（两种路径在观测构建阶段相同）：\n1 2 3 4 5 6 Eigen::Vector6d J; J.head\u0026lt;3\u0026gt;() = p_imu.cross(R.transpose() * normal); J.tail\u0026lt;3\u0026gt;() = normal; HTVH += J * J.transpose(); // 6×6 HTVr += J * residual; // 6×1 注意：累加阶段只汇总几何信息；噪声权重 $\\sigma^2$ 在更新阶段通过 $P_{\\text{inv}}$ 体现，不应在累加时重复乘权。\n3. 路径 A：累加后 Cholesky 还原合成观测 3.1 思路 点云循环内只累加 HTVH/HTVr；循环结束后，用 Cholesky 分解构造 6 行合成观测矩阵，再代入第 2 节的标准更新公式。观测构建与滤波更新在接口上仍可保持分离。\n3.2 Cholesky 构造 要求合成观测 $H_s, Z_s$ 满足：\n$$ H_s^\\top H_s \\text{ 的 } 6 \\times 6 \\text{ 块} = \\text{HTVH}, \\quad H_s^\\top Z_s \\text{ 的前 6 维} = \\text{HTVr} $$分解步骤：\n$$ \\text{HTVH} = L L^\\top $$$$ H_s = \\begin{bmatrix} L \u0026 0_{6 \\times 12} \\end{bmatrix} \\in \\mathbb{R}^{6 \\times 18} $$$$ L^\\top Z_s = \\text{HTVr} $$ 1 2 3 4 5 6 7 Eigen::LLT\u0026lt;Eigen::Matrix6d\u0026gt; llt(HTVH); if (llt.info() != Eigen::Success) return false; Eigen::Matrix6d L = llt.matrixL(); H_synth = Eigen::MatrixXd::Zero(6, 18); H_synth.block\u0026lt;6, 6\u0026gt;(0, 0) = L; Z_synth = L.transpose().triangularView\u0026lt;Eigen::Upper\u0026gt;().solve(HTVr); 3.3 代入标准更新 将 $H_s \\in \\mathbb{R}^{6 \\times 18}$、$Z_s \\in \\mathbb{R}^{6}$ 代入第 2.2 节公式即可，$K$ 的维度由 $18 \\times V$ 降为 $18 \\times 6$：\n1 2 3 4 K = (H_s.transpose() * H_s + (P_in / sigma2).inverse()).inverse() * H_s.transpose(); left = -K * Z_s; right = -(I - K * H_s) * J_inv * delta; P_out = (I - K * H_s) * P_in; 3.4 等价性 因 $H_s^\\top H_s = H^\\top H$ 且 $H_s^\\top Z_s = H^\\top Z$，故信息矩阵 $M$、左项 $KZ$、右项涉及的 $KH$ 与 $V$ 行基准写法相同。\n3.5 路径 A 的特点 代数层面 计算层面 与 $V$ 行 $H$ 严格等价 仍需显式构造 $6 \\times 18$ 的 $H_s$ 并计算 $KH$ 更新公式形式不变，便于对照推导 Cholesky 要求 HTVH 正定，退化时分解可能失败 合成观测保留了\u0026quot;先建 $H$ 再更新\u0026quot;的结构 比路径 B 多一步矩阵分解 4. 路径 B：更新阶段直接消费 HTVH/HTVr 4.1 思路 观测构建阶段只输出汇总量 HTVH/HTVr，更新阶段用信息滤波形式直接计算 $\\Delta x$ 和 $P$，不再分配 $H$、$Z$、$K$。\n4.2 核心等价关系 定义 18 维嵌入矩阵（观测信息只在左上角）：\n$$ \\Lambda = \\begin{bmatrix} \\text{HTVH} \u0026 0 \\\\ 0 \u0026 0 \\end{bmatrix} \\quad (18 \\times 18) $$则有：\n$$ H^\\top H = \\Lambda, \\quad H^\\top Z = \\begin{bmatrix} \\text{HTVr} \\\\ \\mathbf{0} \\end{bmatrix} $$信息矩阵：\n$$ P_{\\text{inv}} = \\left(\\frac{P_{\\text{in}}}{\\sigma^2}\\right)^{-1} $$$$ M = \\Lambda + P_{\\text{inv}} $$$$ Q = M^{-1} \\quad \\text{（对应基准写法中 } K = M^{-1} H^\\top \\text{ 里的 } M^{-1} \\text{）} $$4.3 保留 IE 的 left + right 形式 left 项（等价于 $-KZ$）：\n$$ \\text{left} = -Q \\begin{bmatrix} \\text{HTVr} \\\\ \\mathbf{0} \\end{bmatrix} $$ 1 2 3 4 5 6 7 P_inv = (P_in / sigma2).inverse(); P_inv.block\u0026lt;6, 6\u0026gt;(0, 0) += HTVH; Q = P_inv.inverse(); error_dx.setZero(); error_dx.head\u0026lt;6\u0026gt;() = HTVr; left = -Q * error_dx; right 项（等价于 $-(I-KH) J_{\\text{inv}} \\delta$）：\n$$ KH = Q \\Lambda $$$$ \\text{right} = -(I - Q\\Lambda) \\, J_{\\text{inv}} \\, \\delta $$ 1 2 3 4 5 6 Lambda.setZero(); Lambda.block\u0026lt;6, 6\u0026gt;(0, 0) = HTVH; KH = Q * Lambda; right = -(I - KH) * J_inv * delta; dx = left + right; 协方差更新（等价于 $(I-KH)P_{\\text{in}}$）：\n1 P_out = (I - KH) * P_in; 4.4 等价性推导 left：\n$$ -KZ = -M^{-1} H^\\top Z = -M^{-1} \\begin{bmatrix} \\text{HTVr} \\\\ \\mathbf{0} \\end{bmatrix} = -Q \\, \\text{error\\_dx} $$right：\n$$ KH = M^{-1} H^\\top H = M^{-1} \\Lambda = Q \\Lambda $$故 $\\text{right}$ 与 $P$ 的更新与基准写法一致。\n18 维状态的间接修正： $Q$ 为 $18 \\times 18$ 满阵，$\\text{left}$ 虽只在前 6 维注入观测残差，速度、零偏、重力等分量仍通过 $P$ 的交叉协方差被间接修正。\n4.5 另一种常见写法（不含 right 项） 部分 LIO 实现（如 Super-LIO）的观测更新采用更简形式：\n$$ P_k^{-1} \\mathrel{+}= \\text{HTVH}, \\quad Q_k = (P_k^{-1})^{-1} $$$$ \\text{error\\_dx.head}(6) = \\text{HTVr}, \\quad \\Delta x = Q_k \\cdot \\text{error\\_dx} $$$$ P \\leftarrow (I - Q_k \\cdot \\text{temp\\_cov}) P_k, \\quad \\text{temp\\_cov 的 } 6 \\times 6 \\text{ 块} = \\text{HTVH} $$此形式没有 IE 的 right 项。与保留 left + right 的路径 B 在代数上不等价，迭代收敛行为可能不同。\n更新形式 与基准 IE 写法的关系 路径 B + left + right（4.3 节） 代数等价 仅 dx = Q * error_dx 的简化形式 不等价，属于另一种更新约定 4.6 路径 B 的特点 代数层面 计算层面 保留 left + right 时与基准等价 不显式分配 $H$、$Z$、$K$，矩阵规模最小 直接揭示 $H^\\top H$、$H^\\top Z$ 的压缩结构 需完整理解 $Q$、$Q\\Lambda$ 与 $KH$ 的对应关系 无合成观测矩阵，推导更抽象 无法从汇总量还原逐点残差向量 4.7 单轮迭代的完整流程 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 // 迭代更新（单轮示意） for (int iter = 0; iter \u0026lt; max_iter; ++iter) { delta = computeErrorState(x_est, x_prop); J_inv = computeJacobianInverse(...); P_in = J_inv * P * J_inv.transpose(); // 观测构建：逐点累加 HTVH.setZero(); HTVr.setZero(); buildObservation(state, HTVH, HTVr); // 信息滤波更新 P_inv = (P_in / sigma2).inverse(); P_inv.block\u0026lt;6, 6\u0026gt;(0, 0) += HTVH; Q = P_inv.inverse(); error_dx.setZero(); error_dx.head\u0026lt;6\u0026gt;() = HTVr; left = -Q * error_dx; Lambda.setZero(); Lambda.block\u0026lt;6, 6\u0026gt;(0, 0) = HTVH; KH = Q * Lambda; right = -(I - KH) * J_inv * delta; dx = left + right; // 更新状态、判断收敛 ... } P = (I - KH) * P_in; 5. 三种写法对比 设有效观测点数 $V$，状态维度 $n = 18$，迭代轮数 $T$。\n项目 基准：$V$ 行 $H$ 路径 A：6 行合成 $H_s$ 路径 B：直接 HTVH 存储 $H$ $V \\times n$ $6 \\times n$ 无 存储 $K$ $n \\times V$ $n \\times 6$ 无 Cholesky 无 需要 不需要 显式计算 $H^\\top H$ $O(V n^2)$ $O(6^2)$ 跳过（已累加） 计算 $KH$ $O(n^2 V)$ $O(n^2 \\cdot 6)$ $Q\\Lambda$，$O(n^2 \\cdot 6)$ 与基准代数等价 基准 是 是（保留 left+right） 6. 数值差异与退化 方面 路径 A 路径 B（left+right） 浮点累加顺序 极小差异 极小差异 Cholesky 误差 有 无 HTVH 非正定 分解可能失败 影响 $P_{\\text{inv}}$ 求逆稳定性 逐点残差可还原 否 否 7. 优化结果对比 在实际 LIO 系统中按路径 B 改造后，对 IESKF 两阶段耗时进行了分段统计：\n指标 含义 calc_ms 观测模型阶段：体素查找、点面匹配、雅可比与 HTVH/HTVr 累加 mat_ms 更新阶段：信息矩阵求逆、left/right 计算、协方差更新 下图记录了约 781 帧运行过程中，基准写法与路径 B 的耗时曲线对比：\n从曲线可以观察到：\ncalc_ms（观测构建）：路径 B（红线）整体低于基准（绿线）。基准多在 15–28 ms 波动，路径 B 多在 7–15 ms，约降低一半。 mat_ms（矩阵更新）：路径 B（浅蓝线）几乎贴近零轴；基准（黄线）仍在 5–13 ms 波动。更新阶段耗时下降最为显著。 以第 307 帧为例：\n指标 基准 路径 B 变化 calc_ms 23.02 ms 9.48 ms 约 −59% mat_ms 8.17 ms 0.09 ms 约 −99% 合计 31.19 ms 9.57 ms 约 −69% 这与第 5 节的复杂度分析一致：路径 B 不再分配 $V$ 行 $H$ 和 $K$，mat_ms 从 $O(n^2 V)$ 量级降至 $O(n^2 \\cdot 6)$；calc_ms 的下降则来自观测构建阶段同步改为逐点累加 HTVH/HTVr，避免了完整 $H$ 矩阵的写入开销。\n8. 总结 路径 核心思想 路径 A 用 Cholesky 将 $6 \\times 6$ 信息还原为 6 行合成观测，再走标准 $K$、$KH$ 公式 路径 B 将 $H^\\top H$、$H^\\top Z$ 视为可压缩量，用 $Q$ 与 $Q\\Lambda$ 直接完成 IE 更新 共同认识 压缩的是信息矩阵结构，而非观测几何本身；点面匹配仍须逐点完成 理解这两条路径的关键，在于认清 $H$ 的稀疏列结构与 $H^\\top H$ 低秩块之间的对应关系。路径 A 更贴近\u0026quot;先观测、后滤波\u0026quot;的教科书形式；路径 B 则直接暴露了信息滤波中\u0026quot;只需汇总量\u0026quot;的本质。实测中路径 B 使 mat_ms 降至亚毫秒级，单帧 IESKF 总耗时可降低约 60% 以上。\n参考 ieskf_lio 项目：https://github.com/Zero-Kq/ieskf_lio Super-LIO ESKF 实现：https://github.com/Liansheng-Wang/Super-LIO/blob/ros2/src/super_lio/src/lio/ESKF.cpp FAST-LIO2：https://github.com/hku-mars/FAST_LIO ","permalink":"https://zero-kq.github.io/Pages/posts/slam/ieskf-6x6-observation-optimization/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e在 LIO 的 IESKF 点面观测中，每行雅可比只有前 6 列非零，因此 $H^\\top H$ 与 $H^\\top Z$ 的信息可压缩为 $6 \\times 6$ 与 $6 \\times 1$ 的汇总量。本文记录并推导由此产生的两种等价计算路径：\u003c/p\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e路径\u003c/th\u003e\n          \u003cth\u003e简述\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003cstrong\u003e路径 A\u003c/strong\u003e\u003c/td\u003e\n          \u003ctd\u003e累加 HTVH/HTVr → Cholesky 还原为 6 行合成观测 $H_s, Z_s$ → 走标准更新公式\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003cstrong\u003e路径 B\u003c/strong\u003e\u003c/td\u003e\n          \u003ctd\u003e累加 HTVH/HTVr → 在更新阶段直接消费汇总量，不显式构造 $H$、$Z$、$K$\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003cp\u003e两者观测模型相同，在保留迭代误差（IE）\u003ccode\u003eleft + right\u003c/code\u003e 形式时，与逐点构建 $V$ 行 $H$ 的基准写法代数等价。下文以典型 18 维 LIO 状态为例，逐步展开推导。\u003c/p\u003e\n\u003ch2 id=\"1-背景18-维状态-vs-6-维观测\"\u003e1. 背景：18 维状态 vs 6 维观测\u003c/h2\u003e\n\u003cp\u003e考虑 18 维误差状态：\u003c/p\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e下标\u003c/th\u003e\n          \u003cth\u003e状态\u003c/th\u003e\n          \u003cth\u003eLiDAR 点面观测\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e0–2\u003c/td\u003e\n          \u003ctd\u003e旋转 $\\delta\\theta$\u003c/td\u003e\n          \u003ctd\u003e\u003cstrong\u003e有\u003c/strong\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e3–5\u003c/td\u003e\n          \u003ctd\u003e位置 $\\delta p$\u003c/td\u003e\n          \u003ctd\u003e\u003cstrong\u003e有\u003c/strong\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e6–8\u003c/td\u003e\n          \u003ctd\u003e速度 $\\delta v$\u003c/td\u003e\n          \u003ctd\u003e\u003cstrong\u003e无\u003c/strong\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e9–11\u003c/td\u003e\n          \u003ctd\u003e陀螺零偏 $b_g$\u003c/td\u003e\n          \u003ctd\u003e\u003cstrong\u003e无\u003c/strong\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e12–14\u003c/td\u003e\n          \u003ctd\u003e加计零偏 $b_a$\u003c/td\u003e\n          \u003ctd\u003e\u003cstrong\u003e无\u003c/strong\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e15–17\u003c/td\u003e\n          \u003ctd\u003e重力 $g$\u003c/td\u003e\n          \u003ctd\u003e\u003cstrong\u003e无\u003c/strong\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003cp\u003e点到平面残差只依赖 $R, p$，每行雅可比 $h_i \\in \\mathbb{R}^{1 \\times 18}$ \u003cstrong\u003e只有前 6 列非零\u003c/strong\u003e。\u003c/p\u003e","title":"IESKF 观测更新：6×6 累加两种推导路径"},{"content":"介绍 在三维点云处理与空间几何分析中，平面拟合是一项基础且关键的任务。给定一组三维点坐标 $\\mathbf{p}_i = (x_i, y_i, z_i)^\\top$，本文将探讨如何估计平面参数，并重点对比两种最常用的实现方法：PCA（主成分分析）法和线性最小二乘法。\n平面方程的几种形式 在讨论具体算法前，先回顾一下平面的常见数学表达：\n一般式（最常用） $$ ax + by + cz + d = 0 $$记 $\\mathbf{n} = (a, b, c)^\\top$ 为平面的法向量。若法向量为单位向量，即 $\\|\\mathbf{n}\\| = 1$，则 $|ax + by + cz + d|$ 就是任意一点到平面的几何距离。\n点法式 $$ \\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$ 为平面内两个相互正交的方向向量。\n点到平面的距离 给定一个查询点 $\\mathbf{p}$ 和拟合出的平面 $(\\mathbf{n}, \\mathbf{c})$，该点到平面的有符号距离为：\n$$ r = \\mathbf{n}^\\top (\\mathbf{p} - \\mathbf{c}) $$在点云配准及位姿优化任务中，$r$ 常常被用作观测残差进行最小化。\n方法一：PCA / 协方差特征分解 核心思路 如果一组点云近似落在某个平面上，那么它们在沿平面法线方向上的方差一定是最小的。因此，对去中心化后的点集进行 PCA（主成分分析），其最小特征值对应的特征向量即为平面的法向量。\n计算步骤 假设共有 $N$ 个点 $\\mathbf{p}_1, \\dots, \\mathbf{p}_N$。\nStep 1：计算质心\n$$ \\bar{\\mathbf{p}} = \\frac{1}{N}\\sum_{i=1}^{N}\\mathbf{p}_i $$Step 2：构建协方差矩阵\n$$ \\mathbf{C} = \\frac{1}{N}\\sum_{i=1}^{N}(\\mathbf{p}_i - \\bar{\\mathbf{p}})(\\mathbf{p}_i - \\bar{\\mathbf{p}})^\\top $$在实际工程中，为了支持数据的增量更新，常采用等价的增量形式：\n$$ \\mathbf{C} = \\frac{\\mathbf{P}}{N} - \\bar{\\mathbf{p}}\\bar{\\mathbf{p}}^\\top,\\quad \\mathbf{P} = \\sum_i \\mathbf{p}_i\\mathbf{p}_i^\\top,\\quad \\mathbf{v} = \\sum_i \\mathbf{p}_i $$Step 3：特征分解\n对协方差矩阵进行特征分解：\n$$ \\mathbf{C} = \\mathbf{V}\\mathbf{\\Lambda}\\mathbf{V}^\\top,\\quad \\lambda_0 \\le \\lambda_1 \\le \\lambda_2 $$ 法向：$\\mathbf{n} = \\mathbf{v}_0$（即最小特征值 $\\lambda_0$ 对应的特征向量） 平面上一点：$\\mathbf{c} = \\bar{\\mathbf{p}}$ 参数 d：$d = -\\mathbf{n}^\\top \\mathbf{c}$ Step 4：平面有效性检验\n并非所有点云都能拟合出有效平面。通常需要设定阈值进行判断，例如： 如果最小特征值足够小（如 λ₀ \u0026lt; 0.01）且显著小于次小特征值（如 λ₀ \u0026lt; 0.1 × λ₁），则认为平面有效；否则认为点云分布不够“平”，应拒绝该拟合结果。\n方法特点 优点 缺点 支持增量维护，无需保存每个历史点坐标 拟合的是整块点集的平均平面，对混合结构（如墙角、边缘）容易失效 适合大规模点云统计，抗噪性相对较好 至少需要足够多的点（一般建议 $N \\ge 5$） 计算过程稳定，实现简单 - 方法二：线性最小二乘 核心思路 将平面的一般方程 $ax + by + cz + d = 0$ 在 $d \\neq 0$ 时进行归一化：\n$$ \\frac{a}{d}x + \\frac{b}{d}y + \\frac{c}{d}z = -1 $$令未知数向量 $\\mathbf{x} = (a/d,\\, b/d,\\, c/d)^\\top$，对于每个点 $\\mathbf{p}_i = (x_i, y_i, z_i)^\\top$，有：\n$$ \\begin{bmatrix} x_i \u0026 y_i \u0026 z_i \\end{bmatrix} \\begin{bmatrix} x_1 \\\\ x_2 \\\\ x_3 \\end{bmatrix} = -1 $$将 $N$ 个点堆叠起来，可得到一个超定线性方程组 $\\mathbf{A}\\mathbf{x} = \\mathbf{b}$：\n$$ \\underbrace{\\begin{bmatrix} x_1 \u0026 y_1 \u0026 z_1 \\\\ x_2 \u0026 y_2 \u0026 z_2 \\\\ \\vdots \u0026 \\vdots \u0026 \\vdots \\\\ x_N \u0026 y_N \u0026 z_N \\end{bmatrix}}_{\\mathbf{A}\\,(N\\times 3)} \\mathbf{x} = \\underbrace{\\begin{bmatrix} -1 \\\\ -1 \\\\ \\vdots \\\\ -1 \\end{bmatrix}}_{\\mathbf{b}} $$计算步骤 Step 1：最小二乘求解\n通过最小二乘法求解该超定方程：\n$$ \\mathbf{x} = \\arg\\min_{\\mathbf{x}} \\|\\mathbf{A}\\mathbf{x} - \\mathbf{b}\\|_2^2 $$Step 2：还原平面参数 $(a,b,c,d)$\n设求得的解为 $\\mathbf{w} = \\mathbf{x}$，先得到未归一化的法向 $\\tilde{\\mathbf{n}} = \\mathbf{w}$。然后对其进行归一化：\n$$ \\mathbf{n} = \\frac{\\tilde{\\mathbf{n}}}{\\|\\tilde{\\mathbf{n}}\\|},\\quad d = \\frac{1}{\\|\\tilde{\\mathbf{n}}\\|} $$这样得到的平面系数 $[a, b, c, d]^\\top$ 满足单位法向量的前提。\nStep 3：内点严格检验\n通常会对每个参与拟合的点进行距离检查，以确保拟合质量：\n$$ |a x_i + b y_i + c z_i + d| \\le \\epsilon $$若任一点超出误差允许范围（如 $\\epsilon = 0.1\\text{m}$），则拒绝该平面。\n方法特点 优点 缺点 极其适合少量局部点（如 4～5 个 KNN 近邻点） 需要显式保存并计算点坐标，不能仅依靠统计量 平面高度贴合查询点的局部切面 若点共线或处于特定退化分布，矩阵 $\\mathbf{A}$ 会病态导致求解失败 内点检验机制更为严格 相比增量 PCA，每次都需要进行矩阵求解 两种方法对比汇总 项目 PCA（协方差特征分解） 线性最小二乘法 最少点数 建议 $\\ge 5$ 4～5 个即可 输入数据 区域内点集的统计量（可增量更新） 局部查询到的近邻点坐标 平面含义 区域点集的主导平面 局部的切平面 有效性判断 协方差矩阵的特征值阈值 每个点到平面的实际几何距离 退化情况与处理对策 在实际数据中，点云的分布往往不理想。下面列出了常见的退化情况及对应表现：\n点分布形态 现象表现 处理策略 共线 协方差矩阵秩不足，法向不唯一（PCA 中 $\\lambda_0 \\approx \\lambda_1 \\approx 0$） 检验失败，丢弃该结果 单点 / 两点 自由度不足，无法确定唯一平面 数量不足直接跳过拟合 球状 / 团状 三个方向上的方差接近（$\\lambda_0$ 不够小） 特征值检验失败，拒绝平面 两面混合（如墙角） 会拟合出一个“折中”的错误平面 残差过大，通过几何检验剔除 最小示例 PCA 法代码示例 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 import numpy as np points = np.array([ [0.0, 0.0, 0.0], [1.0, 0.0, 0.01], [0.0, 1.0, -0.01], [1.0, 1.0, 0.0], [0.5, 0.5, 0.0], ]) center = points.mean(axis=0) cov = (points - center).T @ (points - center) / len(points) eigval, eigvec = np.linalg.eigh(cov) # 默认升序排列 normal = eigvec[:, 0] # 最小特征值对应的方向 d = -normal @ center print(\u0026#34;normal:\u0026#34;, normal) print(\u0026#34;center:\u0026#34;, center) print(\u0026#34;d:\u0026#34;, d) 最小二乘法代码示例 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 import numpy as np points = np.array([ [0.0, 0.0, 0.0], [1.0, 0.0, 0.01], [0.0, 1.0, -0.01], [1.0, 1.0, 0.0], ]) A = points b = -np.ones(len(points)) # 最小二乘求解 w = np.linalg.lstsq(A, b, rcond=None)[0] n = w / np.linalg.norm(w) d = 1.0 / np.linalg.norm(w) a, b_, c = n print(f\u0026#34;拟合平面: {a:.4f}x + {b_:.4f}y + {c:.4f}z + {d:.4f} = 0\u0026#34;) 在实际应用中的工作流程 无论是基于网格化的点云处理，还是基于 KNN 的局部处理，平面拟合最终往往都服务于状态估计或位姿优化。一般流程如下：\n1 2 3 4 5 6 7 8 9 点云数据输入 │ ├─► [PCA法] 统计点集特征 → 提取法向 n 与中心 c │ │ │ └─► 计算残差 r = nᵀ(p - c)，构建优化目标 │ └─► [最小二乘法] 提取局部近邻点 → 解算平面系数 abcd │ └─► 计算残差 r = a·x + b·y + c·z + d，构建优化目标 关于优化雅可比的补充： 在进行位姿优化时，假设点 $\\mathbf{p}$ 经过位姿（旋转 $\\mathbf{R}$，平移 $\\mathbf{t}$）变换，已知目标平面的单位法向 $\\mathbf{n}$，其对位姿增量的雅可比通常遵循统一的形式：\n对旋转：$\\frac{\\partial r}{\\partial \\delta\\boldsymbol{\\theta}} = -\\mathbf{n}^\\top \\mathbf{R}\\,[\\mathbf{p}]_\\times$ 对平移：$\\frac{\\partial r}{\\partial \\delta\\mathbf{t}} = \\mathbf{n}^\\top$ 小结 平面拟合的本质是：从一组空间点坐标中估计平面的法向量 $\\mathbf{n}$ 和位置参数（$d$ 或中心点 $\\mathbf{c}$）。 PCA 法考察的是点云在哪个方向最为“扁平”，它非常适合多点聚集的区域，且易于实现增量式统计。 线性最小二乘法通过建立方程组求解，更擅长对少量局部近邻点进行快速的切平面拟合。 拟合算法本身无法辨别噪声和非平面结构，因此拟合完成后必须引入几何检验（如特征值比例、点面距离等）剔除退化情况或外点，才能保证下游任务的稳定。 参考 无 ","permalink":"https://zero-kq.github.io/Pages/posts/slam/plane-fitting-methods/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e在三维点云处理与空间几何分析中，平面拟合是一项基础且关键的任务。给定一组三维点坐标 $\\mathbf{p}_i = (x_i, y_i, z_i)^\\top$，本文将探讨如何估计平面参数，并重点对比两种最常用的实现方法：\u003cstrong\u003ePCA（主成分分析）法\u003c/strong\u003e和\u003cstrong\u003e线性最小二乘法\u003c/strong\u003e。\u003c/p\u003e\n\u003chr\u003e\n\u003ch2 id=\"平面方程的几种形式\"\u003e平面方程的几种形式\u003c/h2\u003e\n\u003cp\u003e在讨论具体算法前，先回顾一下平面的常见数学表达：\u003c/p\u003e\n\u003ch3 id=\"一般式最常用\"\u003e一般式（最常用）\u003c/h3\u003e\n$$\nax + by + cz + d = 0\n$$\u003cp\u003e记 $\\mathbf{n} = (a, b, c)^\\top$ 为平面的法向量。若法向量为单位向量，即 $\\|\\mathbf{n}\\| = 1$，则 $|ax + by + cz + d|$ 就是任意一点到平面的\u003cstrong\u003e几何距离\u003c/strong\u003e。\u003c/p\u003e\n\u003ch3 id=\"点法式\"\u003e点法式\u003c/h3\u003e\n$$\n\\mathbf{n}^\\top (\\mathbf{p} - \\mathbf{c}) = 0\n$$\u003cul\u003e\n\u003cli\u003e$\\mathbf{c}$：平面上的一点（常取点云的质心）\u003c/li\u003e\n\u003cli\u003e$\\mathbf{n}$：单位法向量\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"参数形式\"\u003e参数形式\u003c/h3\u003e\n$$\n\\mathbf{p}(u, v) = \\mathbf{c} + u\\mathbf{e}_1 + v\\mathbf{e}_2\n$$\u003cp\u003e其中 $\\mathbf{e}_1, \\mathbf{e}_2$ 为平面内两个相互正交的方向向量。\u003c/p\u003e\n\u003chr\u003e\n\u003ch2 id=\"点到平面的距离\"\u003e点到平面的距离\u003c/h2\u003e\n\u003cp\u003e给定一个查询点 $\\mathbf{p}$ 和拟合出的平面 $(\\mathbf{n}, \\mathbf{c})$，该点到平面的有符号距离为：\u003c/p\u003e\n$$\nr = \\mathbf{n}^\\top (\\mathbf{p} - \\mathbf{c})\n$$\u003cp\u003e在点云配准及位姿优化任务中，$r$ 常常被用作\u003cstrong\u003e观测残差\u003c/strong\u003e进行最小化。\u003c/p\u003e","title":"平面拟合方法"},{"content":"介绍 BTC 是一种用于 3D 点云位置识别（Place Recognition） 的算法，核心思想是：\n将点云体素化，检测平面 将点云投影到平面上，提取 二进制描述子（BinaryDescriptor） 用二进制描述子构建 稳定三角形描述子（STD, Stable Triangle Descriptor） 通过三角形匹配实现 回环检测 整个流程由 STDescManager 类管理，分为两个核心阶段：\n描述子生成（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\u0026lt;bool\u0026gt; // 占用数组，表示各高度层是否有点 summary_: unsigned char // 占用层数（occupy_array_ 中 true 的个数） location_: Vector3d // 描述子在3D空间中的位置 本质：将一个空间区域按高度分层，用二进制位串描述该区域的几何结构。\nSTD — 稳定三角形描述子 1 2 3 4 5 6 7 triangle_: Vector3d // 三角形三边长（量化后） angle_: Vector3d // 三个顶点处的角度信息 center_: Vector3d // 三角形中心 frame_number_: int // 所属帧号 binary_A_: BinaryDescriptor // 顶点A的二进制描述子 binary_B_: BinaryDescriptor // 顶点B的二进制描述子 binary_C_: BinaryDescriptor // 顶点C的二进制描述子 本质：三个二进制描述子构成的三角形，以三边长作为几何特征。\nBTCPlane — 平面结构 1 2 3 4 5 6 7 8 9 center_: Vector3d // 平面中心 normal_: Vector3d // 法向量 covariance_: Matrix3d // 协方差矩阵 d_: float // 平面方程参数 (ax+by+cz+d=0) radius_: float // 平面覆盖半径 is_plane_: bool // 是否为有效平面 points_size_: int // 包含点数 id_: int // 合并标识ID sub_plane_num_: int // 合并的子平面数 BTCOctoTree — 体素八叉树 1 2 3 voxel_points_: vector\u0026lt;Vector3d\u0026gt; // 体素内的点 plane_ptr_: BTCPlane* // 拟合的平面 octo_state_: int // 0=叶子节点, 1=非叶子 STDMatchList — 匹配列表 1 2 3 match_list_: vector\u0026lt;pair\u0026lt;STD, STD\u0026gt;\u0026gt; // 匹配的STD对 match_id_: pair\u0026lt;int, int\u0026gt; // (当前帧ID, 候选帧ID) match_frame_: int // 候选帧号 哈希结构 BTCVOXEL_LOC：体素坐标 (x, y, z)，用于空间哈希 STD_LOC：STD 量化坐标，用于描述子数据库索引 整体运行流程 主流程（以单帧处理为例） flowchart TD A[输入点云] --\u0026gt; B[GenerateSTDescs: 描述子生成] subgraph GenerateSTDescs C[init_voxel_map: 体素化 \u0026#43; 平面检测] --\u0026gt; D[get_plane: 提取平面点云] D --\u0026gt; E[get_project_plane: 获取投影平面] E --\u0026gt; F[merge_plane: 合并相似平面] F --\u0026gt; G[binary_extractor: 提取二进制描述子] subgraph binary_extractor G1[extract_binary: 单平面投影提取] --\u0026gt; G2[non_maxi_suppression: 非极大值抑制] end G --\u0026gt; G1 G2 --\u0026gt; H[generate_std: 生成三角形描述子] H --\u0026gt; I[清理内存] end B --\u0026gt; J[STD 描述子列表] J --\u0026gt; K[SearchLoop: 回环搜索] J --\u0026gt; L[AddSTDescs: 存入数据库] 回环搜索流程 当前帧 STDs → candidate_selector()（粗匹配：投票选出候选帧）→ 对每个候选帧执行 candidate_verify()（精验证）：\n调用 triangle_solver() 解算三角形变换 投票验证：用变换验证所有匹配对 调用 plane_geometric_verify() 平面几何验证 (ICP score) → 输出：(最佳候选帧 ID, 得分, 变换矩阵)\n各函数详解 read_parameters() — 参数读取 1 void read_parameters(ros::NodeHandle \u0026amp;nh, ConfigSetting \u0026amp;config_setting, int isHighFly) 功能：根据飞行高度（isHighFly）设置算法参数。\nisHighFly = 0（低空）：体素小（1m）、平面阈值严格、投影距离近 isHighFly = 1（高空）：体素大（2m）、平面阈值宽松、投影距离远 两组参数的关键差异：\n参数 低空 高空 影响 voxel_size_ 1 2 体素越大，平面越少但更稳定 plane_detection_thre_ 0.01 0.05 阈值越大，更容易被判定为平面 proj_plane_num_ 2 1 使用的投影平面数 descriptor_near_num_ 15 15 KNN 近邻数 candidate_num_ 20 100 候选帧数量上限 binary_similarity() — 二进制相似度计算 1 double binary_similarity(const BinaryDescriptor \u0026amp;b1, const BinaryDescriptor \u0026amp;b2) 功能：计算两个二进制描述子的相似度。\n算法：改进的 Jaccard 相似度\n1 similarity = 2 * |b1 ∩ b2| / (|b1| + |b2|) 遍历 occupy_array_，统计两个描述子在同一位置都为 true 的个数 分母为两个描述子各自 summary_ 的和 结果范围 [0, 1]，1 表示完全相同 binary_greater_sort() / plane_greater_sort() — 排序比较器 1 2 bool binary_greater_sort(BinaryDescriptor a, BinaryDescriptor b) bool plane_greater_sort(BTCPlane *plane1, BTCPlane *plane2) 功能：用于 std::sort 的降序排列比较器。\nbinary_greater_sort：按 summary_（占用层数）降序 plane_greater_sort：按 points_size_（点数）降序 BTCOctoTree::init_octo_tree() — 体素初始化入口 1 void BTCOctoTree::init_octo_tree() 功能：判断体素内点数是否足够，足够则进行平面拟合。\n逻辑：\n若 voxel_points_.size() \u0026gt; voxel_init_num_（默认 10 个点），则调用 init_plane() 进行平面拟合。\nBTCOctoTree::init_plane() — 平面拟合 1 void BTCOctoTree::init_plane() 功能：对体素内的点进行 PCA 平面拟合。\n算法步骤：\n计算协方差矩阵：\n1 Σ = (1/N) * Σ(pi * pi^T) - μ * μ^T 其中 μ 为点的均值（中心）\n特征值分解：\n1 Σ = V * diag(λ₁, λ₂, λ₃) * V^T 求出三个特征值 λ_min \u0026lt; λ_mid \u0026lt; λ_max 及对应特征向量\n平面判定：\n若 λ_min \u0026lt; plane_detection_thre_（默认 0.01），判定为平面 法向量 = 最小特征值对应的特征向量 平面半径 = √λ_max 计算平面方程：\n1 2 d = -(n · center) 平面: n·p + d = 0 STDescManager::GenerateSTDescs() — 描述子生成主函数 1 2 3 void STDescManager::GenerateSTDescs( pcl::PointCloud\u0026lt;pcl::PointXYZI\u0026gt;::Ptr \u0026amp;input_cloud, std::vector\u0026lt;STD\u0026gt; \u0026amp;stds_vec, int id) 功能：从输入点云生成 STD 描述子集合。这是描述子生成阶段的顶层函数。\n执行流程：\nStep 1: init_voxel_map()(input_cloud, voxel_map) → 体素化 + 平面检测 Step 2: get_plane()(voxel_map, plane_cloud) → 提取所有平面中心 + 法向量，保存到 plane_cloud_vec_ Step 3: get_project_plane()(voxel_map, proj_plane_list) → 获取可用于投影的平面列表；若无平面，创建一个默认水平面；merge_plane() 合并相似平面 Step 4: binary_extractor()(merge_plane_list, input_cloud, binary_list) → 提取二进制描述子 Step 5: generate_std()(binary_list, frame_id, stds_vec) → 生成三角形描述子 Step 6: 清理 voxel_map 内存 STDescManager::init_voxel_map() — 体素化 1 2 3 void STDescManager::init_voxel_map( const pcl::PointCloud\u0026lt;pcl::PointXYZI\u0026gt;::Ptr \u0026amp;input_cloud, std::unordered_map\u0026lt;BTCVOXEL_LOC, BTCOctoTree *\u0026gt; \u0026amp;voxel_map) 功能：将点云分配到哈希体素中，并对每个体素进行平面拟合。\n算法：\n遍历所有点，计算所属体素坐标：\n1 loc = floor(point / voxel_size) 使用 BTCVOXEL_LOC 作为哈希键\n分配点到体素：若体素不存在则创建 BTCOctoTree\n并行初始化：对每个体素调用 init_octo_tree() 进行平面拟合\nSTDescManager::get_plane() — 提取平面点云 1 2 3 void STDescManager::get_plane( const std::unordered_map\u0026lt;BTCVOXEL_LOC, BTCOctoTree *\u0026gt; \u0026amp;voxel_map, pcl::PointCloud\u0026lt;pcl::PointXYZINormal\u0026gt;::Ptr \u0026amp;plane_cloud) 功能：从体素地图中提取所有被判定为平面的体素，输出为点云。\n输出：每个平面体素输出一个点，包含：\n坐标 = 平面中心 法向量 = 平面法向量 这些平面点云会被保存到 plane_cloud_vec_，后续用于几何验证。\nSTDescManager::get_project_plane() — 获取投影平面 1 2 3 void STDescManager::get_project_plane( std::unordered_map\u0026lt;BTCVOXEL_LOC, BTCOctoTree *\u0026gt; \u0026amp;voxel_map, std::vector\u0026lt;BTCPlane *\u0026gt; \u0026amp;project_plane_list) 功能：从体素地图中提取所有平面，并通过聚类合并相似平面。\n算法步骤：\n收集所有平面：遍历体素地图，收集 is_plane_ = true 的平面\n平面聚类（Union-Find 思想）：\n遍历所有平面对 (i, j) 若法向量相似（|n_i - n_j| \u0026lt; threshold 或 |n_i + n_j| \u0026lt; threshold） 且距离相近（点到平面距离 \u0026lt; threshold） 则分配相同的 id_ 合并同组平面：\n合并协方差矩阵：Σ_merged = (P_PT1 + P_PT2) / N - μ * μ^T 合并中心：加权平均 重新计算法向量和半径 STDescManager::merge_plane() — 平面合并 1 2 void STDescManager::merge_plane(std::vector\u0026lt;BTCPlane *\u0026gt; \u0026amp;origin_list, std::vector\u0026lt;BTCPlane *\u0026gt; \u0026amp;merge_plane_list) 功能：与 get_project_plane() 类似的平面合并逻辑，但输入是已排序的平面列表。\n与 get_project_plane 的区别：\n输入已按点数降序排列 合并后的平面会递归计算新的特征值和法向量 未合并的平面（id_ = 0）也保留 合并公式：\n1 2 新中心 = (n₁ * μ₁ + n₂ * μ₂) / (n₁ + n₂) 新协方差 = (ΣP_PT₁ + ΣP_PT₂) / (n₁ + n₂) - 新中心 * 新中心^T STDescManager::binary_extractor() — 二进制描述子提取 1 2 3 4 void STDescManager::binary_extractor( const std::vector\u0026lt;BTCPlane *\u0026gt; proj_plane_list, const pcl::PointCloud\u0026lt;pcl::PointXYZI\u0026gt;::Ptr \u0026amp;input_cloud, std::vector\u0026lt;BinaryDescriptor\u0026gt; \u0026amp;binary_descriptor_list) 功能：对多个投影平面分别提取二进制描述子，并进行后处理。\n流程：\n遍历投影平面（最多 proj_plane_num_ 个）：\n跳过法向量过于相似的连续平面 对每个平面调用 extract_binary() 非极大值抑制：non_maxi_suppression()\n数量控制：保留最多 useful_corner_num_ 个描述子（按 summary_ 排序）\nSTDescManager::extract_binary() — 单平面二进制描述子提取 1 2 3 4 5 void STDescManager::extract_binary( const Eigen::Vector3d \u0026amp;project_center, const Eigen::Vector3d \u0026amp;project_normal, const pcl::PointCloud\u0026lt;pcl::PointXYZI\u0026gt;::Ptr \u0026amp;input_cloud, std::vector\u0026lt;BinaryDescriptor\u0026gt; \u0026amp;binary_list) 功能：将点云投影到指定平面，生成二进制描述子图像。\n这是整个算法最核心的函数，详细步骤如下：\nStep 1: 构建投影坐标系 1 2 3 4 5 6 7 平面法向量: n = (A, B, C) 平面中心: c = project_center x_axis: 与法向量正交的单位向量 y_axis: n × x_axis（叉积） 投影坐标: (px, py) = (点投影到y_axis的分量, 点投影到x_axis的分量) Step 2: 点云投影 对每个点：\n计算到平面的距离 d = |Ax + By + Cz + D| 若 d ∉ [dis_min, dis_max]，跳过 将点投影到平面：proj_point = point - d * n 计算 2D 坐标 (px, py) Step 3: 构建距离图像 1 2 3 4 5 6 分辨率: resolution = 0.5m 图像尺寸: x_axis_len * y_axis_len 对每个像素 (x, y): 统计落入该像素的点数 img_count[x][y] 记录每个点到平面的距离列表 dis_container[x][y] Step 4: 生成二进制编码 1 2 3 4 5 6 高度分层数: cut_num = (dis_max - dis_min) / high_inc 对每个像素: 将距离列表分层统计 cnt_list[layer] 若 cnt_list[layer] \u0026gt;= 1, occupy[layer] = true summary = 占用层数 二进制编码示意：\n1 2 3 4 高度层: [层0] [层1] [层2] [层3] [层4] ... 占用情况: 1 0 1 1 0 ... occupy_array_ = [true, false, true, true, false, ...] summary_ = 3 Step 5: 分段筛选 将图像分成 segmen_base_num * segmen_base_num 的段：\n每段内找 summary 最大的像素 必须 summary \u0026gt;= summary_min_thre_（默认 10） Step 6: 线过滤（可选） 检查候选点是否位于\u0026quot;线\u0026quot;上：\n在 4 个方向（水平、垂直、两个对角）检查 若两侧都有高响应值，说明是线结构，过滤掉 Step 7: 恢复 3D 坐标 1 3D位置 = py * x_axis + px * y_axis + project_center STDescManager::non_maxi_suppression() — 非极大值抑制 1 void STDescManager::non_maxi_suppression(std::vector\u0026lt;BinaryDescriptor\u0026gt; \u0026amp;binary_list) 功能：在局部邻域内保留 summary 最大的描述子，避免聚集。\n算法：\n构建 KD-Tree 对每个描述子，搜索半径 non_max_suppression_radius_ 内的邻居 若存在邻居的 summary 更大，则标记当前描述子为待移除 保留未被标记的描述子 STDescManager::generate_std() — 三角形描述子生成 1 2 3 void STDescManager::generate_std( const std::vector\u0026lt;BinaryDescriptor\u0026gt; \u0026amp;binary_list, const int \u0026amp;frame_id, std::vector\u0026lt;STD\u0026gt; \u0026amp;std_list) 功能：从二进制描述子集合中构建三角形描述子。\n算法：\n构建 KD-Tree：对所有描述子位置建立 KD-Tree\nKNN 搜索：对每个描述子搜索 K 个最近邻（默认 K=15）\n三角形构建：对每三个点 (p1, p2, p3) 构建三角形：\n1 a = |p1 - p2|, b = |p1 - p3|, c = |p2 - p3| 边长约束：\n每条边必须在 [min_len, max_len] 范围内（默认 [2, 50]） 不能退化（|c - (a+b)| \u0026lt; 0.2 表示近似共线） 排序去重：\n对三边排序：a ≤ b ≤ c 用 (a * 1000, b * 1000, c * 1000) 作为哈希键去重 保证相同的三角形只生成一次 构建 STD：\n1 2 3 triangle_ = (a, b, c) * scale // 量化后的边长 center_ = (A + B + C) / 3 // 三角形中心 binary_A_, binary_B_, binary_C_ // 三个顶点的二进制描述子 STDescManager::AddSTDescs() — 添加描述子到数据库 1 void STDescManager::AddSTDescs(const std::vector\u0026lt;STD\u0026gt; \u0026amp;stds_vec) 功能：将当前帧的 STD 描述子存入全局哈希数据库。\n算法：\n递增帧计数器 current_frame_id_ 对每个 STD，计算量化位置 (int)(triangle_[i] + 0.5) 存入 data_base_ 哈希表，键为 STD_LOC STDescManager::SearchLoop() — 回环搜索 1 2 3 4 void STDescManager::SearchLoop( std::vector\u0026lt;STD\u0026gt; \u0026amp;stds_vec, std::pair\u0026lt;int, double\u0026gt; \u0026amp;loop_result, std::pair\u0026lt;Eigen::Vector3d, Eigen::Matrix3d\u0026gt; \u0026amp;loop_transform, std::vector\u0026lt;std::pair\u0026lt;STD, STD\u0026gt;\u0026gt; \u0026amp;loop_std_pair, ...) 功能：在历史数据库中搜索与当前帧匹配的回环帧。\n流程：\nStep 1: candidate_selector()(stds_vec, candidate_matcher_vec) → 粗匹配，投票选出 top-N 候选帧 Step 2: 对每个候选帧执行 candidate_verify() → 精验证：解算变换 + 投票 + 平面几何验证 Step 3: 选择得分最高的候选 → 若 best_score \u0026gt; icp_threshold_，确认回环 输出：\nloop_result = (candidate_id, score)：-1 表示无回环 loop_transform = (t, R)：相对变换 loop_std_pair：匹配的 STD 对 STDescManager::candidate_selector() — 候选帧选择 1 2 3 void STDescManager::candidate_selector( std::vector\u0026lt;STD\u0026gt; \u0026amp;current_STD_list, std::vector\u0026lt;STDMatchList\u0026gt; \u0026amp;candidate_matcher_vec) 功能：通过粗匹配和投票快速筛选候选回环帧。\n算法：\n遍历当前帧的每个 STD：\n在数据库中搜索量化坐标附近的 27 个邻域体素（3×3×3） 对每个匹配的 STD： 检查帧号差 \u0026gt; skip_near_num_（避免近邻帧） 计算三角形边长距离 \u0026lt; rough_dis_threshold_ * ||triangle|| 计算三个二进制描述子的平均相似度 \u0026gt; similarity_threshold_ 若满足条件，记录匹配 投票统计：\n对每个历史帧统计匹配数 match_array[frame_id] += 1 按匹配数降序排列 构建候选列表：\n选择 top-candidate_num_ 个候选帧 每个候选帧需至少 5 个匹配 STDescManager::candidate_verify() — 候选帧验证 1 2 3 4 void STDescManager::candidate_verify( STDMatchList \u0026amp;candidate_matcher, double \u0026amp;verify_score, std::pair\u0026lt;Eigen::Vector3d, Eigen::Matrix3d\u0026gt; \u0026amp;relative_pose, std::vector\u0026lt;std::pair\u0026lt;STD, STD\u0026gt;\u0026gt; \u0026amp;sucess_match_list, ...) 功能：对候选帧进行几何验证，确认回环。\n算法：\n采样：从匹配列表中均匀采样（最多 50 个）\nRANSAC 式验证：\n对每个采样匹配对，调用 triangle_solver() 解算变换 (R, t) 用该变换验证所有匹配对： 将当前帧的 A/B/C 点变换到候选帧坐标系 若三个点都与候选帧对应点距离 \u0026lt; dis_threshold（默认 3m），计一票 保留得票最高的变换 阈值判断：max_vote \u0026gt;= 4 才继续\n几何验证：调用 plane_geometric_verify() 计算最终得分\nSTDescManager::triangle_solver() — 三角形变换解算 1 2 void STDescManager::triangle_solver(std::pair\u0026lt;STD, STD\u0026gt; \u0026amp;std_pair, Eigen::Vector3d \u0026amp;t, Eigen::Matrix3d \u0026amp;rot) 功能：从一对匹配的 STD 解算刚体变换 (R, t)。\n算法（SVD 分解法）：\n构建对应点矩阵：\n1 2 src = [A-c, B-c, C-c] （当前帧，去中心化） ref = [A\u0026#39;-c\u0026#39;, B\u0026#39;-c\u0026#39;, C\u0026#39;-c\u0026#39;] （候选帧，去中心化） 计算协方差矩阵：\n1 H = src * ref^T SVD 分解：\n1 2 H = U * Σ * V^T R = V * U^T 处理反射情况：\n若 det(R) \u0026lt; 0，修正为 R = V * K * U^T（K 为修正矩阵） 计算平移：\n1 t = -R * c_src + c_ref STDescManager::plane_geometric_verify() — 平面几何验证 1 2 3 4 double STDescManager::plane_geometric_verify( const pcl::PointCloud\u0026lt;pcl::PointXYZINormal\u0026gt;::Ptr \u0026amp;source_cloud, const pcl::PointCloud\u0026lt;pcl::PointXYZINormal\u0026gt;::Ptr \u0026amp;target_cloud, const std::pair\u0026lt;Eigen::Vector3d, Eigen::Matrix3d\u0026gt; \u0026amp;transform) 功能：用平面点云进行几何验证，计算匹配得分。\n算法：\n构建 KD-Tree：对目标平面点云建树\n逐点验证：\n将源平面点变换到目标坐标系：p' = R * p + t 同时变换法向量：n' = R * n 搜索最近邻 检查： 法向量一致：|n' - n_target| \u0026lt; normal_threshold 或 |n' + n_target| \u0026lt; normal_threshold 点到平面距离：|n_target^T * (p' - p_target)| \u0026lt; dis_threshold 计算得分：\n1 score = 匹配点数 / 源点云总点数 得分范围 [0, 1]，越高越好\n关键设计思想 为什么用二进制描述子？ 高效：占用数组可用位运算快速比较 鲁棒：对点云密度变化不敏感（只关心\u0026quot;有无点\u0026quot;而非\u0026quot;多少点\u0026quot;） 紧凑：每个描述子仅需一个 vector\u0026lt;bool\u0026gt; + 一个字节 为什么用三角形？ 旋转/平移不变：三角形边长在刚体变换下不变 尺度稳定：通过边长范围约束，避免退化三角形 高效匹配：三边长可量化为整数，用哈希表快速查找 为什么用平面投影？ 降维：3D → 2D，简化描述子提取 结构化：平面是最常见的建筑/地面结构 多视角：多个投影平面可捕获不同方向的几何特征 参考 无\n","permalink":"https://zero-kq.github.io/Pages/posts/slam/btc-algorithm-introduction/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003eBTC 是一种用于 \u003cstrong\u003e3D 点云位置识别（Place Recognition）\u003c/strong\u003e 的算法，核心思想是：\u003c/p\u003e\n\u003col\u003e\n\u003cli\u003e将点云体素化，检测平面\u003c/li\u003e\n\u003cli\u003e将点云投影到平面上，提取 \u003cstrong\u003e二进制描述子（BinaryDescriptor）\u003c/strong\u003e\u003c/li\u003e\n\u003cli\u003e用二进制描述子构建 \u003cstrong\u003e稳定三角形描述子（STD, Stable Triangle Descriptor）\u003c/strong\u003e\u003c/li\u003e\n\u003cli\u003e通过三角形匹配实现 \u003cstrong\u003e回环检测\u003c/strong\u003e\u003c/li\u003e\n\u003c/ol\u003e\n\u003cp\u003e整个流程由 \u003ccode\u003eSTDescManager\u003c/code\u003e 类管理，分为两个核心阶段：\u003c/p\u003e\n\u003cul\u003e\n\u003cli\u003e\u003cstrong\u003e描述子生成\u003c/strong\u003e（\u003ca href=\"#stdescmanagergeneratestddescs--%E6%8F%8F%E8%BF%B0%E5%AD%90%E7%94%9F%E6%88%90%E4%B8%BB%E5%87%BD%E6%95%B0\"\u003e\u003ccode\u003eGenerateSTDescs\u003c/code\u003e\u003c/a\u003e）：从点云提取 STD 描述子\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e回环搜索\u003c/strong\u003e（\u003ca href=\"#stdescmanagersearchloop--%E5%9B%9E%E7%8E%AF%E6%90%9C%E7%B4%A2\"\u003e\u003ccode\u003eSearchLoop\u003c/code\u003e\u003c/a\u003e）：在历史数据库中搜索匹配帧\u003c/li\u003e\n\u003c/ul\u003e\n\u003chr\u003e\n\u003ch2 id=\"数据结构\"\u003e数据结构\u003c/h2\u003e\n\u003ch3 id=\"configsetting--配置参数\"\u003eConfigSetting — 配置参数\u003c/h3\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e参数组\u003c/th\u003e\n          \u003cth\u003e关键参数\u003c/th\u003e\n          \u003cth\u003e说明\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e体素化\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003evoxel_size_\u003c/code\u003e, \u003ccode\u003evoxel_init_num_\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e体素大小、最小点数阈值\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e平面检测\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003eplane_detection_thre_\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e最小特征值阈值，低于此值判定为平面\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e平面合并\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003eplane_merge_normal_thre_\u003c/code\u003e, \u003ccode\u003eplane_merge_dis_thre_\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e法向量/距离阈值\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e投影参数\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003eproj_plane_num_\u003c/code\u003e, \u003ccode\u003eproj_image_resolution_\u003c/code\u003e, \u003ccode\u003eproj_dis_min/max_\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e投影平面数、分辨率、距离范围\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e二进制描述子\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003esummary_min_thre_\u003c/code\u003e, \u003ccode\u003eline_filter_enable_\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e最小占用数、线过滤开关\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eSTD 生成\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003edescriptor_near_num_\u003c/code\u003e, \u003ccode\u003edescriptor_min/max_len_\u003c/code\u003e, \u003ccode\u003estd_side_resolution_\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e近邻数、边长范围、量化分辨率\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e回环检测\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003eskip_near_num_\u003c/code\u003e, \u003ccode\u003ecandidate_num_\u003c/code\u003e, \u003ccode\u003esimilarity_threshold_\u003c/code\u003e, \u003ccode\u003eicp_threshold_\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e跳帧数、候选数、相似度/ICP 阈值\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003ch3 id=\"binarydescriptor--二进制描述子\"\u003eBinaryDescriptor — 二进制描述子\u003c/h3\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"nl\"\u003eoccupy_array_\u003c/span\u003e\u003cspan class=\"p\"\u003e:\u003c/span\u003e \u003cspan class=\"n\"\u003evector\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026lt;\u003c/span\u003e\u003cspan class=\"kt\"\u003ebool\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026gt;\u003c/span\u003e   \u003cspan class=\"c1\"\u003e// 占用数组，表示各高度层是否有点\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"nl\"\u003esummary_\u003c/span\u003e\u003cspan class=\"p\"\u003e:\u003c/span\u003e      \u003cspan class=\"kt\"\u003eunsigned\u003c/span\u003e \u003cspan class=\"kt\"\u003echar\u003c/span\u003e  \u003cspan class=\"c1\"\u003e// 占用层数（occupy_array_ 中 true 的个数）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"nl\"\u003elocation_\u003c/span\u003e\u003cspan class=\"p\"\u003e:\u003c/span\u003e     \u003cspan class=\"n\"\u003eVector3d\u003c/span\u003e       \u003cspan class=\"c1\"\u003e// 描述子在3D空间中的位置\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e\u003cstrong\u003e本质\u003c/strong\u003e：将一个空间区域按高度分层，用二进制位串描述该区域的几何结构。\u003c/p\u003e","title":"BTC (Binary Triangle Context) 算法详解"},{"content":"介绍 在将 VoxelSLAM 的 BTC（Binary Triangle Context）描述子模块移植到 ZLIO 项目时，我们经历了从\u0026quot;检测不出回环（STD 数量为 0）\u0026ldquo;到\u0026quot;性能极慢（searchLoop 单次 60+ ms）\u0026ldquo;再到\u0026quot;极速运行（单次 0.5 ms 以下）\u0026ldquo;的完整过程。本文档记录了这期间的所有致命 Bug、性能瓶颈以及最终的极致性能优化方案。\n算法逻辑修复（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，完全无法满足阈值。 教训：代码里的\u0026quot;兜底默认值\u0026quot;必须与原版保持一致，否则读取失败会引发隐性逻辑断层。 缺少分段局部最大值选择 现象：提取出的 binary 描述子数量远少于参考代码。 原因：重构时为了简化，把原作者的\u0026quot;分段局部找最大值\u0026quot;的网格操作，简化成了\u0026quot;全局固定阈值过滤\u0026rdquo;。 修复：还原 5x5 网格局部最大值选择的机制。 法向量过滤条件逻辑错误 现象：大量投影平面被错误跳过。 原因：把原版的 || 错写成了 \u0026amp;\u0026amp;。if (normal_diff.norm() \u0026lt; 0.3 \u0026amp;\u0026amp; normal_add.norm() \u0026gt; 0.3) 导致条件几近无法触发。 修复：将 \u0026amp;\u0026amp; 更正为 ||。 兜底逻辑缺失 现象：当没有检测到平面时，后续流程完全中断并崩溃。 原因：原作在没找到平面时会强行插入一个默认法向量的伪平面作为兜底，重构时丢失了这一行。 教训：异常处理和边缘情况兜底往往是原作者踩过坑加上的，不能轻易删除。 C++ 工程与性能优化（Performance Optimizations） 在算法逻辑打通后，我们发现代码耗时奇高，随后进行了深度底层优化，取得了 20 倍以上的加速。\n消除 std::vector\u0026lt;bool\u0026gt; 的深拷贝与动态内存分配 问题分析：在进行候选帧筛选（candidateSelector）时，原代码将匹配上的特征（STD）存入一个 std::pair\u0026lt;STD, STD\u0026gt; 数组中。由于 STD 内部包含多个巨大的 std::vector\u0026lt;bool\u0026gt;，这引发了**海量的堆内存分配（Heap Allocation）**和深度复制。 优化原理：重构 STDMatchResult 数据结构，将匹配记录改为使用**常量指针（const STDescriptor*）引用数据库中的已有描述子，实现了零拷贝（Zero-copy）**和零分配。 副作用：匹配结果的生命周期强绑定到了 database_ 上。如果在访问匹配结果时，database_ 被析构或清空，会引发悬空指针（目前生命周期内安全）。 扁平化数据结构拯救 CPU 缓存（Cache Miss） 问题分析：原版在第一轮匹配时使用了嵌套的二维动态数组 std::vector\u0026lt;std::vector\u0026lt;size_t\u0026gt;\u0026gt;。数千个特征同时 push_back 导致内存极度碎片化，CPU L1/L2 Cache 严重未命中。 优化原理：引入扁平化的 MatchRecord 结构体，预先使用 reserve() 分配一整块连续的 std::vector\u0026lt;MatchRecord\u0026gt; 一维内存。CPU 顺序遍历时会触发硬件预取（Prefetching），极大提升访存效率。 副作用：无副作用，内存占用更小、更连续。 -O3 优化与位运算 SIMD 自动向量化 问题分析：计算 binarySimilarity 时，需要对长达数千位的 std::vector\u0026lt;bool\u0026gt; 进行 \u0026amp;\u0026amp; 位运算。在缺乏强优化时，这种按位访问极低效。 优化原理：在 CMakeLists.txt 中强制全局注入 -O3 标志。现代编译器会自动对该位操作循环进行 SIMD 自动向量化，将单比特运算合并为 128 位 / 256 位宽度的整数指令，产生降维打击般的加速。 副作用：编译时间略长；若代码存在未定义行为（UB）可能会在 -O3 下暴露。 移除无效的 O(N) 遍历统计 问题分析：由于想在日志里打印 database_ 的总描述子数量，每次循环都添加了遍历 8 万个哈希桶求 size 的逻辑，直接隐性吃掉了 8 毫秒。 优化原理：果断移除纯打印用的废操作，切忌在热点路径做耗时统计。 参数对齐与计算量控制（Parameter Tuning） 问题分析：在底层代码极度优化后，单帧仍需 30 ms，而原版仅需约 10 ms。排查发现，在 loop_detector.cpp 中的 readParam() 存在旧的硬编码默认值（由于原配置文件未更新导致生效），导致 ZLIO 的数学参数极度庞大。 参数对比：\n参数 ZLIO 旧值 VoxelSLAM 原版 影响 useful_corner_num 100 30 角点数量翻 3 倍以上 descriptor_near_num 15 10 近邻搜索范围增大 descriptor_max_len 50 10 描述子长度膨胀 5 倍 这使得 ZLIO 构建的三角组合数量达到了原版的 12.5 倍，硬生生做了十多倍的冗余运算量。\n优化原理：更新 params.yaml 并同步 loop_detector.cpp 里的代码默认值，确保控制变量严格一致。修改后，生成特征数量断崖式下跌，单帧 searchLoop 暴降至 0.5 毫秒甚至更低，远超原版 VoxelSLAM 的速度。 副作用与调参建议 调低角点提取数和近邻数虽然大幅降低了耗时，但也意味着在极端特征稀疏环境下，回环检测的召回率（Recall）可能会下降。但由于我们的代码已被优化到了 0.5 ms，用户完全可以随时\u0026quot;加大剂量\u0026rdquo;（如将角点数改回 60~100），以在 5 ms 级别的性能下换取比原版更强悍的鲁棒性。\n调试与移植方法论总结 逐层断点 / 打印验证法：在每个阶段（planes → proj → binary → STD）打印提取的数量，数量哪里断崖下跌，bug 就在哪里。 永远不要盲目信任\u0026quot;默认参数\u0026rdquo;：配置文件可能读取失败，代码里的兜底参数可能没同步。一定要在程序启动后 printf 出实际生效的参数值。 警惕 C++ 内存陷阱：算法移植不能只抄公式。vector\u0026lt;bool\u0026gt; 的深拷贝、二维数组的碎片化，在原版代码中就存在的低效写法，移植时若数据量膨胀会被无限放大。 尊重编译器的力量：涉及密集型位运算和循环时，不要忘记在 CMakeLists.txt 里开启 -O3。 参考 VoxelSLAM 源码：https://github.com/ivipsourcecode/VoxelSLAM FAST_LIO 源码：https://github.com/hku-mars/FAST_LIO BTC 描述子原始论文：Binary Triangle Context for Loop Closure Detection ","permalink":"https://zero-kq.github.io/Pages/posts/slam/btc-loop-closure-debug-optimization/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e在将 VoxelSLAM 的 BTC（Binary Triangle Context）描述子模块移植到 ZLIO 项目时，我们经历了从\u0026quot;检测不出回环（STD 数量为 0）\u0026ldquo;到\u0026quot;性能极慢（searchLoop 单次 60+ ms）\u0026ldquo;再到\u0026quot;极速运行（单次 0.5 ms 以下）\u0026ldquo;的完整过程。本文档记录了这期间的所有致命 Bug、性能瓶颈以及最终的极致性能优化方案。\u003c/p\u003e\n\u003chr\u003e\n\u003ch2 id=\"算法逻辑修复bug-fixes\"\u003e算法逻辑修复（Bug Fixes）\u003c/h2\u003e\n\u003ch3 id=\"yaml-cpp-节点引用失效\"\u003eYAML-cpp 节点引用失效\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003e\u003cstrong\u003e现象\u003c/strong\u003e：所有 \u003ccode\u003ebtc/\u003c/code\u003e 前缀的参数读取都失败，使用了硬编码的旧默认值。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e原因\u003c/strong\u003e：\u003ccode\u003eModuleBase\u003c/code\u003e 构造函数中直接用子节点覆盖了根节点 \u003ccode\u003econfig_node_\u003c/code\u003e，由于 YAML-cpp 的 Node 是引用语义，原根节点析构导致子节点变为 \u003ccode\u003eUndefined\u003c/code\u003e。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e修复\u003c/strong\u003e：保留根节点，用单独的 \u003ccode\u003eprefix_node_\u003c/code\u003e 存储子节点。\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"点到平面距离公式符号错误致命\"\u003e点到平面距离公式符号错误（致命）\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003e\u003cstrong\u003e现象\u003c/strong\u003e：所有点的 \u003ccode\u003edis\u003c/code\u003e 值异常大，全部被过滤掉。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e原因\u003c/strong\u003e：重构时凭直觉把点到平面距离错写成了 \u003ccode\u003en·p + n·c\u003c/code\u003e，而正确数学推导应为 \u003ccode\u003en·p - n·c\u003c/code\u003e。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e教训\u003c/strong\u003e：涉及数学公式时，必须逐项对照参考实现验证，绝不可凭直觉重写。\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"proj_image_high_inc-默认值过大\"\u003eproj_image_high_inc 默认值过大\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003e\u003cstrong\u003e现象\u003c/strong\u003e：\u003ccode\u003esummary_min_thre = 10\u003c/code\u003e，要求 10 层必须全部被占据。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e原因\u003c/strong\u003e：参数读取失败后采用了 0.5 的默认值，导致总层数缩减为 10，完全无法满足阈值。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e教训\u003c/strong\u003e：代码里的\u0026quot;兜底默认值\u0026quot;必须与原版保持一致，否则读取失败会引发隐性逻辑断层。\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"缺少分段局部最大值选择\"\u003e缺少分段局部最大值选择\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003e\u003cstrong\u003e现象\u003c/strong\u003e：提取出的 binary 描述子数量远少于参考代码。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e原因\u003c/strong\u003e：重构时为了简化，把原作者的\u0026quot;分段局部找最大值\u0026quot;的网格操作，简化成了\u0026quot;全局固定阈值过滤\u0026rdquo;。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e修复\u003c/strong\u003e：还原 5x5 网格局部最大值选择的机制。\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"法向量过滤条件逻辑错误\"\u003e法向量过滤条件逻辑错误\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003e\u003cstrong\u003e现象\u003c/strong\u003e：大量投影平面被错误跳过。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e原因\u003c/strong\u003e：把原版的 \u003ccode\u003e||\u003c/code\u003e 错写成了 \u003ccode\u003e\u0026amp;\u0026amp;\u003c/code\u003e。\u003ccode\u003eif (normal_diff.norm() \u0026lt; 0.3 \u0026amp;\u0026amp; normal_add.norm() \u0026gt; 0.3)\u003c/code\u003e 导致条件几近无法触发。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e修复\u003c/strong\u003e：将 \u003ccode\u003e\u0026amp;\u0026amp;\u003c/code\u003e 更正为 \u003ccode\u003e||\u003c/code\u003e。\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"兜底逻辑缺失\"\u003e兜底逻辑缺失\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003e\u003cstrong\u003e现象\u003c/strong\u003e：当没有检测到平面时，后续流程完全中断并崩溃。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e原因\u003c/strong\u003e：原作在没找到平面时会强行插入一个默认法向量的伪平面作为兜底，重构时丢失了这一行。\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e教训\u003c/strong\u003e：异常处理和边缘情况兜底往往是原作者踩过坑加上的，不能轻易删除。\u003c/li\u003e\n\u003c/ul\u003e\n\u003chr\u003e\n\u003ch2 id=\"c-工程与性能优化performance-optimizations\"\u003eC++ 工程与性能优化（Performance Optimizations）\u003c/h2\u003e\n\u003cp\u003e在算法逻辑打通后，我们发现代码耗时奇高，随后进行了深度底层优化，取得了 20 倍以上的加速。\u003c/p\u003e","title":"BTC 回环检测模块调试与性能优化经验总结"},{"content":"介绍 ROS 服务（Service）是一种请求-响应模式的通信机制，适合一次性查询或控制操作（区别于持续发布的 Topic）。\n1 2 3 4 5 客户端 (Client) 服务端 (Server) | | |--- 请求 (Request) -------------\u0026gt;| | | 处理逻辑 |\u0026lt;-- 响应 (Response) -------------| 原理 通信模型 ROS 服务基于 TCP/IP 的同步 RPC（远程过程调用）机制：\n服务端在 ROS Master 注册服务名和地址 客户端通过 ROS Master 查找服务端地址 客户端与服务端建立 TCP 直连 客户端发送请求，阻塞等待响应 服务端处理完成后返回响应，客户端解除阻塞 graph LR A[ROS Master\u0026lt;br\u0026gt;注册中心] --\u0026gt;|① 注册服务| B[Server\u0026lt;br\u0026gt;服务端] A --\u0026gt;|② 查找服务| C[Client\u0026lt;br\u0026gt;客户端] B \u0026lt;--\u0026gt;|③ 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-\u0026gt;\u0026gt;MRLO: get_transform(robot_id=1) MRLO-\u0026gt;\u0026gt;MRLO: 从 drone_gicp_poses_ 读取 MRLO--\u0026gt;\u0026gt;Other: transform (Pose) Other-\u0026gt;\u0026gt;Other: 使用变换矩阵转换坐标系 外部节点（如导航、规划模块）需要知道各机器人相对于 robot0 的坐标变换，通过服务按需获取，避免不必要的持续订阅。\n完整步骤 1. 定义 .srv 文件 在 srv/ 目录下创建服务定义文件，格式为：\n1 2 3 4 5 # 请求部分（--- 之上） 类型 字段名 --- # 响应部分（--- 之下） 类型 字段名 示例：srv/GetTransform.srv\n1 2 3 4 5 6 7 # Request int32 robot_id --- # Response bool success string message geometry_msgs/Pose transform 常用类型：int32, float64, bool, string, geometry_msgs/Pose, sensor_msgs/PointCloud2\n2. 修改 CMakeLists.txt 1 2 3 4 5 6 7 8 9 10 11 12 13 14 # ① find_package 中已包含 message_generation（通常已有） # ② 添加服务文件 add_service_files( FILES GetTransform.srv ) # ③ generate_messages 中确保依赖完整 generate_messages( DEPENDENCIES std_msgs geometry_msgs # 如果 srv 用了 geometry_msgs 类型 ) 3. 修改 package.xml 确认已有以下依赖（通常创建包时自动生成）：\n1 2 \u0026lt;build_depend\u0026gt;message_generation\u0026lt;/build_depend\u0026gt; \u0026lt;exec_depend\u0026gt;message_runtime\u0026lt;/exec_depend\u0026gt; 4. 服务端实现（C++） 头文件：\n1 2 3 4 5 6 #include \u0026lt;your_package/YourService.h\u0026gt; // 自动生成 // 类成员 ros::ServiceServer service_server_; bool serviceCallback(your_package::YourService::Request\u0026amp; req, your_package::YourService::Response\u0026amp; res); 构造函数中注册：\n1 2 service_server_ = nh_.advertiseService(\u0026#34;service_name\u0026#34;, \u0026amp;ClassName::serviceCallback, this); 回调实现：\n1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 bool ClassName::serviceCallback(your_package::YourService::Request\u0026amp; req, your_package::YourService::Response\u0026amp; res) { // 读取请求 int id = req.robot_id; // 处理逻辑 if (success) { res.success = true; res.message = \u0026#34;OK\u0026#34;; // 填充其他响应字段... } else { res.success = false; res.message = \u0026#34;Error: xxx\u0026#34;; } return true; // 返回 false 会关闭服务 } 5. 客户端调用 Python 方式 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 #!/usr/bin/env python3 import rospy from your_package.srv import YourService rospy.init_node(\u0026#39;my_client\u0026#39;) # 等待服务上线 rospy.wait_for_service(\u0026#39;service_name\u0026#39;) # 调用 try: proxy = rospy.ServiceProxy(\u0026#39;service_name\u0026#39;, YourService) resp = proxy(robot_id=1) # 参数名对应 srv 中的 Request 字段 if resp.success: print(resp.transform) else: print(\u0026#34;Failed:\u0026#34;, resp.message) except rospy.ServiceException as e: print(\u0026#34;Service call failed:\u0026#34;, e) C++ 方式 1 2 3 4 5 6 7 8 9 10 #include \u0026lt;your_package/YourService.h\u0026gt; your_package::YourService srv; srv.request.robot_id = 1; if (ros::service::call(\u0026#34;service_name\u0026#34;, srv)) { if (srv.response.success) { // 使用 srv.response.transform } } 命令行方式 1 2 3 4 5 # 查看服务类型 rosservice type /service_name # 调用（自动补全参数） rosservice call /service_name \u0026#34;robot_id: 1\u0026#34; 调试技巧 1 2 3 4 5 6 7 8 # 列出所有服务 rosservice list # 查看服务类型 rosservice type /get_transform # 查看服务详细信息（包含请求/响应字段） rossrv show multi_robot_loop_optimization/GetTransform 常见问题 问题 原因 解决 ImportError: No module named yaml shebang 用了 python2 改为 #!/usr/bin/env python3 服务调用超时 服务端未启动 确认 mrlo_node 已运行 rosservice call 报参数错误 参数格式不对 用 Tab 键自动补全 编译找不到 srv 头文件 CMakeLists 未配置 检查 add_service_files 和 generate_messages 参考 ROS Wiki Services：http://wiki.ros.org/Services ROS Wiki Writing a Simple Service and Client (C++)：http://wiki.ros.org/ROS/Tutorials/WritingServiceClient(c%2B%2B) ROS Wiki Writing a Simple Service and Client (Python)：http://wiki.ros.org/ROS/Tutorials/WritingServiceClient(python) ","permalink":"https://zero-kq.github.io/Pages/posts/ros/ros-service-guide/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003eROS 服务（Service）是一种\u003cstrong\u003e请求-响应\u003c/strong\u003e模式的通信机制，适合一次性查询或控制操作（区别于持续发布的 Topic）。\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e5\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-fallback\" data-lang=\"fallback\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e客户端 (Client)                    服务端 (Server)\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e     |                                  |\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e     |--- 请求 (Request) -------------\u0026gt;|\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e     |                                  | 处理逻辑\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e     |\u0026lt;-- 响应 (Response) -------------|\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003chr\u003e\n\u003ch2 id=\"原理\"\u003e原理\u003c/h2\u003e\n\u003ch3 id=\"通信模型\"\u003e通信模型\u003c/h3\u003e\n\u003cp\u003eROS 服务基于 \u003cstrong\u003eTCP/IP\u003c/strong\u003e 的同步 RPC（远程过程调用）机制：\u003c/p\u003e\n\u003col\u003e\n\u003cli\u003e\u003cstrong\u003e服务端\u003c/strong\u003e在 ROS Master 注册服务名和地址\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e客户端\u003c/strong\u003e通过 ROS Master 查找服务端地址\u003c/li\u003e\n\u003cli\u003e客户端与服务端建立 \u003cstrong\u003eTCP 直连\u003c/strong\u003e\u003c/li\u003e\n\u003cli\u003e客户端发送请求，\u003cstrong\u003e阻塞等待\u003c/strong\u003e响应\u003c/li\u003e\n\u003cli\u003e服务端处理完成后返回响应，客户端解除阻塞\u003c/li\u003e\n\u003c/ol\u003e\n\n\u003cdiv class=\"mermaid\"\u003egraph LR\n    A[ROS Master\u0026lt;br\u0026gt;注册中心] --\u0026gt;|① 注册服务| B[Server\u0026lt;br\u0026gt;服务端]\n    A --\u0026gt;|② 查找服务| C[Client\u0026lt;br\u0026gt;客户端]\n    B \u0026lt;--\u0026gt;|③ TCP 直连| C\u003c/div\u003e\n\u003ch3 id=\"与-topic-的对比\"\u003e与 Topic 的对比\u003c/h3\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e特性\u003c/th\u003e\n          \u003cth\u003eTopic（话题）\u003c/th\u003e\n          \u003cth\u003eService（服务）\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e通信模式\u003c/td\u003e\n          \u003ctd\u003e发布-订阅（异步）\u003c/td\u003e\n          \u003ctd\u003e请求-响应（同步）\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e是否阻塞\u003c/td\u003e\n          \u003ctd\u003e否，持续发布\u003c/td\u003e\n          \u003ctd\u003e是，等待响应\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e数据流向\u003c/td\u003e\n          \u003ctd\u003e单向（发布者→订阅者）\u003c/td\u003e\n          \u003ctd\u003e双向（请求+响应）\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e适用场景\u003c/td\u003e\n          \u003ctd\u003e持续数据流（传感器、位姿）\u003c/td\u003e\n          \u003ctd\u003e一次性操作（查询、控制）\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e典型例子\u003c/td\u003e\n          \u003ctd\u003e点云、里程计、地图\u003c/td\u003e\n          \u003ctd\u003e获取变换、保存文件、重置状态\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003ch3 id=\"底层实现\"\u003e底层实现\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003e服务定义文件 \u003ccode\u003e.srv\u003c/code\u003e 编译后自动生成 C++/Python 的请求和响应类\u003c/li\u003e\n\u003cli\u003e通信协议：\u003ccode\u003eroscpp\u003c/code\u003e 使用 TCPROS，\u003ccode\u003erospy\u003c/code\u003e 使用 XML-RPC + TCPROS\u003c/li\u003e\n\u003cli\u003e服务端回调函数在 ROS spinner 线程中执行，多个请求会排队处理\u003c/li\u003e\n\u003c/ul\u003e\n\u003chr\u003e\n\u003ch2 id=\"作用\"\u003e作用\u003c/h2\u003e\n\u003ch3 id=\"适用场景\"\u003e适用场景\u003c/h3\u003e\n\u003col\u003e\n\u003cli\u003e\u003cstrong\u003e状态查询\u003c/strong\u003e：获取某个变量的当前值（如本项目的变换矩阵查询）\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e配置修改\u003c/strong\u003e：运行时修改参数（如切换模式、调整阈值）\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e触发动作\u003c/strong\u003e：执行一次性操作（如保存地图、重置系统）\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e同步确认\u003c/strong\u003e：需要确认操作结果（如配准是否成功）\u003c/li\u003e\n\u003c/ol\u003e\n\u003ch3 id=\"本项目中的应用\"\u003e本项目中的应用\u003c/h3\u003e\n\n\u003cdiv class=\"mermaid\"\u003esequenceDiagram\n    participant Other as 其他节点\n    participant MRLO as MRLO 节点\n\n    Other-\u0026gt;\u0026gt;MRLO: get_transform(robot_id=1)\n    MRLO-\u0026gt;\u0026gt;MRLO: 从 drone_gicp_poses_ 读取\n    MRLO--\u0026gt;\u0026gt;Other: transform (Pose)\n    Other-\u0026gt;\u0026gt;Other: 使用变换矩阵转换坐标系\u003c/div\u003e\n\u003cp\u003e外部节点（如导航、规划模块）需要知道各机器人相对于 robot0 的坐标变换，通过服务按需获取，避免不必要的持续订阅。\u003c/p\u003e","title":"ROS 服务创建与调用指南"},{"content":"介绍 在 LIO-SLAM 系统中，IMU（惯性测量单元）扮演着至关重要的角色。IMU 提供高频（通常 200-400Hz）的角速度和线加速度测量，用于：\n状态初始化：确定系统的初始重力方向、零偏和缩放系数 状态预测（前向传播）：在两帧 LiDAR 之间递推位姿，为滤波器提供预测值 点云去畸变（后向传播）：将一帧扫描周期内的点云补偿到同一时刻，消除运动畸变 ZLIO 中 IMU 处理的核心流程如下：\n1 IMU 初始化 → 前向传播 (递推状态 + 记录位姿序列) → 后向传播 (点云去畸变) 涉及的核心文件：\n文件 作用 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 中：\n1 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：\n1 2 3 4 5 6 struct MeasureGroup { double lidar_begin_time = 0.0; // 点云开始时间 (s) double lidar_end_time = 0.0; // 点云结束时间 (s) std::deque\u0026lt;Imu\u0026gt; imus; // 该帧点云时间段内的 IMU 数据 PointCloud cloud; // 点云数据 }; 设计要点：\n一帧 LiDAR 点云的时间跨度内会有多条 IMU 数据，它们被同步打包到 MeasureGroup 中 点云中每个点的时间信息存储在 curvature 字段中，表示相对于点云起始时刻的偏移（秒） IMU 初始化 原理 IMU 初始化的目的是确定系统的初始状态，包括：\n重力方向：IMU 测量的加速度包含重力分量，通过平均多帧加速度数据可以确定重力方向 加速度计缩放系数：实际 IMU 可能存在缩放误差，通过比较测量值模长与理论重力值 g=9.81 可以计算缩放系数 陀螺仪零偏：静止状态下角速度的平均值即为零偏的估计 数学原理：\n在静止或匀速运动假设下，加速度计测量值为：\n$$a_{measured} = R^T \\cdot (-g) + b_a + n_a$$其中 $R$ 是 IMU 到世界系的旋转，$g$ 是重力加速度向量。当载体静止时：\n$$\\|a_{measured}\\| \\approx g = 9.81 \\text{ m/s}^2$$因此缩放系数为：\n$$s = \\frac{g}{\\|a_{mean}\\|}$$重力方向可由平均加速度方向确定：\n$$g_{vec} = -\\frac{a_{mean}}{\\|a_{mean}\\|} \\cdot g$$代码实现 初始化逻辑在 frontend.cpp:initState() 中实现：\n1 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 void FrontEnd::initState(MeasureGroup \u0026amp;_mg) { static int imu_count = 0; static Eigen::Vector3d mean_acc{0, 0, 0}; // 累加 IMU 数据 for (size_t i = 0; i \u0026lt; _mg.imus.size(); i++) { imu_count++; auto x = ieskf.getX(); mean_acc += _mg.imus[i].acceleration; // 累加加速度 x.bg += _mg.imus[i].gyroscope; // 累加角速度（用于零偏估计） ieskf.setX(x); } // 累计 5 个 IMU 数据后完成初始化 if (imu_count \u0026gt;= 5) { auto x = ieskf.getX(); mean_acc /= double(imu_count); // 计算平均加速度 x.bg /= double(imu_count); // 计算平均角速度（零偏） // 计算缩放系数: 实际重力值 / 测量值模长 imu_scale_ = GRAVITY / mean_acc.norm(); imu_process_ptr_-\u0026gt;imu_scale = imu_scale_; // 初始化 ImuProcess 的历史状态 imu_process_ptr_-\u0026gt;last_imu = _mg.imus.back(); imu_process_ptr_-\u0026gt;last_lidar_end_time = _mg.lidar_end_time; // 确定重力向量（注意符号为负，与 FAST-LIO 公式一致） x.gravity = -mean_acc / mean_acc.norm() * GRAVITY; ieskf.setX(x); imu_inited_ = true; } } 关键步骤解读：\n步骤 操作 作用 1 累加 acceleration 和 gyroscope 收集初始化所需的统计数据 2 imu_count \u0026gt;= 5 判断 确保有足够数据进行平均，避免噪声影响 3 imu_scale_ = GRAVITY / mean_acc.norm() 计算加速度计缩放系数 4 x.gravity = -mean_acc / mean_acc.norm() * GRAVITY 确定重力方向（世界系下） 5 last_imu / last_lidar_end_time 初始化 ImuProcess 的递推起点 重力符号说明：x.gravity 存储的是世界系下的重力加速度向量，取负号是因为在 IESKF 的状态递推公式中：\n$$\\dot{v} = R \\cdot (a_m - b_a) + g$$其中 $g$ 是重力加速度（方向向下），与 FAST-LIO 的公式约定一致。\n前向传播 (Forward Propagation) 原理 前向传播的核心任务是：利用 IMU 数据在两帧 LiDAR 之间递推系统状态，并记录递推过程中的中间位姿序列，为后续的点云去畸变提供位姿参考。\n运动模型（连续时间）：\n$$\\dot{R} = R \\cdot [\\omega_m - b_g]_{\\times}$$$$\\dot{v} = R \\cdot (a_m - b_a) + g$$$$\\dot{p} = v$$其中 $[\\cdot]_{\\times}$ 表示反对称矩阵（叉积的矩阵形式）。\n离散化（中值积分）：\n对于相邻两个 IMU 时刻 $k$ 和 $k+1$，取两端测量值的均值：\n$$\\bar{\\omega} = \\frac{1}{2}(\\omega_k + \\omega_{k+1})$$$$\\bar{a} = \\frac{1}{2}(a_k + a_{k+1})$$状态更新：\n$$R_{k+1} = R_k \\cdot \\exp([\\bar{\\omega} - b_g]_{\\times} \\cdot \\Delta t)$$$$v_{k+1} = v_k + (R_k \\cdot (\\bar{a} - b_a) + g) \\cdot \\Delta t$$$$p_{k+1} = p_k + v_k \\cdot \\Delta t$$ 注意：代码中的位置更新使用了简化的欧拉积分 p += v * dt，而非二阶积分 p += v*dt + 0.5*a*dt^2。这是因为前向传播的主要目的是记录位姿序列用于去畸变，去畸变阶段会使用更精确的二次积分。\n代码实现 前向传播实现在 imu_process.cpp:forwardPropagate() 中：\n1 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 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 void ImuProcess::forwardPropagate(MeasureGroup \u0026amp;_mg, IESKF::Ptr _ieskf_ptr, std::vector\u0026lt;IMUPose6d\u0026gt; \u0026amp;_poses_out) { // ===== 步骤 1: 点云按时间排序 ===== // curvature 字段存储的是相对点云起始时刻的时间偏移 (秒) std::sort(_mg.cloud.cloud_ptr-\u0026gt;points.begin(), _mg.cloud.cloud_ptr-\u0026gt;points.end(), [](const PointType \u0026amp;_x, const PointType \u0026amp;_y) { return _x.curvature \u0026lt; _y.curvature; }); // ===== 步骤 2: 准备 IMU 数据 ===== // 将上一帧最后一条 IMU 数据插入队列头部，保证时间连续性 auto v_imu = _mg.imus; v_imu.push_front(last_imu); const double \u0026amp;pcl_beg_time = _mg.lidar_begin_time; const double \u0026amp;pcl_end_time = _mg.lidar_end_time; auto imu_state = _ieskf_ptr-\u0026gt;getX(); // ===== 步骤 3: 记录递推起始状态 ===== _poses_out.clear(); _poses_out.emplace_back(0.0, acc_s_last, angvel_last, imu_state.velocity, imu_state.position, imu_state.rotation); // ===== 步骤 4: 逐段递推 ===== Eigen::Vector3d angvel_avr, acc_avr; double dt = 0.0; Imu in; for (auto it_imu = v_imu.begin(); it_imu \u0026lt; (v_imu.end() - 1); it_imu++) { auto \u0026amp;\u0026amp;head = *(it_imu); auto \u0026amp;\u0026amp;tail = *(it_imu + 1); // 中值积分：取两端均值 angvel_avr = 0.5 * (head.gyroscope + tail.gyroscope); acc_avr = 0.5 * (head.acceleration + tail.acceleration) * imu_scale; // 计算时间步长，处理与上一帧的衔接 if (head.timestamp \u0026lt; last_lidar_end_time) { dt = tail.timestamp - last_lidar_end_time; // 跨帧衔接 } else { dt = tail.timestamp - head.timestamp; // 正常步长 } if (dt \u0026lt;= 0.0) continue; // 执行 IESKF 预测步 in.acceleration = acc_avr; in.gyroscope = angvel_avr; _ieskf_ptr-\u0026gt;predict(in, dt); // 获取更新后的状态 imu_state = _ieskf_ptr-\u0026gt;getX(); // 计算世界系下的加速度（用于去畸变） angvel_last = angvel_avr - imu_state.bg; acc_s_last = imu_state.rotation * (acc_avr - imu_state.ba); acc_s_last += imu_state.gravity; // 补偿重力 // 记录该时刻位姿，时间戳为相对于点云起始时刻的偏移 double offs_t = tail.timestamp - pcl_beg_time; _poses_out.emplace_back(offs_t, acc_s_last, angvel_last, imu_state.velocity, imu_state.position, imu_state.rotation); } // ===== 步骤 5: 补齐到点云结束时刻 ===== double dt_final = pcl_end_time - v_imu.back().timestamp; if (dt_final \u0026gt; 0.0) { _ieskf_ptr-\u0026gt;predict(in, dt_final); } // 更新历史状态 last_imu = _mg.imus.back(); last_lidar_end_time = pcl_end_time; } IMUPose6d 结构体记录了每个递推时刻的关键信息：\n1 2 3 4 5 6 7 8 struct IMUPose6d { double time; // 相对点云起始时刻的时间偏移 (s) Eigen::Vector3d acc; // 世界系下的加速度（已补偿重力和零偏） Eigen::Vector3d angvel; // 角速度（已减去零偏） Eigen::Vector3d vel; // 世界系速度 Eigen::Vector3d pos; // 世界系位置 Eigen::Quaterniond rot; // 世界系旋转 }; IESKF 预测步实现 ieskf.cpp:predict()：\n1 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 void IESKF::predict(const Imu \u0026amp;_imu, double _dt) { // 1. 状态递推 (Nominal State Prediction) Eigen::Vector3d angvel = _imu.gyroscope - x_.bg; // 去零偏 Eigen::Vector3d acc = _imu.acceleration - x_.ba; // 去零偏 auto rotation_mat = x_.rotation.toRotationMatrix(); // 旋转更新: R_{k+1} = R_k * exp([ω]× * dt) x_.rotation = Eigen::Quaterniond(rotation_mat * so3Exp(angvel * _dt)); x_.rotation.normalize(); // 位置和速度更新 x_.position += x_.velocity * _dt; x_.velocity += (rotation_mat * acc + x_.gravity) * _dt; // 2. 协方差传播 (Covariance Propagation) // P_{k+1} = Fx * P_k * Fx^T + Fw * Q * Fw^T Eigen::Matrix\u0026lt;double, 18, 18\u0026gt; Fx = Eigen::Matrix\u0026lt;double, 18, 18\u0026gt;::Identity(); Eigen::Matrix\u0026lt;double, 18, 12\u0026gt; Fw = Eigen::Matrix\u0026lt;double, 18, 12\u0026gt;::Zero(); // Fx: 状态转移矩阵（误差状态对误差状态的雅可比） Fx.block\u0026lt;3, 3\u0026gt;(0, 0) = so3Exp(-angvel * _dt); // δR 对 δR Fx.block\u0026lt;3, 3\u0026gt;(0, 9) = -A_T(-angvel * _dt) * _dt; // δbg 对 δR Fx.block\u0026lt;3, 3\u0026gt;(3, 6) = Eigen::Matrix3d::Identity() * _dt; // δv 对 δp Fx.block\u0026lt;3, 3\u0026gt;(6, 0) = -rotation_mat * skewSymmetric(acc) * _dt; // δR 对 δv Fx.block\u0026lt;3, 3\u0026gt;(6, 12) = -rotation_mat * _dt; // δba 对 δv Fx.block\u0026lt;3, 3\u0026gt;(6, 15) = Eigen::Matrix3d::Identity() * _dt; // δg 对 δv // Fw: 噪声转移矩阵（误差状态对噪声的雅可比） Fw.block\u0026lt;3, 3\u0026gt;(0, 0) = -A_T(-angvel * _dt) * _dt; // 陀螺仪噪声 Fw.block\u0026lt;3, 3\u0026gt;(6, 3) = -rotation_mat * _dt; // 加速度计噪声 Fw.block\u0026lt;3, 3\u0026gt;(9, 6) = Eigen::Matrix3d::Identity() * _dt; // 零偏随机游走 Fw.block\u0026lt;3, 3\u0026gt;(12, 9) = Eigen::Matrix3d::Identity() * _dt; P_ = Fx * P_ * Fx.transpose() + Fw * Q_ * Fw.transpose(); } 18 维状态向量布局：\n索引 维度 状态 说明 0-2 3 δθ (旋转误差) SO3 李代数表示 3-5 3 δp (位置误差) 6-8 3 δv (速度误差) 9-11 3 δb_g (陀螺仪零偏) 12-14 3 δb_a (加速度计零偏) 15-17 3 δg (重力误差) 12 维噪声向量布局：\n索引 维度 噪声源 0-2 3 陀螺仪测量噪声 3-5 3 加速度计测量噪声 6-8 3 陀螺仪零偏随机游走 9-11 3 加速度计零偏随机游走 后向传播与去畸变 (Undistortion) 原理 由于 LiDAR 扫描一帧点云需要一定时间（例如 100ms），在这段时间内载体是运动的，因此同一帧点云中的不同点实际上是在不同位姿下采集的。这会导致点云出现运动畸变。\n去畸变的目标是：将一帧点云中的所有点变换到同一时刻（通常是扫描结束时刻），消除运动带来的畸变。\n数学推导：\n设扫描结束时刻为 $C$，点 $P_i$ 的采集时刻为 $t_i$，则：\n$R_i, p_i$：时刻 $t_i$ 的位姿（由前向传播插值得到） $R_C, p_C$：扫描结束时刻 $C$ 的位姿 $P_i^{L}$：点在采集时刻雷达坐标系下的坐标 点在世界系下的坐标：\n$$P_i^{G} = R_i \\cdot P_i^{L} + p_i$$变换到结束时刻雷达系：\n$$P_i^{C} = R_C^T \\cdot (P_i^{G} - p_C) = R_C^T \\cdot (R_i \\cdot P_i^{L} + p_i - p_C)$$旋转插值：在 $t_i$ 时刻的旋转通过角速度插值得到：\n$$R_i = R_{head} \\cdot \\exp([\\omega \\cdot (t_i - t_{head})]_{\\times})$$其中 $R_{head}$ 是该 IMU 段起始时刻的旋转，$\\omega$ 是该段的平均角速度。\n位置插值：使用二阶积分：\n$$p_i = p_{head} + v_{head} \\cdot \\Delta t + \\frac{1}{2} a \\cdot \\Delta t^2$$其中 $a$ 是世界系下的加速度（已补偿重力和零偏）。\n代码实现 去畸变实现在 imu_process.cpp:undistortPcl() 中：\n1 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 40 41 42 43 44 45 46 47 48 49 void ImuProcess::undistortPcl(MeasureGroup \u0026amp;_mg, const std::vector\u0026lt;IMUPose6d\u0026gt; \u0026amp;_poses_in, IESKF::Ptr _ieskf_ptr) { auto \u0026amp;pcl_out = *(_mg.cloud.cloud_ptr); auto it_pcl = pcl_out.points.end() - 1; // 从最后一个点开始（时间最晚） // 获取扫描终点 C 的位姿 auto imu_state_final = _ieskf_ptr-\u0026gt;getX(); // 从后往前遍历 IMU 位姿段 for (auto it_kp = _poses_in.end() - 1; it_kp != _poses_in.begin(); it_kp--) { auto head = it_kp - 1; // 当前段起始 auto tail = it_kp; // 当前段结束 Eigen::Matrix3d R_imu = head-\u0026gt;rot.toRotationMatrix(); Eigen::Vector3d vel_imu = head-\u0026gt;vel; Eigen::Vector3d pos_imu = head-\u0026gt;pos; Eigen::Vector3d acc_imu = tail-\u0026gt;acc; // 使用尾部的加速度 Eigen::Vector3d angvel_avr = tail-\u0026gt;angvel; // 使用尾部的角速度 // 遍历落在该时间段内的点 for (; it_pcl-\u0026gt;curvature \u0026gt; head-\u0026gt;time; it_pcl--) { double dt = it_pcl-\u0026gt;curvature - head-\u0026gt;time; // 插值计算点时刻的旋转: R_i = R_head * exp([ω * dt]×) Eigen::Matrix3d R_i(R_imu * so3Exp(angvel_avr * dt)); // 点在采集时刻雷达系下的坐标 Eigen::Vector3d P_i(it_pcl-\u0026gt;x, it_pcl-\u0026gt;y, it_pcl-\u0026gt;z); // 计算点时刻的位置偏移（相对于扫描终点） // T_ei = p_i^G - p_C^G Eigen::Vector3d T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state_final.position); // 变换到扫描终点时刻的雷达系 // P_C = R_G^C * (R_i * P_i + T_ei) Eigen::Vector3d P_compensate = imu_state_final.rotation.conjugate() * (R_i * P_i + T_ei); // 写回点云 it_pcl-\u0026gt;x = P_compensate(0); it_pcl-\u0026gt;y = P_compensate(1); it_pcl-\u0026gt;z = P_compensate(2); if (it_pcl == pcl_out.points.begin()) break; } } } 去畸变流程图：\n1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 时间轴: ─────────────────────────────────────────────► | IMU段0 | IMU段1 | IMU段2 | IMU段3 | ↑ ↑ 扫描开始 扫描结束 (C) 点云点: • • • • •• • •• • • •• • • • • ──────────────────────────────────────────→ 从后往前遍历点，按时间分配到对应 IMU 段 对每个点 P_i: 1. 确定所在 IMU 段 [head, tail] 2. dt = P_i.curvature - head.time 3. R_i = R_head * exp(ω * dt) // 旋转插值 4. p_i = p_head + v*dt + 0.5*a*dt² // 位置插值 5. P_out = R_C^T * (R_i * P_i + p_i - p_C) // 变换到终点系 为什么从后往前遍历？\n点云已按时间排序（前向传播的第一步） 从后往前遍历可以保证每个点只需检查当前 IMU 段，时间复杂度为 O(N) 如果从前往后遍历，每个点都需要重新查找所属的 IMU 段 数学工具 IMU 处理依赖的核心数学工具定义在 math_utils.h 中：\n反对称矩阵 (Skew Symmetric Matrix) 向量 $v = [v_1, v_2, v_3]^T$ 的反对称矩阵：\n$$[v]_{\\times} = \\begin{bmatrix} 0 \u0026 -v_3 \u0026 v_2 \\\\ v_3 \u0026 0 \u0026 -v_1 \\\\ -v_2 \u0026 v_1 \u0026 0 \\end{bmatrix}$$作用：将叉积转化为矩阵乘法 $a \\times b = [a]_{\\times} \\cdot b$\nSO3 指数映射 (Rodrigues 公式) 将旋转向量 $\\theta \\cdot n$ 转换为旋转矩阵：\n$$\\exp([\\theta n]_{\\times}) = I + \\sin\\theta \\cdot [n]_{\\times} + (1 - \\cos\\theta) \\cdot [n]_{\\times}^2$$ 1 2 3 4 5 6 7 8 static inline Eigen::Matrix3d so3Exp(const Eigen::Vector3d \u0026amp;_v) { double theta = _v.norm(); if (theta \u0026lt; 1e-7) return Eigen::Matrix3d::Identity(); // 小角度近似 Eigen::Vector3d n = _v / theta; Eigen::Matrix3d n_skew = skewSymmetric(n); return Eigen::Matrix3d::Identity() + std::sin(theta) * n_skew + (1.0 - std::cos(theta)) * n_skew * n_skew; } SO3 对数映射 将旋转矩阵转换回旋转向量：\n$$\\theta = \\arccos\\left(\\frac{tr(R) - 1}{2}\\right)$$$$[\\theta n]_{\\times} = \\frac{\\theta}{2\\sin\\theta}(R - R^T)$$右雅可比 A_T 用于 IESKF 协方差传播中的误差状态转移：\n$$J_r(\\theta n) = I + \\frac{1 - \\cos\\theta}{\\theta^2} [n]_{\\times} + \\frac{\\theta - \\sin\\theta}{\\theta^3} [n]_{\\times}^2$$ 完整数据流 将所有部分串联起来，IMU 处理在 ZLIO 系统中的完整数据流如下：\n1 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 40 41 ┌─────────────────────────────────────────────────────────────────┐ │ FrontEnd::track() │ │ │ │ ┌──────────────────┐ │ │ │ syncMeasureGroup │ ← 同步 IMU 和点云数据 │ │ └────────┬─────────┘ │ │ │ │ │ ▼ │ │ ┌──────────────────┐ No ┌───────────────┐ │ │ │ imu_inited_? │ ─────────→ │ initState() │ │ │ └────────┬─────────┘ │ · 累加 acc │ │ │ │ Yes │ · 累加 gyro │ │ │ │ │ · 计算 gravity │ │ │ │ │ · 计算 scale │ │ │ │ └───────┬───────┘ │ │ ▼ │ │ │ ┌──────────────────────────────────────────┐ │ │ │ imu_process_ptr_-\u0026gt;propagate(mg, ieskf) │ │ │ │ │ │ │ │ ┌─────────────────────────────────────┐ │ │ │ │ │ 1. forwardPropagate() │ │ │ │ │ │ · 点云按时间排序 │ │ │ │ │ │ · 逐段中值积分递推 │ │ │ │ │ │ · 调用 ieskf-\u0026gt;predict() 更新状态 │ │ │ │ │ │ · 记录 IMUPose6d 位姿序列 │ │ │ │ │ └──────────────┬──────────────────────┘ │ │ │ │ │ │ │ │ │ ▼ │ │ │ │ ┌─────────────────────────────────────┐ │ │ │ │ │ 2. undistortPcl() │ │ │ │ │ │ · 从后往前遍历点云 │ │ │ │ │ │ · 旋转插值 + 位置二阶积分 │ │ │ │ │ │ · 变换到扫描终点坐标系 │ │ │ │ │ └─────────────────────────────────────┘ │ │ │ └───────────────────────────────────────────┘ │ │ │ │ │ ▼ │ │ ┌──────────────────┐ │ │ │ IESKF::update() │ ← 观测更新（点到面残差） │ │ └──────────────────┘ │ └─────────────────────────────────────────────────────────────────┘ 关键设计总结 设计点 选择 原因 积分方式 中值积分 比欧拉积分精度更高，比 RK4 计算量小 时间衔接 last_lidar_end_time 保证帧间 IMU 数据不丢失不重复 去畸变基准 扫描结束时刻 与 IESKF 更新步的状态时间对齐 遍历方向 从后往前 点云已排序，单次遍历即可完成去畸变 旋转表示 四元数 + SO3 李代数 避免万向锁，指数映射计算高效 状态维度 18 维 包含重力估计，提高鲁棒性 参考 FAST-LIO 源码：https://github.com/hku-mars/FAST_LIO IESKF 理论：Iterated Extended Kalman Filter for SLAM SO3 李代数：State Estimation for Robotics (Timothy D. Barfoot) ","permalink":"https://zero-kq.github.io/Pages/posts/slam/lio%E5%AD%A6%E4%B9%A0%E7%AC%94%E8%AE%B003/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e在 LIO-SLAM 系统中，IMU（惯性测量单元）扮演着至关重要的角色。IMU 提供高频（通常 200-400Hz）的角速度和线加速度测量，用于：\u003c/p\u003e\n\u003col\u003e\n\u003cli\u003e\u003cstrong\u003e状态初始化\u003c/strong\u003e：确定系统的初始重力方向、零偏和缩放系数\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e状态预测（前向传播）\u003c/strong\u003e：在两帧 LiDAR 之间递推位姿，为滤波器提供预测值\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e点云去畸变（后向传播）\u003c/strong\u003e：将一帧扫描周期内的点云补偿到同一时刻，消除运动畸变\u003c/li\u003e\n\u003c/ol\u003e\n\u003cp\u003eZLIO 中 IMU 处理的核心流程如下：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-fallback\" data-lang=\"fallback\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eIMU 初始化 → 前向传播 (递推状态 + 记录位姿序列) → 后向传播 (点云去畸变)\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e涉及的核心文件：\u003c/p\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e文件\u003c/th\u003e\n          \u003cth\u003e作用\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003eimu_type.h\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003eIMU 数据结构定义\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003eimu_process.h/cpp\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003eIMU 前向/后向传播与去畸变\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003eieskf.h/cpp\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003eIESKF 滤波器，包含 predict 步\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003efrontend.h/cpp\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e前端模块，协调 IMU 初始化与数据同步\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003emath_utils.h\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e李群李代数数学工具\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003chr\u003e\n\u003ch2 id=\"imu-数据结构\"\u003eIMU 数据结构\u003c/h2\u003e\n\u003cp\u003eIMU 数据结构定义在 \u003ca href=\"src/zlio/include/core/type/imu_type.h\"\u003eimu_type.h\u003c/a\u003e 中：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e6\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"k\"\u003eclass\u003c/span\u003e \u003cspan class=\"nc\"\u003eImu\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e \u003cspan class=\"k\"\u003epublic\u003c/span\u003e\u003cspan class=\"o\"\u003e:\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e  \u003cspan class=\"n\"\u003eEigen\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eVector3d\u003c/span\u003e \u003cspan class=\"n\"\u003eacceleration\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003eEigen\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eVector3d\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eZero\u003c/span\u003e\u003cspan class=\"p\"\u003e();\u003c/span\u003e \u003cspan class=\"c1\"\u003e// 线加速度 (m/s^2)\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e  \u003cspan class=\"n\"\u003eEigen\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eVector3d\u003c/span\u003e \u003cspan class=\"n\"\u003egyroscope\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003eEigen\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eVector3d\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eZero\u003c/span\u003e\u003cspan class=\"p\"\u003e();\u003c/span\u003e    \u003cspan class=\"c1\"\u003e// 角速度 (rad/s)\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e  \u003cspan class=\"kt\"\u003edouble\u003c/span\u003e \u003cspan class=\"n\"\u003etimestamp\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.0\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e                                 \u003cspan class=\"c1\"\u003e// 时间戳 (s)\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"p\"\u003e};\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003eIMU 数据以\u003cstrong\u003e测量组 (MeasureGroup)\u003c/strong\u003e 的形式与点云同步后一起处理，定义在 \u003ca href=\"src/zlio/include/core/type/measure_group_type.h\"\u003emeasure_group_type.h\u003c/a\u003e：\u003c/p\u003e","title":"LIO学习笔记03"},{"content":"介绍 体素地图是 ZLIO 系统的核心数据结构之一，用于维护局部三维环境的紧凑表示。相比传统的 KD-Tree + 暴力搜索方案，体素地图通过哈希表实现 O(1) 的体素查找，并利用点集统计量 (PointCluster) 避免存储原始点云，大幅降低内存占用。\n核心设计思想：\n空间离散化：将连续的三维空间划分为固定大小的立方体（体素），每个体素由整数坐标 (x, y, z) 索引 增量统计：不存储原始点，而是维护每个体素内点的一阶矩（坐标和）和二阶矩（坐标外积和），可在线计算质心和协方差 平面检测：通过协方差矩阵的特征值分解判断体素内点是否构成平面，为 IESKF 提供点到平面的观测约束 涉及的核心文件：\n文件 作用 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 \u0026amp;other) const { return (x == other.x \u0026amp;\u0026amp; y == other.y \u0026amp;\u0026amp; z == other.z); } }; 设计要点：\n使用 int64_t 而非 int，支持极大的坐标范围（避免负数取整溢出） 重载 operator== 以支持哈希表的相等比较 三个整数唯一确定一个体素在空间中的位置 哈希函数 VoxelHash 1 2 3 4 5 struct VoxelHash { size_t operator()(const VOXEL_LOC \u0026amp;v) const { return (((v.x * 73856093) ^ (v.y * 19349663) ^ (v.z * 83492791)) % 10000000); } }; 这是一个经典的空间哈希函数，三个大质数分别与坐标异或后取模，将三维整数坐标映射到一维哈希值。选择大质数可以减少哈希冲突。\n点集统计 PointCluster 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 class PointCluster { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix3d P = Eigen::Matrix3d::Zero(); // 二阶矩: sum(p * p^T) Eigen::Vector3d v = Eigen::Vector3d::Zero(); // 一阶矩: sum(p) int N = 0; // 点数 void add(const Eigen::Vector3d \u0026amp;point) { P += point * point.transpose(); // 累加外积 v += point; // 累加坐标 N++; } Eigen::Vector3d getCenter() const { return v / static_cast\u0026lt;double\u0026gt;(N); // 质心 = 一阶矩 / 点数 } Eigen::Matrix3d getCovariance() const { if (N \u0026lt; 3) return Eigen::Matrix3d::Identity(); Eigen::Vector3d center = getCenter(); return P / static_cast\u0026lt;double\u0026gt;(N) - center * center.transpose(); // 协方差 = E[pp^T] - E[p]E[p]^T } }; 核心数学原理：\nPointCluster 利用在线算法维护统计量，避免存储全部原始点：\n统计量 公式 含义 一阶矩 $v$ $\\sum_{i=1}^{N} p_i$ 坐标之和 二阶矩 $P$ $\\sum_{i=1}^{N} p_i p_i^T$ 外积之和 质心 $\\bar{p}$ $v / N$ 几何中心 协方差 $\\Sigma$ $P/N - \\bar{p}\\bar{p}^T$ 分散程度 增量更新：每来一个新点，只需执行一次向量加法和一次矩阵加法，无需回溯历史数据。这使得 add() 操作的时间复杂度为 O(1)。\n为什么不用存储原始点？\n在 IESKF 更新步中，我们只需要：\n体素的质心（用于计算点到平面距离） 体素的协方差矩阵（用于判断是否为平面、提取法向量） 这两个量都可以从 PointCluster 的三个统计量 (N, v, P) 直接计算得到。\n体素地图管理器 VoxelMapManager 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 class VoxelMapManager : public ModuleBase { public: using Ptr = std::shared_ptr\u0026lt;VoxelMapManager\u0026gt;; void reset(); void addScan(PCLPointCloudPtr _curr_scan, const Eigen::Quaterniond \u0026amp;_att_q, const Eigen::Vector3d \u0026amp;_pos_t); void saveMapPCD(const std::string \u0026amp;_file_path); // 保存质心 PCD void saveMapBinary(const std::string \u0026amp;_file_path); // 保存二进制格式 private: // 局部地图：用于 IESKF 跟踪，会自动清理远离当前位置的体素 std::unordered_map\u0026lt;VOXEL_LOC, PointCluster, VoxelHash\u0026gt; voxel_map_local_; // 全局地图：用于保存，只增不减 std::unordered_map\u0026lt;VOXEL_LOC, PointCluster, VoxelHash\u0026gt; voxel_map_global_; mutable std::mutex mtx_map_; double voxel_size_ = 0.5; // 体素大小 (m) double map_side_ = 50.0; // 局部地图维护范围 (m) }; 双地图设计：\n地图 用途 特点 voxel_map_local_ IESKF 跟踪的局部地图 会定期清理远离当前位姿的体素，控制内存 voxel_map_global_ 完整地图保存 只增不减，用于最终地图导出 体素索引原理 将世界坐标转换为体素整数坐标的公式：\n$$voxel\\_x = \\lfloor \\frac{p_x}{voxel\\_size} \\rfloor, \\quad voxel\\_y = \\lfloor \\frac{p_y}{voxel\\_size} \\rfloor, \\quad voxel\\_z = \\lfloor \\frac{p_z}{voxel\\_size} \\rfloor$$其中 $\\lfloor \\cdot \\rfloor$ 表示向下取整。\n1 2 3 4 5 6 7 8 9 10 11 示意图 (2D 视角, voxel_size = 1.0): y 3 ┌───┬───┬───┬───┐ │ │ │ │ │ 2 ├───┼───┼───┼───┤ │ │ ● │ │ │ ● = 点 (1.3, 2.7) 1 ├───┼───┼───┼───┤ 体素索引 = (⌊1.3/1⌋, ⌊2.7/1⌋) = (1, 2) │ │ │ │ │ 0 ├───┼───┼───┼───┤ 0 1 2 3 4 x 代码实现：\n1 2 3 4 5 VOXEL_LOC loc( static_cast\u0026lt;int64_t\u0026gt;(std::floor(world_point.x() / voxel_size_)), static_cast\u0026lt;int64_t\u0026gt;(std::floor(world_point.y() / voxel_size_)), static_cast\u0026lt;int64_t\u0026gt;(std::floor(world_point.z() / voxel_size_)) ); std::floor 保证负坐标也能正确映射（例如 -0.3 会映射到 -1 而非 0）。\n地图构建流程 addScan 整体流程 flowchart TD A(\u0026#34;输入: 当前帧点云 _curr_scan (雷达系) \u0026#43; 位姿 (_att_q, _pos_t)\u0026#34;) A --\u0026gt; B[\u0026#34;**步骤 1: 并行预计算世界坐标和体素索引 (不涉及锁)**\u0026lt;br\u0026gt;\u0026lt;br\u0026gt;for each point p in _curr_scan:\u0026lt;br\u0026gt;\u0026amp;nbsp;\u0026amp;nbsp;p_world = R * p \u0026#43; t\u0026lt;br\u0026gt;\u0026amp;nbsp;\u0026amp;nbsp;voxel_loc = floor(p_world / voxel_size)\u0026#34;] B --\u0026gt; C[\u0026#34;**步骤 2: 串行更新地图 (带锁)**\u0026lt;br\u0026gt;\u0026lt;br\u0026gt;for each (p_world, voxel_loc):\u0026lt;br\u0026gt;\u0026amp;nbsp;\u0026amp;nbsp;voxel_map_local_[voxel_loc].add(p_world)\u0026lt;br\u0026gt;\u0026amp;nbsp;\u0026amp;nbsp;voxel_map_global_[voxel_loc].add(p_world)\u0026#34;] C --\u0026gt; D[\u0026#34;**步骤 3: 地图维护 (每 50 帧执行一次)**\u0026lt;br\u0026gt;\u0026lt;br\u0026gt;遍历 voxel_map_local_，剔除距离当前位置\u0026lt;br\u0026gt;超过 map_side 的体素\u0026#34;] style B text-align:left style C text-align:left 代码实现 voxel_map_manager.cpp:addScan()：\n1 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 40 41 42 43 44 45 46 47 48 49 50 51 void VoxelMapManager::addScan(PCLPointCloudPtr _curr_scan, const Eigen::Quaterniond \u0026amp;_att_q, const Eigen::Vector3d \u0026amp;_pos_t) { if (_curr_scan-\u0026gt;empty()) return; int cloud_size = _curr_scan-\u0026gt;size(); std::vector\u0026lt;Eigen::Vector3d\u0026gt; world_points(cloud_size); std::vector\u0026lt;VOXEL_LOC\u0026gt; voxel_locs(cloud_size); Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); transform.rotate(_att_q); transform.pretranslate(_pos_t); // ===== 步骤 1: 并行预计算 (只读，无锁) ===== #ifdef MP_EN omp_set_num_threads(MP_PROC_NUM); #pragma omp parallel for #endif for (int i = 0; i \u0026lt; cloud_size; i++) { const auto \u0026amp;p = _curr_scan-\u0026gt;points[i]; world_points[i] = transform * Eigen::Vector3d(p.x, p.y, p.z); voxel_locs[i] = VOXEL_LOC( static_cast\u0026lt;int64_t\u0026gt;(std::floor(world_points[i].x() / voxel_size_)), static_cast\u0026lt;int64_t\u0026gt;(std::floor(world_points[i].y() / voxel_size_)), static_cast\u0026lt;int64_t\u0026gt;(std::floor(world_points[i].z() / voxel_size_))); } // ===== 步骤 2: 串行更新地图 (写操作，加锁) ===== { std::lock_guard\u0026lt;std::mutex\u0026gt; lock(mtx_map_); for (int i = 0; i \u0026lt; cloud_size; i++) { voxel_map_local_[voxel_locs[i]].add(world_points[i]); voxel_map_global_[voxel_locs[i]].add(world_points[i]); } // ===== 步骤 3: 定期清理局部地图 ===== static int cleanup_counter = 0; if (map_side_ \u0026gt; 0 \u0026amp;\u0026amp; ++cleanup_counter \u0026gt; 50) { cleanup_counter = 0; for (auto it = voxel_map_local_.begin(); it != voxel_map_local_.end(); ) { const auto \u0026amp;loc = it-\u0026gt;first; Eigen::Vector3d v_pos(loc.x * voxel_size_, loc.y * voxel_size_, loc.z * voxel_size_); if ((v_pos - _pos_t).cwiseAbs().maxCoeff() \u0026gt; map_side_) { it = voxel_map_local_.erase(it); } else { ++it; } } } } } 并行策略分析：\n步骤 并行化 原因 世界坐标计算 + 体素索引 ✅ OpenMP 并行 每个点独立计算，无数据竞争 哈希表更新 ❌ 串行 unordered_map 的写操作非线程安全 地图清理 ❌ 串行 需要遍历 + 删除，与更新共享锁 地图维护策略 局部地图采用滑动窗口策略，每 50 帧执行一次清理：\n1 2 // 清理条件：体素中心到当前位置的 L∞ 距离 \u0026gt; map_side (v_pos - _pos_t).cwiseAbs().maxCoeff() \u0026gt; map_side_ 为什么用 L∞ 距离（切比雪夫距离）而不是 L2（欧氏距离）？\nL∞ 距离等价于判断体素是否在一个立方体窗口内 计算只需 max(abs(dx), abs(dy), abs(dz))，比 sqrt(dx²+dy²+dz²) 更快 体素本身就是立方体，用立方体窗口更自然 flowchart TD subgraph OutOfBounds [\u0026#34;超出范围的区域 (将被删除)\u0026#34;] direction TB subgraph InBounds [\u0026#34;保留范围: 100m × 100m (局部地图)\u0026#34;] direction TB Center((\u0026#34;● 当前位置\u0026#34;)) Note[\u0026#34;向四周延伸 map_side (50m)\u0026#34;] Center -.- Note end end style OutOfBounds fill:#fff0f0,stroke:#ff6b6b,stroke-width:2px,stroke-dasharray: 5 5 style InBounds fill:#f0fff0,stroke:#51cf66,stroke-width:2px style Center fill:#fff,stroke:#333,stroke-width:2px 设计权衡：\n参数 值 影响 voxel_size = 0.5m 较小 更精细的地图表示，但体素数量更多 map_side = 50m 中等 保留 100m×100m 范围，平衡精度与内存 清理周期 = 50 帧 较长 减少清理开销，但可能短暂保留过期体素 观测模型：点到平面残差 平面拟合原理 对于一个体素内的点集，其协方差矩阵为：\n$$\\Sigma = \\frac{1}{N}\\sum_{i=1}^{N}(p_i - \\bar{p})(p_i - \\bar{p})^T = \\frac{P}{N} - \\bar{p}\\bar{p}^T$$对 $\\Sigma$ 进行特征值分解：\n$$\\Sigma = U \\cdot \\text{diag}(\\lambda_0, \\lambda_1, \\lambda_2) \\cdot U^T, \\quad \\lambda_0 \\leq \\lambda_1 \\leq \\lambda_2$$平面判定条件：\n$$\\lambda_0 \u003c 0.01 \\quad \\text{且} \\quad \\lambda_0 \u003c 0.1 \\cdot \\lambda_1$$ $\\lambda_0 \\approx 0$：点集在一个方向上几乎没有分散，说明是平面 $\\lambda_0 \\ll \\lambda_1$：最小特征值远小于次小特征值，平面性好 对应的法向量为最小特征值对应的特征向量 $n = U[:,0]$。\n1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 特征值分解示意: λ₂ (最大) ─── 沿平面的最大分散方向 λ₁ (中间) ─── 沿平面的次大分散方向 λ₀ (最小) ─── 垂直于平面的方向 (法向量方向) 如果 λ₀ ≈ 0，则点集近似共面: λ₁ 方向 ↑ │ • • • │• • • • • → λ₂ 方向 │ • • • │ (平面) 点到平面距离 对于一个世界系下的点 $p_w$，到平面 $(n, c)$ 的距离为：\n$$d = n^T \\cdot (p_w - c)$$其中：\n$n$：平面法向量（单位向量） $c$：平面质心 $p_w = R \\cdot p_{imu} + t$：点从 IMU 系变换到世界系 残差 $z = d$，当点恰好在平面上时 $z = 0$。\n雅可比矩阵推导 IESKF 更新需要观测方程对状态的雅可比 $H$。状态为 18 维 $(\\delta\\theta, \\delta p, \\delta v, \\delta b_g, \\delta b_a, \\delta g)$，但观测只与旋转和位置有关：\n$$z = n^T \\cdot (R \\cdot p_{imu} + t - c)$$对旋转误差 $\\delta\\theta$（使用一阶近似 $R \\approx R_0 \\cdot (I + [\\delta\\theta]_\\times)$）：\n$$\\frac{\\partial z}{\\partial \\delta\\theta} = n^T \\cdot R_0 \\cdot [p_{imu}]_\\times \\cdot (-1) = -n^T \\cdot R_0 \\cdot [p_{imu}]_\\times$$对位置误差 $\\delta p$：\n$$\\frac{\\partial z}{\\partial \\delta p} = n^T$$因此雅可比矩阵 $H$ 的一行：\n$$H_i = \\begin{bmatrix} -n^T R [p_{imu}]_\\times \u0026 n^T \u0026 0_{1\\times3} \u0026 0_{1\\times3} \u0026 0_{1\\times3} \u0026 0_{1\\times3} \\end{bmatrix}$$推导细节：\n$$z(\\delta\\theta, \\delta p) = n^T \\cdot (R_0(I + [\\delta\\theta]_\\times) \\cdot p_{imu} + t + \\delta p - c)$$$$\\approx n^T \\cdot (R_0 \\cdot p_{imu} + t - c) + n^T \\cdot R_0 [\\delta\\theta]_\\times p_{imu} + n^T \\cdot \\delta p$$利用 $[a]_\\times b = -[b]_\\times a$：\n$$n^T R_0 [\\delta\\theta]_\\times p_{imu} = n^T R_0 (-[p_{imu}]_\\times \\delta\\theta) = -n^T R_0 [p_{imu}]_\\times \\delta\\theta$$所以：\n$$H_{\\delta\\theta} = -n^T R_0 [p_{imu}]_\\times, \\quad H_{\\delta p} = n^T$$LIOZHVoxelModel 代码实现 lio_zh_voxel_model.h 中的 calculate() 方法实现了完整的观测模型：\n1 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 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 bool calculate(const State18 \u0026amp;_state, Eigen::MatrixXd \u0026amp;_Z, Eigen::MatrixXd \u0026amp;_H) override { int cloud_size = current_cloud_ptr_-\u0026gt;size(); // ===== 步骤 1: 预计算每个点对应的体素位置 ===== std::vector\u0026lt;VOXEL_LOC\u0026gt; point_voxel_locs(cloud_size); std::unordered_map\u0026lt;VOXEL_LOC, int, VoxelHash\u0026gt; unique_voxels; for (int i = 0; i \u0026lt; cloud_size; i++) { const auto \u0026amp;p_imu_pcl = current_cloud_ptr_-\u0026gt;points[i]; Eigen::Vector3d p_world = _state.rotation * Eigen::Vector3d(p_imu_pcl.x, p_imu_pcl.y, p_imu_pcl.z) + _state.position; VOXEL_LOC loc(static_cast\u0026lt;int64_t\u0026gt;(std::floor(p_world.x() / voxel_size_)), static_cast\u0026lt;int64_t\u0026gt;(std::floor(p_world.y() / voxel_size_)), static_cast\u0026lt;int64_t\u0026gt;(std::floor(p_world.z() / voxel_size_))); point_voxel_locs[i] = loc; unique_voxels[loc] = -1; // 占位 } // ===== 步骤 2: 提取唯一体素，计算平面参数 (并行) ===== struct VoxelPlane { Eigen::Vector3d normal; Eigen::Vector3d center; bool is_valid = false; }; std::vector\u0026lt;VOXEL_LOC\u0026gt; active_v_list; for (auto \u0026amp;kv : unique_voxels) { kv.second = active_v_list.size(); active_v_list.push_back(kv.first); } std::vector\u0026lt;VoxelPlane\u0026gt; active_planes(active_v_list.size()); #ifdef MP_EN omp_set_num_threads(MP_PROC_NUM); #pragma omp parallel for #endif for (int i = 0; i \u0026lt; (int)active_v_list.size(); i++) { auto it = voxel_map_ptr_-\u0026gt;find(active_v_list[i]); if (it != voxel_map_ptr_-\u0026gt;end() \u0026amp;\u0026amp; it-\u0026gt;second.N \u0026gt;= 5) { const auto \u0026amp;cluster = it-\u0026gt;second; Eigen::Matrix3d cov = cluster.getCovariance(); Eigen::SelfAdjointEigenSolver\u0026lt;Eigen::Matrix3d\u0026gt; saes(cov); Eigen::Vector3d eigen_values = saes.eigenvalues(); // 平面判定: 最小特征值 \u0026lt; 0.01 且 \u0026lt; 0.1 * 次小特征值 if (eigen_values[0] \u0026lt; 0.01 \u0026amp;\u0026amp; eigen_values[0] \u0026lt; 0.1 * eigen_values[1]) { active_planes[i].normal = saes.eigenvectors().col(0); // 最小特征值对应法向量 active_planes[i].center = cluster.getCenter(); active_planes[i].is_valid = true; } } } // ===== 步骤 3: 并行计算残差 ===== int valid_points_num = 0; std::vector\u0026lt;LossInfo\u0026gt; losses(cloud_size); std::vector\u0026lt;bool\u0026gt; is_effect_point(cloud_size, false); #ifdef MP_EN #pragma omp parallel for reduction(+:valid_points_num) #endif for (int i = 0; i \u0026lt; cloud_size; i++) { int v_idx = unique_voxels.at(point_voxel_locs[i]); const auto \u0026amp;vp = active_planes[v_idx]; if (vp.is_valid) { const auto \u0026amp;p_imu_pcl = current_cloud_ptr_-\u0026gt;points[i]; Eigen::Vector3d p_imu(p_imu_pcl.x, p_imu_pcl.y, p_imu_pcl.z); Eigen::Vector3d p_world = _state.rotation * p_imu + _state.position; double residual = vp.normal.dot(p_world - vp.center); // 点到平面距离 if (std::abs(residual) \u0026lt; 0.5) { // 残差阈值: 0.5m losses[i] = {p_imu, vp.normal, residual}; is_effect_point[i] = true; valid_points_num++; } } } if (valid_points_num \u0026lt; 10) return false; // 有效点太少，跳过更新 // ===== 步骤 4: 构建 H 和 Z 矩阵 ===== _H = Eigen::MatrixXd::Zero(valid_points_num, 18); _Z = Eigen::MatrixXd::Zero(valid_points_num, 1); int vi = 0; auto R_mat = _state.rotation.toRotationMatrix(); for (int i = 0; i \u0026lt; cloud_size; i++) { if (is_effect_point[i]) { const auto \u0026amp;loss = losses[i]; // H 的旋转部分: -n^T * R * [p_imu]× _H.block\u0026lt;1, 3\u0026gt;(vi, 0) = -loss.normal.transpose() * R_mat * skewSymmetric(loss.p_imu); // H 的位置部分: n^T _H.block\u0026lt;1, 3\u0026gt;(vi, 3) = loss.normal.transpose(); // 残差 _Z(vi, 0) = loss.residual; vi++; } } return true; } 优化技巧：\n优化点 做法 效果 唯一体素去重 先收集 unique_voxels，避免重复计算平面 体素数远小于点数 平面参数并行计算 对唯一体素列表做 #pragma omp parallel for 多核加速 残差并行计算 #pragma omp parallel for reduction(+:valid_points_num) 多核加速 残差阈值过滤 abs(residual) \u0026lt; 0.5 剔除异常匹配 有效点数检查 valid_points_num \u0026lt; 10 避免病态更新 地图保存 策略 A：质心 PCD 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 void VoxelMapManager::saveMapPCD(const std::string \u0026amp;_file_path) { PCLPointCloudPtr cloud(new PCLPointCloud()); cloud-\u0026gt;reserve(voxel_map_global_.size()); for (auto const\u0026amp; [loc, cluster] : voxel_map_global_) { if (cluster.N \u0026lt; 3) continue; PointType pt; Eigen::Vector3d center = cluster.getCenter(); pt.x = center.x(); pt.y = center.y(); pt.z = center.z(); pt.intensity = 100; cloud-\u0026gt;push_back(pt); } pcl::io::savePCDFileBinaryCompressed(_file_path, *cloud); } 每个体素保存为一个点（质心位置），输出文件可用 PCL 工具或 CloudCompare 查看。\n策略 B：自定义二进制 1 2 3 4 5 6 7 8 9 10 11 12 13 void VoxelMapManager::saveMapBinary(const std::string \u0026amp;_file_path) { std::ofstream outfile(_file_path, std::ios::binary); size_t map_size = voxel_map_global_.size(); outfile.write(reinterpret_cast\u0026lt;const char*\u0026gt;(\u0026amp;map_size), sizeof(size_t)); for (auto const\u0026amp; [loc, cluster] : voxel_map_global_) { outfile.write(reinterpret_cast\u0026lt;const char*\u0026gt;(\u0026amp;loc), sizeof(VOXEL_LOC)); // 16 bytes outfile.write(reinterpret_cast\u0026lt;const char*\u0026gt;(\u0026amp;cluster.N), sizeof(int)); // 4 bytes outfile.write(reinterpret_cast\u0026lt;const char*\u0026gt;(cluster.v.data()), sizeof(double) * 3); // 24 bytes outfile.write(reinterpret_cast\u0026lt;const char*\u0026gt;(cluster.P.data()), sizeof(double) * 9); // 72 bytes } } 二进制格式保留了完整的统计量 (VOXEL_LOC, N, v, P)，可用于后续加载和进一步处理。每个体素占 116 bytes。\n与 VoxelSLAM 参考实现的对比 特性 ZLIO (VoxelMapManager) VoxelSLAM (OctoTree) 数据结构 unordered_map\u0026lt;VOXEL_LOC, PointCluster\u0026gt; unordered_map\u0026lt;VOXEL_LOC, OctoTree*\u0026gt; 空间组织 扁平哈希表，单层体素 八叉树，支持多层细分 平面检测 特征值分解 (运行时) 特征值分解 (维护时) 存储方式 增量统计量 (N, v, P) 原始点 + 统计量 滑动窗口 无，直接清理远距离体素 OctoTree 内部 SlideWindow 观测模型 点到平面 (IESKF) 点到平面 (BA 优化) 多帧优化 单帧 IESKF 滑动窗口 BA 内存占用 较低 (仅统计量) 较高 (存储原始点) 实现复杂度 简单 复杂 (八叉树管理) ZLIO 的简化设计：\nVoxelSLAM 的 OctoTree 是一个完整的八叉树实现，支持：\n多层细分（max_layer 控制） 滑动窗口优化（SlideWindow） 平面不确定性传播（plane_var） LM 优化器（Lidar_BA_Optimizer） ZLIO 选择了一个更简洁的方案：单层体素 + IESKF 滤波，牺牲了一些精度换取了实现的简洁性和计算效率。\n完整数据流 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 ┌─────────────────────────────────────────────────────────────────────┐ │ FrontEnd::track() │ │ │ │ ┌───────────────────────────────────────────┐ │ │ │ imu_process_ptr_-\u0026gt;propagate(mg, ieskf) │ ← 前向传播 + 去畸变 │ │ └───────────────────────────────────────────┘ │ │ │ │ ┌───────────────────────────────────────────┐ │ │ │ 第一帧? │ │ │ │ Yes → addScan(cloud, R, p) 初始化地图 │ │ │ │ No → 进入 IESKF 更新流程 │ │ │ └───────────────────────────────────────────┘ │ │ │ │ ┌───────────────────────────────────────────┐ │ │ │ lio_model_ptr_-\u0026gt;setData(cloud, voxel_map) │ ← 设置观测数据源 │ │ └───────────────────────────────────────────┘ │ │ │ │ ┌───────────────────────────────────────────┐ │ │ │ ieskf_ptr_-\u0026gt;update() │ │ │ │ │ │ │ │ │ ├─ for iter = 1 to iter_times: │ │ │ │ │ ├─ calc_zh_ptr_-\u0026gt;calculate(state,Z,H)│ │ │ │ │ │ ├─ 遍历点云，查找对应体素 │ │ │ │ │ │ ├─ 特征值分解，判断平面 │ │ │ │ │ │ ├─ 计算点到平面残差 │ │ │ │ │ │ └─ 构建 H 和 Z 矩阵 │ │ │ │ │ ├─ 计算卡尔曼增益 K │ │ │ │ │ ├─ 计算状态增量 │ │ │ │ │ └─ 收敛判断 │ │ │ │ └─ 更新协方差 P │ │ │ └───────────────────────────────────────────┘ │ │ │ │ ┌───────────────────────────────────────────┐ │ │ │ addScan(cloud, R, p) 更新地图 │ ← 每帧都更新 │ │ │ ├─ 并行: 世界坐标 + 体素索引 │ │ │ │ ├─ 串行: 哈希表写入 (local + global) │ │ │ │ └─ 定期: 清理远离的体素 (仅 local) │ │ │ └───────────────────────────────────────────┘ │ └─────────────────────────────────────────────────────────────────────┘ 关键设计总结 设计点 选择 原因 哈希表 vs KD-Tree 哈希表 O(1) 查找，适合体素索引场景 增量统计 vs 存原始点 增量统计 内存 O(体素数) 而非 O(点数) 双地图 local + global 局部地图控制内存，全局地图保留完整数据 平面检测 特征值分解 通用、鲁棒，可同时获取法向量 观测模型 点到平面 比点到点更鲁棒，适合平面环境 并行策略 预计算并行 + 更新串行 平衡性能与线程安全 参考 FAST-LIO 源码：https://github.com/hku-mars/FAST_LIO VoxelSLAM 论文：Voxel-SLAM: A Complete, Accurate, and Versatile LiDAR-Inertial SLAM System IESKF 理论：Iterated Extended Kalman Filter for SLAM PCL 点云库：https://pointclouds.org/ ","permalink":"https://zero-kq.github.io/Pages/posts/slam/lio%E5%AD%A6%E4%B9%A0%E7%AC%94%E8%AE%B004/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e体素地图是 ZLIO 系统的核心数据结构之一，用于维护局部三维环境的紧凑表示。相比传统的 KD-Tree + 暴力搜索方案，体素地图通过\u003cstrong\u003e哈希表\u003c/strong\u003e实现 O(1) 的体素查找，并利用\u003cstrong\u003e点集统计量 (PointCluster)\u003c/strong\u003e 避免存储原始点云，大幅降低内存占用。\u003c/p\u003e\n\u003cp\u003e核心设计思想：\u003c/p\u003e\n\u003col\u003e\n\u003cli\u003e\u003cstrong\u003e空间离散化\u003c/strong\u003e：将连续的三维空间划分为固定大小的立方体（体素），每个体素由整数坐标 \u003ccode\u003e(x, y, z)\u003c/code\u003e 索引\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e增量统计\u003c/strong\u003e：不存储原始点，而是维护每个体素内点的\u003cstrong\u003e一阶矩（坐标和）\u003cstrong\u003e和\u003c/strong\u003e二阶矩（坐标外积和）\u003c/strong\u003e，可在线计算质心和协方差\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e平面检测\u003c/strong\u003e：通过协方差矩阵的特征值分解判断体素内点是否构成平面，为 IESKF 提供点到平面的观测约束\u003c/li\u003e\n\u003c/ol\u003e\n\u003cp\u003e涉及的核心文件：\u003c/p\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e文件\u003c/th\u003e\n          \u003cth\u003e作用\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003evoxel_map_manager.h/cpp\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e体素地图数据结构与管理接口\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003elio_zh_voxel_model.h\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e基于体素的点到平面观测模型\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003chr\u003e\n\u003ch2 id=\"核心数据结构\"\u003e核心数据结构\u003c/h2\u003e\n\u003ch3 id=\"体素位置-voxel_loc\"\u003e体素位置 VOXEL_LOC\u003c/h3\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e 1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 9\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e10\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"k\"\u003estruct\u003c/span\u003e \u003cspan class=\"nc\"\u003eVOXEL_LOC\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e  \u003cspan class=\"kt\"\u003eint64_t\u003c/span\u003e \u003cspan class=\"n\"\u003ex\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ey\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ez\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e  \u003cspan class=\"n\"\u003eVOXEL_LOC\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"kt\"\u003eint64_t\u003c/span\u003e \u003cspan class=\"n\"\u003e_x\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"kt\"\u003eint64_t\u003c/span\u003e \u003cspan class=\"n\"\u003e_y\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"kt\"\u003eint64_t\u003c/span\u003e \u003cspan class=\"n\"\u003e_z\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e      \u003cspan class=\"o\"\u003e:\u003c/span\u003e \u003cspan class=\"n\"\u003ex\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003e_x\u003c/span\u003e\u003cspan class=\"p\"\u003e),\u003c/span\u003e \u003cspan class=\"n\"\u003ey\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003e_y\u003c/span\u003e\u003cspan class=\"p\"\u003e),\u003c/span\u003e \u003cspan class=\"n\"\u003ez\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003e_z\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e \u003cspan class=\"p\"\u003e{}\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e  \u003cspan class=\"kt\"\u003ebool\u003c/span\u003e \u003cspan class=\"k\"\u003eoperator\u003c/span\u003e\u003cspan class=\"o\"\u003e==\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"k\"\u003econst\u003c/span\u003e \u003cspan class=\"n\"\u003eVOXEL_LOC\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026amp;\u003c/span\u003e\u003cspan class=\"n\"\u003eother\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e \u003cspan class=\"k\"\u003econst\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"k\"\u003ereturn\u003c/span\u003e \u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ex\u003c/span\u003e \u003cspan class=\"o\"\u003e==\u003c/span\u003e \u003cspan class=\"n\"\u003eother\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ex\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026amp;\u0026amp;\u003c/span\u003e \u003cspan class=\"n\"\u003ey\u003c/span\u003e \u003cspan class=\"o\"\u003e==\u003c/span\u003e \u003cspan class=\"n\"\u003eother\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ey\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026amp;\u0026amp;\u003c/span\u003e \u003cspan class=\"n\"\u003ez\u003c/span\u003e \u003cspan class=\"o\"\u003e==\u003c/span\u003e \u003cspan class=\"n\"\u003eother\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ez\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e  \u003cspan class=\"p\"\u003e}\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"p\"\u003e};\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e\u003cstrong\u003e设计要点\u003c/strong\u003e：\u003c/p\u003e\n\u003cul\u003e\n\u003cli\u003e使用 \u003ccode\u003eint64_t\u003c/code\u003e 而非 \u003ccode\u003eint\u003c/code\u003e，支持极大的坐标范围（避免负数取整溢出）\u003c/li\u003e\n\u003cli\u003e重载 \u003ccode\u003eoperator==\u003c/code\u003e 以支持哈希表的相等比较\u003c/li\u003e\n\u003cli\u003e三个整数唯一确定一个体素在空间中的位置\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"哈希函数-voxelhash\"\u003e哈希函数 VoxelHash\u003c/h3\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e5\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"k\"\u003estruct\u003c/span\u003e \u003cspan class=\"nc\"\u003eVoxelHash\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e  \u003cspan class=\"n\"\u003esize_t\u003c/span\u003e \u003cspan class=\"nf\"\u003eoperator\u003c/span\u003e\u003cspan class=\"p\"\u003e()(\u003c/span\u003e\u003cspan class=\"k\"\u003econst\u003c/span\u003e \u003cspan class=\"n\"\u003eVOXEL_LOC\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026amp;\u003c/span\u003e\u003cspan class=\"n\"\u003ev\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e \u003cspan class=\"k\"\u003econst\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"k\"\u003ereturn\u003c/span\u003e \u003cspan class=\"p\"\u003e(((\u003c/span\u003e\u003cspan class=\"n\"\u003ev\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ex\u003c/span\u003e \u003cspan class=\"o\"\u003e*\u003c/span\u003e \u003cspan class=\"mi\"\u003e73856093\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e \u003cspan class=\"o\"\u003e^\u003c/span\u003e \u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ev\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ey\u003c/span\u003e \u003cspan class=\"o\"\u003e*\u003c/span\u003e \u003cspan class=\"mi\"\u003e19349663\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e \u003cspan class=\"o\"\u003e^\u003c/span\u003e \u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ev\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ez\u003c/span\u003e \u003cspan class=\"o\"\u003e*\u003c/span\u003e \u003cspan class=\"mi\"\u003e83492791\u003c/span\u003e\u003cspan class=\"p\"\u003e))\u003c/span\u003e \u003cspan class=\"o\"\u003e%\u003c/span\u003e \u003cspan class=\"mi\"\u003e10000000\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e  \u003cspan class=\"p\"\u003e}\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"p\"\u003e};\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e这是一个经典的\u003cstrong\u003e空间哈希函数\u003c/strong\u003e，三个大质数分别与坐标异或后取模，将三维整数坐标映射到一维哈希值。选择大质数可以减少哈希冲突。\u003c/p\u003e","title":"LIO学习笔记04"},{"content":"介绍 对于通过解压即可使用的 Linux 应用程序（如 Antigravity IDE），系统默认不会为其创建桌面入口。本指南介绍如何通过 .desktop 文件将它们注册到系统应用菜单、任务栏，并配置独立桌面图标。\n一键快速配置（推荐模板） 只需在系统存放快捷方式的目录下创建一个 .desktop 配置文件。\n1. 创建配置文件 打开终端，执行以下命令：\n1 nano ~/.local/share/applications/antigravity-ide.desktop 2. 粘贴并修改配置内容 在打开的编辑器中，粘贴以下标准配置。请务必将路径中的 yj 替换为系统实际用户名，并确保路径与软件存放位置一致：\n1 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=\u0026quot;/home/yj/下载/Antigravity IDE/antigravity-ide\u0026quot; 路径中的用户名：将 yj 替换为实际系统用户名 3. 保存并退出 在 nano 编辑器中，按下 Ctrl + O，然后按 Enter 确认保存，最后按 Ctrl + X 退出。\n激活与刷新菜单 配置文件创建后，需要赋予执行权限并刷新系统缓存，否则应用菜单中可能不会立即显示。\n依次执行以下命令：\n1 2 3 4 5 6 7 8 # 1. 确保软件主程序具有执行权限 chmod +x /home/yj/Antigravity_IDE/antigravity-ide # 2. 赋予桌面快捷方式文件执行权限 chmod +x ~/.local/share/applications/antigravity-ide.desktop # 3. 强制刷新系统应用菜单数据库 update-desktop-database ~/.local/share/applications/ 完成后，按下 Super 键（Windows 键），在应用菜单中搜索 \u0026ldquo;Antigravity IDE\u0026rdquo; 即可找到并启动。\n常见问题排查 问题一：应用菜单中找不到图标 原因：.desktop 文件格式不合法。\n解决方案：使用 desktop-file-validate 验证文件格式：\n1 desktop-file-validate ~/.local/share/applications/antigravity-ide.desktop 如果没有任何输出，说明语法正确 如果有错误提示（如 contains a reserved character '~'），请根据提示修改文件 问题二：应用能启动但没有图标 原因：Icon= 指定的图片路径错误，或图标缓存未刷新。\n解决方案：\n查找精确图标路径： 1 find ~/Antigravity_IDE -name \u0026#34;*.png\u0026#34; -o -name \u0026#34;*.svg\u0026#34; 将输出中最像应用 Logo 的文件（如 icon.png 或 logo.png）的绝对路径填入 Icon= 字段\n强制刷新系统图标缓存：\n1 2 sudo gtk-update-icon-cache /usr/share/icons/hicolor killall gnome-shell -HUP 临时替代方案：可将 Icon= 设为系统通用图标名称，如 Icon=code 或 Icon=utilities-terminal\n问题三：如何直接放到桌面上双击启动 解决方案：将快捷方式复制到桌面目录：\n1 2 3 4 5 # 中文系统 cp ~/.local/share/applications/antigravity-ide.desktop ~/桌面/ # 英文系统 # cp ~/.local/share/applications/antigravity-ide.desktop ~/Desktop/ 参考 Desktop Entry Specification：https://specifications.freedesktop.org/desktop-entry-spec/latest/ Antigravity IDE：https://antigravity-fpga.com ","permalink":"https://zero-kq.github.io/Pages/posts/linux/linux-desktop-shortcut-guide/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e对于通过解压即可使用的 Linux 应用程序（如 Antigravity IDE），系统默认不会为其创建桌面入口。本指南介绍如何通过 \u003ccode\u003e.desktop\u003c/code\u003e 文件将它们注册到系统应用菜单、任务栏，并配置独立桌面图标。\u003c/p\u003e\n\u003chr\u003e\n\u003ch2 id=\"一键快速配置推荐模板\"\u003e一键快速配置（推荐模板）\u003c/h2\u003e\n\u003cp\u003e只需在系统存放快捷方式的目录下创建一个 \u003ccode\u003e.desktop\u003c/code\u003e 配置文件。\u003c/p\u003e\n\u003ch3 id=\"1-创建配置文件\"\u003e1. 创建配置文件\u003c/h3\u003e\n\u003cp\u003e打开终端，执行以下命令：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003enano ~/.local/share/applications/antigravity-ide.desktop\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"2-粘贴并修改配置内容\"\u003e2. 粘贴并修改配置内容\u003c/h3\u003e\n\u003cp\u003e在打开的编辑器中，粘贴以下标准配置。请务必将路径中的 \u003ccode\u003eyj\u003c/code\u003e 替换为系统实际用户名，并确保路径与软件存放位置一致：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e9\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-ini\" data-lang=\"ini\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"k\"\u003e[Desktop Entry]\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"na\"\u003eVersion\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"s\"\u003e1.0\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"na\"\u003eType\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"s\"\u003eApplication\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"na\"\u003eName\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"s\"\u003eAntigravity IDE\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"na\"\u003eComment\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"s\"\u003eMy Antigravity IDE\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"na\"\u003eExec\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"s\"\u003e/home/yj/Antigravity_IDE/antigravity-ide\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"na\"\u003eIcon\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"s\"\u003e/home/yj/Antigravity_IDE/resources/app/resources/linux/code.png\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"na\"\u003eTerminal\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"s\"\u003efalse\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"na\"\u003eCategories\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"s\"\u003eDevelopment;IDE;\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch4 id=\"配置字段说明\"\u003e配置字段说明\u003c/h4\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e字段\u003c/th\u003e\n          \u003cth\u003e说明\u003c/th\u003e\n          \u003cth\u003e示例\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003eExec\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e可执行程序的\u003cstrong\u003e绝对路径\u003c/strong\u003e\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003e/home/yj/Antigravity_IDE/antigravity-ide\u003c/code\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003eIcon\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e应用图标的\u003cstrong\u003e绝对路径\u003c/strong\u003e\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003e/home/yj/Antigravity_IDE/resources/icon.png\u003c/code\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003eCategories\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e应用分类（分号分隔）\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003eDevelopment;IDE;\u003c/code\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003e\u003ccode\u003eTerminal\u003c/code\u003e\u003c/td\u003e\n          \u003ctd\u003e是否在终端中运行\u003c/td\u003e\n          \u003ctd\u003e\u003ccode\u003efalse\u003c/code\u003e\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003ch4 id=\"避坑关键点\"\u003e避坑关键点\u003c/h4\u003e\n\u003cul\u003e\n\u003cli\u003e\u003cstrong\u003e禁止使用波浪号（~）\u003c/strong\u003e：\u003ccode\u003eExec\u003c/code\u003e 和 \u003ccode\u003eIcon\u003c/code\u003e 必须使用绝对路径（如 \u003ccode\u003e/home/yj/...\u003c/code\u003e），系统无法解析 \u003ccode\u003e~/\u003c/code\u003e\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e处理路径空格\u003c/strong\u003e：若文件夹名包含空格（如 \u003ccode\u003eAntigravity IDE\u003c/code\u003e），请给路径加上英文双引号，例如：\u003ccode\u003eExec=\u0026quot;/home/yj/下载/Antigravity IDE/antigravity-ide\u0026quot;\u003c/code\u003e\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e路径中的用户名\u003c/strong\u003e：将 \u003ccode\u003eyj\u003c/code\u003e 替换为实际系统用户名\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"3-保存并退出\"\u003e3. 保存并退出\u003c/h3\u003e\n\u003cp\u003e在 nano 编辑器中，按下 \u003ccode\u003eCtrl + O\u003c/code\u003e，然后按 \u003ccode\u003eEnter\u003c/code\u003e 确认保存，最后按 \u003ccode\u003eCtrl + X\u003c/code\u003e 退出。\u003c/p\u003e","title":"Linux 桌面快捷方式配置指南"},{"content":"介绍 多机器人系统初始化时，各机器人的局部坐标系往往不一致，需要通过点云配准将它们统一到同一个世界坐标系。本文介绍一种三阶段配准策略：重力对齐消除姿态差异、RANSAC 全局粗配准找到大致变换、GICP 精配准优化精度。三个阶段层层递进，兼顾鲁棒性和精度。\n三阶段配准策略 整个配准流程分为三个阶段，每个阶段解决不同层次的问题：\ngraph TD A[\u0026#34;原始点云 (body frame)\u0026#34;] --\u0026gt; B[\u0026#34;Stage 1: 重力对齐\u0026#34;] B --\u0026gt; C[\u0026#34;Stage 2: RANSAC 全局配准\u0026#34;] C --\u0026gt; D[\u0026#34;Stage 3: GICP 精配准\u0026#34;] D --\u0026gt; E[\u0026#34;变换矩阵 T (src → target)\u0026#34;] B -.- B1[\u0026#34;估计重力方向\\n消除 Roll/Pitch 姿态差异\u0026#34;] C -.- C1[\u0026#34;FPFH 特征提取\\n基于特征匹配的采样一致性估计\u0026#34;] D -.- D1[\u0026#34;协方差矩阵加权\\n迭代最近点优化\u0026#34;] 阶段 目标 方法 输出 Stage 1 消除 Roll/Pitch 姿态差异 水平平面法向量统计 旋转矩阵 $T_{gravity}$ Stage 2 全局粗配准 FPFH + RANSAC 粗配准变换 $T_{ransac}$ Stage 3 高精度精配准 GICP 协方差加权 精配准变换 $T_{fine}$ 数据准备 点云融合 每个机器人采集 init_frame_num（默认 16）帧点云并融合，提高配准鲁棒性。单帧点云的特征信息有限，多帧融合后点云密度更高，几何特征更丰富，配准成功率显著提升。\n机器人 融合方式 说明 Robot 0（目标） $target = \\sum_{i=0}^{15} measure[0][i].lidar$ 作为参考坐标系 Robot 1（源） $src = \\sum_{i=0}^{15} measure[1][i].lidar$ 与 Robot 0 配准 Robot N（源） $src = \\sum_{i=0}^{15} measure[N][i].lidar$ 与 Robot 0 配准 Robot 0 作为参考坐标系（世界坐标系） Robot 1 ~ N 分别与 Robot 0 进行配准 配准结果存储在 drone_gicp_poses_[robot_id] Stage 1：重力对齐 目的 不同机器人的安装姿态可能不同，导致 Roll/Pitch 存在差异。重力对齐通过估计重力方向，将点云旋转到统一的重力对齐坐标系，消除姿态差异。\n实现步骤 graph TD A[\u0026#34;输入: 点云 P (body frame)\u0026#34;] --\u0026gt; B[\u0026#34;Step 1: 法向量估计\\nPCA 拟合局部平面\u0026#34;] B --\u0026gt; C[\u0026#34;Step 2: 筛选水平平面\\n|n.z| \u0026gt; cos(30°)\u0026#34;] C --\u0026gt; D[\u0026#34;Step 3: 法向量统一朝上\\nn.z \u0026lt; 0 则取反\u0026#34;] D --\u0026gt; E[\u0026#34;Step 4: 累加归一化\\ngravity = normalize(Σn)\u0026#34;] E --\u0026gt; F[\u0026#34;Step 5: 计算旋转矩阵\\nq = FromTwoVectors(gravity, Z)\u0026#34;] F --\u0026gt; G[\u0026#34;输出: 旋转矩阵 T_gravity\u0026#34;] 步骤 操作 参数 法向量估计 对每个点使用半径邻近点进行 PCA 拟合 半径 $r = 0.5m$ 筛选水平平面 法向量与 Z 轴夹角 \u0026lt; 30° 的平面 $\\|n.z\\| \u003e 0.866$ 法向量统一 如果 $n.z \u003c 0$，则 $n = -n$ - 累加归一化 $gravity_{est} = normalize(\\sum n_i)$ - 旋转矩阵 $q = FromTwoVectors(gravity_{est}, [0,0,1])$ - 原理说明 场景中的水平平面（地面、天花板、桌面）的法向量接近重力方向。通过统计这些法向量，可以估计重力方向并进行对齐。\n1 2 3 4 5 6 重力对齐前: 重力对齐后: ╱╲ │ ╱ ╲ Roll=10° │ Roll=0° ╱ ╲ │ ╱______╲ ────┼──── 局限性 需要场景中有足够的水平平面 如果点云已经重力对齐（来自 LiDAR-IMU 里程计），此步骤可能多余 法向量估计对点云密度和噪声敏感 Stage 2：RANSAC 全局配准 目的 在重力对齐的基础上，进行全局粗配准，找到大致的平移和 Yaw 旋转。RANSAC 通过随机采样和一致性检验，能够在存在大量误匹配的情况下找到正确变换。\nFPFH 特征提取 FPFH（Fast Point Feature Histograms）是一种局部几何特征描述子，用于描述点云中每个点的局部几何形状，具有旋转不变性和平移不变性。每个点的 FPFH 特征为 33 维向量。\n局部坐标系建立 对点 $p$ 和其邻近点 $q$，建立局部坐标系 $(u, v, w)$：\n轴 定义 说明 $u$ $n_p$ 点 $p$ 的法向量 $v$ $u \\times (q - p) / \\|q - p\\|$ 与连线垂直 $w$ $u \\times v$ 右手坐标系 1 2 3 4 5 6 7 8 9 10 11 w ↑ │ │ q ────────┼──────── p │ │ ↓ v u 指向屏幕外 (n_p 方向) 三个特征值 对点 $p$ 和其邻近点 $q$，计算三个几何特征：\n特征 1：$\\alpha$ (alpha)\n$$\\alpha = v \\cdot n_q$$含义为邻近点法向量 $n_q$ 在 $v$ 方向的投影，范围 $[-1, 1]$。\n1 2 3 4 5 6 7 8 9 n_q ↑ │ │ α = cos(angle between v and n_q) │ ╱ │ ╱ ─────┼───── q │ ↓ v 特征 2：$\\varphi$ (phi)\n$$\\varphi = u \\cdot (q - p) / \\|q - p\\|$$含义为点 $p$ 的法向量 $u$ 与连线 $(q-p)$ 的夹角余弦，范围 $[-1, 1]$。\n1 2 3 4 5 6 7 8 9 10 n_p = u ↑ │ │ φ = cos(angle between u and (q-p)) │ ╱ │ ╱ ─────┼───── p │ ↓ q 特征 3：$\\theta$ (theta)\n$$\\theta = atan2(w \\cdot n_q, u \\cdot n_q)$$含义为邻近点法向量 $n_q$ 在 $u$-$w$ 平面的旋转角，范围 $[0, 2\\pi]$。\n1 2 3 4 5 6 7 8 9 w ↑ │ n_q │ ╱ │ ╱ θ │╱ ────┼──── u │ ↓ 直方图构建 将三个特征值分别离散化为直方图，最终拼接为 33 维特征向量：\n特征 Bin 数 范围 维度 $\\alpha$ 11 $[-1, 1]$ 11 $\\varphi$ 11 $[-1, 1]$ 11 $\\theta$ 11 $[0, 2\\pi]$ 11 合计 - - 33 特征的物理含义 特征 物理含义 平面区域 边缘区域 曲面区域 $\\alpha$ 邻近点法向量在 $v$ 方向的分量 集中在 0 分散 分散 $\\varphi$ 当前点法向量与连线的夹角 集中在 $\\pm 1$ 分散 中等 $\\theta$ 邻近点法向量在 $u$-$w$ 平面的旋转角 集中 分散 中等 SPFH 到 FPFH 的加权累加 SPFH 只考虑直接邻近点（1 邻域），FPFH 通过加权累加考虑更远邻域（2 邻域）的影响：\n$$FPFH(p) = SPFH(p) + \\frac{1}{k} \\sum_{i=1}^{k} \\frac{1}{w_i} \\cdot SPFH(q_i)$$其中 $k$ 为点 $p$ 的邻近点数量，$w_i$ 为点 $p$ 到 $q_i$ 的距离。\n1 2 3 4 5 6 7 8 9 10 11 1 邻域 (SPFH): 2 邻域 (FPFH): q1 q1 │ │ │ │ p ─── q2 p ─── q2 ─── q3 │ │ │ │ q4 q4 ─── q5 只考虑 p 的直接邻近点 考虑 p 的邻近点的邻近点 特征不变性 旋转不变性：基于相对角度 $(\\alpha, \\varphi, \\theta)$，不依赖绝对坐标系 平移不变性：基于相对位置 $(q - p)$，不依赖绝对位置 尺度鲁棒性：对点云密度变化有一定容忍度，直方图的分布模式保持稳定 RANSAC 采样一致性 RANSAC 通过随机采样和内点检验，在存在大量误匹配的情况下找到正确的刚体变换。\ngraph TD A[\u0026#34;开始\u0026#34;] --\u0026gt; B[\u0026#34;随机采样 3 个点对\\n基于 FPFH 特征匹配\u0026#34;] B --\u0026gt; C[\u0026#34;计算变换矩阵 T\\n刚体变换 6DOF\u0026#34;] C --\u0026gt; D[\u0026#34;将源点云变换到目标坐标系\u0026#34;] D --\u0026gt; E[\u0026#34;统计内点\\n距离 \u0026lt; max_corr_dist\u0026#34;] E --\u0026gt; F{\u0026#34;内点比例 \u0026gt; inlier_fraction?\u0026#34;} F --\u0026gt;|是| G[\u0026#34;接受此变换\u0026#34;] F --\u0026gt;|否| H[\u0026#34;丢弃\u0026#34;] G --\u0026gt; I{\u0026#34;达到最大迭代次数?\u0026#34;} H --\u0026gt; I I --\u0026gt;|否| B I --\u0026gt;|是| J[\u0026#34;输出最优变换 T_ransac\u0026#34;] FPFH 在 RANSAC 中的作用：传统 ICP 只用距离找对应点，容易陷入局部最优。RANSAC + FPFH 基于几何特征匹配，找到更可靠的对应关系，对初始位置不敏感，适合全局配准。\nRANSAC 参数配置 参数 值 说明 体素大小 0.3 m 粗配准降采样 法向量估计半径 0.5 m PCA 拟合邻域 FPFH 特征半径 0.8 m 特征计算邻域 最大迭代次数 50000 RANSAC 采样上限 每次采样点数 3 计算刚体变换所需最少点 最大对应距离 2.5 m 内点判定阈值 内点比例阈值 0.33 接受变换的最低内点比例 Stage 3：GICP 精配准 目的 在 RANSAC 粗配准的基础上，进行高精度精配准。GICP（Generalized Iterative Closest Point）在传统 ICP 基础上引入协方差矩阵加权，提高配准精度和鲁棒性。\n误差函数 $$E(T) = \\sum_i (T \\cdot p_i - q_i)^T \\cdot (C_{q[i]} + T \\cdot C_{p[i]} \\cdot T^T)^{-1} \\cdot (T \\cdot p_i - q_i)$$ 符号 含义 $T$ 4×4 变换矩阵 $p_i$ 源点云中的点 $q_i$ 目标点云中的对应点 $C_{p[i]}, C_{q[i]}$ 协方差矩阵（描述局部表面几何） 协方差矩阵的作用 协方差矩阵描述了局部表面的几何特性，不同几何区域的协方差差异决定了配准时的权重分配：\n区域类型 法向量方向 协方差矩阵 权重 约束强度 平坦区域 确定 大 高 强 边缘/角点 不确定 小 低 弱 1 2 3 4 5 平坦区域: 边缘/角点: ───────── │ 协方差矩阵大 │ 协方差矩阵小 权重高 权重低 约束强 约束弱 实现步骤 graph TD A[\u0026#34;输入: src_al, tgt_al, T_ransac\u0026#34;] --\u0026gt; B[\u0026#34;Step 1: 降采样\\n体素大小 0.1m\u0026#34;] B --\u0026gt; C[\u0026#34;Step 2: GICP 配准\\n初始值: T_ransac\u0026#34;] C --\u0026gt; D[\u0026#34;迭代过程\u0026#34;] D --\u0026gt; D1[\u0026#34;建立对应关系 (KNN)\u0026#34;] D1 --\u0026gt; D2[\u0026#34;估计协方差矩阵\u0026#34;] D2 --\u0026gt; D3[\u0026#34;最小化加权误差函数\\nLevenberg-Marquardt 求解\u0026#34;] D3 --\u0026gt; D4{\u0026#34;收敛?\u0026#34;} D4 --\u0026gt;|否| D1 D4 --\u0026gt;|是| E[\u0026#34;输出: 精配准变换 T_fine\u0026#34;] GICP 参数配置 参数 值 说明 体素大小 0.1 m 精配准降采样 最大迭代次数 100 迭代上限 最大对应距离 1.0 m 对应点搜索半径 邻近点数 20 协方差估计用 KNN 变换矩阵合并 DOF 约束 根据运动学约束，限制变换的自由度：\n模式 自由度 锁定项 3-DOF X, Y 平移 + Yaw 旋转 Z=0, Roll=0, Pitch=0 4-DOF X, Y, Z 平移 + Yaw 旋转 Roll=0, Pitch=0 最终变换矩阵 $$T_{total} = T_{gravity\\_tgt}^{-1} \\cdot T_{fine} \\cdot T_{gravity\\_src}$$合并过程：\n$T_{gravity\\_src}$：将源点云旋转到重力对齐坐标系 $T_{fine}$：在重力对齐坐标系下进行配准 $T_{gravity\\_tgt}^{-1}$：旋转回原始坐标系 多机器人并行配准 各机器人的 GICP 配准相互独立，可以并行执行以加速初始化过程：\n1 2 3 4 5 6 7 std::vector\u0026lt;std::thread\u0026gt; threads; for (int drone_id = 1; drone_id \u0026lt; num_robots_; ++drone_id) { threads.emplace_back([this, drone_id, \u0026amp;target_cloud]() { drone_gicp_poses_[drone_id] = GicpRegistration(src_cloud, target_cloud); }); } for (auto\u0026amp; t : threads) t.join(); 配准完成后，构建 GTSAM 因子图：\nRobot 0：PriorFactor（先验，固定为世界坐标系原点） Robot 1 ~ N：BetweenFactor（相对约束） 配置参数汇总 类别 参数 值 说明 体素降采样 coarse_voxel_size 0.3 m 粗配准体素大小 体素降采样 fine_voxel_size 0.1 m 精配准体素大小 法向量估计 normal_radius 0.5 m 法向量估计半径 FPFH 特征 fpfh_radius 0.8 m FPFH 特征半径 RANSAC ransac_max_iterations 50000 最大迭代次数 RANSAC ransac_max_corr_dist 2.5 m 最大对应距离 RANSAC ransac_inlier_fraction 0.33 内点比例阈值 GICP gicp_max_iterations 100 最大迭代次数 GICP gicp_max_corr_dist 1.0 m 最大对应距离 GICP gicp_correspondence_randomness 20 邻近点数 DOF dof_mode 4 3-DOF 或 4-DOF 流程总结 graph TD A[\u0026#34;1. 数据采集\\n每个机器人采集 16 帧点云并融合\u0026#34;] --\u0026gt; B[\u0026#34;2. 重力对齐\\n估计水平平面法向量\\n旋转使重力方向对齐到 Z 轴\u0026#34;] B --\u0026gt; C[\u0026#34;3. RANSAC 全局配准\\nFPFH 特征提取 (33 维)\\n采样一致性估计变换\u0026#34;] C --\u0026gt; D[\u0026#34;4. GICP 精配准\\n协方差矩阵加权\\nLevenberg-Marquardt 优化\u0026#34;] D --\u0026gt; E[\u0026#34;5. 变换合并\\nT = T_tgt⁻¹ · T_fine · T_src\u0026#34;] E --\u0026gt; F[\u0026#34;6. 因子图构建\\n添加先验和相对约束\u0026#34;] 参考 Open3D FPFH 文档：http://www.open3d.org/docs/latest/tutorial/pipelines/global_registration.html PCL FPFH 估计：https://pointclouds.org/documentation/classpcl_1_1_f_p_f_h_estimation.html GICP 论文：Segal, A., Haehnel, D., \u0026amp; Thrun, S. (2009). Generalized-ICP. RSS. FPFH 论文：Rusu, R. B., Blodow, N., \u0026amp; Beetz, M. (2009). Fast Point Feature Histograms (FPFH) for 3D registration. ICRA. GTSAM 因子图：https://gtsam.org/ ","permalink":"https://zero-kq.github.io/Pages/posts/slam/%E7%82%B9%E4%BA%91%E9%85%8D%E5%87%86/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e多机器人系统初始化时，各机器人的局部坐标系往往不一致，需要通过点云配准将它们统一到同一个世界坐标系。本文介绍一种\u003cstrong\u003e三阶段配准策略\u003c/strong\u003e：重力对齐消除姿态差异、RANSAC 全局粗配准找到大致变换、GICP 精配准优化精度。三个阶段层层递进，兼顾鲁棒性和精度。\u003c/p\u003e\n\u003ch2 id=\"三阶段配准策略\"\u003e三阶段配准策略\u003c/h2\u003e\n\u003cp\u003e整个配准流程分为三个阶段，每个阶段解决不同层次的问题：\u003c/p\u003e\n\n\u003cdiv class=\"mermaid\"\u003egraph TD\n    A[\u0026#34;原始点云 (body frame)\u0026#34;] --\u0026gt; B[\u0026#34;Stage 1: 重力对齐\u0026#34;]\n    B --\u0026gt; C[\u0026#34;Stage 2: RANSAC 全局配准\u0026#34;]\n    C --\u0026gt; D[\u0026#34;Stage 3: GICP 精配准\u0026#34;]\n    D --\u0026gt; E[\u0026#34;变换矩阵 T (src → target)\u0026#34;]\n\n    B -.- B1[\u0026#34;估计重力方向\\n消除 Roll/Pitch 姿态差异\u0026#34;]\n    C -.- C1[\u0026#34;FPFH 特征提取\\n基于特征匹配的采样一致性估计\u0026#34;]\n    D -.- D1[\u0026#34;协方差矩阵加权\\n迭代最近点优化\u0026#34;]\u003c/div\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e阶段\u003c/th\u003e\n          \u003cth\u003e目标\u003c/th\u003e\n          \u003cth\u003e方法\u003c/th\u003e\n          \u003cth\u003e输出\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eStage 1\u003c/td\u003e\n          \u003ctd\u003e消除 Roll/Pitch 姿态差异\u003c/td\u003e\n          \u003ctd\u003e水平平面法向量统计\u003c/td\u003e\n          \u003ctd\u003e旋转矩阵 $T_{gravity}$\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eStage 2\u003c/td\u003e\n          \u003ctd\u003e全局粗配准\u003c/td\u003e\n          \u003ctd\u003eFPFH + RANSAC\u003c/td\u003e\n          \u003ctd\u003e粗配准变换 $T_{ransac}$\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eStage 3\u003c/td\u003e\n          \u003ctd\u003e高精度精配准\u003c/td\u003e\n          \u003ctd\u003eGICP 协方差加权\u003c/td\u003e\n          \u003ctd\u003e精配准变换 $T_{fine}$\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003ch2 id=\"数据准备\"\u003e数据准备\u003c/h2\u003e\n\u003ch3 id=\"点云融合\"\u003e点云融合\u003c/h3\u003e\n\u003cp\u003e每个机器人采集 \u003ccode\u003einit_frame_num\u003c/code\u003e（默认 16）帧点云并融合，提高配准鲁棒性。单帧点云的特征信息有限，多帧融合后点云密度更高，几何特征更丰富，配准成功率显著提升。\u003c/p\u003e","title":"多机器人初始配准方法"},{"content":"介绍 本文主要介绍 LIO SLAM 系统中的可视化工具 Pangolin 和日志工具 glog 的使用方法。\n主要依赖 Pangolin：3D 可视化库 glog：Google 日志库 可视化 可视化功能在 visualize.cpp 中独立实现，创建单独的显示线程，使用原子布尔进行线程控制：\n1 std::atomic\u0026lt;bool\u0026gt; exit_flag_{false}; 头文件 1 2 #include \u0026lt;pangolin/pangolin.h\u0026gt; #include \u0026lt;GL/gl.h\u0026gt; 原子布尔 原子布尔需要进行读写操作：\n1 2 exit_flag_.store(true); // 写入 exit_flag_.load(); // 读取 原子布尔有锁机制，会强制同步线程的缓存值到主内存。\n可视化窗口设置 1 pangolin::CreateWindowAndBind(\u0026#34;ZLIO Visualize\u0026#34;, 1920, 1080); 投影参数配置：\n1 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 轴向上 按钮设置 创建左侧菜单面板：\n1 CreatePanel(\u0026#34;menu\u0026#34;).SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(180)); 参数 值 说明 第一个参数 0.0 左边界：屏幕最左边 第二个参数 1.0 右边界：屏幕最右边 第三个参数 0.0 下边界：屏幕最下面 第四个参数 pangolin::Attach::Pix(180) 上边界：离底部 180 像素 其他 Attach 用法：\n用法 说明 Attach::Pix(180) 固定像素值 Attach::Fam(1.0) 相对比例（1.0 = 100%） Attach::Ref(0.5) 相对于父容器 配置按钮：\n1 2 pangolin::Var\u0026lt;bool\u0026gt; menu_reset_view(\u0026#34;menu.Reset View\u0026#34;, false, false); // 重置视角按钮 pangolin::Var\u0026lt;float\u0026gt; menu_intensity(\u0026#34;menu.Intensity\u0026#34;, 1.0f, 0.1f, 10.0f); // 点云亮度调节 按钮无法修改 UI，据 AI 反馈可以和 ImGui 进行组合，后续再尝试。\n显示 使用 while 循环进行显示，通过线程休眠控制帧率：\n1 std::this_thread::sleep_for(std::chrono::milliseconds(30)); 睡眠时间 约等于帧率 sleep_for(16ms) ~60 FPS sleep_for(30ms) ~33 FPS sleep_for(100ms) ~10 FPS 点云绘制 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 void Visualize::RenderCurrentScan(float intensity_scale) { std::lock_guard\u0026lt;std::mutex\u0026gt; lock(mtx_cloud_); if (current_cloud_.points.empty()) { return; } // 清除并预分配变换后的点云数据缓冲区 cloud_xyz_.clear(); cloud_color_.clear(); cloud_xyz_.reserve(current_cloud_.points.size()); cloud_color_.reserve(current_cloud_.points.size()); // 遍历每个点，进行坐标系变换和颜色计算 for (const auto\u0026amp; pt : current_cloud_.points) { // 将点从机体坐标系变换到世界坐标系 Eigen::Vector3f pt_body(pt.x, pt.y, pt.z); Eigen::Vector3f pt_world = TransformPoint(pt_body, current_pose_); cloud_xyz_.push_back(pt_world); // 根据点云强度值计算灰度颜色，并应用菜单缩放 // intensity 归一化到 [0, 1] 范围 float intensity = pt.intensity / 255.0f * intensity_scale; intensity = std::min(intensity, 1.0f); cloud_color_.push_back(Eigen::Vector3f(intensity, intensity, intensity)); } // 使用 OpenGL 渲染点云 glPointSize(2.0f); glBegin(GL_POINTS); for (size_t i = 0; i \u0026lt; cloud_xyz_.size(); ++i) { const auto\u0026amp; pt = cloud_xyz_[i]; const auto\u0026amp; color = cloud_color_[i]; glColor3f(color.x(), color.y(), color.z()); glVertex3f(pt.x(), pt.y(), pt.z()); } glEnd(); } 轨迹绘制 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 void Visualize::RenderTrajectory() { std::lock_guard\u0026lt;std::mutex\u0026gt; lock(mtx_traj_); if (trajectory_pts_.empty()) { return; } // 设置线宽和颜色，绘制轨迹线 glLineWidth(2.0f); glColor3f(traj_color_[0], traj_color_[1], traj_color_[2]); glBegin(GL_LINE_STRIP); for (const auto\u0026amp; pt : trajectory_pts_) { glVertex3f(pt.x(), pt.y(), pt.z()); } glEnd(); // 在轨迹终点绘制红色标记点，表示当前位置 if (!trajectory_pts_.empty()) { const auto\u0026amp; last_pt = trajectory_pts_.back(); glPointSize(5.0f); glColor3f(1.0f, 0.0f, 0.0f); glBegin(GL_POINTS); glVertex3f(last_pt.x(), last_pt.y(), last_pt.z()); glEnd(); } } 机体坐标轴绘制 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 void Visualize::RenderCar() { std::lock_guard\u0026lt;std::mutex\u0026gt; lock(mtx_state_); // 从导航状态获取旋转矩阵和位置 Eigen::Matrix3f R = nav_state_.orientation.matrix(); Eigen::Vector3f t = nav_state_.position; // 计算车体坐标系三个轴的世界坐标表示 // X轴: 前进方向(1,0,0), Y轴: 右侧(0,1,0), Z轴: 上方(0,0,1) Eigen::Vector3f forward = R * Eigen::Vector3f(1, 0, 0) * car_size_; Eigen::Vector3f right = R * Eigen::Vector3f(0, 1, 0) * car_size_ * 0.5f; Eigen::Vector3f up = R * Eigen::Vector3f(0, 0, 1) * car_size_ * 0.3f; glLineWidth(2.0f); // 使用 GL_LINES 绘制三条坐标轴 glBegin(GL_LINES); // X 轴（前进方向）- 红色 glColor3f(1.0f, 0.0f, 0.0f); glVertex3f(t.x(), t.y(), t.z()); glVertex3f(t.x() + forward.x(), t.y() + forward.y(), t.z() + forward.z()); // Y 轴 - 绿色 glColor3f(0.0f, 1.0f, 0.0f); glVertex3f(t.x(), t.y(), t.z()); glVertex3f(t.x() + right.x(), t.y() + right.y(), t.z() + right.z()); // Z 轴 - 蓝色 glColor3f(0.0f, 0.0f, 1.0f); glVertex3f(t.x(), t.y(), t.z()); glVertex3f(t.x() + up.x(), t.y() + up.y(), t.z() + up.z()); glEnd(); } log 日志打印 日志打印直接设置参数并调用即可：\n1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 #include \u0026lt;glog/logging.h\u0026gt; // 头文件 // 初始化日志系统 google::InitGoogleLogging(\u0026#34;zlio_test\u0026#34;); // 设置日志保存路径 google::SetLogDestination(google::GLOG_INFO, \u0026#34;/home/yj/ZLIO_ws/logs/zlio_test\u0026#34;); // 设置日志级别（小于该级别的日志不输出） FLAGS_stderrthreshold = google::GLOG_WARNING; // 立即刷新日志到文件 FLAGS_logbufsecs = 0; // 使用日志宏 LOG(INFO) \u0026lt;\u0026lt; \u0026#34;程序开始\u0026#34;; // 写入 INFO 日志 LOG(WARNING) \u0026lt;\u0026lt; \u0026#34;警告\u0026#34;; // 写入 WARNING 日志 LOG(ERROR) \u0026lt;\u0026lt; \u0026#34;错误\u0026#34;; // 写入 ERROR 日志 // 关闭 google 日志 google::ShutdownGoogleLogging(); 只要链接到同一个程序，引用头文件 #include \u0026lt;glog/logging.h\u0026gt; 就可以直接调用宏进行日志打印。\n参考 无 ","permalink":"https://zero-kq.github.io/Pages/posts/slam/lio%E5%AD%A6%E4%B9%A0%E7%AC%94%E8%AE%B002/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e本文主要介绍 LIO SLAM 系统中的可视化工具 Pangolin 和日志工具 glog 的使用方法。\u003c/p\u003e\n\u003ch2 id=\"主要依赖\"\u003e主要依赖\u003c/h2\u003e\n\u003cul\u003e\n\u003cli\u003ePangolin：3D 可视化库\u003c/li\u003e\n\u003cli\u003eglog：Google 日志库\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch2 id=\"可视化\"\u003e可视化\u003c/h2\u003e\n\u003cp\u003e可视化功能在 \u003ccode\u003evisualize.cpp\u003c/code\u003e 中独立实现，创建单独的显示线程，使用原子布尔进行线程控制：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003estd\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eatomic\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026lt;\u003c/span\u003e\u003cspan class=\"kt\"\u003ebool\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026gt;\u003c/span\u003e \u003cspan class=\"n\"\u003eexit_flag_\u003c/span\u003e\u003cspan class=\"p\"\u003e{\u003c/span\u003e\u003cspan class=\"nb\"\u003efalse\u003c/span\u003e\u003cspan class=\"p\"\u003e};\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"头文件\"\u003e头文件\u003c/h3\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;pangolin/pangolin.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;GL/gl.h\u0026gt;\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"原子布尔\"\u003e原子布尔\u003c/h3\u003e\n\u003cp\u003e原子布尔需要进行读写操作：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003eexit_flag_\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003estore\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"nb\"\u003etrue\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e   \u003cspan class=\"c1\"\u003e// 写入\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003eexit_flag_\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eload\u003c/span\u003e\u003cspan class=\"p\"\u003e();\u003c/span\u003e        \u003cspan class=\"c1\"\u003e// 读取\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e原子布尔有\u003cstrong\u003e锁\u003c/strong\u003e机制，会强制同步线程的缓存值到主内存。\u003c/p\u003e\n\u003ch3 id=\"可视化窗口设置\"\u003e可视化窗口设置\u003c/h3\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003epangolin\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eCreateWindowAndBind\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"s\"\u003e\u0026#34;ZLIO Visualize\u0026#34;\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e1920\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e1080\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e投影参数配置：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003epangolin\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eOpenGlRenderState\u003c/span\u003e \u003cspan class=\"n\"\u003es_cam\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003epangolin\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eProjectionMatrix\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"mi\"\u003e1920\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e1080\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e5000\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e5000\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e960\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e540\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e1000\u003c/span\u003e\u003cspan class=\"p\"\u003e),\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003epangolin\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eModelViewLookAt\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"o\"\u003e-\u003c/span\u003e\u003cspan class=\"mi\"\u003e10\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e20\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epangolin\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eAxisY\u003c/span\u003e\u003cspan class=\"p\"\u003e));\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e参数\u003c/th\u003e\n          \u003cth\u003e值\u003c/th\u003e\n          \u003cth\u003e说明\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eProjectionMatrix 第1/2个参数\u003c/td\u003e\n          \u003ctd\u003e1920, 1080\u003c/td\u003e\n          \u003ctd\u003e屏幕分辨率\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eProjectionMatrix 第3/4个参数\u003c/td\u003e\n          \u003ctd\u003e5000, 5000\u003c/td\u003e\n          \u003ctd\u003efx, fy 焦距\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eProjectionMatrix 第5/6个参数\u003c/td\u003e\n          \u003ctd\u003e960, 540\u003c/td\u003e\n          \u003ctd\u003ecx, cy 主点（屏幕中心）\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eProjectionMatrix 第7/8个参数\u003c/td\u003e\n          \u003ctd\u003e1, 1000\u003c/td\u003e\n          \u003ctd\u003enear, far 显示深度\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eModelViewLookAt 第1/2/3个参数\u003c/td\u003e\n          \u003ctd\u003e0, -10, 20\u003c/td\u003e\n          \u003ctd\u003e相机位置\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eModelViewLookAt 第4/5/6个参数\u003c/td\u003e\n          \u003ctd\u003e0, 0, 0\u003c/td\u003e\n          \u003ctd\u003e相机观察目标点\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eModelViewLookAt 第7个参数\u003c/td\u003e\n          \u003ctd\u003epangolin::AxisY\u003c/td\u003e\n          \u003ctd\u003eY 轴向上\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003ch3 id=\"按钮设置\"\u003e按钮设置\u003c/h3\u003e\n\u003cp\u003e创建左侧菜单面板：\u003c/p\u003e","title":"LIO 学习笔记 02"},{"content":"介绍 打算实现读书时应该完成的工作，写一个自己的 SLAM，来对 SLAM 的实现有一个更深层次的了解，项目参考了 FAST_LIO2、Voxel-SLAM、lightning-lm\n数据结构定义和数据缓存 首先是对数据结构体进行创建，点、点云、IMU、同步后的数据体，为了结构清楚，我单独写一个头文件common.h方便查看和管理 点和点云通过PCL注册，hku是通过PointXYZNomal的曲率存储时间戳，lm是通过重新注册一个 IMU 包含时间戳、加速度计、陀螺仪 同步后的数据measuregroup 包含点云 两帧点云之间的所有IMU 之所以单独定义结构体，更多的是增强独立性，这样其他如ros或者雷达调用接口就可调用该里程计\n数据缓存 imu_buffer_.push_back(imu) lidar_buffer_.push_back(cloud)\n点云创建和注册 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\u0026lt;std::mutex\u0026gt; lock(mtx_buffer_); 因为回调函数出发存储，避免和其他位置同时操作缓存导致错误\n说明：std::lock_guard 能自动解锁，在作用域结束、return、异常时自动解锁，作用域是从声明开始到所在的大括号结束\nROS接口 创建ROS接口实现，算法模块与ROS系统解耦，虽然现在基本是在ROS系统中运行，但是解耦后更方便移植在ROS1和ROS2中，以及万一之后在其他嵌入式上运行\n主要是对应的话题回调函数中调用slam借口实现\nRosApp::CloudCallback RosApp::IMUCallback\n开始我自己创建的点云结构体，没有用PCL，所以不是很理解time_buffer的作用，三个参考项目都加了time_buffer，time_buffer中存储的是话题的时间戳，在时间同步时作为点云的扫描开始时间，那为什么不直接在点云存储时间戳，我就省略了time_buffer 我知道了，因为他们创建的点云结构体没有时间戳，我是自己定义的点云，考虑到点云只在同步时才需要时间，确实没必要多一个成员\nfastlio对livox雷达会进行IMU和雷达的时间软同步处理，不是很知道区别对待的原图，其他的算法似乎没有\n参考算法似乎对不同雷达还会有不同的处理，但是最后还是只用到了XYZI，以及用curvature记录不同雷达的点云时间偏移，lm则是用的动态平均值处理的点云时间偏移，计划先用lm的方法，后面再两个都加上吧\nlm在IMU回调中还额外EKF处理了，估计是为了高频输出，后续再进行添加吧\n回调函数中还通过跳点逻辑进行了降采样，我之前都是用的体素滤波，非常耗时\n1 2 3 4 5 6 7 8 9 inline PointCloudType DownSamplePointCloud(const PointCloudType\u0026amp; cloud, int point_filter_num) { PointCloudType downsampled; downsampled.reserve(cloud.size() / point_filter_num + 1); for (size_t i = 0; i \u0026lt; cloud.points.size(); i += point_filter_num) { downsampled.points.push_back(cloud.points[i]); } return downsampled; } 跳点常用间隔参考：\n项目 LiDAR 类型 point_filter_num FAST-LIO AVIA 3 Velodyne 4 Mid360 3 Horizon 3 SE3 1（不降采样） Voxel-SLAM Mid360 3 AVIA 3 Velodyne 3 Ouster 1 Hesai 1 lightning-lm Livox 4 NCLT 6 Robosense 6 UTBM 10 还有自定义点云使用相关函数如体素滤波需要在头文件中宏定义\n1 #define PCL_NO_PRECOMPILE // 禁用PCL预编译，启用自定义点云类型模板实例化 不然编译会报错链接失败之类的\n数据同步 三个好像都差不多，简单的逻辑，以雷达点云为标准，如果IMU数据不够（时间比雷达的时间早）就返回false，然后吧两帧之间的IMU及当前点云帧全部放入一个结构体中，同步的数据结构除了点云和IMU容器，还需要点云的开始扫描和结束扫描时间，用于去畸变。\n参考 FAST_LIO源码：https://github.com/hku-mars/FAST_LIO Voxel-SLAM源码：https://github.com/hku-mars/Voxel-SLAM lightning-lm源码：https://github.com/gaoxiang12/lightning-lm ","permalink":"https://zero-kq.github.io/Pages/posts/slam/lio%E5%AD%A6%E4%B9%A0%E7%AC%94%E8%AE%B001/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e打算实现读书时应该完成的工作，写一个自己的 SLAM，来对 SLAM 的实现有一个更深层次的了解，项目参考了 \u003ca href=\"https://github.com/hku-mars/FAST_LIO\"\u003eFAST_LIO2\u003c/a\u003e、\u003ca href=\"https://github.com/hku-mars/Voxel-SLAM\"\u003eVoxel-SLAM\u003c/a\u003e、\u003ca href=\"https://github.com/gaoxiang12/lightning-lm\"\u003elightning-lm\u003c/a\u003e\u003c/p\u003e\n\u003ch2 id=\"数据结构定义和数据缓存\"\u003e数据结构定义和数据缓存\u003c/h2\u003e\n\u003cp\u003e首先是对数据结构体进行创建，点、点云、IMU、同步后的数据体，为了结构清楚，我单独写一个头文件common.h方便查看和管理\n点和点云通过PCL注册，hku是通过PointXYZNomal的曲率存储时间戳，lm是通过重新注册一个\nIMU 包含时间戳、加速度计、陀螺仪\n同步后的数据measuregroup 包含点云 两帧点云之间的所有IMU\n之所以单独定义结构体，更多的是增强独立性，这样其他如ros或者雷达调用接口就可调用该里程计\u003c/p\u003e\n\u003cp\u003e数据缓存\nimu_buffer_.push_back(imu)\nlidar_buffer_.push_back(cloud)\u003c/p\u003e\n\u003ch3 id=\"点云创建和注册\"\u003e点云创建和注册\u003c/h3\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e 1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 9\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e10\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e11\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// PCL兼容点类型 (必须在全局或pcl命名空间定义)\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"k\"\u003estruct\u003c/span\u003e \u003cspan class=\"nc\"\u003ePointXYZIT\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003ePCL_ADD_POINT4D\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003ePCL_ADD_INTENSITY\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"kt\"\u003edouble\u003c/span\u003e \u003cspan class=\"n\"\u003etimestamp\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003ePointXYZIT\u003c/span\u003e\u003cspan class=\"p\"\u003e()\u003c/span\u003e \u003cspan class=\"p\"\u003e{}\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003eEIGEN_MAKE_ALIGNED_OPERATOR_NEW\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"p\"\u003e};\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003ePOINT_CLOUD_REGISTER_POINT_STRUCT\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003ePointXYZIT\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"kt\"\u003efloat\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ex\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ex\u003c/span\u003e\u003cspan class=\"p\"\u003e)(\u003c/span\u003e\u003cspan class=\"kt\"\u003efloat\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ey\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ey\u003c/span\u003e\u003cspan class=\"p\"\u003e)(\u003c/span\u003e\u003cspan class=\"kt\"\u003efloat\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ez\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ez\u003c/span\u003e\u003cspan class=\"p\"\u003e)(\u003c/span\u003e\u003cspan class=\"kt\"\u003efloat\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003eintensity\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003eintensity\u003c/span\u003e\u003cspan class=\"p\"\u003e)(\u003c/span\u003e\u003cspan class=\"kt\"\u003edouble\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003etimestamp\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003etimestamp\u003c/span\u003e\u003cspan class=\"p\"\u003e))\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"注意\"\u003e注意\u003c/h3\u003e\n\u003cp\u003e每个数据在存储时会用到 \u003ccode\u003estd::lock_guard\u0026lt;std::mutex\u0026gt; lock(mtx_buffer_);\u003c/code\u003e 因为回调函数出发存储，避免和其他位置同时操作缓存导致错误\u003c/p\u003e\n\u003cblockquote\u003e\n\u003cp\u003e\u003cstrong\u003e说明\u003c/strong\u003e：\u003ccode\u003estd::lock_guard\u003c/code\u003e 能自动解锁，在作用域结束、return、异常时自动解锁，作用域是从声明开始到所在的大括号结束\u003c/p\u003e\u003c/blockquote\u003e\n\u003ch2 id=\"ros接口\"\u003eROS接口\u003c/h2\u003e\n\u003cp\u003e创建ROS接口实现，算法模块与ROS系统解耦，虽然现在基本是在ROS系统中运行，但是解耦后更方便移植在ROS1和ROS2中，以及万一之后在其他嵌入式上运行\u003c/p\u003e\n\u003cp\u003e主要是对应的话题回调函数中调用slam借口实现\u003c/p\u003e\n\u003cp\u003eRosApp::CloudCallback\nRosApp::IMUCallback\u003c/p\u003e\n\u003cp\u003e开始我自己创建的点云结构体，没有用PCL，所以不是很理解time_buffer的作用，三个参考项目都加了time_buffer，time_buffer中存储的是话题的时间戳，在时间同步时作为点云的扫描开始时间，那为什么不直接在点云存储时间戳，我就省略了time_buffer\n我知道了，因为他们创建的点云结构体没有时间戳，我是自己定义的点云，考虑到点云只在同步时才需要时间，确实没必要多一个成员\u003c/p\u003e\n\u003cp\u003efastlio对livox雷达会进行IMU和雷达的时间软同步处理，不是很知道区别对待的原图，其他的算法似乎没有\u003c/p\u003e\n\u003cp\u003e参考算法似乎对不同雷达还会有不同的处理，但是最后还是只用到了XYZI，以及用curvature记录不同雷达的点云时间偏移，lm则是用的动态平均值处理的点云时间偏移，计划先用lm的方法，后面再两个都加上吧\u003c/p\u003e\n\u003cp\u003elm在IMU回调中还额外EKF处理了，估计是为了高频输出，后续再进行添加吧\u003c/p\u003e\n\u003cp\u003e回调函数中还通过跳点逻辑进行了降采样，我之前都是用的体素滤波，非常耗时\u003c/p\u003e","title":"LIO 学习笔记 01"},{"content":"介绍 部署 YOLOv8 目标检测算法到 K230 开发板上运行，本文记录完整的部署流程。\n环境准备 软件依赖 CanMV IDE 已训练好的 YOLOv8 模型（best.pt） Python 环境（建议使用 conda） 硬件要求 K230 开发板（已刷好固件） 摄像头 模型转换 PyTorch → ONNX 将训练好的模型导出为 ONNX 格式，支持模型量化优化。\n1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 from ultralytics import YOLO if __name__ == \u0026#34;__main__\u0026#34;: # 1. 加载训练好的 YOLO 模型 model = YOLO(r\u0026#34;..\\best.pt\u0026#34;) # 2. 调用 YOLO 模型实例的 export 方法 # 注意：这里不需要重新赋值 model，export 方法会直接生成 ONNX 文件 model.export( format=\u0026#34;onnx\u0026#34;, # 指定导出格式为 ONNX simplify=True, # 简化 ONNX 模型 imgsz=640, # 输入图像尺寸 opset=12 # ONNX 算子集版本 ) print(\u0026#34;ONNX 模型导出成功！文件会保存在 best.pt 同目录下\u0026#34;) ONNX → KModel 在 Windows 环境下完成模型转换。\n1. 安装 .NET 7.0 下载并安装 .NET 7.0。\n2. 安装 Python 依赖 1 pip install nncase onnx onnxruntime onnxsim 3. 安装 nncase_kpu 从 nncase releases 下载 nncase_kpu-2*-py2.py3-none-win_amd64.whl，然后安装：\n1 pip install nncase_kpu-2*-py2.py3-none-win_amd64.whl 4. 验证版本 1 pip list | findstr nncase 确保 nncase 和 nncase_kpu 版本一致。\n5. 复制依赖库 下载 libomp140.x86_64.zip，将 libomp140.x86_64.dll 复制到 C:\\Windows\\System32。\n6. 下载官方转换工具 1 2 wget https://kendryte-download.canaan-creative.com/developer/k230/yolo_files/test_yolov8.zip unzip test_yolov8.zip 7. 转换为 KModel 1 python to_kmodel.py --target k230 --model ../best.onnx --dataset ../test --input_width 640 --input_height 640 --ptq_option 0 参数 说明 model ONNX 模型路径 dataset 验证图片文件夹 input_width/height 输入图像尺寸 ptq_option 量化选项（0 = int8） 警告说明 以下警告不影响转换结果：\n1 2 3 4 5 6 7 warn: Nncase.Hosting.PluginLoader[0] NNCASE_PLUGIN_PATH is not set. to_kmodel.py:25: DeprecationWarning: `mapping.TENSOR_TYPE_TO_NP_TYPE` is now deprecated and will be removed in a future release.To silence this warning, please use `helper.tensor_dtype_to_np_dtype` instead. input_dict[\u0026#39;dtype\u0026#39;] = onnx.mapping.TENSOR_TYPE_TO_NP_TYPE[onnx_type.elem_type] WARNING: The argument `input_shapes` is deprecated. Please use `overwrite_input_shapes` and/or `test_input_shapes` instead. An error will be raised in the future. 部署代码 将 KModel 文件放入 K230 开发板，运行以下代码：\n1 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 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 from libs.PipeLine import PipeLine, ScopedTiming from libs.AIBase import AIBase from libs.AI2D import Ai2d import os import ujson from media.media import * from time import * import nncase_runtime as nn import ulab.numpy as np import time import utime import image import random import gc import sys import aidemo # 自定义YOLOv8检测类 class ObjectDetectionApp(AIBase): def __init__(self,kmodel_path,labels,model_input_size,max_boxes_num,confidence_threshold=0.5,nms_threshold=0.2,rgb888p_size=[224,224],display_size=[1920,1080],debug_mode=0): super().__init__(kmodel_path,model_input_size,rgb888p_size,debug_mode) self.kmodel_path=kmodel_path self.labels=labels # 模型输入分辨率 self.model_input_size=model_input_size # 阈值设置 self.confidence_threshold=confidence_threshold self.nms_threshold=nms_threshold self.max_boxes_num=max_boxes_num # sensor给到AI的图像分辨率 self.rgb888p_size=[ALIGN_UP(rgb888p_size[0],16),rgb888p_size[1]] # 显示分辨率 self.display_size=[ALIGN_UP(display_size[0],16),display_size[1]] self.debug_mode=debug_mode # 检测框预置颜色值 self.color_four=[(255, 220, 20, 60), (255, 119, 11, 32), (255, 0, 0, 142), (255, 0, 0, 230), (255, 106, 0, 228), (255, 0, 60, 100), (255, 0, 80, 100), (255, 0, 0, 70), (255, 0, 0, 192), (255, 250, 170, 30), (255, 100, 170, 30), (255, 220, 220, 0), (255, 175, 116, 175), (255, 250, 0, 30), (255, 165, 42, 42), (255, 255, 77, 255), (255, 0, 226, 252), (255, 182, 182, 255), (255, 0, 82, 0), (255, 120, 166, 157)] # 宽高缩放比例 self.x_factor = float(self.rgb888p_size[0])/self.model_input_size[0] self.y_factor = float(self.rgb888p_size[1])/self.model_input_size[1] # Ai2d实例，用于实现模型预处理 self.ai2d=Ai2d(debug_mode) # 设置Ai2d的输入输出格式和类型 self.ai2d.set_ai2d_dtype(nn.ai2d_format.NCHW_FMT,nn.ai2d_format.NCHW_FMT,np.uint8, np.uint8) # 配置预处理操作，这里使用了resize，Ai2d支持crop/shift/pad/resize/affine，具体代码请打开/sdcard/app/libs/AI2D.py查看 def config_preprocess(self,input_image_size=None): with ScopedTiming(\u0026#34;set preprocess config\u0026#34;,self.debug_mode \u0026gt; 0): # 初始化ai2d预处理配置，默认为sensor给到AI的尺寸，您可以通过设置input_image_size自行修改输入尺寸 ai2d_input_size=input_image_size if input_image_size else self.rgb888p_size self.ai2d.resize(nn.interp_method.tf_bilinear, nn.interp_mode.half_pixel) self.ai2d.build([1,3,ai2d_input_size[1],ai2d_input_size[0]],[1,3,self.model_input_size[1],self.model_input_size[0]]) # 自定义当前任务的后处理 def postprocess(self,results): with ScopedTiming(\u0026#34;postprocess\u0026#34;,self.debug_mode \u0026gt; 0): result=results[0] result = result.reshape((result.shape[0] * result.shape[1], result.shape[2])) output_data = result.transpose() boxes_ori = output_data[:,0:4] scores_ori = output_data[:,4:] confs_ori = np.max(scores_ori,axis=-1) inds_ori = np.argmax(scores_ori,axis=-1) boxes,scores,inds = [],[],[] for i in range(len(boxes_ori)): if confs_ori[i] \u0026gt; self.confidence_threshold: scores.append(confs_ori[i]) inds.append(inds_ori[i]) x = boxes_ori[i,0] y = boxes_ori[i,1] w = boxes_ori[i,2] h = boxes_ori[i,3] left = int((x - 0.5 * w) * self.x_factor) top = int((y - 0.5 * h) * self.y_factor) right = int((x + 0.5 * w) * self.x_factor) bottom = int((y + 0.5 * h) * self.y_factor) boxes.append([left,top,right,bottom]) if len(boxes)==0: return [] boxes = np.array(boxes) scores = np.array(scores) inds = np.array(inds) # NMS过程 keep = self.nms(boxes,scores,nms_threshold) dets = np.concatenate((boxes, scores.reshape((len(boxes),1)), inds.reshape((len(boxes),1))), axis=1) dets_out = [] for keep_i in keep: dets_out.append(dets[keep_i]) dets_out = np.array(dets_out) dets_out = dets_out[:self.max_boxes_num, :] return dets_out # 绘制结果 def draw_result(self,pl,dets): with ScopedTiming(\u0026#34;display_draw\u0026#34;,self.debug_mode \u0026gt;0): if dets: pl.osd_img.clear() for det in dets: x1, y1, x2, y2 = map(lambda x: int(round(x, 0)), det[:4]) x= x1*self.display_size[0] // self.rgb888p_size[0] y= y1*self.display_size[1] // self.rgb888p_size[1] w = (x2 - x1) * self.display_size[0] // self.rgb888p_size[0] h = (y2 - y1) * self.display_size[1] // self.rgb888p_size[1] pl.osd_img.draw_rectangle(x,y, w, h, color=self.get_color(int(det[5])),thickness=4) pl.osd_img.draw_string_advanced( x , y-50,32,\u0026#34; \u0026#34; + self.labels[int(det[5])] + \u0026#34; \u0026#34; + str(round(det[4],2)) , color=self.get_color(int(det[5]))) else: pl.osd_img.clear() # 多目标检测 非最大值抑制方法实现 def nms(self,boxes,scores,thresh): \u0026#34;\u0026#34;\u0026#34;Pure Python NMS baseline.\u0026#34;\u0026#34;\u0026#34; x1,y1,x2,y2 = boxes[:, 0],boxes[:, 1],boxes[:, 2],boxes[:, 3] areas = (x2 - x1 + 1) * (y2 - y1 + 1) order = np.argsort(scores,axis = 0)[::-1] keep = [] while order.size \u0026gt; 0: i = order[0] keep.append(i) new_x1,new_y1,new_x2,new_y2,new_areas = [],[],[],[],[] for order_i in order: new_x1.append(x1[order_i]) new_x2.append(x2[order_i]) new_y1.append(y1[order_i]) new_y2.append(y2[order_i]) new_areas.append(areas[order_i]) new_x1 = np.array(new_x1) new_x2 = np.array(new_x2) new_y1 = np.array(new_y1) new_y2 = np.array(new_y2) xx1 = np.maximum(x1[i], new_x1) yy1 = np.maximum(y1[i], new_y1) xx2 = np.minimum(x2[i], new_x2) yy2 = np.minimum(y2[i], new_y2) w = np.maximum(0.0, xx2 - xx1 + 1) h = np.maximum(0.0, yy2 - yy1 + 1) inter = w * h new_areas = np.array(new_areas) ovr = inter / (areas[i] + new_areas - inter) new_order = [] for ovr_i,ind in enumerate(ovr): if ind \u0026lt; thresh: new_order.append(order[ovr_i]) order = np.array(new_order,dtype=np.uint8) return keep # 根据当前类别索引获取框的颜色 def get_color(self, x): idx=x%len(self.color_four) return self.color_four[idx] if __name__==\u0026#34;__main__\u0026#34;: # 显示模式，默认\u0026#34;hdmi\u0026#34;,可以选择\u0026#34;hdmi\u0026#34;和\u0026#34;lcd\u0026#34; display_mode=\u0026#34;hdmi\u0026#34; if display_mode==\u0026#34;hdmi\u0026#34;: display_size=[1920,1080] else: display_size=[800,480] # 模型路径 kmodel_path=\u0026#34;/sdcard/examples/kmodel/best.kmodel\u0026#34; labels = [\u0026#34;Black_footed_Albatross\u0026#34;, \u0026#34;Laysan_Albatross\u0026#34;, \u0026#34;Sooty_Albatross\u0026#34;, \u0026#34;Groove_billed_Ani\u0026#34;, \u0026#34;Crested_Auklet\u0026#34;] # 其它参数设置 confidence_threshold = 0.2 nms_threshold = 0.2 max_boxes_num = 50 rgb888p_size=[320,320] # 初始化PipeLine pl=PipeLine(rgb888p_size=rgb888p_size,display_size=display_size,display_mode=display_mode) pl.create() # 初始化自定义目标检测实例 ob_det=ObjectDetectionApp(kmodel_path,labels=labels,model_input_size=[640,640],max_boxes_num=max_boxes_num,confidence_threshold=confidence_threshold,nms_threshold=nms_threshold,rgb888p_size=rgb888p_size,display_size=display_size,debug_mode=0) ob_det.config_preprocess() try: while True: os.exitpoint() with ScopedTiming(\u0026#34;total\u0026#34;,1): # 获取当前帧数据 img=pl.get_frame() # 推理当前帧 res=ob_det.run(img) # 绘制结果到PipeLine的osd图像 ob_det.draw_result(pl,res) # 显示当前的绘制结果 pl.show_image() gc.collect() except Exception as e: sys.print_exception(e) finally: ob_det.deinit() pl.destroy() 参考 Kendryte K230 YOLO 大作战 ","permalink":"https://zero-kq.github.io/Pages/posts/%E6%84%9F%E7%9F%A5/k230%E9%83%A8%E7%BD%B2yolov8/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e部署 YOLOv8 目标检测算法到 K230 开发板上运行，本文记录完整的部署流程。\u003c/p\u003e\n\u003chr\u003e\n\u003ch2 id=\"环境准备\"\u003e环境准备\u003c/h2\u003e\n\u003ch3 id=\"软件依赖\"\u003e软件依赖\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003eCanMV IDE\u003c/li\u003e\n\u003cli\u003e已训练好的 YOLOv8 模型（best.pt）\u003c/li\u003e\n\u003cli\u003ePython 环境（建议使用 conda）\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch3 id=\"硬件要求\"\u003e硬件要求\u003c/h3\u003e\n\u003cul\u003e\n\u003cli\u003eK230 开发板（已刷好固件）\u003c/li\u003e\n\u003cli\u003e摄像头\u003c/li\u003e\n\u003c/ul\u003e\n\u003chr\u003e\n\u003ch2 id=\"模型转换\"\u003e模型转换\u003c/h2\u003e\n\u003ch3 id=\"pytorch--onnx\"\u003ePyTorch → ONNX\u003c/h3\u003e\n\u003cp\u003e将训练好的模型导出为 ONNX 格式，支持模型量化优化。\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e 1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 9\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e10\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e11\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e12\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e13\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e14\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e15\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e16\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-python\" data-lang=\"python\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"kn\"\u003efrom\u003c/span\u003e \u003cspan class=\"nn\"\u003eultralytics\u003c/span\u003e \u003cspan class=\"kn\"\u003eimport\u003c/span\u003e \u003cspan class=\"n\"\u003eYOLO\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"k\"\u003eif\u003c/span\u003e \u003cspan class=\"vm\"\u003e__name__\u003c/span\u003e \u003cspan class=\"o\"\u003e==\u003c/span\u003e \u003cspan class=\"s2\"\u003e\u0026#34;__main__\u0026#34;\u003c/span\u003e\u003cspan class=\"p\"\u003e:\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"c1\"\u003e# 1. 加载训练好的 YOLO 模型\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003emodel\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003eYOLO\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"sa\"\u003er\u003c/span\u003e\u003cspan class=\"s2\"\u003e\u0026#34;..\\best.pt\u0026#34;\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"c1\"\u003e# 2. 调用 YOLO 模型实例的 export 方法\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"c1\"\u003e# 注意：这里不需要重新赋值 model，export 方法会直接生成 ONNX 文件\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003emodel\u003c/span\u003e\u003cspan class=\"o\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eexport\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e        \u003cspan class=\"nb\"\u003eformat\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"s2\"\u003e\u0026#34;onnx\u0026#34;\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e  \u003cspan class=\"c1\"\u003e# 指定导出格式为 ONNX\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e        \u003cspan class=\"n\"\u003esimplify\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"kc\"\u003eTrue\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e   \u003cspan class=\"c1\"\u003e# 简化 ONNX 模型\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e        \u003cspan class=\"n\"\u003eimgsz\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"mi\"\u003e640\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e      \u003cspan class=\"c1\"\u003e# 输入图像尺寸\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e        \u003cspan class=\"n\"\u003eopset\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"mi\"\u003e12\u003c/span\u003e        \u003cspan class=\"c1\"\u003e# ONNX 算子集版本\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"p\"\u003e)\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"nb\"\u003eprint\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"s2\"\u003e\u0026#34;ONNX 模型导出成功！文件会保存在 best.pt 同目录下\u0026#34;\u003c/span\u003e\u003cspan class=\"p\"\u003e)\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"onnx--kmodel\"\u003eONNX → KModel\u003c/h3\u003e\n\u003cp\u003e在 Windows 环境下完成模型转换。\u003c/p\u003e","title":"K230 部署 YOLOv8"},{"content":"点云配准方法介绍 常见的有 ICP、GICP、NDT 等，快捷实现基本基于 PCL 库中自带的点云处理实现。\nGICP 原理 Generalized Iterative Closest Point (GICP) 是一种改进的 ICP 算法，通过协方差矩阵优化对应点匹配，提升配准精度。\n头文件 1 2 3 4 5 6 7 #include \u0026lt;pcl/point_types.h\u0026gt; // 点类型定义 #include \u0026lt;pcl/point_cloud.h\u0026gt; // 点云容器 #include \u0026lt;pcl/registration/gicp.h\u0026gt; // GICP 算法核心 #include \u0026lt;pcl/registration/icp.h\u0026gt; // ICP 基类（部分功能依赖） #include \u0026lt;pcl/features/normal_3d.h\u0026gt; // 法向量估计（GICP 需要） #include \u0026lt;pcl/filters/voxel_grid.h\u0026gt; // 下采样（可选） #include \u0026lt;pcl/io/pcd_io.h\u0026gt; // 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。\n初始让 AI 以 Score 为基准进行调参，获得\u0026quot;最优参数\u0026quot;，但实际误差依旧很大\n测试概述 分类 参数 值 配置 max_iterations 500 配置 transformation_epsilon 1e-10 配置 euclidean_fitness_epsilon 0.0001 配置 max_correspondence_distance 50.0 配置 correspondence_randomness 20 配置 ransac_iterations 1000 配置 ransac_outlier_rejection_threshold 0.05 配置 transformation_rotation_epsilon 1e-10 配置 use_reciprocal_correspondences false 输出 Score 0.891884 输出 Time 862.025 ms 输出 Trans Error 2.14007 m 输出 Rot Error 2.03481 deg 效果展示 图例：绿色=Target cloud，白色=Source cloud (before)，蓝色=Registration result\n测试分析 通过遍历调参优化 Score 指标，发现 mcd=50, cr=20 时 Score 可降至 0.89，但实际 Trans Error 仍达 2.14m。\nScore vs Trans/Rot Error 差异原因：\nScore 是局部误差，Trans/Rot Error 是全局误差\nGICP 优化的是点到邻近点平面的距离（局部对齐质量） 即使对应关系完全错误，只要局部结构相似，Score 也可能很低 对称性欺骗\n例如 180 度旋转后，点云局部结构仍然相似 GICP 可能收敛到\u0026quot;错误但局部最优\u0026quot;的解 初始估计敏感\nGICP 是局部优化算法 从单位矩阵出发，没有好的初始估计 可能收敛到错误的局部最优而非全局最优 mcd 太大的影响\nmcd=50 太大，允许更远的对应点匹配 可能接受错误匹配，陷入局部最优 然后以 Trans Error 和 Rot Error 作为基准进行参数调整\n测试概述 分类 参数 值 配置 max_iterations 500 配置 transformation_epsilon 1e-10 配置 euclidean_fitness_epsilon 0.0001 配置 max_correspondence_distance 17 配置 correspondence_randomness 18.5 配置 ransac_iterations 1000 配置 ransac_outlier_rejection_threshold 0.05 配置 transformation_rotation_epsilon 1e-10 配置 use_reciprocal_correspondences false 输出 Score 2.9e-07 输出 Time 777 ms 输出 Trans Error 0.0002 m 输出 Rot Error 0 deg 效果展示 分析总结 mcd cr Trans Error Rot Error Score 50 20 2.14m 2.03deg 0.89 17 18.5 0.0002m 0deg 2.9e-07 优化目标从 Score 改为 Trans/Rot Error 后，找到真正有效的参数组合。\n关键发现：\nScore 与 Trans/Rot Error 无直接关联\nScore 是点到平面距离的 RMSE（局部对齐质量） Trans/Rot Error 是全局位姿精度 Score 低不等于配准准确 陷入错误局部最优的原因\nmcd=50 太大：允许更远的对应点匹配，接受错误匹配 初始估计敏感：从单位矩阵出发，没有好的初始估计 对称性欺骗：180 度旋转后局部结构仍相似 核心参数：mcd 和 cr 决定配准成败\nmcd 太大：接受错误匹配 mcd 太小：找不到正确对应 cr 偏离 18-20：陷入局部最优 参数对耗时和分数的影响：\n参数 影响程度 对分数的影响 对耗时的影响 max_correspondence_distance ★★★★★ 决定是否收敛到正确解 mcd 较小时耗时增加 correspondence_randomness ★★★★★ 决定是否收敛到正确解 cr 太大或太小都会增加耗时 ransac_outlier_rejection_threshold ☆☆☆☆☆ 无影响 无影响 ransac_iterations ☆☆☆☆☆ 无影响 无影响 max_iterations ☆☆☆☆☆ 无影响 无影响 transformation_epsilon ☆☆☆☆☆ 无影响 无影响 euclidean_fitness_epsilon ☆☆☆☆☆ 无影响 无影响 transformation_rotation_epsilon ☆☆☆☆☆ 无影响 无影响 use_reciprocal_correspondences ☆☆☆☆☆ 无影响 无影响 耗时分析：\nmcd cr Time Trans Error 5 18 889ms 17.3m 17 18.5 777ms 0.0002m 20 18 461ms 0.0012m 50 20 519ms 20.1m 追求精度：mcd=17, cr=18.5，耗时约 777ms 追求速度：mcd=20, cr=18，耗时约 461ms，精度仍可达标 最优配置：\n1 2 3 max_correspondence_distance: 17 correspondence_randomness: 18.5 max_iterations: 500 NDT + GICP 参数配置 参数 值 说明 ndt.transformation_epsilon 0.005 NDT 变换矩阵收敛阈值 ndt.step_size 1 NDT 步长 ndt.resolution 150 NDT 栅格分辨率 ndt.max_iterations 300 NDT 最大迭代次数 gicp.max_iterations 200 GICP 最大迭代次数 gicp.transformation_epsilon 1e-10 GICP 变换矩阵收敛阈值 gicp.euclidean_fitness_epsilon 0.0001 GICP 欧氏距离收敛阈值 gicp.max_correspondence_distance 17.0 GICP 对应点搜索半径上限 gicp.correspondence_randomness 50 GICP 随机对应点数量 gicp.ransac_outlier_rejection_threshold 0.05 GICP RANSAC 离群点阈值 测试概述 分类 参数 值 点数 Target/Source 21528 points 实际变换 T [10, 10, 10] m 实际变换 R [90, 0, 0] deg NDT transformation_epsilon 0.005 NDT step_size 1 NDT resolution 150 NDT max_iterations 300 GICP max_iterations 200 GICP transformation_epsilon 1e-10 GICP euclidean_fitness_epsilon 0.0001 GICP max_correspondence_distance 17.0 GICP correspondence_randomness 50 GICP ransac_outlier_rejection_threshold 0.05 输出 NDT Time 5023.26 ms 输出 GICP Time 198.718 ms 输出 Total Time 5225.32 ms 输出 Score 4.43e-07 输出 Trans Error 0.0002 m 输出 Rot Error 0 deg 效果展示 总结分析 NDT的耗时过长，并不适合进行slam的回环配准，可用于初始化配准，因为GICP需要一个比较好的初始位姿，但是按纯GICP配准中，似乎初始位姿不好也能实现配准？\n","permalink":"https://zero-kq.github.io/Pages/posts/slam/pointcloudregistration/","summary":"\u003ch2 id=\"点云配准方法介绍\"\u003e点云配准方法介绍\u003c/h2\u003e\n\u003cp\u003e常见的有 ICP、GICP、NDT 等，快捷实现基本基于 PCL 库中自带的点云处理实现。\u003c/p\u003e\n\u003ch3 id=\"gicp\"\u003eGICP\u003c/h3\u003e\n\u003ch4 id=\"原理\"\u003e原理\u003c/h4\u003e\n\u003cp\u003eGeneralized Iterative Closest Point (GICP) 是一种改进的 ICP 算法，通过协方差矩阵优化对应点匹配，提升配准精度。\u003c/p\u003e\n\u003ch4 id=\"头文件\"\u003e头文件\u003c/h4\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e7\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-cpp\" data-lang=\"cpp\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;pcl/point_types.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e          \u003c/span\u003e\u003cspan class=\"c1\"\u003e// 点类型定义\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;pcl/point_cloud.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e          \u003c/span\u003e\u003cspan class=\"c1\"\u003e// 点云容器\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;pcl/registration/gicp.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e    \u003c/span\u003e\u003cspan class=\"c1\"\u003e// GICP 算法核心\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;pcl/registration/icp.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e     \u003c/span\u003e\u003cspan class=\"c1\"\u003e// ICP 基类（部分功能依赖）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;pcl/features/normal_3d.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e    \u003c/span\u003e\u003cspan class=\"c1\"\u003e// 法向量估计（GICP 需要）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;pcl/filters/voxel_grid.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e   \u003c/span\u003e\u003cspan class=\"c1\"\u003e// 下采样（可选）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;pcl/io/pcd_io.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e            \u003c/span\u003e\u003cspan class=\"c1\"\u003e// PCD 文件读写\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch4 id=\"参数配置\"\u003e参数配置\u003c/h4\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e参数\u003c/th\u003e\n          \u003cth\u003e值\u003c/th\u003e\n          \u003cth\u003e说明\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003esetMaximumIterations\u003c/td\u003e\n          \u003ctd\u003e500\u003c/td\u003e\n          \u003ctd\u003e最大迭代次数，达到后停止配准\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003esetTransformationEpsilon\u003c/td\u003e\n          \u003ctd\u003e1e-10\u003c/td\u003e\n          \u003ctd\u003e变换矩阵变化的收敛阈值，小于此值认为已收敛\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003esetEuclideanFitnessEpsilon\u003c/td\u003e\n          \u003ctd\u003e0.0001\u003c/td\u003e\n          \u003ctd\u003e点到点欧氏距离误差的收敛阈值\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003esetMaxCorrespondenceDistance\u003c/td\u003e\n          \u003ctd\u003e50.0\u003c/td\u003e\n          \u003ctd\u003e对应点搜索半径上限，超过则不考虑\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003esetCorrespondenceRandomness\u003c/td\u003e\n          \u003ctd\u003e20\u003c/td\u003e\n          \u003ctd\u003e每次迭代随机采样对应点数量，影响计算效率和精度\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003esetRANSACOutlierRejectionThreshold\u003c/td\u003e\n          \u003ctd\u003e0.05\u003c/td\u003e\n          \u003ctd\u003eRANSAC 离群点阈值，超过此距离的点被标记为异常值\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003esetRANSACIterations\u003c/td\u003e\n          \u003ctd\u003e1000\u003c/td\u003e\n          \u003ctd\u003eRANSAC 迭代次数\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003esetTransformationRotationEpsilon\u003c/td\u003e\n          \u003ctd\u003e1e-10\u003c/td\u003e\n          \u003ctd\u003e旋转矩阵变化的收敛阈值\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003esetUseReciprocalCorrespondences\u003c/td\u003e\n          \u003ctd\u003efalse\u003c/td\u003e\n          \u003ctd\u003e是否使用互对应关系（source-target双向匹配）\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003chr\u003e\n\u003ch2 id=\"测试结果\"\u003e测试结果\u003c/h2\u003e\n\u003cp\u003e通过将目标点云变换作为原始点云进行测试，实际变换为 T=[10, 10, 10]m, R=[90, 0, 0]deg。\u003c/p\u003e","title":"点云配准"},{"content":"介绍Gtsam Gtsam（General and Tunable Structure-from-Motion）是一个用于结构化从运动学（SFM）的优化库。Gtsam提供了一种基于图优化的算法，用于解决各种类型的结构化从运动学问题。Gtsam支持多种优化算法，如Levenberg-Marquardt、Gauss-Newton、Dogleg、Gauss-Newton-Dogleg等，以及多种优化目标，如最小二乘、最小 Trust Region等。Gtsam提供了多种数据结构，如变量、因子、 优化问题等，以及多种工具函数，如线性方程求解、矩阵运算、向量运算等。（AI生成）\nGtsam库 https://github.com/borglab/gtsam\nGtsam的使用 快速使用 引入头文件 1 2 3 4 5 6 #include \u0026lt;gtsam/slam/PriorFactor.h\u0026gt; // 先验因子，为Pose提供绝对约束（如首帧初始位姿） #include \u0026lt;gtsam/nonlinear/ISAM2.h\u0026gt; // 增量平滑与建图优化器，支持增量式求解因子图 #include \u0026lt;gtsam/nonlinear/Values.h\u0026gt; // 优化变量值的容器，存储键值对形式 #include \u0026lt;gtsam/slam/BetweenFactor.h\u0026gt; // 里程计/回环约束因子，表示两Pose间相对变换 #include \u0026lt;gtsam/inference/Symbol.h\u0026gt; // GTSAM符号系统，用于生成因子图变量唯一键 ，当直接用帧创建key时无需引入 #include \u0026lt;gtsam/geometry/Pose3.h\u0026gt; // 3D位姿（Rot3旋转+Point3位置），表示机器人姿态 创建变量（Values） 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 // 使用 Symbol 创建唯一键：\u0026#39;x\u0026#39; + 0 = 键 \u0026#34;x0\u0026#34; gtsam::Key key1 = gtsam::symbol(\u0026#39;x\u0026#39;, 0); gtsam::Key key2 = gtsam::symbol(\u0026#39;x\u0026#39;, 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() \u0026lt;\u0026lt; 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() \u0026lt;\u0026lt; 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\u0026lt;gtsam::Pose3\u0026gt;(key1, pose1, priorNoise)); // 里程计因子（两帧之间的相对变换） gtsam::Pose3 odom_delta = pose1.between(pose2); // 计算相对变换 graph.add(gtsam::BetweenFactor\u0026lt;gtsam::Pose3\u0026gt;(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(\u0026#39;x\u0026#39;, 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\u0026lt;gtsam::Pose3\u0026gt;(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 \u0026lt;gtsam/slam/PriorFactor.h\u0026gt; #include \u0026lt;gtsam/nonlinear/ISAM2.h\u0026gt; #include \u0026lt;gtsam/nonlinear/Values.h\u0026gt; #include \u0026lt;gtsam/slam/BetweenFactor.h\u0026gt; #include \u0026lt;gtsam/inference/Symbol.h\u0026gt; #include \u0026lt;gtsam/geometry/Pose3.h\u0026gt; int main() { // 1. 创建变量 gtsam::Key key1 = gtsam::symbol(\u0026#39;x\u0026#39;, 0); gtsam::Key key2 = gtsam::symbol(\u0026#39;x\u0026#39;, 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\u0026lt;gtsam::Pose3\u0026gt;(key1, pose1, priorNoise)); // 先验 graph.add(gtsam::BetweenFactor\u0026lt;gtsam::Pose3\u0026gt;(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\u0026lt;gtsam::Pose3\u0026gt;(key2); std::cout \u0026lt;\u0026lt; \u0026#34;Optimized pose: \u0026#34; \u0026lt;\u0026lt; optimized_pose2 \u0026lt;\u0026lt; std::endl; return 0; } 注意事项 每一个Key必须有初始值 一个图必须创建先验约束 不能有孤立节点，必须与其他节点建立约束 优化效果 原始x轴位移\n1 2 3 4 vector\u0026lt;double\u0026gt; cumulative_x = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 } 优化后\n关键函数速查 函数 说明 gtsam::symbol( 'x', id) 创建变量键，如 \u0026lsquo;x0\u0026rsquo;, \u0026lsquo;x1\u0026rsquo; Pose3::Ypr(yaw, pitch, roll) 从欧拉角创建旋转 pose1.between(pose2) 计算 pose1 到 pose2 的相对变换 graph.add(Factor(...)) 添加因子到因子图 isam.update(graph, values) 增量更新优化器 isam.calculateEstimate() 获取所有优化结果 result.at\u0026lt;Pose3\u0026gt;(key) 获取特定键的优化值 引用文献 [无]\n","permalink":"https://zero-kq.github.io/Pages/posts/slam/gtsam/","summary":"\u003ch2 id=\"介绍gtsam\"\u003e介绍Gtsam\u003c/h2\u003e\n\u003cp\u003eGtsam（General and Tunable Structure-from-Motion）是一个用于结构化从运动学（SFM）的优化库。Gtsam提供了一种基于图优化的算法，用于解决各种类型的结构化从运动学问题。Gtsam支持多种优化算法，如Levenberg-Marquardt、Gauss-Newton、Dogleg、Gauss-Newton-Dogleg等，以及多种优化目标，如最小二乘、最小 Trust Region等。Gtsam提供了多种数据结构，如变量、因子、 优化问题等，以及多种工具函数，如线性方程求解、矩阵运算、向量运算等。（AI生成）\u003c/p\u003e\n\u003ch2 id=\"gtsam库\"\u003eGtsam库\u003c/h2\u003e\n\u003cp\u003e\u003ca href=\"https://github.com/borglab/gtsam\"\u003ehttps://github.com/borglab/gtsam\u003c/a\u003e\u003c/p\u003e\n\u003ch2 id=\"gtsam的使用\"\u003eGtsam的使用\u003c/h2\u003e\n\u003ch3 id=\"快速使用\"\u003e快速使用\u003c/h3\u003e\n\u003col\u003e\n\u003cli\u003e引入头文件\u003c/li\u003e\n\u003c/ol\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e6\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-c++\" data-lang=\"c++\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/slam/PriorFactor.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e      \u003c/span\u003e\u003cspan class=\"c1\"\u003e// 先验因子，为Pose提供绝对约束（如首帧初始位姿）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/nonlinear/ISAM2.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e       \u003c/span\u003e\u003cspan class=\"c1\"\u003e// 增量平滑与建图优化器，支持增量式求解因子图\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/nonlinear/Values.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e      \u003c/span\u003e\u003cspan class=\"c1\"\u003e// 优化变量值的容器，存储键值对形式\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/slam/BetweenFactor.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e   \u003c/span\u003e\u003cspan class=\"c1\"\u003e// 里程计/回环约束因子，表示两Pose间相对变换\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/inference/Symbol.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e     \u003c/span\u003e\u003cspan class=\"c1\"\u003e// GTSAM符号系统，用于生成因子图变量唯一键 ，当直接用帧创建key时无需引入\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/geometry/Pose3.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e        \u003c/span\u003e\u003cspan class=\"c1\"\u003e// 3D位姿（Rot3旋转+Point3位置），表示机器人姿态\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003col start=\"2\"\u003e\n\u003cli\u003e创建变量（Values）\u003c/li\u003e\n\u003c/ol\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e 1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 9\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e10\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e11\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e12\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e13\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e14\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e15\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e16\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-c++\" data-lang=\"c++\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 使用 Symbol 创建唯一键：\u0026#39;x\u0026#39; + 0 = 键 \u0026#34;x0\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eKey\u003c/span\u003e \u003cspan class=\"n\"\u003ekey1\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003esymbol\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"sc\"\u003e\u0026#39;x\u0026#39;\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eKey\u003c/span\u003e \u003cspan class=\"n\"\u003ekey2\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003esymbol\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"sc\"\u003e\u0026#39;x\u0026#39;\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e1\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e//直接用帧数也可以\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eKey\u003c/span\u003e \u003cspan class=\"n\"\u003ekey1\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e           \u003cspan class=\"c1\"\u003e// 直接用整数\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 创建 Pose3 位姿（旋转 + 平移）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eRot3\u003c/span\u003e \u003cspan class=\"n\"\u003erotation\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eRot3\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eYpr\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"mf\"\u003e0.0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.0\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e  \u003cspan class=\"c1\"\u003e// yaw, pitch, roll\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePoint3\u003c/span\u003e \u003cspan class=\"n\"\u003etranslation\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"mf\"\u003e0.0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.0\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e                \u003cspan class=\"c1\"\u003e// x, y, z\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e \u003cspan class=\"n\"\u003epose1\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003erotation\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003etranslation\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 存入 Values 容器\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eValues\u003c/span\u003e \u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003einsert\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epose1\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e        \u003cspan class=\"c1\"\u003e// 初始值\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003einsert\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey2\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epose1\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e         \u003cspan class=\"c1\"\u003e// 第二帧初始值\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003col start=\"3\"\u003e\n\u003cli\u003e创建噪声模型（NoiseModel）\u003c/li\u003e\n\u003c/ol\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e9\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-c++\" data-lang=\"c++\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 高斯噪声模型（6维：旋转3维 + 平移3维）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eVector6\u003c/span\u003e \u003cspan class=\"n\"\u003evariance\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eVector6\u003c/span\u003e\u003cspan class=\"p\"\u003e()\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026lt;\u0026lt;\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.01\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.01\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.01\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.1\u003c/span\u003e\u003cspan class=\"p\"\u003e).\u003c/span\u003e\u003cspan class=\"n\"\u003efinished\u003c/span\u003e\u003cspan class=\"p\"\u003e();\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eSharedNoiseModel\u003c/span\u003e \u003cspan class=\"n\"\u003enoise\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003enoiseModel\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eGaussian\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eCovariance\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eMatrix6\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eIdentity\u003c/span\u003e\u003cspan class=\"p\"\u003e()\u003c/span\u003e \u003cspan class=\"o\"\u003e*\u003c/span\u003e \u003cspan class=\"n\"\u003evariance\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003easDiagonal\u003c/span\u003e\u003cspan class=\"p\"\u003e());\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 或使用对角噪声模型\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"k\"\u003eauto\u003c/span\u003e \u003cspan class=\"n\"\u003eodometryNoise\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003enoiseModel\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eDiagonal\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eSigmas\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eVector6\u003c/span\u003e\u003cspan class=\"p\"\u003e()\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026lt;\u0026lt;\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.05\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.05\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.05\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.1\u003c/span\u003e\u003cspan class=\"p\"\u003e).\u003c/span\u003e\u003cspan class=\"n\"\u003efinished\u003c/span\u003e\u003cspan class=\"p\"\u003e());\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"k\"\u003eauto\u003c/span\u003e \u003cspan class=\"n\"\u003epriorNoise\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003enoiseModel\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eIsotropic\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eSigma\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"mi\"\u003e6\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.01\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003col start=\"4\"\u003e\n\u003cli\u003e创建因子（Factor）\u003c/li\u003e\n\u003c/ol\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e8\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-c++\" data-lang=\"c++\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eNonlinearFactorGraph\u003c/span\u003e \u003cspan class=\"n\"\u003egraph\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 先验因子（为第一帧提供绝对约束）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egraph\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eadd\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePriorFactor\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026lt;\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026gt;\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epose1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epriorNoise\u003c/span\u003e\u003cspan class=\"p\"\u003e));\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 里程计因子（两帧之间的相对变换）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e \u003cspan class=\"n\"\u003eodom_delta\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003epose1\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ebetween\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003epose2\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e  \u003cspan class=\"c1\"\u003e// 计算相对变换\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egraph\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eadd\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eBetweenFactor\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026lt;\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026gt;\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ekey2\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003eodom_delta\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003eodometryNoise\u003c/span\u003e\u003cspan class=\"p\"\u003e));\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003col start=\"5\"\u003e\n\u003cli\u003e创建 ISAM2 优化器\u003c/li\u003e\n\u003c/ol\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e5\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-c++\" data-lang=\"c++\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eISAM2Params\u003c/span\u003e \u003cspan class=\"n\"\u003eparams\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003eparams\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003erelinearizeThreshold\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.01\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e   \u003cspan class=\"c1\"\u003e// 重新线性化阈值\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003eparams\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003erelinearizeSkip\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"mi\"\u003e10\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e          \u003cspan class=\"c1\"\u003e// 跳帧数\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eISAM2\u003c/span\u003e \u003cspan class=\"n\"\u003eisam\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003eparams\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003eisam\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eupdate\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egraph\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e    \u003cspan class=\"c1\"\u003e// 首次更新\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003col start=\"6\"\u003e\n\u003cli\u003e增量优化\u003c/li\u003e\n\u003c/ol\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e 1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 9\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e10\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e11\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e12\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e13\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-c++\" data-lang=\"c++\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 添加新帧时\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eKey\u003c/span\u003e \u003cspan class=\"n\"\u003ekey3\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003esymbol\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"sc\"\u003e\u0026#39;x\u0026#39;\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e2\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eclear\u003c/span\u003e\u003cspan class=\"p\"\u003e();\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003einsert\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey3\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epose_guess\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e  \u003cspan class=\"c1\"\u003e// 新帧初始值\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 更新优化器（增量式，只优化新节点）\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003eisam\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eupdate\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egraph\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 获取优化结果\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eValues\u003c/span\u003e \u003cspan class=\"n\"\u003eresult\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003eisam\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ecalculateEstimate\u003c/span\u003e\u003cspan class=\"p\"\u003e();\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e// 读取特定节点\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e \u003cspan class=\"n\"\u003eoptimized_pose\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003eresult\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eat\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026lt;\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026gt;\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey3\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"完整示例\"\u003e完整示例\u003c/h3\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e 1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 9\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e10\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e11\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e12\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e13\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e14\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e15\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e16\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e17\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e18\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e19\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e20\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e21\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e22\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e23\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e24\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e25\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e26\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e27\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e28\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e29\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e30\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e31\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e32\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e33\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e34\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e35\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e36\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e37\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e38\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e39\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-c++\" data-lang=\"c++\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/slam/PriorFactor.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/nonlinear/ISAM2.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/nonlinear/Values.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/slam/BetweenFactor.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/inference/Symbol.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e#include\u003c/span\u003e \u003cspan class=\"cpf\"\u003e\u0026lt;gtsam/geometry/Pose3.h\u0026gt;\u003c/span\u003e\u003cspan class=\"cp\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"cp\"\u003e\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"kt\"\u003eint\u003c/span\u003e \u003cspan class=\"nf\"\u003emain\u003c/span\u003e\u003cspan class=\"p\"\u003e()\u003c/span\u003e \u003cspan class=\"p\"\u003e{\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"c1\"\u003e// 1. 创建变量\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eKey\u003c/span\u003e \u003cspan class=\"n\"\u003ekey1\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003esymbol\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"sc\"\u003e\u0026#39;x\u0026#39;\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eKey\u003c/span\u003e \u003cspan class=\"n\"\u003ekey2\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003esymbol\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"sc\"\u003e\u0026#39;x\u0026#39;\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e1\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e \u003cspan class=\"n\"\u003epose1\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e                        \u003cspan class=\"c1\"\u003e// 单位位姿\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e \u003cspan class=\"n\"\u003epose2\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eRot3\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eYpr\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"mf\"\u003e0.1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e),\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePoint3\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"mi\"\u003e1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e));\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"c1\"\u003e// 2. 创建噪声模型\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e    \u003cspan class=\"k\"\u003eauto\u003c/span\u003e \u003cspan class=\"n\"\u003epriorNoise\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003enoiseModel\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eIsotropic\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eSigma\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"mi\"\u003e6\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.01\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"k\"\u003eauto\u003c/span\u003e \u003cspan class=\"n\"\u003eodomNoise\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003enoiseModel\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eIsotropic\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eSigma\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"mi\"\u003e6\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"mf\"\u003e0.05\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"c1\"\u003e// 3. 创建因子图\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eNonlinearFactorGraph\u003c/span\u003e \u003cspan class=\"n\"\u003egraph\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003egraph\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eadd\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePriorFactor\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026lt;\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026gt;\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epose1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epriorNoise\u003c/span\u003e\u003cspan class=\"p\"\u003e));\u003c/span\u003e  \u003cspan class=\"c1\"\u003e// 先验\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e    \u003cspan class=\"n\"\u003egraph\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eadd\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eBetweenFactor\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026lt;\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026gt;\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003ekey2\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epose1\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ebetween\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003epose2\u003c/span\u003e\u003cspan class=\"p\"\u003e),\u003c/span\u003e \u003cspan class=\"n\"\u003eodomNoise\u003c/span\u003e\u003cspan class=\"p\"\u003e));\u003c/span\u003e \u003cspan class=\"c1\"\u003e// 里程计\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"c1\"\u003e// 4. 创建初始值\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eValues\u003c/span\u003e \u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003einsert\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey1\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epose1\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003einsert\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey2\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003epose2\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"c1\"\u003e// 5. ISAM2 优化\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eISAM2\u003c/span\u003e \u003cspan class=\"n\"\u003eisam\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003eisam\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eupdate\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003egraph\u003c/span\u003e\u003cspan class=\"p\"\u003e,\u003c/span\u003e \u003cspan class=\"n\"\u003einitial_values\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eValues\u003c/span\u003e \u003cspan class=\"n\"\u003eresult\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003eisam\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003ecalculateEstimate\u003c/span\u003e\u003cspan class=\"p\"\u003e();\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"c1\"\u003e// 6. 读取结果\n\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e\u003c/span\u003e    \u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e \u003cspan class=\"n\"\u003eoptimized_pose2\u003c/span\u003e \u003cspan class=\"o\"\u003e=\u003c/span\u003e \u003cspan class=\"n\"\u003eresult\u003c/span\u003e\u003cspan class=\"p\"\u003e.\u003c/span\u003e\u003cspan class=\"n\"\u003eat\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026lt;\u003c/span\u003e\u003cspan class=\"n\"\u003egtsam\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ePose3\u003c/span\u003e\u003cspan class=\"o\"\u003e\u0026gt;\u003c/span\u003e\u003cspan class=\"p\"\u003e(\u003c/span\u003e\u003cspan class=\"n\"\u003ekey2\u003c/span\u003e\u003cspan class=\"p\"\u003e);\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"n\"\u003estd\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003ecout\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026lt;\u0026lt;\u003c/span\u003e \u003cspan class=\"s\"\u003e\u0026#34;Optimized pose: \u0026#34;\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026lt;\u0026lt;\u003c/span\u003e \u003cspan class=\"n\"\u003eoptimized_pose2\u003c/span\u003e \u003cspan class=\"o\"\u003e\u0026lt;\u0026lt;\u003c/span\u003e \u003cspan class=\"n\"\u003estd\u003c/span\u003e\u003cspan class=\"o\"\u003e::\u003c/span\u003e\u003cspan class=\"n\"\u003eendl\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e    \u003cspan class=\"k\"\u003ereturn\u003c/span\u003e \u003cspan class=\"mi\"\u003e0\u003c/span\u003e\u003cspan class=\"p\"\u003e;\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"p\"\u003e}\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"注意事项\"\u003e注意事项\u003c/h3\u003e\n\u003col\u003e\n\u003cli\u003e每一个Key必须有初始值\u003c/li\u003e\n\u003cli\u003e一个图必须创建先验约束\u003c/li\u003e\n\u003cli\u003e不能有孤立节点，必须与其他节点建立约束\u003c/li\u003e\n\u003c/ol\u003e\n\u003ch3 id=\"优化效果\"\u003e优化效果\u003c/h3\u003e\n\u003cp\u003e原始x轴位移\u003c/p\u003e","title":"Gtsam 学习笔记"},{"content":"我的第一篇学习笔记 今天开始使用 Hugo 搭建我的学习笔记网站。\n为什么选择 Hugo？ Hugo 是一个静态网站生成器，具有以下优点：\n构建速度快 主题丰富 易于部署到 GitHub Pages 下一步计划 学习更多 Hugo 知识 定制 PaperMod 主题 写更多学习笔记 持续更新中\u0026hellip;\nMermaid 图表示例 Hugo PaperMod 主题内置支持 Mermaid.js，可以直接使用。\n流程图 flowchart TD A[开始] --\u0026gt; B{是否有问题?} B --\u0026gt;|是| C[分析问题] B --\u0026gt;|否| D[继续学习] C --\u0026gt; E[查找资料] E --\u0026gt; F[解决问题] F --\u0026gt; G[总结经验] G --\u0026gt; D D --\u0026gt; H[结束] 函数调用关系图 graph LR A[main 函数] --\u0026gt; B[init] A --\u0026gt; C[processData] C --\u0026gt; D[validateInput] C --\u0026gt; E[transformData] E --\u0026gt; F[saveResult] D --\u0026gt; F 类图/模块关系 classDiagram class App { \u0026#43;main() \u0026#43;init() } class DataProcessor { \u0026#43;processData() \u0026#43;validate() } class Storage { \u0026#43;save() \u0026#43;load() } App --\u0026gt; DataProcessor DataProcessor --\u0026gt; Storage 时序图 sequenceDiagram participant U as 用户 participant H as Hugo participant P as PaperMod participant G as GitHub Pages U-\u0026gt;\u0026gt;H: 编写 Markdown H-\u0026gt;\u0026gt;P: 生成静态页面 P-\u0026gt;\u0026gt;G: 推送到仓库 G-\u0026gt;\u0026gt;G: 自动部署 G--\u0026gt;\u0026gt;U: 网站上线 ","permalink":"https://zero-kq.github.io/Pages/posts/demo/first-post/","summary":"\u003ch1 id=\"我的第一篇学习笔记\"\u003e我的第一篇学习笔记\u003c/h1\u003e\n\u003cp\u003e今天开始使用 Hugo 搭建我的学习笔记网站。\u003c/p\u003e\n\u003ch2 id=\"为什么选择-hugo\"\u003e为什么选择 Hugo？\u003c/h2\u003e\n\u003cp\u003eHugo 是一个静态网站生成器，具有以下优点：\u003c/p\u003e\n\u003cul\u003e\n\u003cli\u003e构建速度快\u003c/li\u003e\n\u003cli\u003e主题丰富\u003c/li\u003e\n\u003cli\u003e易于部署到 GitHub Pages\u003c/li\u003e\n\u003c/ul\u003e\n\u003ch2 id=\"下一步计划\"\u003e下一步计划\u003c/h2\u003e\n\u003col\u003e\n\u003cli\u003e学习更多 Hugo 知识\u003c/li\u003e\n\u003cli\u003e定制 PaperMod 主题\u003c/li\u003e\n\u003cli\u003e写更多学习笔记\u003c/li\u003e\n\u003c/ol\u003e\n\u003cp\u003e持续更新中\u0026hellip;\u003c/p\u003e\n\u003ch2 id=\"mermaid-图表示例\"\u003eMermaid 图表示例\u003c/h2\u003e\n\u003cp\u003eHugo PaperMod 主题内置支持 Mermaid.js，可以直接使用。\u003c/p\u003e\n\u003ch3 id=\"流程图\"\u003e流程图\u003c/h3\u003e\n\n\u003cdiv class=\"mermaid\"\u003eflowchart TD\n    A[开始] --\u0026gt; B{是否有问题?}\n    B --\u0026gt;|是| C[分析问题]\n    B --\u0026gt;|否| D[继续学习]\n    C --\u0026gt; E[查找资料]\n    E --\u0026gt; F[解决问题]\n    F --\u0026gt; G[总结经验]\n    G --\u0026gt; D\n    D --\u0026gt; H[结束]\u003c/div\u003e\n\u003ch3 id=\"函数调用关系图\"\u003e函数调用关系图\u003c/h3\u003e\n\n\u003cdiv class=\"mermaid\"\u003egraph LR\n    A[main 函数] --\u0026gt; B[init]\n    A --\u0026gt; C[processData]\n    C --\u0026gt; D[validateInput]\n    C --\u0026gt; E[transformData]\n    E --\u0026gt; F[saveResult]\n    D --\u0026gt; F\u003c/div\u003e\n\u003ch3 id=\"类图模块关系\"\u003e类图/模块关系\u003c/h3\u003e\n\n\u003cdiv class=\"mermaid\"\u003eclassDiagram\n    class App {\n        \u0026#43;main()\n        \u0026#43;init()\n    }\n    class DataProcessor {\n        \u0026#43;processData()\n        \u0026#43;validate()\n    }\n    class Storage {\n        \u0026#43;save()\n        \u0026#43;load()\n    }\n    App --\u0026gt; DataProcessor\n    DataProcessor --\u0026gt; Storage\u003c/div\u003e\n\u003ch3 id=\"时序图\"\u003e时序图\u003c/h3\u003e\n\n\u003cdiv class=\"mermaid\"\u003esequenceDiagram\n    participant U as 用户\n    participant H as Hugo\n    participant P as PaperMod\n    participant G as GitHub Pages\n\n    U-\u0026gt;\u0026gt;H: 编写 Markdown\n    H-\u0026gt;\u0026gt;P: 生成静态页面\n    P-\u0026gt;\u0026gt;G: 推送到仓库\n    G-\u0026gt;\u0026gt;G: 自动部署\n    G--\u0026gt;\u0026gt;U: 网站上线\u003c/div\u003e","title":"我的第一篇学习笔记"},{"content":"Hugo 快速入门指南 安装 Hugo 在 Windows 上可以使用 Chocolatey 或 Scoop 安装：\n1 scoop install hugo 创建新站点 1 hugo new site my-site 添加主题 1 2 3 cd my-site git init git submodule add https://github.com/adityatelange/hugo-PaperMod.git themes/PaperMod 启动开发服务器 1 hugo server 构建生产版本 1 hugo 这样就可以生成静态网站文件了！\n","permalink":"https://zero-kq.github.io/Pages/posts/demo/hugo-quickstart/","summary":"\u003ch1 id=\"hugo-快速入门指南\"\u003eHugo 快速入门指南\u003c/h1\u003e\n\u003ch2 id=\"安装-hugo\"\u003e安装 Hugo\u003c/h2\u003e\n\u003cp\u003e在 Windows 上可以使用 Chocolatey 或 Scoop 安装：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003escoop install hugo\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch2 id=\"创建新站点\"\u003e创建新站点\u003c/h2\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003ehugo new site my-site\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch2 id=\"添加主题\"\u003e添加主题\u003c/h2\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"nb\"\u003ecd\u003c/span\u003e my-site\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003egit init\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003egit submodule add https://github.com/adityatelange/hugo-PaperMod.git themes/PaperMod\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch2 id=\"启动开发服务器\"\u003e启动开发服务器\u003c/h2\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003ehugo server\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch2 id=\"构建生产版本\"\u003e构建生产版本\u003c/h2\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003ehugo\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e这样就可以生成静态网站文件了！\u003c/p\u003e","title":"Hugo 快速入门指南"},{"content":"","permalink":"https://zero-kq.github.io/Pages/about/","summary":"","title":"关于"},{"content":"介绍 本文介绍在 Ubuntu 系统上安装与配置 ROS 环境的完整流程，包括系统编码与软件源检查、ROS2 Foxy 安装、ROS1 与 ROS2 环境切换、编译命令差异及常用终端指令对照。\n1. 检查系统设置 1.1 检查编码 UTF-8 终端输入：\n1 locale 出现如下输出信息说明编码为 UTF-8：\n1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 LANG=zh_CN.UTF-8 LANGUAGE=zh_CN:en_US:en LC_CTYPE=\u0026#34;zh_CN.UTF-8\u0026#34; LC_NUMERIC=zh_CN.UTF-8 LC_TIME=zh_CN.UTF-8 LC_COLLATE=\u0026#34;zh_CN.UTF-8\u0026#34; LC_MONETARY=zh_CN.UTF-8 LC_MESSAGES=\u0026#34;zh_CN.UTF-8\u0026#34; LC_PAPER=\u0026#34;zh_CN.UTF-8\u0026#34; LC_NAME=\u0026#34;zh_CN.UTF-8\u0026#34; LC_ADDRESS=\u0026#34;zh_CN.UTF-8\u0026#34; LC_TELEPHONE=\u0026#34;zh_CN.UTF-8\u0026#34; LC_MEASUREMENT=\u0026#34;zh_CN.UTF-8\u0026#34; LC_IDENTIFICATION=\u0026#34;zh_CN.UTF-8\u0026#34; LC_ALL= 1.2 检查是否启用 Ubuntu Universe 存储库 终端输入：\n1 apt-cache policy | grep universe 若输出如下则表示已启用：\n1 2 3 4 500 http://security.ubuntu.com/ubuntu focal-security/universe i386 Packages release v=20.04,o=Ubuntu,a=focal-security,n=focal,l=Ubuntu,c=universe,b=i386 500 http://security.ubuntu.com/ubuntu focal-security/universe amd64 Packages release v=20.04,o=Ubuntu,a=focal-security,n=focal,l=Ubuntu,c=universe,b=amd64 若无输出，执行以下命令启用：\n1 2 sudo apt install software-properties-common sudo add-apt-repository universe 1.3 设置软件包签名密钥 1 2 3 sudo apt update \u0026amp;\u0026amp; sudo apt install curl gnupg2 lsb-release sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg echo \u0026#34;deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release \u0026amp;\u0026amp; echo $UBUNTU_CODENAME) main\u0026#34; | sudo tee /etc/apt/sources.list.d/ros2.list \u0026gt; /dev/null 2. 安装 1 sudo apt install ros-foxy-desktop 3. 配置环境变量 为方便在 ROS1 和 ROS2 之间切换，可在 ~/.bashrc 中添加以下函数：\n1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 # \u0026gt;\u0026gt;\u0026gt; type in ros to choose which to initialize \u0026gt;\u0026gt;\u0026gt; ros(){ echo \u0026#34;ros: noetic(1) foxy(2)?\u0026#34; read edition if [ \u0026#34;$edition\u0026#34; -eq \u0026#34;1\u0026#34; ];then source /opt/ros/noetic/setup.bash echo using ROS1 Noetic else source /opt/ros/foxy/setup.bash echo using ROS2 Foxy fi } # \u0026lt;\u0026lt;\u0026lt; type in ros/ros2 to initialize \u0026lt;\u0026lt;\u0026lt; 根据需要修改工作空间路径，然后执行 source ~/.bashrc 使配置生效。\n4. 编译命令差异 1 2 3 4 5 6 7 8 9 10 11 12 13 14 # ROS1 catkin_create_pkg \u0026lt;package_name\u0026gt; [depend1] [depend2] [depend3]... # catkin_create_pkg test_pkg std_msgs rospy roscpp catkin_make catkin_make -DCATKIN_WHITELIST_PACKAGES=\u0026#34;\u0026#34; # ROS2 colcon build # 只编译指定包 colcon build --packages-select PACKAGE_NAME # 忽略指定包 colcon build --packages-ignore PACKAGE_NAME # 遇到编译错误继续编译其他模块 colcon build --continue-on-error 5. 工作空间配置 1 2 source ~/catkin_ws/devel/setup.bash source ~/colcon_ws/install/setup.bash 6. 终端命令对照 ROS1 命令 ROS2 命令 rosrun ros2 run rosnode ros2 node roslaunch ros2 launch rosparam ros2 param rospkg ros2 pkg rosservice ros2 service rossrv ros2 srv rostopic ros2 topic rosaction ros2 action 参考 ROS2 官方安装文档：https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html ROS Noetic 安装文档：http://wiki.ros.org/noetic/Installation/Ubuntu colcon 编译工具文档：https://colcon.readthedocs.io/ ","permalink":"https://zero-kq.github.io/Pages/posts/ros/ros-install-guide/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e本文介绍在 Ubuntu 系统上安装与配置 ROS 环境的完整流程，包括系统编码与软件源检查、ROS2 Foxy 安装、ROS1 与 ROS2 环境切换、编译命令差异及常用终端指令对照。\u003c/p\u003e\n\u003ch2 id=\"1-检查系统设置\"\u003e1. 检查系统设置\u003c/h2\u003e\n\u003ch3 id=\"11-检查编码-utf-8\"\u003e1.1 检查编码 UTF-8\u003c/h3\u003e\n\u003cp\u003e终端输入：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003elocale\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e出现如下输出信息说明编码为 UTF-8：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e 1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 4\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 5\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 6\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 7\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 8\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e 9\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e10\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e11\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e12\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e13\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e14\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e15\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-fallback\" data-lang=\"fallback\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLANG=zh_CN.UTF-8\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLANGUAGE=zh_CN:en_US:en\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_CTYPE=\u0026#34;zh_CN.UTF-8\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_NUMERIC=zh_CN.UTF-8\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_TIME=zh_CN.UTF-8\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_COLLATE=\u0026#34;zh_CN.UTF-8\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_MONETARY=zh_CN.UTF-8\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_MESSAGES=\u0026#34;zh_CN.UTF-8\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_PAPER=\u0026#34;zh_CN.UTF-8\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_NAME=\u0026#34;zh_CN.UTF-8\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_ADDRESS=\u0026#34;zh_CN.UTF-8\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_TELEPHONE=\u0026#34;zh_CN.UTF-8\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_MEASUREMENT=\u0026#34;zh_CN.UTF-8\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_IDENTIFICATION=\u0026#34;zh_CN.UTF-8\u0026#34;\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLC_ALL=\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"12-检查是否启用-ubuntu-universe-存储库\"\u003e1.2 检查是否启用 Ubuntu Universe 存储库\u003c/h3\u003e\n\u003cp\u003e终端输入：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eapt-cache policy \u003cspan class=\"p\"\u003e|\u003c/span\u003e grep universe\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e若输出如下则表示已启用：\u003c/p\u003e","title":"Ubuntu ROS 环境安装与配置指南"},{"content":"介绍 本文介绍 GitHub 日常开发中的基础操作，包括使用 Personal Access Token 克隆私人仓库、保存 Git 凭据避免重复输入，以及配置全局与局部的用户名和邮箱。\n1. 拉取私人仓库 1.1 创建 Personal Access Token 登录 GitHub，点击右上角头像 → Settings 左侧菜单选择 Developer settings 选择 Personal access tokens → Fine-grained tokens 点击 Generate new token → Generate new token (classic) 设置名称（如：我的开发令牌） 选择过期时间（建议选择较长时间，如 90 天或自定义） 选择仓库范围 All repositories 所有仓库，或者其他选项 勾选权限： 选择对应的权限领域 repositories（仓库）、Account（账户） 仓库中选择 Contents 可读写，其它权限按需设置 点击 Generate token 重要：复制生成的 token（只显示一次，请妥善保存） 1.2 使用 Token 克隆仓库 在命令行中执行：\n1 git clone https://github.com/用户名/仓库名.git 当提示输入用户名和密码时：\n提示项 填写内容 Username 你的 GitHub 用户名 Password 刚才复制的 Personal Access Token（不是你的 GitHub 密码） 示例：\n1 2 # Username: zhangsan # Password: ghp_xxxxxxxxxxxxxxxxxxxx（你的 token） 2. Git 保存密码 在 git pull 输入用户名和密码之后，执行以下命令保存凭据：\n1 git config --global credential.helper store 执行后，Git 会将凭据写入 ~/.git-credentials，后续操作无需重复输入。\n3. 设置用户名与邮箱 3.1 全局配置 适用于当前设备上的所有仓库：\n1 2 git config --global user.name \u0026#34;你的用户名\u0026#34; git config --global user.email \u0026#34;zhangsan@example.com\u0026#34; 3.2 局部配置 仅在当前仓库目录下生效，会覆盖全局配置：\n1 2 git config user.name \u0026#34;zhangsan\u0026#34; git config user.email \u0026#34;zhangsan@example.com\u0026#34; 3.3 查看配置 命令 说明 git config user.name 查看当前仓库的用户名 git config user.email 查看当前仓库的邮箱 git config --global user.name 查看全局用户名 git config --global user.email 查看全局邮箱 参考 GitHub Personal Access Token 文档：https://docs.github.com/en/authentication/keeping-your-account-and-data-secure/managing-your-personal-access-tokens Git 凭据存储文档：https://git-scm.com/docs/gitcredentials Git 配置文档：https://git-scm.com/docs/git-config ","permalink":"https://zero-kq.github.io/Pages/posts/other/github-basic-guide/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e本文介绍 GitHub 日常开发中的基础操作，包括使用 Personal Access Token 克隆私人仓库、保存 Git 凭据避免重复输入，以及配置全局与局部的用户名和邮箱。\u003c/p\u003e\n\u003ch2 id=\"1-拉取私人仓库\"\u003e1. 拉取私人仓库\u003c/h2\u003e\n\u003ch3 id=\"11-创建-personal-access-token\"\u003e1.1 创建 Personal Access Token\u003c/h3\u003e\n\u003col\u003e\n\u003cli\u003e登录 GitHub，点击右上角头像 → \u003ccode\u003eSettings\u003c/code\u003e\u003c/li\u003e\n\u003cli\u003e左侧菜单选择 \u003ccode\u003eDeveloper settings\u003c/code\u003e\u003c/li\u003e\n\u003cli\u003e选择 \u003ccode\u003ePersonal access tokens\u003c/code\u003e → \u003ccode\u003eFine-grained tokens\u003c/code\u003e\u003c/li\u003e\n\u003cli\u003e点击 \u003ccode\u003eGenerate new token\u003c/code\u003e → \u003ccode\u003eGenerate new token (classic)\u003c/code\u003e\u003c/li\u003e\n\u003cli\u003e设置名称（如：我的开发令牌）\u003c/li\u003e\n\u003cli\u003e选择过期时间（建议选择较长时间，如 90 天或自定义）\u003c/li\u003e\n\u003cli\u003e选择仓库范围 \u003ccode\u003eAll repositories\u003c/code\u003e 所有仓库，或者其他选项\u003c/li\u003e\n\u003cli\u003e勾选权限：\n\u003cul\u003e\n\u003cli\u003e选择对应的权限领域 \u003ccode\u003erepositories\u003c/code\u003e（仓库）、\u003ccode\u003eAccount\u003c/code\u003e（账户）\u003c/li\u003e\n\u003cli\u003e仓库中选择 \u003ccode\u003eContents\u003c/code\u003e 可读写，其它权限按需设置\u003c/li\u003e\n\u003c/ul\u003e\n\u003c/li\u003e\n\u003cli\u003e点击 \u003ccode\u003eGenerate token\u003c/code\u003e\u003c/li\u003e\n\u003cli\u003e\u003cstrong\u003e重要\u003c/strong\u003e：复制生成的 token（只显示一次，请妥善保存）\u003c/li\u003e\n\u003c/ol\u003e\n\u003ch3 id=\"12-使用-token-克隆仓库\"\u003e1.2 使用 Token 克隆仓库\u003c/h3\u003e\n\u003cp\u003e在命令行中执行：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003egit clone https://github.com/用户名/仓库名.git\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e当提示输入用户名和密码时：\u003c/p\u003e\n\u003ctable\u003e\n  \u003cthead\u003e\n      \u003ctr\u003e\n          \u003cth\u003e提示项\u003c/th\u003e\n          \u003cth\u003e填写内容\u003c/th\u003e\n      \u003c/tr\u003e\n  \u003c/thead\u003e\n  \u003ctbody\u003e\n      \u003ctr\u003e\n          \u003ctd\u003eUsername\u003c/td\u003e\n          \u003ctd\u003e你的 GitHub 用户名\u003c/td\u003e\n      \u003c/tr\u003e\n      \u003ctr\u003e\n          \u003ctd\u003ePassword\u003c/td\u003e\n          \u003ctd\u003e刚才复制的 Personal Access Token（不是你的 GitHub 密码）\u003c/td\u003e\n      \u003c/tr\u003e\n  \u003c/tbody\u003e\n\u003c/table\u003e\n\u003cp\u003e示例：\u003c/p\u003e","title":"GitHub 基础使用指南"},{"content":"介绍 本文记录无人机仿真开发环境的完整搭建流程，涵盖 ROS 安装、PX4-Autopilot 源码编译、Gazebo 仿真环境配置、MAVROS 通信中间件安装，以及 QGroundControl 地面站部署。\n1. 安装 ROS 使用鱼香 ROS 一键安装脚本：\n1 wget http://fishros.com/install -O fishros \u0026amp;\u0026amp; . fishros 2. 安装 PX4-Autopilot 2.1 下载源码 1 git clone https://github.com/PX4/PX4-Autopilot.git 2.2 安装依赖 解压缩后执行：\n1 2 cd ~/Desktop/PX4-Autopilot bash ./Tools/setup/ubuntu.sh 2.3 添加环境路径 编辑 ~/.bashrc 文件，添加以下内容：\n1 2 3 4 source ~/桌面/PX4-Autopilot/Tools/setup_gazebo.bash ~/桌面/PX4-Autopilot ~/桌面/PX4-Autopilot/build/px4_sitl_default export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/桌面/PX4-Autopilot export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/桌面/PX4-Autopilot/Tools/sitl_gazebo # 具体路径取决于实际文件环境 使环境变量生效：\n1 source ~/.bashrc 此时开启终端会打印以下信息：\n1 2 3 GAZEBO_PLUGIN_PATH $GAZEBO_PLUGIN_PATH GAZEBO_MODEL_PATH $GAZEBO_MODEL_PATH LD_LIBRARY_PATH $LD_LIBRARY_PATH 可通过注释 setup_gazebo.bash 最后三句关闭打印。\n2.4 编译 PX4 SITL 在 PX4-Autopilot 目录下执行：\n1 make px4_sitl_default gazebo 3. 安装 MAVROS 3.1 二进制安装 1 sudo apt-get install ros-noetic-mavros ros-noetic-mavros-extras ros-noetic-mavros-msgs 3.2 安装 GeographicLib 数据集 1 2 3 wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh sudo chmod a+x ./install_geographiclib_datasets.sh sudo ./install_geographiclib_datasets.sh 由于网络问题容易失败，可通过 GitHub 镜像获取：\n1 git clone https://github.91chi.fun/https://github.com/wyfroom/mavros-GeographicLib.git 将 Geographiclib 文件夹放在 /usr/share 路径下。\n4. 安装 QGroundControl 4.1 配置环境 1 2 3 4 5 sudo usermod -a -G dialout $USER sudo apt-get remove modemmanager -y sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl -y sudo apt install libfuse2 -y sudo apt install libxcb-xinerama0 libxkbcommon-x11-0 libxcb-cursor-dev -y 4.2 下载启动程序 从 QGroundControl 发布页面下载 QGroundControl.AppImage：\n1 2 # 下载地址 # https://github.com/mavlink/qgroundcontrol/releases/tag/v4.3.0 下载后设置为可执行文件：\n1 2 chmod +x QGroundControl.AppImage ./QGroundControl.AppImage 参考 PX4-Autopilot 源码：https://github.com/PX4/PX4-Autopilot 鱼香 ROS 一键安装：http://fishros.com/ MAVROS 文档：https://github.com/mavlink/mavros QGroundControl 发布页：https://github.com/mavlink/qgroundcontrol/releases/tag/v4.3.0 ","permalink":"https://zero-kq.github.io/Pages/posts/ros/drone-simulation-setup-guide/","summary":"\u003ch2 id=\"介绍\"\u003e介绍\u003c/h2\u003e\n\u003cp\u003e本文记录无人机仿真开发环境的完整搭建流程，涵盖 ROS 安装、PX4-Autopilot 源码编译、Gazebo 仿真环境配置、MAVROS 通信中间件安装，以及 QGroundControl 地面站部署。\u003c/p\u003e\n\u003ch2 id=\"1-安装-ros\"\u003e1. 安装 ROS\u003c/h2\u003e\n\u003cp\u003e使用鱼香 ROS 一键安装脚本：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003ewget http://fishros.com/install -O fishros \u003cspan class=\"o\"\u003e\u0026amp;\u0026amp;\u003c/span\u003e . fishros\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch2 id=\"2-安装-px4-autopilot\"\u003e2. 安装 PX4-Autopilot\u003c/h2\u003e\n\u003ch3 id=\"21-下载源码\"\u003e2.1 下载源码\u003c/h3\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003egit clone https://github.com/PX4/PX4-Autopilot.git\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"22-安装依赖\"\u003e2.2 安装依赖\u003c/h3\u003e\n\u003cp\u003e解压缩后执行：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"nb\"\u003ecd\u003c/span\u003e ~/Desktop/PX4-Autopilot\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003ebash ./Tools/setup/ubuntu.sh\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"23-添加环境路径\"\u003e2.3 添加环境路径\u003c/h3\u003e\n\u003cp\u003e编辑 \u003ccode\u003e~/.bashrc\u003c/code\u003e 文件，添加以下内容：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e4\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"nb\"\u003esource\u003c/span\u003e ~/桌面/PX4-Autopilot/Tools/setup_gazebo.bash ~/桌面/PX4-Autopilot ~/桌面/PX4-Autopilot/build/px4_sitl_default\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"nb\"\u003eexport\u003c/span\u003e \u003cspan class=\"nv\"\u003eROS_PACKAGE_PATH\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"nv\"\u003e$ROS_PACKAGE_PATH\u003c/span\u003e:~/桌面/PX4-Autopilot\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"nb\"\u003eexport\u003c/span\u003e \u003cspan class=\"nv\"\u003eROS_PACKAGE_PATH\u003c/span\u003e\u003cspan class=\"o\"\u003e=\u003c/span\u003e\u003cspan class=\"nv\"\u003e$ROS_PACKAGE_PATH\u003c/span\u003e:~/桌面/PX4-Autopilot/Tools/sitl_gazebo\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e# 具体路径取决于实际文件环境\u003c/span\u003e\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e使环境变量生效：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"nb\"\u003esource\u003c/span\u003e ~/.bashrc\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e此时开启终端会打印以下信息：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e3\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-fallback\" data-lang=\"fallback\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eGAZEBO_PLUGIN_PATH $GAZEBO_PLUGIN_PATH\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eGAZEBO_MODEL_PATH $GAZEBO_MODEL_PATH\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eLD_LIBRARY_PATH $LD_LIBRARY_PATH\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e可通过注释 \u003ccode\u003esetup_gazebo.bash\u003c/code\u003e 最后三句关闭打印。\u003c/p\u003e","title":"无人机仿真环境搭建指南"},{"content":"1. Nvidia 驱动配置 1.1 禁用开源 nouveau 驱动 与 NVIDIA 专有驱动冲突，核心步骤如下：\n1 2 # 编辑黑名单文件 sudo nano /etc/modprobe.d/blacklist-nouveau.conf 写入以下内容：\n1 2 blacklist nouveau options nouveau modeset=0 更新并重启：\n1 2 sudo update-initramfs -u sudo reboot 验证是否禁用：\n1 lsmod | grep nouveau 查看可用驱动版本：\n1 ubuntu-drivers devices 安装指定版本驱动：\n1 sudo apt install nvidia-driver-535 nvidia-settings -y 重启使配置生效：\n1 2 sudo reboot nvidia-smi 1.2 避免自动安装问题 1 sudo ubuntu-drivers autoinstall 注意：此命令会安装新内核，可能导致兼容性问题，建议手动安装。\n2. 桌面环境故障排除 2.1 桌面无法进入（左上角光标闪烁） 可能原因：/var/log/syslog.1 过大导致系统卡顿或无法继续引导。\n排查与处理步骤：\n首先检查磁盘与内存使用情况：\n1 2 df -h # 查看磁盘使用情况 free -h # 查看内存使用情况 若确定是日志文件过大，可清空（谨慎操作）：\n1 sudo sh -c \u0026#39;echo \u0026gt; /var/log/syslog.1\u0026#39; 使用 logrotate 管理日志大小（可编辑 rsyslog 的 logrotate 配置）：\n编辑文件：/etc/logrotate.d/rsyslog\n示例配置（控制 /var/log/syslog 大小与轮转）：\n1 2 3 4 5 6 7 8 9 10 11 12 13 /var/log/syslog { size 200M rotate 4 daily missingok notifempty delaycompress compress postrotate /usr/lib/rsyslog/rsyslog-rotate endscript } 保存后退出编辑器（例如 nano 使用 Ctrl+O 保存，Ctrl+X 退出）。\n重启 rsyslog 服务：\n1 sudo systemctl restart rsyslog 3. WPS Office 配置 3.1 WPS 字体缺失问题 符号或特殊字体缺失的解决方案：\n参考文档：WPS 字体配置详解\n方法一（解压到 WPS 的字体目录）：\n1 2 sudo unzip wps_symbol_fonts.zip -d /usr/share/fonts/wps-office # 若目录不存在，可先创建：sudo mkdir -p /usr/share/fonts/wps-office 方法二（通用流程：解压 → 生成索引 → 更新缓存）：\n1 2 3 4 # 将字体文件放入任意系统字体目录（如 /usr/share/fonts 或 ~/.local/share/fonts） sudo mkfontscale sudo mkfontdir sudo fc-cache -fv 说明：完成后重启 WPS 或重新登录会话以确保字体生效。\n3.2 安装宋体（Simsun） 参考文档：Ubuntu 安装宋体详细教程\n可从可信网站下载 Simsun.ttf，然后安装：\n1 2 3 4 5 sudo mkdir -p /usr/share/fonts/chinese # 在 fonts 目录下创建自己的目录 sudo cp Simsun.ttf /usr/share/fonts/chinese # 将字体文件拷贝过去 sudo mkfontscale sudo mkfontdir sudo fc-cache -fv 完成后重新启动需要使用该字体的程序。\n4. 输入法配置 修改候选词数量与显示可以提升输入体验。\n根据不同输入法（如 Fcitx、IBus）在设置中调整候选词数量与样式。\n5. 终端配置 5.1 安装 Terminator 终端 1 2 sudo apt-get update sudo apt-get install terminator 安装后右键终端窗口可以进入首选项，调整配色、字体和布局。\n5.2 设置右键打开 Terminator 在文件管理器右键菜单中集成 Terminator 的步骤：\n1. 安装插件：\n1 sudo apt-get install nautilus-actions filemanager-actions 2. 运行配置工具：\n1 fma-config-tool 3. 新建动作：\n4. 新建命令（设置执行路径与参数）：\n路径：/usr/bin/terminator 参数：--working-directory=%d/%b 5. 修改 Filemanager 配置（点击左上角 Filemanager…tool 的 Preference）：\n6. 重启文件管理器以应用更改：\n1 nautilus -q 6. GNOME 桌面美化 6.1 安装扩展管理器 使用 GNOME 扩展管理器来安装美化扩展：\n1 sudo apt install gnome-shell-extension-manager 6.2 推荐扩展 在 gnome-shell-extension-manager 中搜索并安装以下扩展：\nDash to Dock：用于管理并美化 Dock（任务栏） Hide Top Bar：隐藏或自动折叠顶部栏以节省屏幕空间 6.3 管理 Ubuntu Dock 若需要删除默认的 Ubuntu Dock，使用：\n1 sudo apt remove -y gnome-shell-extension-ubuntu-dock 如需恢复或重新安装：\n1 sudo apt install -y gnome-shell-extension-ubuntu-dock 6.4 解决双侧边栏冲突 Ubuntu Dock 和 Dash to Dock 冲突导致，需要卸载 Ubuntu Dock 然后安装 Dash to Dock：\n1 2 3 4 5 sudo apt install git make gettext # 安装依赖 git clone https://github.com/micheleg/dash-to-dock.git -b gnome-3.38 cd dash-to-dock/ make make install 7. 多媒体播放支持 7.1 MP4 播放支持 安装 VLC 播放器及相关编解码器：\n1 2 sudo apt install ubuntu-restricted-extras sudo apt-get install vlc 8. 系统恢复 8.1 设置黑屏崩溃恢复 当遇到黑屏崩溃时，使用以下步骤恢复：\n按 Ctrl+Alt+F3 进入终端 执行系统升级和重新安装桌面环境： 1 2 sudo apt upgrade sudo apt install --reinstall ubuntu-desktop 参考文档 NVIDIA 驱动安装官方指南 Ubuntu 日志管理最佳实践 WPS Linux 版字体配置 Ubuntu 安装宋体详细教程 GNOME Extensions 官方文档 Terminator 终端配置指南 ","permalink":"https://zero-kq.github.io/Pages/posts/linux/linux-system-guide/","summary":"\u003ch2 id=\"1-nvidia-驱动配置\"\u003e1. Nvidia 驱动配置\u003c/h2\u003e\n\u003ch3 id=\"11-禁用开源-nouveau-驱动\"\u003e1.1 禁用开源 nouveau 驱动\u003c/h3\u003e\n\u003cp\u003e与 NVIDIA 专有驱动冲突，核心步骤如下：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003e\u003cspan class=\"c1\"\u003e# 编辑黑名单文件\u003c/span\u003e\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003esudo nano /etc/modprobe.d/blacklist-nouveau.conf\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e写入以下内容：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-text\" data-lang=\"text\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eblacklist nouveau\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eoptions nouveau modeset=0\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e更新并重启：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003esudo update-initramfs -u\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003esudo reboot\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e验证是否禁用：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003elsmod \u003cspan class=\"p\"\u003e|\u003c/span\u003e grep nouveau\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e查看可用驱动版本：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003eubuntu-drivers devices\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e安装指定版本驱动：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003esudo apt install nvidia-driver-535 nvidia-settings -y\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cp\u003e重启使配置生效：\u003c/p\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003cspan class=\"lnt\"\u003e2\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003esudo reboot\n\u003c/span\u003e\u003c/span\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003envidia-smi\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003ch3 id=\"12-避免自动安装问题\"\u003e1.2 避免自动安装问题\u003c/h3\u003e\n\u003cdiv class=\"highlight\"\u003e\u003cdiv class=\"chroma\"\u003e\n\u003ctable class=\"lntable\"\u003e\u003ctr\u003e\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode\u003e\u003cspan class=\"lnt\"\u003e1\n\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\n\u003ctd class=\"lntd\"\u003e\n\u003cpre tabindex=\"0\" class=\"chroma\"\u003e\u003ccode class=\"language-bash\" data-lang=\"bash\"\u003e\u003cspan class=\"line\"\u003e\u003cspan class=\"cl\"\u003esudo ubuntu-drivers autoinstall\u003c/span\u003e\u003c/span\u003e\u003c/code\u003e\u003c/pre\u003e\u003c/td\u003e\u003c/tr\u003e\u003c/table\u003e\n\u003c/div\u003e\n\u003c/div\u003e\n\u003cblockquote\u003e\n\u003cp\u003e\u003cstrong\u003e注意\u003c/strong\u003e：此命令会安装新内核，可能导致兼容性问题，建议手动安装。\u003c/p\u003e\u003c/blockquote\u003e\n\u003chr\u003e\n\u003ch2 id=\"2-桌面环境故障排除\"\u003e2. 桌面环境故障排除\u003c/h2\u003e\n\u003ch3 id=\"21-桌面无法进入左上角光标闪烁\"\u003e2.1 桌面无法进入（左上角光标闪烁）\u003c/h3\u003e\n\u003cp\u003e可能原因：\u003ccode\u003e/var/log/syslog.1\u003c/code\u003e 过大导致系统卡顿或无法继续引导。\u003c/p\u003e","title":"Linux 系统配置与问题解决指南"}]