优化

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

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

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

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