【3D SLAM源码解读系列】(二)Small_gicp——5 个积木搭出最优点云配准

前言

上一期我们深入解析了 Scan Context 与 Scan Context++ 的回环检测原理，它们可以输出回环候选帧以及两帧之间的 yaw 偏转角，已经具备了直接输入图优化的前置条件
往前内容：
- 第一期：【3D SLAM源码解读系列】(一)Scan Context与Scan Context++：3D点云回环检测源码解析
然而，对于一个 6-DOF 自由度的机器人，仅仅输出 yaw 角还不足以完成完整的位姿约束------我们还需要一个能够估计完整 (x, y, z, roll, pitch, yaw) 的配准模块，也就是 点云配准（Point Cloud Registration）
刚好，由日本产综研（AIST）的 Kenji Koide 开发的 small_gicp 是目前综合性能最出色的 3D 点云配准库之一，它用极小的体积实现了 ICP、Point-to-Plane ICP、GICP、VGICP 等全套算法，并且比它的前身 fast_gicp 还要快将近 2 倍
本文将基于 small_gicp 源码，从预处理到四种 ICP 变体再到 VGICP 的体素化加速，逐层拆解其数学原理与 C++ 实现，完整覆盖公式推导与代码逐行对照

文章目录

- 前言
- [1 Small_gicp 简介](#1 Small_gicp 简介)
- - - [1-1 作者与背景](#1-1 作者与背景)
    - [1-2 算法总览](#1-2 算法总览)
    - [1-3 整体架构设计](#1-3 整体架构设计)
    - [1-4 输入、输出与应用场景](#1-4 输入、输出与应用场景)
- [2 预处理算法](#2 预处理算法)
- - - [2-1 VoxelGrid 降采样](#2-1 VoxelGrid 降采样)
    - [2-2 KD-tree 最近邻搜索](#2-2 KD-tree 最近邻搜索)
    - [2-3 法向量与协方差估计](#2-3 法向量与协方差估计)
    - - [2-3-1 什么是特征分解](#2-3-1 什么是特征分解)
      - [2-3-2 法向量与协方差计算](#2-3-2 法向量与协方差计算)
    - [2-4 OpenMP 与 TBB 并行后端](#2-4 OpenMP 与 TBB 并行后端)
    - [2-5 Gaussian Voxel Map（体素化数据结构）](#2-5 Gaussian Voxel Map（体素化数据结构）)
    - [2-6 外点剔除（Correspondence Rejector）](#2-6 外点剔除（Correspondence Rejector）)
    - [2-7 全局约束因子（GeneralFactor）](#2-7 全局约束因子（GeneralFactor）)
- [3 优化器：Gauss-Newton 与 Levenberg-Marquardt](#3 优化器：Gauss-Newton 与 Levenberg-Marquardt)
- - - [3-1 介绍](#3-1 介绍)
    - [3-2 Gauss-Newton 原理](#3-2 Gauss-Newton 原理)
    - [3-3 一维例子](#3-3 一维例子)
    - [3-4 多维例子](#3-4 多维例子)
    - [3-5 Levenberg-Marquardt 原理](#3-5 Levenberg-Marquardt 原理)
    - [3-6 LDLT 分解：求解 H δ = − b H\delta = -b Hδ=−b](#3-6 LDLT 分解：求解 H δ = − b H\delta = -b Hδ=−b)
    - [3-7 small_gicp 中的实现](#3-7 small_gicp 中的实现)
- [4 Point-to-Point ICP](#4 Point-to-Point ICP)
- - - [4-1 算法介绍](#4-1 算法介绍)
    - [4-2 算法原理](#4-2 算法原理)
    - [4-3 完整代码流程](#4-3 完整代码流程)
- [5 Point-to-Plane ICP](#5 Point-to-Plane ICP)
- - - [5-1 算法介绍](#5-1 算法介绍)
    - [5-2 算法原理](#5-2 算法原理)
    - [5-3 完整代码流程](#5-3 完整代码流程)
- [6 GICP（核心）](#6 GICP（核心）)
- - - [6-1 算法介绍](#6-1 算法介绍)
    - [6-2 算法原理](#6-2 算法原理)
    - [6-3 完整代码流程](#6-3 完整代码流程)
- [7 VGICP（最出名）](#7 VGICP（最出名）)
- - - [7-1 算法介绍](#7-1 算法介绍)
    - [7-2 算法原理](#7-2 算法原理)
    - [7-3 完整代码流程](#7-3 完整代码流程)
- [8 VGICP-CUDA（GPU 加速）](#8 VGICP-CUDA（GPU 加速）)
- - - [8-1 VGICP-CUDA 的核心思路](#8-1 VGICP-CUDA 的核心思路)
    - [8-2 代码架构对比](#8-2 代码架构对比)
    - [8-3 如何获取](#8-3 如何获取)
- [9 NDT（正态分布变换）](#9 NDT（正态分布变换）)
- - - [9-1 NDT 的数学原理](#9-1 NDT 的数学原理)
    - [9-2 NDT 与 VGICP 的对比](#9-2 NDT 与 VGICP 的对比)
    - [9-3 如何选择](#9-3 如何选择)
- [10 应用：API 使用指南与算法流程总结](#10 应用：API 使用指南与算法流程总结)
- - - [10-1 快速开始：一键配准](#10-1 快速开始：一键配准)
    - [10-2 分步 API：预处理与配准分离](#10-2 分步 API：预处理与配准分离)
    - [10-3 Registration 模板：完全自定义](#10-3 Registration 模板：完全自定义)
    - [10-4 Python 接口](#10-4 Python 接口)
    - [10-5 自定义点云类型（Traits 机制）](#10-5 自定义点云类型（Traits 机制）)
    - [10-6 VGICP 体素化 API](#10-6 VGICP 体素化 API)
    - [10-7 算法全流程总结](#10-7 算法全流程总结)
- [11 Gauss-Newton 可视化 Python 源码](#11 Gauss-Newton 可视化 Python 源码)
- 总结

1 Small_gicp 简介

1-1 作者与背景

small_gicp 的作者是 Kenji Koide（小出健司），来自日本国立产业技术综合研究所（AIST）
他在 SLAM 与点云配准领域有非常深厚的技术积累，此前的作品 fast_gicp 已经是领域内广泛使用的高效 GICP 实现，被多个知名 SLAM 系统（如 HDL Graph SLAM、LiDAR-SLAM 等）所集成
small_gicp 是 fast_gicp 的 完全重写版本 ，从零开始重新设计，核心目标只有三个：
- 更快：核心配准逻辑进一步优化，单线程比 fast_gicp 快约 1.9 倍，比 PCL 的 GICP 快约 2.4 倍
- 更小：header-only 库，强依赖仅 Eigen，nanoflann 和 Sophus 的 SO3 代码直接内嵌
- 更灵活：基于 C++17 模板 + Traits 机制，用户可以接入任意自定义点云类型，也可以自由组合 Factor、Reduction、Optimizer 等模块

small_gicp 的设计哲学：把配准算法的每个环节（最近邻搜索、误差因子、线性系统归约、优化器）全部解耦成独立的模板参数，用户可以像搭积木一样自由替换任意模块

1-2 算法总览

small_gicp 库内实现了以下 4 种 CPU 配准算法，此外本文还将介绍 VGICP-CUDA（GPU 加速版，来自 fast_gicp）和 NDT（正态分布变换，PCL 实现）作为对比和扩展：

算法	误差类型	是否使用协方差	来源	特点
ICP	Point-to-Point	否	small_gicp	最基础，点到点距离最小化
Plane ICP	Point-to-Plane	否（但需要法向量）	small_gicp	沿法向量投影，对平面结构更鲁棒
GICP	Distribution-to-Distribution	是	small_gicp	同时考虑 target 和 source 的局部几何分布
VGICP	Distribution-to-Voxel	是	small_gicp	将 target 体素化为高斯体素，O(1) 哈希查找替代 O(logN) KD-tree
VGICP-CUDA	Distribution-to-Voxel	是	fast_gicp	VGICP 的 GPU 加速版，配准速度 5-10 倍提升
NDT	Distribution-to-Grid	是	PCL	基于高斯混合模型的对数似然最大化，HD Map 标配

说人话就是：
- ICP 只管"这两个点离得够不够近"
- Plane ICP 进一步考虑"这个点是不是落在目标平面上"
- GICP 更进一步考虑"这两片局部点云的形状分布匹配得怎么样"
- VGICP 是对 GICP 的工程加速------把 target 点云聚合成一个个带协方差的体素，用 O(1) 的哈希查找替代 O(logN) 的 KD-tree 查找
- VGICP-CUDA 则是对 VGICP 的进一步加速------把体素查找和线性系统归约搬到 GPU，用上千个 CUDA 核心并行处理
- NDT 走的是另一条路：把点云建模成高斯混合，直接最大化观测似然，不需要做点对点的最近邻搜索

1-3 整体架构设计

在深入具体算法之前，我们先看一眼 small_gicp 的核心模板架构：

cpp 复制代码

// include/small_gicp/registration/registration.hpp
template <
  typename PointFactor,          // 误差因子类型（ICP/GICP/PlaneICP）
  typename Reduction,            // 并行归约策略（串行 / OpenMP / TBB）
  typename GeneralFactor = NullFactor,        // 全局约束因子
  typename CorrespondenceRejector = DistanceRejector,  // 外点剔除
  typename Optimizer = LevenbergMarquardtOptimizer>    // 数值优化器
struct Registration {
  RegistrationResult align(
    const TargetPointCloud& target,
    const SourcePointCloud& source,
    const TargetTree& target_tree,
    const Eigen::Isometry3d& init_T) const;
};

五个模板参数各管一摊，团队分工如下：

模板参数	管什么	默认值	文章章节
`PointFactor`	误差怎么算：残差 e i e_i ei 和 Jacobian J i J_i Ji 的定义	（无默认，必选）	第 4~7 章
`Reduction`	怎么并行：把每个点的 ( H i , b i , e i ) (H_i, b_i, e_i) (Hi,bi,ei) 累加起来	`SerialReduction`	[2-4](#模板参数管什么默认值文章章节 PointFactor 误差怎么算：残差 e i e_i ei 和 Jacobian J i J_i Ji 的定义（无默认，必选）第 4~7 章 Reduction 怎么并行：把每个点的 ( H i , b i , e i ) (H_i, b_i, e_i) (Hi,bi,ei) 累加起来 SerialReduction 2-4 GeneralFactor 怎么注入先验：求解前修改 H , b , e H, b, e H,b,e，软约束 NullFactor 2-7 CorrespondenceRejector 怎么剔除误匹配：距离/法向量/特征不过关就丢弃 DistanceRejector 2-6 Optimizer 怎么优化：GN 或 LM 求解 ( H + λ I ) δ = − b (H+\lambda I)\delta=-b (H+λI)δ=−b LevenbergMarquardtOptimizer 第 3 章)
`GeneralFactor`	怎么注入先验：求解前修改 H , b , e H, b, e H,b,e，软约束	`NullFactor`	[2-7](#模板参数管什么默认值文章章节 PointFactor 误差怎么算：残差 e i e_i ei 和 Jacobian J i J_i Ji 的定义（无默认，必选）第 4~7 章 Reduction 怎么并行：把每个点的 ( H i , b i , e i ) (H_i, b_i, e_i) (Hi,bi,ei) 累加起来 SerialReduction 2-4 GeneralFactor 怎么注入先验：求解前修改 H , b , e H, b, e H,b,e，软约束 NullFactor 2-7 CorrespondenceRejector 怎么剔除误匹配：距离/法向量/特征不过关就丢弃 DistanceRejector 2-6 Optimizer 怎么优化：GN 或 LM 求解 ( H + λ I ) δ = − b (H+\lambda I)\delta=-b (H+λI)δ=−b LevenbergMarquardtOptimizer 第 3 章)
`CorrespondenceRejector`	怎么剔除误匹配：距离/法向量/特征不过关就丢弃	`DistanceRejector`	[2-6](#模板参数管什么默认值文章章节 PointFactor 误差怎么算：残差 e i e_i ei 和 Jacobian J i J_i Ji 的定义（无默认，必选）第 4~7 章 Reduction 怎么并行：把每个点的 ( H i , b i , e i ) (H_i, b_i, e_i) (Hi,bi,ei) 累加起来 SerialReduction 2-4 GeneralFactor 怎么注入先验：求解前修改 H , b , e H, b, e H,b,e，软约束 NullFactor 2-7 CorrespondenceRejector 怎么剔除误匹配：距离/法向量/特征不过关就丢弃 DistanceRejector 2-6 Optimizer 怎么优化：GN 或 LM 求解 ( H + λ I ) δ = − b (H+\lambda I)\delta=-b (H+λI)δ=−b LevenbergMarquardtOptimizer 第 3 章)
`Optimizer`	怎么优化：GN 或 LM 求解 ( H + λ I ) δ = − b (H+\lambda I)\delta=-b (H+λI)δ=−b	`LevenbergMarquardtOptimizer`	[第 3 章](#模板参数管什么默认值文章章节 PointFactor 误差怎么算：残差 e i e_i ei 和 Jacobian J i J_i Ji 的定义（无默认，必选）第 4~7 章 Reduction 怎么并行：把每个点的 ( H i , b i , e i ) (H_i, b_i, e_i) (Hi,bi,ei) 累加起来 SerialReduction 2-4 GeneralFactor 怎么注入先验：求解前修改 H , b , e H, b, e H,b,e，软约束 NullFactor 2-7 CorrespondenceRejector 怎么剔除误匹配：距离/法向量/特征不过关就丢弃 DistanceRejector 2-6 Optimizer 怎么优化：GN 或 LM 求解 ( H + λ I ) δ = − b (H+\lambda I)\delta=-b (H+λI)δ=−b LevenbergMarquardtOptimizer 第 3 章)

五者在迭代优化循环中的配合关系：

#mermaid-svg-ufOLjFEYLVgGS3sf{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-ufOLjFEYLVgGS3sf .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-ufOLjFEYLVgGS3sf .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-ufOLjFEYLVgGS3sf .error-icon{fill:#552222;}#mermaid-svg-ufOLjFEYLVgGS3sf .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-ufOLjFEYLVgGS3sf .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-ufOLjFEYLVgGS3sf .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-ufOLjFEYLVgGS3sf .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-ufOLjFEYLVgGS3sf .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-ufOLjFEYLVgGS3sf .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-ufOLjFEYLVgGS3sf .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-ufOLjFEYLVgGS3sf .marker{fill:#333333;stroke:#333333;}#mermaid-svg-ufOLjFEYLVgGS3sf .marker.cross{stroke:#333333;}#mermaid-svg-ufOLjFEYLVgGS3sf svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-ufOLjFEYLVgGS3sf p{margin:0;}#mermaid-svg-ufOLjFEYLVgGS3sf .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-ufOLjFEYLVgGS3sf .cluster-label text{fill:#333;}#mermaid-svg-ufOLjFEYLVgGS3sf .cluster-label span{color:#333;}#mermaid-svg-ufOLjFEYLVgGS3sf .cluster-label span p{background-color:transparent;}#mermaid-svg-ufOLjFEYLVgGS3sf .label text,#mermaid-svg-ufOLjFEYLVgGS3sf span{fill:#333;color:#333;}#mermaid-svg-ufOLjFEYLVgGS3sf .node rect,#mermaid-svg-ufOLjFEYLVgGS3sf .node circle,#mermaid-svg-ufOLjFEYLVgGS3sf .node ellipse,#mermaid-svg-ufOLjFEYLVgGS3sf .node polygon,#mermaid-svg-ufOLjFEYLVgGS3sf .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-ufOLjFEYLVgGS3sf .rough-node .label text,#mermaid-svg-ufOLjFEYLVgGS3sf .node .label text,#mermaid-svg-ufOLjFEYLVgGS3sf .image-shape .label,#mermaid-svg-ufOLjFEYLVgGS3sf .icon-shape .label{text-anchor:middle;}#mermaid-svg-ufOLjFEYLVgGS3sf .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-ufOLjFEYLVgGS3sf .rough-node .label,#mermaid-svg-ufOLjFEYLVgGS3sf .node .label,#mermaid-svg-ufOLjFEYLVgGS3sf .image-shape .label,#mermaid-svg-ufOLjFEYLVgGS3sf .icon-shape .label{text-align:center;}#mermaid-svg-ufOLjFEYLVgGS3sf .node.clickable{cursor:pointer;}#mermaid-svg-ufOLjFEYLVgGS3sf .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-ufOLjFEYLVgGS3sf .arrowheadPath{fill:#333333;}#mermaid-svg-ufOLjFEYLVgGS3sf .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-ufOLjFEYLVgGS3sf .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-ufOLjFEYLVgGS3sf .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-ufOLjFEYLVgGS3sf .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-ufOLjFEYLVgGS3sf .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-ufOLjFEYLVgGS3sf .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-ufOLjFEYLVgGS3sf .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-ufOLjFEYLVgGS3sf .cluster text{fill:#333;}#mermaid-svg-ufOLjFEYLVgGS3sf .cluster span{color:#333;}#mermaid-svg-ufOLjFEYLVgGS3sf div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-ufOLjFEYLVgGS3sf .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-ufOLjFEYLVgGS3sf rect.text{fill:none;stroke-width:0;}#mermaid-svg-ufOLjFEYLVgGS3sf .icon-shape,#mermaid-svg-ufOLjFEYLVgGS3sf .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-ufOLjFEYLVgGS3sf .icon-shape p,#mermaid-svg-ufOLjFEYLVgGS3sf .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-ufOLjFEYLVgGS3sf .icon-shape .label rect,#mermaid-svg-ufOLjFEYLVgGS3sf .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-ufOLjFEYLVgGS3sf .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-ufOLjFEYLVgGS3sf .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-ufOLjFEYLVgGS3sf :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 通过
通过
未收敛
PointFactor

算 e_i + J_i

得 H_i, b_i, e_i
Rejector

距离超阈值?

法向量打架?

→ 丢弃此匹配
Reduction

H=ΣH_i, b=Σb_i

OMP / TBB 并行
GeneralFactor

注入先验约束

修改 H, b, e
Optimizer

GN / LM + LDLT

解 δ, 更新 T

说人话：五个积木在每轮迭代里依次登场------Factor 算误差、Rejector 把不靠谱的匹配丢出去、Reduction 把剩下的累加起来、GeneralFactor 给系统加上先验约束、Optimizer 求解并更新位姿------没收敛就再来一圈

可复用的预处理（降采样 → KD-tree → 法向量/协方差）在迭代开始前一次性完成

核心执行流程：目标点云 → 降采样 → KD-tree → 法向量/协方差估计 → 迭代优化（线性化 + 归约 + LM 求解）→ 输出位姿

1-4 输入、输出与应用场景

输入：只需要三样东西------
- target 点云：目标/参考点云（"地图帧"），通常比 source 稠密或经过体素化
- source 点云：待配准点云（"当前帧"），会被变换到 target 坐标系下
- 初始位姿 T init T_{\text{init}} Tinit：一个 4 × 4 4 \times 4 4×4 的初始猜测（Eigen::Isometry3d），配准从这个位姿开始迭代优化。如果完全不知道初值，就用 Eigen::Isometry3d::Identity()
T T T 的结构 ：点云配准中的变换矩阵 T T T 是 4 × 4 4 \times 4 4×4 的齐次变换：

T = $R 3 \times 3 t 3 \times 1 0 1 \times 3 1$ ∈ S E ( 3 ) T = \begin{bmatrix} R_{3\times3} & t_{3\times1} \\ 0_{1\times3} & 1 \end{bmatrix} \in SE(3) T= $R3\times301\times3t3\times11$ ∈SE(3)
- R R R 是 3 × 3 3 \times 3 3×3 旋转矩阵（正交矩阵， det ⁡ ( R ) = 1 \det(R)=1 det(R)=1），包含 3 个旋转自由度（roll, pitch, yaw）
- t = $t x , t y , t z$ T t = $t_x, t_y, t_z$ ^T t= $tx,ty,tz$ T 是 3 × 1 3 \times 1 3×1 平移向量，包含 3 个平移自由度
- 总共 6 个自由度。用齐次坐标 ( x , y , z , 1 ) T (x, y, z, 1)^T (x,y,z,1)T 表示点，旋转和平移合并为一次矩阵乘法： p target = T ⋅ p source p_{\text{target}} = T \cdot p_{\text{source}} ptarget=T⋅psource
- 这就是为什么 Gauss-Newton 优化中的扰动 δ \delta δ 是 6 维向量：对应这 6 个自由度的微调量
输出：RegistrationResult 结构体，包含以下字段：

字段	类型	含义
`T_target_source`	`Eigen::Isometry3d`	source 到 target 的最优变换矩阵（核心输出）
`converged`	`bool`	优化是否收敛
`iterations`	`size_t`	实际迭代次数
`num_inliers`	`size_t`	内点数量（未超最大匹配距离的匹配点数）
`H`	`Eigen::Matrix<double, 6, 6>`	最终的 6×6 Hessian 矩阵（= 信息矩阵，反映位姿估计的不确定性）
`b`	`Eigen::Matrix<double, 6, 1>`	最终的 6×1 梯度向量
`error`	`double`	最终的总误差值

说人话 ：给定两坨点云和一个粗糙的初始猜测，"把 source 转一转、挪一挪，让它和 target 尽可能重合"------返回那个"最佳转挪"（ T T T），顺带告诉你它动了几次才收敛、到底重合得怎么样

典型应用场景 ：
- LiDAR SLAM 前端配准：相邻两帧激光点云做 ICP/GICP，输出帧间变换，作为里程计约束
- scan-to-model 配准 ：当前帧配准到累积的局部子地图（用 VGICP + IncrementalVoxelMap），漂移更小
- 回环验证 ：Scan Context 给出候选回环帧 + 初始 yaw 角后，用 small_gicp 算出完整的 6-DOF 相对位姿，送入图优化
- 多传感器标定：LiDAR-LiDAR、LiDAR-Camera 外参标定中，用点云配准求解传感器之间的变换

2 预处理算法

在正式介绍四种配准算法之前，我们必须先理解 small_gicp 中的三个预处理模块------它们是整个配准流水线的地基
正式配准前，target 和 source 两帧点云都需要经过：降采样 → KD-tree 构建 → 法向量与协方差估计

2-1 VoxelGrid 降采样

为什么要降采样？
- 原始 LiDAR 点云可能有几十万个点，如果全部参与配准，每一步迭代都需要对所有 source 点做最近邻搜索 → 计算量爆炸
- 降采样的本质是用少量"代表性"点替代稠密点云，在保证几何结构的前提下大幅减少点数
small_gicp 实现的 voxelgrid_sampling 使用了 位打包排序法：

cpp 复制代码

// include/small_gicp/util/downsampling.hpp
template <typename InputPointCloud, typename OutputPointCloud = InputPointCloud>
std::shared_ptr<OutputPointCloud> voxelgrid_sampling(const InputPointCloud& points, double leaf_size) {
  const double inv_leaf_size = 1.0 / leaf_size;
  constexpr std::uint64_t invalid_coord = std::numeric_limits<std::uint64_t>::max();
  constexpr int coord_bit_size = 21;  // 每个坐标分量 21bit，打包 63bit 进 64bit 整数
  constexpr size_t coord_bit_mask = (1 << 21) - 1;         // 位掩码
  constexpr int coord_offset = 1 << (coord_bit_size - 1);  // 偏移量，确保坐标为正

  // 1. 计算每个点的体素编码（64bit：0|z|y|x，各占 21bit）
  std::vector<std::pair<std::uint64_t, size_t>> coord_pt(traits::size(points));
  for (size_t i = 0; i < traits::size(points); i++) {
    const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;
    const std::uint64_t bits =
      (static_cast<std::uint64_t>(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) |
      (static_cast<std::uint64_t>(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) |
      (static_cast<std::uint64_t>(coord[2] & coord_bit_mask) << (coord_bit_size * 2));
    coord_pt[i] = {bits, i};
  }

  // 2. 按体素编码排序（同体素的点会相邻）
  std::sort(coord_pt.begin(), coord_pt.end(), compare);

  // 3. 同一体素内的点求平均
  size_t num_points = 0;
  Eigen::Vector4d sum_pt = traits::point(points, coord_pt.front().second);
  for (size_t i = 1; i < traits::size(points); i++) {
    if (coord_pt[i].first == invalid_coord) {
      continue;
    }

    if (coord_pt[i - 1].first != coord_pt[i].first) {
      traits::set_point(*downsampled, num_points++, sum_pt / sum_pt.w());
      sum_pt.setZero();
    }

    sum_pt += traits::point(points, coord_pt[i].second);
  }
  traits::set_point(*downsampled, num_points++, sum_pt / sum_pt.w());
}

说人话：把空间划分成 leaf_size 大小的立方体格子，每个格子里不管有多少原始点，最终只输出一个"平均点"。关键是它用排序而不是 unordered_map 来做体素归属------排序后同一个格子的点自然排在一起，扫描一遍就出结果，内存局部性极好

核心优化细节：
- 使用 std::uint64_t 位打包编码三维体素坐标（每维 21bit），一次 64 位整数比较就等价于三维坐标比较
- 使用 fast_floor（位运算取整）而非 std::floor，避免浮点截断开销
- 支持 OpenMP 和 TBB 两种并行后端，多线程下可达 3.2 倍加速

2-2 KD-tree 最近邻搜索

以防你不知道，KD-tree 是一种 空间划分数据结构，它把三维空间中的点递归地沿着坐标轴切成两半，形成一棵二叉树。搜索最近邻时，你只需要沿着 query 点所在的子树往下找，通常 O(logN) 就能找到，而不需要暴力遍历所有 N 个点
配准的每一步迭代中，我们需要对每个 source 点找到它在 target 中最近的点------这就是 KD-tree 的用武之地
small_gicp 的 KD-tree 是从头手写的（灵感来自 nanoflann），但做了针对性优化：

cpp 复制代码

// include/small_gicp/ann/kdtree.hpp
template <typename PointCloud, typename Projection_ = AxisAlignedProjection>
struct UnsafeKdTree {
  // 建树：递归地在每个维度上找中位数切分
  NodeIndexType create_node(..., IndexConstIterator first, IndexConstIterator last) {
    if (N <= max_leaf_size) {
      // 叶子节点：直接存点索引范围
      node.node_type.lr.first = std::distance(global_first, first);
      node.node_type.lr.last = std::distance(global_first, last);
      return node_index;
    }
    // 非叶节点：沿最优投影轴找中位数切分
    const auto proj = Projection::find_axis(points, first, last, projection_setting);
    const auto median_itr = first + N / 2;
    std::nth_element(first, median_itr, last, [&](size_t i, size_t j) {
      return proj(traits::point(points, i)) < proj(traits::point(points, j));
    });
    node.node_type.sub.proj = proj;
    node.node_type.sub.thresh = proj(traits::point(points, *median_itr));
    // 递归构建左右子树
  }

  // 搜索：best-bin-first 策略
  bool knn_search(const Eigen::Vector4d& query, NodeIndexType node_index, Result& result) {
    // 叶节点 → 暴力比较
    // 非叶节点 → 先搜 query 所在侧，再根据 worst_distance 决定是否搜另一侧
  }
};

从代码可以清晰看到两种节点类型：
- 叶子节点 ：当 N <= max_leaf_size（默认 20 个点），不再继续切分，直接存入这组点的索引范围 [first, last)。搜索时对这至多 20 个点逐一暴力比较距离
- 非叶节点：当点数超过阈值，沿当前"最优切分轴"找中位数，将空间一分为二。左子树存小于中位数的点，右子树存大于中位数的点。搜索时根据 query 点在切分轴上的坐标决定先走左边还是右边

说人话：KD-tree 就像一本"三维空间的二分查找字典"------非叶节点告诉你"往左翻还是往右翻"，叶子节点告诉你"这一页上所有候选点的位置"，你只需要翻 O(logN) 次书页 + 最后在叶子节点里暴力比 20 次

核心优化：
- 使用 std::nth_element（O(N) 部分排序）而非 std::sort（O(NlogN) 全排序）来找中位数切分点
- 支持 OpenMP 和 TBB 两种并行建树，多线程下可达 6 倍加速
- 叶子节点大小可配置（默认 20），在构建开销和查询开销间做折中

2-3 法向量与协方差估计

这是最关键的一步预处理------因为 GICP 和 Plane ICP 都需要每个点的 局部几何信息（法向量和协方差矩阵）
核心算法：对每个点，搜索其 k 个最近邻，计算这 k 个点的样本协方差矩阵，然后做特征分解

2-3-1 什么是特征分解

以防你忘记，特征分解（Eigendecomposition）就是把一个 n × n n \times n n×n 方阵拆成"方向 + 缩放"的乘积形式：
- C = R Λ R T C = R \Lambda R^T C=RΛRT
- R R R 是 n × n n \times n n×n 的正交矩阵，每一列就是一个 特征向量，代表点云局部形状的一个"主轴方向"------彼此正交，构成一个局部坐标系
- Λ \Lambda Λ 是对角矩阵 diag ( λ 1 , λ 2 , λ 3 ) \text{diag}(\lambda_1, \lambda_2, \lambda_3) diag(λ1,λ2,λ3)，每个对角线上的值就是对应的 特征值，代表沿该主轴方向的"方差大小"（点在这个方向上散得有多开）
举个三维点云的例子：
- 如果 k 个近邻点大致分布在一个平面上，那么特征值应该是 λ 1 ≈ λ 2 ≫ λ 3 \lambda_1 \approx \lambda_2 \gg \lambda_3 λ1≈λ2≫λ3------两个大方差沿平面，一个小方差沿法向
- 如果 k 个点呈球状分布，那么 λ 1 ≈ λ 2 ≈ λ 3 \lambda_1 \approx \lambda_2 \approx \lambda_3 λ1≈λ2≈λ3------各向同性，没有明显的平面结构
- 如果 k 个点呈线状分布（比如墙角边上），那么 λ 1 ≫ λ 2 ≈ λ 3 \lambda_1 \gg \lambda_2 \approx \lambda_3 λ1≫λ2≈λ3------一个主方向，两个几乎没分布的方向

说人话：特征分解就像给一团点做"主成分分析（PCA）"------找出这团点"最长的方向"（第一主成分）、"次长的方向"（第二主成分）和"最短的方向"（第三主成分，即法向）

可以回忆我们在【ICP点云配准】从数学原理到手写C++------SVD求解与多分辨率优化进行分解的时候使用到的是 SVD------当时我们需要分解的是 3 × 3 3 \times 3 3×3 的互协方差矩阵 W = ∑ ( p i s − p ˉ s ) ( p j t − p ˉ t ) T W = \sum (p_i^s - \bar{p}^s)(p_j^t - \bar{p}^t)^T W=∑(pis−pˉs)(pjt−pˉt)T， W W W 不一定对称，所以 必须用 SVD （ W = U Σ V T W = U \Sigma V^T W=UΣVT）来求解旋转 R = V U T R = V U^T R=VUT。而这里要分解的是 对称的协方差矩阵 C i C_i Ci，特征分解和 SVD 等价，直接用 SelfAdjointEigenSolver 更快

2-3-2 法向量与协方差计算

cpp 复制代码

// include/small_gicp/util/normal_estimation.hpp
template <typename Setter, typename PointCloud, typename Tree>
void estimate_local_features(PointCloud& cloud, Tree& kdtree, int num_neighbors, size_t point_index) {
  // 1. KNN 搜索 k 个最近邻
  kdtree.knn_search(traits::point(cloud, point_index), num_neighbors, k_indices.data(), k_sq_dists.data());

  // 2. 计算 k 个点的样本协方差
  Eigen::Vector4d sum_points = Eigen::Vector4d::Zero();
  Eigen::Matrix4d sum_cross = Eigen::Matrix4d::Zero();
  for (size_t i = 0; i < n; i++) {
    const auto& pt = traits::point(cloud, k_indices[i]);
    sum_points += pt;
    sum_cross += pt * pt.transpose();
  }
  const Eigen::Vector4d mean = sum_points / n;
  const Eigen::Matrix4d cov = (sum_cross - mean * sum_points.transpose()) / n;

  // 3. 特征分解
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig;
  eig.computeDirect(cov.block<3, 3>(0, 0));

  // 4. 根据 Setter 策略输出法向量 和/或 协方差
  Setter::set(cloud, point_index, eig.eigenvectors());
}

样本协方差矩阵的数学定义：

C i = 1 k ∑ j = 1 k ( p j − p ˉ ) ( p j − p ˉ ) T C_i = \frac{1}{k} \sum_{j=1}^{k} (p_j - \bar{p})(p_j - \bar{p})^T Ci=k1j=1∑k(pj−pˉ)(pj−pˉ)T

其中 p ˉ = 1 k ∑ j = 1 k p j \bar{p} = \frac{1}{k}\sum_{j=1}^{k} p_j pˉ=k1∑j=1kpj 是 k 个近邻点的均值
法向量的提取逻辑（NormalSetter）：
- 对协方差矩阵 C i C_i Ci 的 3 × 3 3 \times 3 3×3 左上块做特征分解
- 最小特征值对应的特征向量 就是该点的法向量方向（因为局部点云沿平面延伸，法向方向方差最小）
- 通过 point.dot(normal) > 0 判断法向的方向，确保法向始终指向传感器（外侧）
协方差的提取逻辑（CovarianceSetter）：
- 使用相同的特征向量矩阵，但人为设定特征值为 ( 10 − 3 , 1.0 , 1.0 ) (10^{-3}, 1.0, 1.0) (10−3,1.0,1.0)
说人话：沿法向方向给极小方差（ 10 − 3 10^{-3} 10−3），沿切平面方向给大方差（ 1.0 1.0 1.0），从而构造出"这个点在法向上很确定、切向上很松弛"的各向异性协方差

C i = R $10 - 3 0 0 0 1.0 0 0 0 1.0$ R T C_i = R \begin{bmatrix} 10^{-3} & 0 & 0 \\ 0 & 1.0 & 0 \\ 0 & 0 & 1.0 \end{bmatrix} R^T Ci=R 10−30001.00001.0 RT

其中 R R R 是特征向量矩阵，第一列是法向量方向

协方差的本质作用：告诉 GICP "这个点在哪个方向上可信、哪个方向上不可信"，从而让优化器只在可信方向上施加约束

2-4 OpenMP 与 TBB 并行后端

small_gicp 的所有并行化都通过两种后端实现：OpenMP（OMP） 和 Intel TBB，用户可以在编译时选择其一（或两者都不选，退化为串行）
并行化覆盖的模块包括：降采样（voxelgrid_sampling_omp/tbb）、KD-tree 构建（KdTreeBuilderOMP/TBB）、法向量与协方差估计（estimate_normals_covariances_omp/tbb）、以及配准归约（ParallelReductionOMP/TBB）
OpenMP 方案 （reduction_omp.hpp）：
- 使用 #pragma omp parallel for 将 source 点的遍历分发给多个线程
- 每个线程独立维护自己的 (H_thread, b_thread, e_thread) 累加器，避免锁竞争
- 循环结束后，主线程将所有线程的累加器归约求和
关键代码：

cpp 复制代码

// include/small_gicp/registration/reduction_omp.hpp
struct ParallelReductionOMP {
  int num_threads = 4;

  auto linearize(...) {
    // 每个线程一个独立的累加器，避免 false sharing
    std::vector<Eigen::Matrix<double, 6, 6>> Hs(num_threads, Zero());
    std::vector<Eigen::Matrix<double, 6, 1>> bs(num_threads, Zero());
    std::vector<double> es(num_threads, 0.0);

#pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
    for (int64_t i = 0; i < factors.size(); i++) {
      // 每个线程独立计算并累加到自己的 slot
      const int thread_id = omp_get_thread_num();
      factors[i].linearize(..., &H, &b, &e);
      Hs[thread_id] += H; bs[thread_id] += b; es[thread_id] += e;
    }
    // 主线程串行归约所有线程的结果
    for (int i = 1; i < num_threads; i++) {
      Hs[0] += Hs[i]; bs[0] += bs[i]; es[0] += es[i];
    }
    return {Hs[0], bs[0], es[0]};
  }
};

TBB 方案 （reduction_tbb.hpp）：
- 使用 TBB 的 parallel_reduce 模式------定义可拆分（split）和可合并（join）的累加器对象
- TBB 的任务窃取（work-stealing）调度器自动管理线程分配和负载均衡
- 不需要手动管理线程 ID 和线程本地存储，代码更简洁但依赖 TBB 运行时
关键代码：

cpp 复制代码

// include/small_gicp/registration/reduction_tbb.hpp
struct LinearizeSum {
  // 构造（split 版本用于 TBB 拆分任务）
  LinearizeSum(LinearizeSum& x, tbb::split) : ... { /* 从 x 复制引用，累加器置零 */ }

  // 核心计算：处理一个区间内的所有点
  void operator()(const tbb::blocked_range<size_t>& r) {
    for (size_t i = r.begin(); i != r.end(); i++) {
      factors[i].linearize(..., &Hi, &bi, &ei);
      Ht += Hi; bt += bi; et += ei;
    }
  }

  // 合并：将另一个任务的累加结果加到自己身上
  void join(const LinearizeSum& y) { H += y.H; b += y.b; e += y.e; }
};

struct ParallelReductionTBB {
  auto linearize(...) {
    LinearizeSum sum(...);
    tbb::parallel_reduce(tbb::blocked_range<size_t>(0, factors.size(), 8), sum);
    return {sum.H, sum.b, sum.e};
  }
};

对比维度	OpenMP	Intel TBB
并行模型	Fork-Join（编译器指令）	任务图（C++ 模板库）
线程管理	`#pragma omp parallel for`	`tbb::parallel_reduce`
负载均衡	`schedule(guided, 8)` 自适应	work-stealing 自动调度
依赖	编译器内置支持（`-fopenmp`）	需安装 TBB 运行时
代码侵入性	低（pragma 注解即可）	中（需定义可 split/join 的函子）
可扩展性	中等（适合 4-16 线程）	优秀（可扩展到 128+ 线程）
跨平台	良好（MSVC 有部分限制）	优秀

说人话：

* OpenMP 就是给 for 循环前面加一行 #pragma omp parallel for，编译器自动帮你把循环拆成多线程------省事，但扩展性到几十个线程后就不太行了

* TBB 是需要你写一个"能拆开也能合并"的累加器类，然后交给 TBB 的任务调度器去跑------麻烦一点，但可以轻松扩展到上百个线程，而且配合 TBB flow graph 还能做流水线并行

在 benchmark 中，TBB flow graph 版本在 128 线程时仍然保持良好的扩展性，而 OpenMP 版本在 16 线程后加速比就开始下降
对于大多数用户，small_gicp 默认推荐 OpenMP（4 线程） 就能获得很好的性能------无需额外安装依赖，且 4-8 线程下 OMP 和 TBB 性能差距不大

一句话总结：OMP 是"开箱即用"的并行，TBB 是"可定制"的并行------small_gicp 通过模板参数让你自由选择，接口完全一致

2-5 Gaussian Voxel Map（体素化数据结构）

Gaussian Voxel Map 是 VGICP 的数据核心，它是一个 增量式、支持 LRU 淘汰的体素哈希表：

cpp 复制代码

// include/small_gicp/ann/gaussian_voxelmap.hpp
struct GaussianVoxel {
  size_t num_points;         // 体素内的点数
  Eigen::Vector4d mean;      // 均值
  Eigen::Matrix4d cov;       // 协方差

  // 添加一个点到体素（累加模式）
  void add(const Eigen::Vector4d& transformed_pt, const PointCloud& points, size_t i, const Eigen::Isometry3d& T) {
    num_points++;
    this->mean += transformed_pt;                                          // 累加均值
    this->cov += T.matrix() * traits::cov(points, i) * T.matrix().transpose();  // 累加协方差（变换后）
  }

  // 归一化（除以点数）
  void finalize() {
    mean /= num_points;
    cov /= num_points;
  }
};

体素聚合的核心公式：

p ˉ v = 1 N v ∑ k ∈ v p k , C ˉ v = 1 N v ∑ k ∈ v T C k T T \bar{p}v = \frac{1}{N_v} \sum{k \in v} p_k, \quad \bar{C}v = \frac{1}{N_v} \sum{k \in v} T C_k T^T pˉv=Nv1k∈v∑pk,Cˉv=Nv1k∈v∑TCkTT

这里的 T T T 是当前帧到体素地图的变换------因为协方差是相对坐标系的，需要随点一起被变换
而 IncrementalVoxelMap 则是体素地图的容器：

cpp 复制代码

// include/small_gicp/ann/incremental_voxelmap.hpp
template <typename VoxelContents>
struct IncrementalVoxelMap {
  double inv_leaf_size;                     // 体素大小的倒数
  std::unordered_map<Eigen::Vector3i, size_t> voxels;     // 体素坐标 → flat_voxels 索引
  std::vector<...> flat_voxels;             // 体素内容列表
  size_t lru_counter;                       // LRU 计数器

  void insert(const PointCloud& points, const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()) {
    for (size_t i = 0; i < traits::size(points); i++) {
      const Eigen::Vector4d pt = T * traits::point(points, i);
      const Eigen::Vector3i coord = fast_floor(pt * inv_leaf_size).template head<3>();

      auto found = voxels.find(coord);
      if (found == voxels.end()) {
        auto voxel = std::make_shared<std::pair<VoxelInfo, VoxelContents>>(VoxelInfo(coord, lru_counter), VoxelContents());
        found = voxels.emplace_hint(found, coord, flat_voxels.size());
        flat_voxels.emplace_back(voxel);
      }

      auto& [info, voxel] = *flat_voxels[found->second];
      info.lru = lru_counter;
      voxel.add(voxel_setting, pt, points, i, T);
    }

    if ((++lru_counter) % lru_clear_cycle == 0) {
      auto remove_counter = std::remove_if(flat_voxels.begin(), flat_voxels.end(), [&](const auto& voxel) {
        return voxel->first.lru + lru_horizon < lru_counter;
      });
      flat_voxels.erase(remove_counter, flat_voxels.end());

      voxels.clear();
      for (size_t i = 0; i < flat_voxels.size(); i++) {
        voxels[flat_voxels[i]->first.coord] = i;
      }
    }

    // 每帧 insert 后都要 finalize，不管是否触发 LRU 清理
    for (auto& voxel : flat_voxels) {
      voxel->second.finalize();
    }
  }

  size_t nearest_neighbor_search(const Eigen::Vector4d& pt, size_t* index, double* sq_dist) const {
    const Eigen::Vector3i center = fast_floor(pt * inv_leaf_size).template head<3>();
    size_t voxel_index = 0;
    const auto index_transform = [&](size_t i) { return calc_index(voxel_index, i); };
    KnnResult<1, decltype(index_transform)> result(index, sq_dist, -1, index_transform);

    for (const auto& offset : search_offsets) {
      const Eigen::Vector3i coord = center + offset;
      const auto found = voxels.find(coord);
      if (found == voxels.end()) {
        continue;
      }

      voxel_index = found->second;
      const auto& voxel = flat_voxels[voxel_index]->second;
      traits::Traits<VoxelContents>::knn_search(voxel, pt, result);
    }

    return result.num_found();
  }
};

关键设计：
- 最近邻搜索不是 KD-tree，而是直接哈希查找 ：因为 query 点落在哪个体素是 O(1) 可计算的（coord = floor(pt * inv_leaf_size)），直接去哈希表找就行
- 搜索邻域可配：可以只搜中心体素（1 个），或 ±1 相邻（7 个），或 3×3×3 范围（27 个），体素分辨率越大越需要搜更多邻域
- LRU 淘汰：对于 SLAM 中的 scan-to-model 配准，体素地图不断增长。LRU 机制会自动删除长期未使用的体素，防止内存无限膨胀
- Traits 适配 ：GaussianVoxelMap 通过 Traits 机制伪装成一个"点云"------它有 point()、cov()、nearest_neighbor_search() 等方法，所以 GICPFactor 根本不需要知道自己在和体素还是点云打交道

Gaussian Voxel Map 的本质：用空间哈希+均值聚合，把 O(NlogN) 的最近邻问题降为 O(1) 的哈希查找，同时几乎不损失信息

2-6 外点剔除（Correspondence Rejector）

KD-tree 最近邻只能找到几何上最近的点，但它不能判断这个匹配是否"合理"------比如两帧点云 overlap 不大的区域，最近邻可能隔了好几米，强行当做正确匹配会拖偏整个配准
small_gicp 通过 CorrespondenceRejector 模板参数在每次匹配时做"安检"------匹配点对必须通过检查才能参与线性化
DistanceRejector（默认）：最简单的策略------只保留距离小于阈值的匹配：

cpp 复制代码

// include/small_gicp/registration/rejector.hpp
struct DistanceRejector {
  double max_dist_sq = 1.0;  // 最大匹配距离平方（默认 1m² → 1m）

  template <typename Target, typename Source>
  bool operator()(const Target& target, const Source& source,
                  const Eigen::Isometry3d& T,
                  size_t target_index, size_t source_index,
                  double sq_dist) const {
    return sq_dist > max_dist_sq;  // true = 拒绝
  }
};

它在每次 linearize() 调用时介入------KD-tree 找到最近邻后先过 rejector：

cpp 复制代码

// 在 icp_factor.hpp 的 linearize() 中：
size_t k_index; double k_sq_dist;
if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist)
    || rejector(target, source, T, k_index, source_index, k_sq_dist)) {
  return false;  // 找不到最近邻 OR 距离超阈值 → 该 source 点被丢弃，不参与本次迭代
}

这个"安检"发生在 每次迭代、每个 source 点------随着位姿收敛，越来越多的匹配点对落入阈值内，内点数量逐渐增加
NullRejector：来者不拒，不做任何剔除。适合点云完全重叠、无离群点的理想场景
自定义 Rejector：可以实现更复杂的策略，例如结合法向量夹角、颜色/强度一致性、特征描述子距离（FPFH、SHOT 等）进行多维度外点剔除：

cpp 复制代码

struct MyRejector {
  double max_dist_sq = 1.0;
  double max_normal_angle_cos = 0.5;  // 法向量夹角余弦阈值（<0.5 → 夹角>60° → 拒绝）

  template <typename Target, typename Source>
  bool operator()(const Target& target, const Source& source,
                  const Eigen::Isometry3d& T,
                  size_t target_index, size_t source_index, double sq_dist) const {
    if (sq_dist > max_dist_sq) return true;  // 距离太远 → 拒绝
    // 还可以检查法向量一致性、颜色差异、特征距离...
    return false;  // 通过
  }
};

说人话：KD-tree 只会告诉你"谁最近"，但它不管"近得合不合理"。Rejector 就是那个把关的------隔太远的不要、法向量打架的不要、颜色差太多的不要...只让"靠谱"的匹配参与优化，配准精度才有保障

2-7 全局约束因子（GeneralFactor）

前六个小节覆盖了"数据准备"，第 3 章将介绍"优化器"，但中间还有一个环节：在求解之前，你可以向线性系统注入额外的先验约束
这就是 GeneralFactor------它在每轮迭代的线性系统 H δ = − b H\delta=-b Hδ=−b 求解之前被调用，可以修改 H H H、 b b b、 e e e，实现"软约束"
NullFactor（默认）：什么都不做，配准完全由数据驱动：

cpp 复制代码

// include/small_gicp/factors/general_factor.hpp
struct NullFactor {
  template <typename Target, typename Source, typename Tree>
  void update_linearized_system(const Target&, const Source&, const Tree&,
                                 const Eigen::Isometry3d&,
                                 Eigen::Matrix<double, 6, 6>* H,
                                 Eigen::Matrix<double, 6, 1>* b,
                                 double* e) const {}  // 空实现
};

RestrictDoFFactor：限制优化自由度。例如地面机器人只需估计 x, y, yaw，可以"软固定" roll, pitch, z：

cpp 复制代码

struct RestrictDoFFactor {
  double lambda = 1e9;                    // 约束强度（越大越"硬"）
  Eigen::Array<double, 6, 1> mask;       // [rx, ry, rz, tx, ty, tz], 1=活跃 0=固定

  void update_linearized_system(..., Eigen::Matrix<double, 6, 6>* H, ...) const {
    // 对 mask=0 的自由度，在 H 对角加一个巨大的惩罚项 λ
    *H += lambda * (mask - 1.0).abs().matrix().asDiagonal();
    // 等价于："这个自由度你给我老实待着别动"
  }
};

典型用法：

cpp 复制代码

Registration<GICPFactor, ParallelReductionOMP, RestrictDoFFactor> registration;
registration.general_factor.set_rotation_mask({0.0, 0.0, 1.0});   // 只优化 yaw
registration.general_factor.set_translation_mask({1.0, 1.0, 0.0}); // 只优化 x, y
registration.general_factor.lambda = 1e8;

GeneralFactor 的两个回调时机：
1. update_linearized_system()：在 H、b 归约完成后、求解 δ \delta δ 之前调用------此时修改 H、b 会直接影响增量方向
2. update_error()：在 LM 内循环验算误差时调用------如果你修改了误差项，需要在这里保持一致性

说人话：GeneralFactor 就像给优化器"戴上手铐"------你可以指定哪些自由度能动、哪些不能动（或者能动但很费劲）。没有它，配准是纯数据驱动；有了它，你可以把里程计、IMU 等先验信息注入进去，让优化在数据约束 + 先验约束的共同作用下找到最优解

3 优化器：Gauss-Newton 与 Levenberg-Marquardt

在正式推导 ICP 的 Jacobian 之前，我们需要先理解两个核心优化器------因为 ICP、Plane ICP、GICP、VGICP 全都用它们做优化，只是误差定义不同
Gauss-Newton（GN）是基础，收敛快但鲁棒性不足；Levenberg-Marquardt（LM）在 GN 上加自适应阻尼，是 small_gicp 的 默认优化器

3-1 介绍

Gauss-Newton（高斯-牛顿法） 是一种求解 非线性最小二乘 问题的迭代优化算法
它的核心思想只有一句话：把非线性的误差函数在当前点做一阶泰勒展开，把原问题变成线性最小二乘，解完更新，再展开------反复迭代直到收敛
和梯度下降的区别：梯度下降只用一阶信息（梯度），Gauss-Newton 用 Jacobian 近似了 Hessian（二阶信息），所以 收敛更快，但需要误差函数在最优解附近"比较线性"；如果初始值离最优解太远，GN 可能发散

说人话：梯度下降是"闭着眼睛往下走，走一步算一步"；Gauss-Newton 是"睁开眼看清脚下的曲率，算好步长再迈"------代价是每一步要多算一个 Jacobian 矩阵。但要是起点离谷底太远，"看清曲率"反而可能把路看岔了------这时候就需要 LM 来兜底

3-2 Gauss-Newton 原理

设我们有 N N N 个误差项 e i ( T ) e_i(T) ei(T)，总代价为 1 2 ∑ ∥ e i ( T ) ∥ 2 \frac{1}{2} \sum \|e_i(T)\|^2 21∑∥ei(T)∥2。 T T T 是我们想优化的变量（在点云配准中是一个 4 × 4 4 \times 4 4×4 位姿矩阵）
因为 T T T 含旋转，不能直接做加法（ R new = R + Δ R R_{\text{new}} = R + \Delta R Rnew=R+ΔR 会破坏正交性），我们在 T T T 处引入 6 维扰动 δ = $r x , r y , r z , t x , t y , t z$ T \delta = $r_x, r_y, r_z, t_x, t_y, t_z$ ^T δ= $rx,ry,rz,tx,ty,tz$ T，用李代数来参数化更新
四步迭代：
- 第 1 步：线性化 ------每个残差在 δ = 0 \delta = 0 δ=0 处一阶泰勒展开：
  e i ( δ ) ≈ e i ( 0 ) + J e i ⋅ δ , J e i = ∂ e i ∂ δ ∣ δ = 0 e_i(\delta) \approx e_i(0) + J_{e_i} \cdot \delta, \quad J_{e_i} = \left.\frac{\partial e_i}{\partial \delta}\right|_{\delta=0} ei(δ)≈ei(0)+Jei⋅δ,Jei=∂δ∂ei δ=0
- 第 2 步：构建正规方程 ------代入总误差，对 δ \delta δ 求导令为 0：
  ( ∑ J e i T J e i ) δ = − ∑ J e i T e i ⟹ H δ = − b (\sum J_{e_i}^T J_{e_i}) \delta = -\sum J_{e_i}^T e_i \quad\Longrightarrow\quad H \delta = -b (∑JeiTJei)δ=−∑JeiTei⟹Hδ=−b
- 第 3 步：求解增量 ------解 H δ = − b H \delta = -b Hδ=−b，得到最优增量 δ ∗ = − H − 1 b \delta^* = -H^{-1} b δ∗=−H−1b
- 第 4 步：更新 ------ T new = T ⋅ exp ⁡ ( δ ∧ ) T_{\text{new}} = T \cdot \exp(\delta^\wedge) Tnew=T⋅exp(δ∧)（详见 [3-6 节 se3_exp](#3-6 节 se3_exp)），然后回到第 1 步
Jacobian 的直觉： J e i J_{e_i} Jei 的每一列对应 δ \delta δ 的一个自由度，该列的模越大 = 残差对这个自由度越敏感 = 约束越强。如果某列趋近 0，说明该自由度在当前场景下几乎不可观------这是退化检测的核心

3-3 一维例子

我们用 指数曲线 来建立直觉：生成 20 个带噪声的观测点，真实模型为 y = exp ⁡ ( θ ⋅ x ) y = \exp(\theta \cdot x) y=exp(θ⋅x)（ θ true = 0.7 \theta_{\text{true}} = 0.7 θtrue=0.7），叠加高斯噪声 N ( 0 , 0.6 2 ) \mathcal{N}(0, 0.6^2) N(0,0.62)
模型 f ( x ; θ ) = exp ⁡ ( θ ⋅ x ) f(x; \theta) = \exp(\theta \cdot x) f(x;θ)=exp(θ⋅x)（关于 θ \theta θ 非线性 ------这才是真正的曲线拟合），误差 e i ( θ ) = exp ⁡ ( θ x i ) − y i e_i(\theta) = \exp(\theta x_i) - y_i ei(θ)=exp(θxi)−yi，Jacobian J i = ∂ e i ∂ θ = x i ⋅ exp ⁡ ( θ x i ) J_i = \frac{\partial e_i}{\partial \theta} = x_i \cdot \exp(\theta x_i) Ji=∂θ∂ei=xi⋅exp(θxi)
从 θ 0 = 0.3 \theta_0 = 0.3 θ0=0.3（故意偏离真实值 0.7，初始曲线"瘪"很多）开始迭代：
- 计算所有 20 个点的残差 e e e 和 Jacobian J J J
- 构建 H = J T J H = J^T J H=JTJ（1×1 标量）， b = − J T e b = -J^T e b=−JTe
- 解 Δ θ = b / H \Delta \theta = b / H Δθ=b/H，更新 θ new = θ + Δ θ \theta_{\text{new}} = \theta + \Delta \theta θnew=θ+Δθ
- 重复直到 Δ θ \Delta \theta Δθ 足够小------每一步的 J J J 和 e e e 都依赖当前 θ \theta θ，必须在每轮重新计算
这就是 Gauss-Newton------把弯的代价函数在当前点用抛物线近似，跳到抛物线最低点，到了新位置再重新近似

说人话：就像在一条弯弯曲曲的山路上，每一步你都假设脚下的路是抛物线形状（二次近似），朝"抛物线的最低点"迈一步，到了新位置重新拟合一条新抛物线，再迈------直到走到真正的谷底

上图展示了 真正的曲线拟合 （不是直线！）：绿色虚线是真实指数曲线 y = exp ⁡ ( 0.7 x ) y = \exp(0.7x) y=exp(0.7x)，灰色点是 20 个带噪声观测，彩色曲线是 GN 每次迭代的拟合结果------颜色从暗紫（初始，θ=0.3，曲线"趴着"）到亮黄（收敛，θ≈0.688，几乎与真实曲线重合），右侧颜色条标注了迭代步数的对应关系。左上角小图是代价收敛的半对数坐标------前 5 步就已经降了 2 个数量级

3-4 多维例子

现在推广到 m m m 维参数 θ ∈ R m \theta \in \mathbb{R}^m θ∈Rm， n n n 个观测（ n > m n > m n>m，超定）。以 3D 曲面拟合 为例（这是真正的曲面，不是曲线！）：模型 z = a x 2 + b y 2 z = a x^2 + b y^2 z=ax2+by2，参数 θ = a , b T ∈ R 2 \theta = a, b^T \in \mathbb{R}^2 θ=a,bT∈R2，50 个散落在 3D 空间中的观测点：
- 残差向量 e ( θ ) = $e 1 , ... , e n$ T e(\theta) = $e_1, \\dots, e_n$ ^T e(θ)= $e1,...,en$ T，其中 e i = a x i 2 + b y i 2 − z i e_i = a x_i^2 + b y_i^2 - z_i ei=axi2+byi2−zi（每个观测有 ( x i , y i , z i ) (x_i, y_i, z_i) (xi,yi,zi) 三个坐标）
- Jacobian 矩阵 J ( θ ) = ∂ e ∂ θ ∈ R n × 2 J(\theta) = \frac{\partial e}{\partial \theta} \in \mathbb{R}^{n \times 2} J(θ)=∂θ∂e∈Rn×2，第 i i i 行为 $x i 2 , y i 2$ $x_i\^2, \\, y_i\^2$ $xi2,yi2$ （对 a 的导数是 x 2 x^2 x2，对 b 的导数是 y 2 y^2 y2）
- 正规方程： J T J Δ θ = − J T e J^T J \Delta \theta = -J^T e JTJΔθ=−JTe（2×2 线性系统------50 个 3D 观测点，Hessian 仍然只是 2×2）
- 更新： θ new = θ + Δ θ \theta_{\text{new}} = \theta + \Delta \theta θnew=θ+Δθ，同时更新 a 和 b，曲面形状随之改变
关键洞察 ：在点云配准中：
- θ = δ = $r x , r y , r z , t x , t y , t z$ T \theta = \delta = $r_x, r_y, r_z, t_x, t_y, t_z$ ^T θ=δ= $rx,ry,rz,tx,ty,tz$ T（6 维位姿扰动）
- n n n = source 点的数量 × 残差维数（百万级 3D 点）
- J ∈ R ( 3 N ) × 6 J \in \mathbb{R}^{(3N) \times 6} J∈R(3N)×6， H ∈ R 6 × 6 H \in \mathbb{R}^{6 \times 6} H∈R6×6
- 不管 N N N 有多大， H H H 永远是 6×6------求解复杂度 O(1)！
这也是为什么点云配准特别适合 Gauss-Newton：参数空间极小（仅 6 维），观测极多（百万点），正规方程的规模永远可控

上图展示了 真正的 3D 曲面拟合 ：（主图）绿色半透明曲面是真实曲面 z = 2 x 2 + 1.5 y 2 z = 2x^2 + 1.5y^2 z=2x2+1.5y2，灰色点是 50 个带噪声的 3D 观测散点，彩色线框从暗紫（初始 a=0.3, b=0.2，曲面"扁塌"）渐变到金色（收敛 a≈2.08, b≈1.45，几乎与真实曲面重合）------这是真正的多维 Gauss-Newton：同时优化 2 个参数，拟合一个 3D 曲面；（右上小图）代价半对数坐标收敛------相比 1D 稍慢但仍是二次收敛

3-5 Levenberg-Marquardt 原理

GN 虽然收敛快，但有一个致命弱点：如果初始值离最优解太远，或者 H H H 接近奇异， δ \delta δ 可能算出巨大步长，直接飞出收敛域
Levenberg-Marquardt（LM） 在 GN 的正规方程里加了一个阻尼项 λ I \lambda I λI：

( H + λ I ) δ = − b (H + \lambda I) \delta = -b (H+λI)δ=−b

LM 的核心思想：在 GN 和梯度下降之间自适应切换
- λ → 0 \lambda \to 0 λ→0： ( H + λ I ) ≈ H (H + \lambda I) \approx H (H+λI)≈H，退化为 GN------步长大、收敛快（二次收敛），但可能不稳
- λ → ∞ \lambda \to \infty λ→∞： ( H + λ I ) ≈ λ I (H + \lambda I) \approx \lambda I (H+λI)≈λI， δ ≈ − 1 λ b \delta \approx -\frac{1}{\lambda} b δ≈−λ1b------退化为梯度下降，步长小、收敛慢但 保证下降
自适应策略------每轮迭代用一个内循环动态调整 λ \lambda λ：
- 第 1 步：试算 ------用当前的 λ \lambda λ 求解 ( H + λ I ) δ = − b (H + \lambda I)\delta = -b (H+λI)δ=−b，算出候选位姿 T new = T ⋅ exp ⁡ ( δ ) T_{\text{new}} = T \cdot \exp(\delta) Tnew=T⋅exp(δ)
- 第 2 步：验算 ------在新位姿下重新计算总误差 e new e_{\text{new}} enew
- 第 3 步：接受 / 拒绝 ：
  - 若 e new ≤ e e_{\text{new}} \leq e enew≤e（误差下降）→ 接受更新 ， λ ← λ / 10 \lambda \gets \lambda \,/\, 10 λ←λ/10（更"激进"，下一步更像 GN）
  - 若 e new > e e_{\text{new}} > e enew>e（误差上升）→ 拒绝更新 ， λ ← λ × 10 \lambda \gets \lambda \times 10 λ←λ×10（更"保守"，下一步更像梯度下降），回到第 1 步重试
- 第 4 步：收敛判断 ------ ∥ δ rot ∥ ≤ ε r \|\delta_{\text{rot}}\| \leq \varepsilon_r ∥δrot∥≤εr 且 ∥ δ trans ∥ ≤ ε t \|\delta_{\text{trans}}\| \leq \varepsilon_t ∥δtrans∥≤εt 时停止
内循环最多尝试 max_inner_iterations（默认 10）次------如果 10 次调整都找不到使误差下降的 λ \lambda λ，说明已经卡在局部极小点，放弃本轮

说人话：LM = GN + 安全绳。GN 每一步都"信任"二次近似，大步流星往下跳------近的时候跳得准，远的时候可能跳下悬崖。LM 每跳一步前先拿脚尖试探：踩实了（误差下降）就站稳，松一松绳子（ λ \lambda λ 变小），下一步跳大点；踩空了（误差上升）就缩回来，拉紧绳子（ λ \lambda λ 变大），下一步保守点。这种"试探-调整-再迈"的策略让它 无论初始值好坏都能稳定收敛

GN vs LM 对比：

特性	Gauss-Newton	Levenberg-Marquardt
正规方程	H δ = − b H\delta = -b Hδ=−b	( H + λ I ) δ = − b (H + \lambda I)\delta = -b (H+λI)δ=−b
收敛速度	二次收敛（快）	近 GN 时二次，远时线性
对初值敏感度	敏感，初值差可能发散	不敏感，总能收敛
每次迭代代价	1 次 LDLT 求解	内循环多次 LDLT 求解
适用场景	初值较准、问题良态	初值粗糙、鲁棒优先
`small_gicp` 中	`GaussNewtonOptimizer`	`LevenbergMarquardtOptimizer`（默认）

3-6 LDLT 分解：求解 H δ = − b H\delta = -b Hδ=−b

每轮迭代最后都要解一个 6 × 6 6 \times 6 6×6 线性系统 ( H + λ I ) δ = − b (H + \lambda I)\delta = -b (H+λI)δ=−b。对于这种 小规模对称正定矩阵，选什么求解方法直接影响速度和数值稳定性
三种常见方法对比：

方法	原理	优点	缺点
直接求逆 δ = − H − 1 b \delta = -H^{-1}b δ=−H−1b	算 H − 1 H^{-1} H−1 再乘 b b b	直观	数值不稳定、计算量大，从不用于生产
Cholesky H = L L T H = LL^T H=LLT	分解为下三角阵乘其转置	利用对称性，比 LU 快一倍	需要开平方根（`std::sqrt`），对角元可能为负时崩溃
LDLT H = L D L T H = LDL^T H=LDLT	分解为对角矩阵 D D D 夹在两个三角阵之间	不需要开平方根，比 Cholesky 更快更稳	H H H 必须正定（点云配准中 H H H 天然正定，无此顾虑）

为什么 LDLT 不需要开方？ Cholesky 的 l i i = h i i − ∑ k < i l i k 2 l_{ii} = \sqrt{h_{ii} - \sum_{k<i} l_{ik}^2} lii=hii−∑k<ilik2 ，每次算对角元都要 sqrt。LDLT 把对角元单独存到 D D D 里： d i i = h i i − ∑ k < i d k k l i k 2 d_{ii} = h_{ii} - \sum_{k<i} d_{kk} l_{ik}^2 dii=hii−∑k<idkklik2，不需要 sqrt。开方在浮点运算中既慢又容易引入舍入误差
为什么 H H H 天然正定？ H = J T W J H = J^T W J H=JTWJ（ W W W 为权重矩阵），对于任意非零向量 v v v， v T H v = v T J T W J v = ∥ W 1 / 2 J v ∥ 2 ≥ 0 v^T H v = v^T J^T W J v = \|W^{1/2} J v\|^2 \ge 0 vTHv=vTJTWJv=∥W1/2Jv∥2≥0，所以 H H H 是半正定的。加上 LM 的阻尼项 λ I \lambda I λI 后， H + λ I H + \lambda I H+λI 严格正定------LDLT 解 6×6 正定系统快速且稳定
Eigen 中的使用------一句代码完成 LDLT 求解：

cpp 复制代码

// include/small_gicp/registration/optimizer.hpp（GN 和 LM 中都是这一行）
const Eigen::Matrix<double, 6, 1> delta =
  (H + lambda * Eigen::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);

H.ldlt() 返回一个 Eigen::LDLT 对象（不计算分解，惰性求值），.solve(-b) 触发实际的 LDLT 分解 + 前代回代，一步到位
对于 6×6 矩阵，LDLT 的计算量约为 6 3 / 3 = 72 6^3/3 = 72 63/3=72 FLOP------在动辄百万点的计算量面前完全可忽略
这也是为什么 GN/LM 两种优化器都用同一行代码：线性系统求解这一步不需要变，变的只是怎么构建 H H H 和 b b b（也就是 Factor 的职责）

说人话： H H H 只有 6×6，不管用 LDLT、Cholesky 还是高斯消元，反正 CPU 都不到 1 微秒------但 LDLT 是这三个里最"优雅"的那个：不开方、不取逆、稳准快

3-7 small_gicp 中的实现

small_gicp 把两种优化器都封装在 include/small_gicp/registration/optimizer.hpp 中：
GaussNewtonOptimizer------四步迭代的直译：

cpp 复制代码

struct GaussNewtonOptimizer {
  template <typename Factor, typename Target, typename Source, typename Tree,
            typename Rejector, typename Reduction, typename GeneralFactor>
  RegistrationResult optimize(const Target& target, const Source& source, const Tree& tree,
                               const Eigen::Isometry3d& init_T, const Rejector& rejector,
                               Reduction& reduction, GeneralFactor& general_factor,
                               const TerminationCriteria& criteria) {
    RegistrationResult result(init_T);
    for (int i = 0; i < max_iterations; i++) {
      // 1. 线性化 + 归约：对每个 source 点计算 J_i, e_i → 累加 H, b
      auto [H, b, e] = reduction.linearize(target, source, tree, result.T_target_source, rejector, factors);
      general_factor.update_linearized_system(target, source, tree, result.T_target_source, &H, &b, &e);

      // 2. 求解正规方程（带固定小阻尼 1e-6 防奇异）
      const Eigen::Matrix<double, 6, 1> delta =
        (H + lambda * Eigen::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);

      // 3. 更新位姿
      result.T_target_source = result.T_target_source * se3_exp(delta);

      // 4. 收敛判断
      if (criteria.converged(delta)) break;
    }
    return result;
  }
};

LevenbergMarquardtOptimizer------在 GN 外套了一层自适应阻尼内循环：

cpp 复制代码

struct LevenbergMarquardtOptimizer {
  template <typename Factor, typename Target, typename Source, typename Tree,
            typename Rejector, typename Reduction, typename GeneralFactor>
  RegistrationResult optimize(const Target& target, const Source& source, const Tree& tree,
                               const Eigen::Isometry3d& init_T, const Rejector& rejector,
                               Reduction& reduction, GeneralFactor& general_factor,
                               const TerminationCriteria& criteria) {
    RegistrationResult result(init_T);
    double lambda = init_lambda;  // 初始阻尼因子
    for (int i = 0; i < max_iterations; i++) {
      // 1. 线性化 + 归约 + 全局约束
      auto [H, b, e] = reduction.linearize(target, source, tree, result.T_target_source, rejector, factors);
      general_factor.update_linearized_system(target, source, tree, result.T_target_source, &H, &b, &e);

      bool success = false;
      // 2. 内循环：自适应调整 lambda
      for (int j = 0; j < max_inner_iterations; j++) {
        const Eigen::Matrix<double, 6, 1> delta =
          (H + lambda * Eigen::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);
        const Eigen::Isometry3d new_T = result.T_target_source * se3_exp(delta);
        double new_e = reduction.error(target, source, new_T, factors);
        general_factor.update_error(target, source, new_T, &new_e);

        if (new_e <= e) {
          // 误差下降 → 接受更新，减小 lambda（更接近 GN）
          result.T_target_source = new_T;
          lambda /= lambda_factor;  // 默认 /10
          e = new_e;
          success = true;
          break;
        } else {
          // 误差上升 → 拒绝更新，增大 lambda（更接近梯度下降）
          lambda *= lambda_factor;  // 默认 *10
        }
      }
      if (!success) break;  // 内循环耗尽仍未下降 → 收敛

      // 3. 收敛判断
      if (criteria.converged(delta)) break;
    }
    return result;
  }
};

se3_exp：位姿更新 ------每次迭代算出的 δ \delta δ（6 维向量）需要通过 SE(3) 指数映射转回 4 × 4 4 \times 4 4×4 变换矩阵：

cpp 复制代码

// include/small_gicp/util/lie.hpp
inline Eigen::Isometry3d se3_exp(const Eigen::Matrix<double, 6, 1>& a) {
  const Eigen::Vector3d omega = a.head<3>();  // 旋转部分 [r_x, r_y, r_z]
  const double theta_sq = omega.dot(omega);
  const double theta = std::sqrt(theta_sq);
  Eigen::Isometry3d se3 = Eigen::Isometry3d::Identity();
  se3.linear() = so3_exp(omega).toRotationMatrix();  // SO(3) 指数映射（Rodrigues 公式）

  if (theta < 1e-10) {
    se3.translation() = se3.linear() * a.tail<3>();  // 小角度近似
  } else {
    const Eigen::Matrix3d Omega = skew(omega);
    const Eigen::Matrix3d V = Eigen::Matrix3d::Identity()
      + (1.0 - std::cos(theta)) / theta_sq * Omega
      + (theta - std::sin(theta)) / (theta_sq * theta) * Omega * Omega;
    se3.translation() = V * a.tail<3>();  // 精确公式
  }
  return se3;
}

这里的 SO(3) 指数映射代码取自 Sophus，是业界标准的 Rodrigues 公式实现
位姿更新就是右乘： T new = T ⋅ se3_exp ( δ ) T_{\text{new}} = T \cdot \text{se3\_exp}(\delta) Tnew=T⋅se3_exp(δ)------把 6 维"小扰动"变成 4 × 4 4 \times 4 4×4 矩阵，右乘到当前位姿上

Gauss-Newton 的通用性：只要你能定义误差函数 e i ( T ) e_i(T) ei(T) 并推导出它的 Jacobian J e i J_{e_i} Jei，GN/LM 就能帮你算出最优位姿 。ICP、Plane ICP、GICP、VGICP 的区别仅仅在于"误差和 Jacobian 怎么算"，整体的迭代框架完全一样------这也是 small_gicp 能用同一套模板代码覆盖所有算法的原因

4 Point-to-Point ICP

4-1 算法介绍

ICP（Iterative Closest Point）是最经典的配准算法，small_gicp 的实现极其精简，却蕴含了完整的数学推导

4-2 算法原理

理解了 Gauss-Newton 之后，ICP 要做的事就很清楚了------只需要定义好误差函数 e i ( T ) e_i(T) ei(T)，算出对应的 Jacobian：
对于 source 点云中的每个点 p i s p_i^s pis，ICP 的误差定义为变换后与最近 target 点 p j t p_j^t pjt 的欧氏距离：

e i = p j t − T p i s e_i = p_j^t - T p_i^s ei=pjt−Tpis

优化目标是最小化所有点的误差平方和：

E ( T ) = ∑ i 1 2 ∥ e i ∥ 2 2 = ∑ i 1 2 ( p j t − T p i s ) T ( p j t − T p i s ) E(T) = \sum_i \frac{1}{2}\|e_i\|^2_2 = \sum_i \frac{1}{2} (p_j^t - T p_i^s)^T (p_j^t - T p_i^s) E(T)=i∑21∥ei∥22=i∑21(pjt−Tpis)T(pjt−Tpis)

这是一个关于 T T T 的非线性最小二乘问题，其解法已在 [3 Gauss-Newton 最小二乘优化](#3 Gauss-Newton 最小二乘优化) 中详细说明------下面直接进入 ICP 的 Jacobian 推导
以防你忘记：
- T T T：从 source 到 target 的 4 × 4 4 \times 4 4×4 变换矩阵
- p i s p_i^s pis：source 点云中的第 i 个点（齐次坐标，第四维为 1）
- p j t p_j^t pjt：target 点云中的第 j 个点（最近邻，齐次坐标，第四维为 1）
Jacobian 矩阵推导：设 δ = $r x , r y , r z , t x , t y , t z$ T \delta = $r_x, r_y, r_z, t_x, t_y, t_z$ ^T δ= $rx,ry,rz,tx,ty,tz$ T 为 6 维扰动，变换后的 point 对 δ \delta δ 的导数：

J i = ∂ ( T p i s ) ∂ δ = $− R \[ p i s$ × R ] 4 × 6 J_i = \frac{\partial (T p_i^s)}{\partial \delta} = \begin{bmatrix} -R $p_i\^s$ {\times} & R \end{bmatrix}{4\times6} Ji=∂δ∂(Tpis)= $−R\[pis$ ×R]4×6

其中 $a$ × $a$ {\times} $a$ × 是 a a a 的反对称矩阵（skew-symmetric）， R $p i s$ × R $p_i\^s$ {\times} R $pis$ × 表示旋转矩阵 R R R 左乘反对称矩阵； R R R 对应平移部分的导数（RIGHT perturbation T ⋅ exp ⁡ ( δ ∧ ) T \cdot \exp(\delta^\wedge) T⋅exp(δ∧) 下，平移对 δ t \delta_t δt 的导数是 R R R）
注意：源码用 RIGHT perturbation（ T new = T ⋅ exp ⁡ ( δ ∧ ) T_{\text{new}} = T \cdot \exp(\delta^\wedge) Tnew=T⋅exp(δ∧)），所以平移部分的导数是 R R R 而非 I I I
则 residual 对 δ \delta δ 的 Jacobian 为：

J e i = ∂ e i ∂ δ = ∂ ( p j t − T p i s ) ∂ δ = − ∂ ( T p i s ) ∂ δ = $R \[ p i s$ × − R ] J_{e_i} = \frac{\partial e_i}{\partial \delta} = \frac{\partial (p_j^t - T p_i^s)}{\partial \delta} = -\frac{\partial (T p_i^s)}{\partial \delta} = \begin{bmatrix} R $p_i\^s$ _{\times} & -R \end{bmatrix} Jei=∂δ∂ei=∂δ∂(pjt−Tpis)=−∂δ∂(Tpis)= $R\[pis$ ×−R]

信息矩阵和梯度向量（Gauss-Newton 近似）：

H i = J e i T J e i , b i = J e i T e i , E i = 1 2 e i T e i H_i = J_{e_i}^T J_{e_i}, \quad b_i = J_{e_i}^T e_i, \quad E_i = \frac{1}{2}e_i^T e_i Hi=JeiTJei,bi=JeiTei,Ei=21eiTei

4-3 完整代码流程

代码实现（对照上面的公式）：

cpp 复制代码

// include/small_gicp/factors/icp_factor.hpp
template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector>
bool linearize(..., Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b, double* e) {
  // 1. 将 source 点变换到 target 坐标系
  const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index);

  // 2. 在 target 的 KD-tree 中找最近邻
  size_t k_index; double k_sq_dist;
  if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist)
      || rejector(target, source, T, k_index, source_index, k_sq_dist)) {
    return false;  // 找不到匹配点或距离超阈值 → 标记为外点
  }

  // 3. 计算残差 e = p_target - T * p_source
  const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt;

  // 4. 计算 Jacobian (4x6)
  //    J.block<3,3>(0,0) = T.linear() * skew(p_source.head<3>())  // 旋转部分
  //    J.block<3,3>(0,3) = -T.linear()                             // 平移部分
  Eigen::Matrix<double, 4, 6> J = Eigen::Matrix<double, 4, 6>::Zero();
  J.block<3, 3>(0, 0) = T.linear() * skew(traits::point(source, source_index).template head<3>());
  J.block<3, 3>(0, 3) = -T.linear();

  // 5. 构建信息矩阵 H(6x6) = J^T * J, 梯度 b(6x1) = J^T * e, 误差 e
  *H = J.transpose() * J;
  *b = J.transpose() * residual;
  *e = 0.5 * residual.squaredNorm();

  return true;
}

说人话：ICP 就是"把 source 每个点先旋转平移过去，在 target 里找离它最近的，然后算距离"。这个距离对所有 source 点求和就是我们要最小化的总误差。代码里的 linearize 函数做的事就是：给每个 source 点算出它对位姿 δ \delta δ 的 Jacobian，然后用 Gauss-Newton 把它累加进 H * delta = -b 这个 6x6 的线性系统中
一句话总结 ICP：最近点对之间欧氏距离的 Gauss-Newton 非线性最小二乘

5 Point-to-Plane ICP

5-1 算法介绍

Point-to-Point ICP 有一个致命问题：当表面是光滑平面时，点沿平面滑动的方向上没有任何约束------因为平面上随便一个点都是"最近点"
Point-to-Plane ICP 的解法是：不直接最小化点到点的距离，而是最小化点到目标平面（沿法向）的距离

5-2 算法原理

误差定义为：

e i = n j T ( p j t − T p i s ) e_i = n_j^T (p_j^t - T p_i^s) ei=njT(pjt−Tpis)

其中 n j n_j nj 是 target 点在 p j t p_j^t pjt 处的法向量

说人话：我们不再关心 source 点和 target 点在三维空间里差了多远，而是只关心 source 点 沿 target 点法向方向 偏移了多少------也就是"source 点到 target 局部平面的垂直距离"

Jacobian 的推导类似，但多了一项 n j T n_j^T njT 投影：

J e i = n j T ⋅ $R \[ p i s$ × − R ] J_{e_i} = n_j^T \cdot \begin{bmatrix} R $p_i\^s$ _{\times} & -R \end{bmatrix} Jei=njT⋅ $R\[pis$ ×−R]

由于 n j n_j nj 是 4 × 1 4 \times 1 4×1 向量（第四维为 0），这个操作等价于对原先的 4×6 Jacobian 矩阵按法向方向加权

5-3 完整代码流程

代码实现：

cpp 复制代码

// include/small_gicp/factors/plane_icp_factor.hpp
bool linearize(..., Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b, double* e) {
  const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index);

  size_t k_index; double k_sq_dist;
  if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist)
      || rejector(...)) {
    return false;
  }

  const auto& target_normal = traits::normal(target, target_index);  // target 点的法向量
  const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt;
  const Eigen::Vector4d err = target_normal.array() * residual.array();  // 逐元素乘法

  Eigen::Matrix<double, 4, 6> J = Eigen::Matrix<double, 4, 6>::Zero();
  // 用 target_normal 的 x,y,z 分量对 Jacobian 逐行加权
  J.block<3, 3>(0, 0) = target_normal.template head<3>().asDiagonal() * T.linear() * skew(...);
  J.block<3, 3>(0, 3) = target_normal.template head<3>().asDiagonal() * (-T.linear());

  *H = J.transpose() * J;
  *b = J.transpose() * err;
  *e = 0.5 * err.squaredNorm();

  return true;
}

注意代码里用了 .array() * .array()（逐元素乘法）而不是矩阵乘法------因为 residual 和 normal 都是 4 维向量， n j T ⋅ r e s i d u a l n_j^T \cdot residual njT⋅residual 就是每个分量相乘后再求和（通过 J 的 .asDiagonal() 加权实现）

对比维度	Point-to-Point ICP	Point-to-Plane ICP
误差定义	∣ p j t − T p i s ∣ 2 \|p_j^t - T p_i^s\|_2 ∣pjt−Tpis∣2	n j T ( p j t − T p i s ) n_j^T(p_j^t - T p_i^s) njT(pjt−Tpis)
Jacobian 形状	4×6	4×6（无法向加权）
对平面的收敛性	慢（沿平面有自由滑动方向）	快（法向约束消除了平面滑动）
需要额外信息	只需坐标	需要法向量
退化情况	平面场景收敛慢	曲率不足时可能退化

Point-to-Plane ICP 的核心思想：点云配准本质是一个平面匹配问题------我们只需要保证 source 点落在 target 的局部平面上，而不需要 exact 的点点对齐

6 GICP（核心）

6-1 算法介绍

GICP（Generalized ICP）是 small_gicp 最核心的算法，也是整个库的名字来源
它融合了 ICP 的简洁性和概率推断的鲁棒性：将点云建模为高斯分布，做分布到分布的匹配

6-2 算法原理

回忆一下协方差估计的输出：每个点都有一个 4 × 4 4 \times 4 4×4 的协方差矩阵 C i C_i Ci，描述了这个点在各方向上的不确定度
GICP 的误差定义为：

e i = p j t − T p i s e_i = p_j^t - T p_i^s ei=pjt−Tpis

这和 ICP 一模一样！区别在于 目标函数------残差被一个融合协方差矩阵加权：

E ( T ) = ∑ i 1 2 e i T ( C j t + T C i s T T ) 3 × 3 − 1 e i E(T) = \sum_i \frac{1}{2} e_i^T \left( C_j^t + T C_i^s T^T \right)^{-1}_{3\times3} e_i E(T)=i∑21eiT(Cjt+TCisTT)3×3−1ei

这里的 ( Σ i j ) − 1 (\Sigma_{ij})^{-1} (Σij)−1 就是 马氏距离（Mahalanobis distance） 的权重矩阵。马氏距离的定义：

d M ( p , q ) = ( p − q ) T Σ − 1 ( p − q ) d_M(p, q) = \sqrt{(p - q)^T \Sigma^{-1} (p - q)} dM(p,q)=(p−q)TΣ−1(p−q)

欧氏距离 d E ( p , q ) = ( p − q ) T I ( p − q ) d_E(p, q) = \sqrt{(p-q)^T I (p-q)} dE(p,q)=(p−q)TI(p−q) 把各个方向等同看待------差 1 米就是差 1 米
马氏距离则在 Σ − 1 \Sigma^{-1} Σ−1 的"拉伸"下，把各个方向的差异 按不确定度归一化 ------它衡量的是"差了几个标准差"，而不是"差了几米"
- 当 Σ = I \Sigma = I Σ=I（各向同性，方差为 1），马氏距离退化为欧氏距离------GICP 退化为 ICP
- 当 Σ \Sigma Σ 极度各向异性（法向量方向方差 → 0），马氏距离在法向方向上惩罚极重------GICP 趋近于 Point-to-Plane ICP

说人话：不确定度低的方向（协方差小）→ 权重高 → 差一点就要追究；不确定度高的方向（协方差大）→ 权重低 → 差很多也可以容忍。GICP 用马氏距离的权重矩阵来自动完成这个"区别对待"

以防你忘记：
- C j t C_j^t Cjt：target 第 j 个点的协方差矩阵（ 4 × 4 4 \times 4 4×4，只有左上 3 × 3 3 \times 3 3×3 有效）
- C i s C_i^s Cis：source 第 i 个点的协方差矩阵
- T C i s T T T C_i^s T^T TCisTT：将 source 协方差旋转到 target 坐标系下（协方差是坐标系相关的）
我们记 Σ i j = C j t + T C i s T T \Sigma_{ij} = C_j^t + T C_i^s T^T Σij=Cjt+TCisTT，取其左上 3 × 3 3 \times 3 3×3 块的逆作为马氏距离（Mahalanobis distance）的权重矩阵

6-3 完整代码流程

代码实现：

cpp 复制代码

// include/small_gicp/factors/gicp_factor.hpp
bool linearize(..., Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b, double* e) {
  const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index);

  // 最近邻搜索
  size_t k_index; double k_sq_dist;
  if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist)
      || rejector(...)) {
    return false;
  }

  // 1. 计算融合协方差 RCR = C_target + T * C_source * T^T
  const Eigen::Matrix4d RCR = traits::cov(target, target_index)
                            + T.matrix() * traits::cov(source, source_index) * T.matrix().transpose();

  // 2. 取左上 3x3 块的逆作为马氏权重矩阵
  mahalanobis.block<3, 3>(0, 0) = RCR.block<3, 3>(0, 0).inverse();

  const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt;

  // 3. Jacobian（和 ICP 一模一样）
  Eigen::Matrix<double, 4, 6> J = Eigen::Matrix<double, 4, 6>::Zero();
  J.block<3, 3>(0, 0) = T.linear() * skew(traits::point(source, source_index).template head<3>());
  J.block<3, 3>(0, 3) = -T.linear();

  // 4. 唯一的区别：H 和 b 都用 mahalanobis 矩阵加权了
  *H = J.transpose() * mahalanobis * J;
  *b = J.transpose() * mahalanobis * residual;
  *e = 0.5 * residual.transpose() * mahalanobis * residual;

  return true;
}

仔细对比 ICP 和 GICP 的代码，唯一的区别就是这三行：

	ICP	GICP
H	`J^T * I * J`	`J^T * mahalanobis * J`
b	`J^T * I * residual`	`J^T * mahalanobis * residual`
e	`0.5 * r^T * I * r`	`0.5 * r^T * mahalanobis * r`

其中 mahalanobis = (C_target + T*C_source*T^T)^{-1}_{3×3}

GICP 的本质：它退化到 ICP 还是退化到 Plane ICP，完全取决于协方差矩阵的形状！

我们来看协方差矩阵是如何让 GICP 自动"退化"的：

C i = R $λ 1 0 0 0 λ 2 0 0 0 λ 3$ R T C_i = R \begin{bmatrix} \lambda_1 & 0 & 0 \\ 0 & \lambda_2 & 0 \\ 0 & 0 & \lambda_3 \end{bmatrix} R^T Ci=R λ1000λ2000λ3 RT

特征值 λ 1 \lambda_1 λ1（最小）对应法向量方向， λ 2 , λ 3 \lambda_2, \lambda_3 λ2,λ3（较大）对应切平面方向
场景 1：所有协方差都设为单位矩阵
- C j t = C i s = I ⟹ Σ i j = I + T I T T = 2 I ⟹ Σ i j − 1 ∝ I C_j^t = C_i^s = I \implies \Sigma_{ij} = I + T I T^T = 2I \implies \Sigma_{ij}^{-1} \propto I Cjt=Cis=I⟹Σij=I+TITT=2I⟹Σij−1∝I
- 代入目标函数： ∑ 1 2 ( p j t − T p i s ) T I ( p j t − T p i s ) = ∑ 1 2 ∥ p j t − T p i s ∥ 2 \sum \frac{1}{2}(p_j^t - T p_i^s)^T I (p_j^t - T p_i^s) = \sum \frac{1}{2}\|p_j^t - T p_i^s\|^2 ∑21(pjt−Tpis)TI(pjt−Tpis)=∑21∥pjt−Tpis∥2
说人话：GICP 退化为 Point-to-Point ICP
场景 2：target 协方差设为法向极小、切向极大（即 target 被建模为高置信度的平面），source 设为单位矩阵
- C j t = R ⋅ diag ( 10 − 3 , 1 , 1 ) ⋅ R T C_j^t = R \cdot \text{diag}(10^{-3}, 1, 1) \cdot R^T Cjt=R⋅diag(10−3,1,1)⋅RT， C i s = I C_i^s = I Cis=I
- Σ i j − 1 ≈ R ⋅ diag ( 10 3 , 0.5 , 0.5 ) ⋅ R T \Sigma_{ij}^{-1} \approx R \cdot \text{diag}(10^3, 0.5, 0.5) \cdot R^T Σij−1≈R⋅diag(103,0.5,0.5)⋅RT（近似，因为融合后在法向方向权重极大）
- 代入目标函数：残差在法向方向上被极度放大，在切向方向上被抑制
说人话：GICP 退化为 Point-to-Plane ICP------只有法向方向的误差被惩罚
这就是 GICP 的精妙之处：它不需要你显式指定"用 ICP 还是 Plane ICP"，而是通过协方差矩阵的形状自动选择最合理的误差度量！

一个公式，一套代码，从 ICP 到 Plane ICP 全覆盖。这就是 GICP 被称为"Generalized"的原因

7 VGICP（最出名）

7-1 算法介绍

VGICP（Voxelized GICP）是 Koide 在 fast_gicp 时期提出的加速方案，也是 small_gicp 中对实际 SLAM 系统最有用的算法
在标准 GICP 中，每一步迭代都要：
- 对每个 source 点 → 在 target 的 KD-tree 中做最近邻搜索 → 复杂度 O ( N s log ⁡ N t ) O(N_s \log N_t) O(NslogNt)
- N t N_t Nt（target 点数）可能有数十万， N s N_s Ns（source 点数）也类似------每一步迭代的最近邻搜索都非常耗时
VGICP 的核心思路：用体素化把 target 点云压缩成少量"高斯体素"（Gaussian Voxel），每个体素存储其内部所有点的均值和协方差，从而把 target 点数从数十万降到数千

7-2 算法原理

数学上，VGICP 把同一体素内的 target 点聚合为一个分布：

μ v = 1 N v ∑ k ∈ v p k , C v = 1 N v ∑ k ∈ v C k \mu_v = \frac{1}{N_v} \sum_{k \in v} p_k, \quad C_v = \frac{1}{N_v} \sum_{k \in v} C_k μv=Nv1k∈v∑pk,Cv=Nv1k∈v∑Ck

其中 v v v 是体素索引， N v N_v Nv 是体素内的点数， C k C_k Ck 是每个 target 点自己的协方差
聚合后，每个体素作为一个"大分布"参与 GICP 配准，误差公式完全不变：

e i = μ v − T p i s e_i = \mu_v - T p_i^s ei=μv−Tpis

E = ∑ i 1 2 e i T ( C v + T C i s T T ) − 1 e i E = \sum_i \frac{1}{2} e_i^T \left( C_v + T C_i^s T^T \right)^{-1} e_i E=i∑21eiT(Cv+TCisTT)−1ei

说人话：VGICP = GICP + 把 target 体素化。原来要对几十万个 target 点做最近邻搜索，现在只需要对几千个高斯体素做搜索------精度几乎无损，速度提升 1-2 个数量级

7-3 完整代码流程

VGICP 使用时的关键代码路径：

cpp 复制代码

// src/small_gicp/registration/registration_helper.cpp --- align() 函数
if (setting.type == RegistrationSetting::VGICP) {
  // 先创建 GaussianVoxelMap（内部自动聚合均值和协方差）
  auto target_voxelmap = create_gaussian_voxelmap(*target_points, setting.voxel_resolution);
  // VGICP 仍然使用 GICPFactor，只是 target 换成了 GaussianVoxelMap
  // 同时 GaussianVoxelMap 自己就是它自己的 search 结构（通过 Traits 适配）
  return align(*target_voxelmap, *source_points, init_T, setting);
}

注意：VGICP 使用的 Factor 仍然是 GICPFactor------因为数学形式完全一样，区别仅在于 target 的数据结构从"稠密点云+KD-tree"变成了"稀疏体素+哈希表直接查找"

对比维度	GICP	VGICP
target 数据结构	点云 + KD-tree	高斯体素 + 哈希表
target "点数"	数十万	数千
最近邻搜索复杂度	O ( log ⁡ N t ) O(\log N_t) O(logNt)	O ( 1 ) O(1) O(1)（直接哈希查找）
target 协方差来源	每个点局部 kNN 估计	体素内点协方差的均值
单步迭代速度	中等	极快
内存占用	存 N_t 个点	存体素（远少于 N_t）

8 VGICP-CUDA（GPU 加速）

small_gicp 的 README 明确声明："GPU-based implementations are NOT included in this package."
也就是说，small_gicp 是一个纯 CPU 库，不包含任何 CUDA 代码
但是，VGICP 的 GPU 加速版本确实存在------就在它的前身 fast_gicp 仓库中，以及 Koide 后续维护的一些 SLAM 系统中

8-1 VGICP-CUDA 的核心思路

VGICP-CUDA 的数学原理与 VGICP 完全相同 ------都是将 target 体素化为高斯体素，然后用 GICPFactor 做分布到分布的配准
区别仅在于：将最耗时的两个步骤从 CPU 搬到 GPU 上并行执行：
- 体素化插入：CPU 上需要串行/少量并行遍历点云，GPU 上可以数千个线程同时处理
- 线性化 + 归约：CPU 上用 OMP/TBB 将 source 点的因子计算分发到 4-16 个线程，GPU 上可以分发到数千个 CUDA 核心
性能提升：在典型场景（KITTI 数据集，Velodyne HDL-64E 点云），VGICP-CUDA 比 CPU 版 VGICP 快约 5-10 倍

8-2 代码架构对比

CPU 版（small_gicp）的配准核心：

cpp 复制代码

// 并行归约：CPU 上用 OpenMP
Registration<GICPFactor, ParallelReductionOMP> registration;
registration.reduction.num_threads = 4;
auto result = registration.align(*target_voxelmap, *source, init_T);

GPU 版（fast_gicp 的 CUDA 分支）的核心思路：

cpp 复制代码

// 伪代码------展示 CUDA 版本的架构思路
// 1. 将 target 体素和 source 点上传到 GPU 显存
cudaMemcpy(d_voxels, h_voxels, ...);
cudaMemcpy(d_source, h_source, ...);

// 2. GPU kernel：每个线程处理一个 source 点
__global__ void linearize_kernel(...) {
  int i = blockIdx.x * blockDim.x + threadIdx.x;
  // 在 GPU 上做体素哈希查找 + 计算 (H_i, b_i, e_i)
}

// 3. GPU parallel reduction：将每个线程的结果归约成 6x6 系统
cub::DeviceReduce::Reduce(...);

8-3 如何获取

如果你需要 GPU 加速的 VGICP，可以从以下途径获取：
- fast_gicp 仓库的 CUDA 分支：https://github.com/SMRT-AIST/fast_gicp
- 自己基于 small_gicp 的 GICPFactor + cub::DeviceReduce 做移植------得益于模板架构，Factor 的定义可以直接复用，只需替换 Reduction 后端

VGICP-CUDA 的本质：VGICP 的数学形式完全不变，只是把 Reduction 后端从 CPU（OMP/TBB）换成了 GPU（CUDA Cores）

9 NDT（正态分布变换）

NDT（Normal Distributions Transform）是另一种经典的点云配准算法，最早由 Biber 和 Straßer 在 2003 年提出
small_gicp 不包含 NDT 实现，但它与 VGICP 的关系非常密切，理解二者的区别有助于在项目中选择正确的算法

9-1 NDT 的数学原理

NDT 同样需要先将 target 点云体素化，每个体素计算均值和协方差：

μ v = 1 N v ∑ k ∈ v p k , Σ v = 1 N v − 1 ∑ k ∈ v ( p k − μ v ) ( p k − μ v ) T \mu_v = \frac{1}{N_v} \sum_{k \in v} p_k, \quad \Sigma_v = \frac{1}{N_v-1} \sum_{k \in v} (p_k - \mu_v)(p_k - \mu_v)^T μv=Nv1k∈v∑pk,Σv=Nv−11k∈v∑(pk−μv)(pk−μv)T

注意这里 Σ v \Sigma_v Σv 使用的是 样本协方差 （除以 N v − 1 N_v-1 Nv−1），而 VGICP 是除以 N v N_v Nv（总体协方差）------这是第一个细微差别
NDT 把每个体素建模为一个高斯分布 N ( μ v , Σ v ) \mathcal{N}(\mu_v, \Sigma_v) N(μv,Σv)，然后定义 配准评分函数（Score Function）：

S ( T ) = ∑ i exp ⁡ ( − 1 2 ( T p i s − μ v ) T Σ v − 1 ( T p i s − μ v ) ) S(T) = \sum_i \exp\left(-\frac{1}{2} (T p_i^s - \mu_v)^T \Sigma_v^{-1} (T p_i^s - \mu_v)\right) S(T)=i∑exp(−21(Tpis−μv)TΣv−1(Tpis−μv))

说人话：NDT 不是在最小化"距离"，而是在最大化"概率"------对于每个变换后的 source 点，计算它落在对应 target 体素的高斯分布里的概率，然后对所有点的概率求和。概率越大，说明配准越好

优化目标是最大化 S ( T ) S(T) S(T)，等价于最小化 − S ( T ) -S(T) −S(T)。实际优化时，NDT 同样用 Gauss-Newton 或 LM 求解

9-2 NDT 与 VGICP 的对比

对比维度	VGICP	NDT
目标函数	马氏距离加权最小二乘	高斯混合似然最大化
残差形式	e T ( C v + T C s T T ) − 1 e e^T (C_v + T C_s T^T)^{-1} e eT(Cv+TCsTT)−1e	− exp ⁡ ( − 1 2 e T Σ v − 1 e ) -\exp(-\frac{1}{2} e^T \Sigma_v^{-1} e) −exp(−21eTΣv−1e)
协方差计算	体素内各点协方差的均值（ N v N_v Nv 除）	体素内点坐标的样本协方差（ N v − 1 N_v-1 Nv−1 除）
source 协方差	使用	不使用（只有 target 体素有协方差）
对离群点	较好（马氏距离天然有鲁棒性）	非常好（指数函数压缩了远距离的影响）
适用场景	结构丰富的城市/室内场景	稀疏场景、大尺度室外
实现位置	`small_gicp` / `fast_gicp`	PCL / `fast_gicp` / Open3D

9-3 如何选择

选 VGICP：当你的场景有丰富的几何结构（建筑、墙壁、道路），且需要高精度配准时
选 NDT：当场景比较稀疏（植被、旷野），或者存在大量动态物体（行人、车辆）导致很多离群匹配时------NDT 的指数评分函数天然对离群点不敏感

NDT 和 VGICP 是一对"远房表亲"------都做体素化+协方差，但优化目标不同：VGICP 是最小二乘范式，NDT 是最大似然范式。实践中两者常被一同提及，但 small_gicp 只实现了 VGICP 这条线

如果你需要 NDT，可以参考：
- PCL 的 pcl::NormalDistributionsTransform
- fast_gicp 仓库中的 NDT 变体
- Open3D 的 o3d.pipelines.registration

10 应用：API 使用指南与算法流程总结

前面 9 章从数学原理到源码实现逐层拆解了 small_gicp 的全部模块，本章回到"用户视角"------如果你要在自己的项目里用 small_gicp，有哪些 API 可以调用？每种 API 适合什么场景？最后再串一遍完整的算法流程

10-1 快速开始：一键配准

最简单的用法------你只有两堆原始点云和一个初始位姿，什么都不想管：

cpp 复制代码

#include <small_gicp/registration/registration_helper.hpp>

std::vector<Eigen::Vector4f> target_points = ...;  // N×4 (x, y, z, 1)
std::vector<Eigen::Vector4f> source_points = ...;

RegistrationSetting setting;
setting.type = RegistrationSetting::GICP;       // 选算法：ICP / PLANE_ICP / GICP / VGICP
setting.downsampling_resolution = 0.25;          // 降采样分辨率
setting.max_correspondence_distance = 1.0;       // 最大匹配距离（外点阈值）
setting.num_threads = 4;                         // 线程数
setting.max_iterations = 20;                     // 最大迭代次数

Eigen::Isometry3d init_T = Eigen::Isometry3d::Identity();
auto result = align(target_points, source_points, init_T, setting);

// result.T_target_source  → 最优变换矩阵
// result.converged         → 是否收敛
// result.iterations        → 实际迭代次数
// result.num_inliers       → 内点数
// result.H                 → 6×6 Hessian（信息矩阵）
// result.error             → 最终误差

align() 在内部自动完成了降采样 → KD-tree → 法向量/协方差 → 配准的全流程，适合快速原型和一次性配准

说人话：align() + RegistrationSetting 就是一个"一键配准"函数------把两坨点云和初始位姿扔进去，最优变换矩阵就出来了

10-2 分步 API：预处理与配准分离

如果需要 复用预处理结果（比如 target 是同一张地图帧，source 不断变化），就应该手动分步调用：

cpp 复制代码

#include <small_gicp/registration/registration_helper.hpp>

// ===== 第 1 步：预处理（只做一次）=====
auto [target, target_tree] = preprocess_points(
    target_points,
    0.25,   // downsampling_resolution
    10,     // num_neighbors（法向量/协方差估计的最近邻数）
    4       // num_threads
);
auto [source, source_tree] = preprocess_points(source_points, 0.25, 10, 4);

// ===== 第 2 步：配准（可多次执行）=====
RegistrationSetting setting;
setting.type = RegistrationSetting::GICP;
RegistrationResult result = align(*target, *source, *target_tree, init_T, setting);

preprocess_points() 返回两个东西：
- PointCloud::Ptr：降采样后的点云（每个点带有法向量和协方差）
- KdTree<PointCloud>::Ptr：KD-tree 索引，配准时的最近邻搜索靠它
如果想手动控制预处理每一步（比如用不同的降采样分辨率、不同的协方差估计参数）：

cpp 复制代码

#include <small_gicp/util/downsampling_omp.hpp>
#include <small_gicp/util/normal_estimation_omp.hpp>
#include <small_gicp/ann/kdtree_omp.hpp>

auto target = std::make_shared<PointCloud>(target_points);
target = voxelgrid_sampling_omp(*target, 0.25, 4);          // 降采样
auto target_tree = std::make_shared<KdTree<PointCloud>>(
    target, KdTreeBuilderOMP(4));                            // 构建 KD-tree
estimate_covariances_omp(*target, *target_tree, 10, 4);     // 估计协方差
// estimate_normals_omp() 如果只需要法向量

10-3 Registration 模板：完全自定义

当需要自定义误差因子、并行策略、外点剔除器、优化器时，直接用 Registration 模板类：

cpp 复制代码

#include <small_gicp/factors/gicp_factor.hpp>
#include <small_gicp/factors/plane_icp_factor.hpp>
#include <small_gicp/registration/reduction_omp.hpp>
#include <small_gicp/registration/registration.hpp>

// GICP + OpenMP 并行 + 自定义外点距离 + LM 优化器（默认）
Registration<GICPFactor, ParallelReductionOMP> registration;
registration.reduction.num_threads = 4;
registration.rejector.max_dist_sq = 1.0;          // 1m² 匹配距离阈值
registration.criteria.rotation_eps = 0.1 * M_PI / 180.0;  // 0.1° 收敛
registration.criteria.translation_eps = 1e-3;              // 1mm 收敛
registration.optimizer.max_iterations = 20;

auto result = registration.align(*target, *source, *target_tree, init_T);

五个模板参数（后三个有默认值）：

模板参数	含义	可选值
`PointFactor`	误差因子类型	`ICPFactor`, `PointToPlaneICPFactor`, `GICPFactor`
`Reduction`	并行归约策略	`ParallelReductionOMP`, `ParallelReductionTBB`, `SerialReduction`
`GeneralFactor`	全局约束（如固定 DOF）	`NullFactor`（默认），或自定义
`CorrespondenceRejector`	外点剔除	`DistanceRejector`（默认），或自定义
`Optimizer`	优化器	`LevenbergMarquardtOptimizer`（默认），`GaussNewtonOptimizer`

运行时配置（registration.xxx）：

配置项	类型	说明
`reduction.num_threads`	`int`	并行线程数
`rejector.max_dist_sq`	`double`	最大匹配距离平方（超过即外点）
`criteria.rotation_eps`	`double`	旋转收敛阈值 $rad$
`criteria.translation_eps`	`double`	平移收敛阈值 $m$
`optimizer.max_iterations`	`int`	最大迭代次数

10-4 Python 接口

small_gicp 提供 pybind11 绑定的 Python 包，API 风格和 C++ 一一对应：

python 复制代码

import numpy as np
import small_gicp

# === 一键配准 ===
target_np = np.random.rand(1000, 4)   # N×4 或 N×3 numpy 数组
source_np = np.random.rand(1000, 4)
result = small_gicp.align(target_np, source_np,
                          downsampling_resolution=0.25,
                          registration_type="GICP")
print(result.T_target_source)  # 4×4 变换矩阵
print(result.converged)        # 是否收敛

# === 分步预处理 ===
target, target_tree = small_gicp.preprocess_points(target_np,
                                                    downsampling_resolution=0.25)
source, source_tree = small_gicp.preprocess_points(source_np,
                                                    downsampling_resolution=0.25)
result = small_gicp.align(target, source, target_tree)

# === 手动控制每一步 ===
target = small_gicp.voxelgrid_sampling(target_np, 0.25)
target_tree = small_gicp.KdTree(target)
small_gicp.estimate_covariances(target, target_tree)

# === 读写点云 ===
cloud = small_gicp.read_ply("target.ply")
points_np = cloud.points()   # 导出为 N×4 numpy 数组

Python API 一览：

函数	说明
`small_gicp.align()`	一键配准（自动预处理 + 配准）
`small_gicp.preprocess_points()`	预处理（降采样 + KD-tree + 法向量/协方差）
`small_gicp.voxelgrid_sampling()`	仅降采样
`small_gicp.KdTree()`	构建 KD-tree
`small_gicp.estimate_covariances()`	估计协方差矩阵
`small_gicp.estimate_normals()`	仅估计法向量
`small_gicp.PointCloud()`	从 numpy 数组构造点云
`small_gicp.read_ply()`	读取 PLY 文件

10-5 自定义点云类型（Traits 机制）

small_gicp 最大的灵活性来自 Traits 机制------你可以定义自己的点云类型，只要实现对应的 getter/setter，就能直接喂给配准和预处理算法：

cpp 复制代码

struct MyPoint {
  std::array<double, 3> point;
  std::array<double, 3> normal;
  std::array<double, 36> features;  // 自定义特征（如 FPFH、SHOT）
};
using MyPointCloud = std::vector<MyPoint>;

namespace small_gicp { namespace traits {
template <>
struct Traits<MyPointCloud> {
  static size_t size(const MyPointCloud& pts) { return pts.size(); }
  static bool has_points(const MyPointCloud&) { return true; }
  static bool has_normals(const MyPointCloud&) { return true; }
  static Eigen::Vector4d point(const MyPointCloud& pts, size_t i) {
    const auto& p = pts[i].point;
    return Eigen::Vector4d(p[0], p[1], p[2], 1.0);
  }
  static Eigen::Vector4d normal(const MyPointCloud& pts, size_t i) {
    const auto& n = pts[i].normal;
    return Eigen::Vector4d(n[0], n[1], n[2], 0.0);
  }
  // 如果需要协方差（GICP/VGICP），还要实现 has_covs() 和 cov()
  // 如果要做预处理，还要实现 resize() / set_point() / set_normal()
};
}}

自定义最近邻搜索也是同样的套路------实现 knn_search() 或 nearest_neighbor_search()，就能替代 KD-tree
自定义外点剔除器：实现 operator()(target, source, T, target_idx, source_idx, sq_dist) → bool
自定义全局约束（GeneralFactor）：实现 update_linearized_system() 来注入先验约束（如固定 roll/pitch）

说人话：Traits 机制就是"适配器模式"------你不需要继承任何基类，只需给你的数据类型实现一套 getter/setter，small_gicp 就能"认识"你的数据格式。这个设计让库的适用范围从标准的 PointCloud 扩展到了任意自定义点类型

10-6 VGICP 体素化 API

使用 VGICP 时，不需要 KD-tree------用体素地图替代：

cpp 复制代码

#include <small_gicp/ann/gaussian_voxelmap.hpp>

// 创建高斯体素地图（O(1) 哈希查找替代 O(logN) KD-tree）
auto target_voxelmap = create_gaussian_voxelmap(*target, voxel_resolution);

// VGICP 配准
RegistrationSetting setting;
setting.type = RegistrationSetting::VGICP;
setting.voxel_resolution = 1.0;
auto result = align(*target_voxelmap, *source, init_T, setting);

IncrementalVoxelMap------支持动态插入 + LRU 淘汰的增量体素地图，适合 scan-to-model 场景（不断把新帧插入已有的局部子地图）：

cpp 复制代码

#include <small_gicp/ann/incremental_voxelmap.hpp>

// 创建支持增量更新的体素地图
auto inc_voxelmap = std::make_shared<IncrementalVoxelMap<GaussianVoxel>>(voxel_resolution);

// 逐帧插入（配准时每来一帧 source，配准后插入一次）
inc_voxelmap->insert(*source_frame, T_frame);
// LRU 自动淘汰长时间未被引用的体素

10-7 算法全流程总结

回顾整个 small_gicp 的端到端流程------从原始点云到最优位姿：

#mermaid-svg-WXvxuuaNzt9jhaOz{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-WXvxuuaNzt9jhaOz .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-WXvxuuaNzt9jhaOz .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-WXvxuuaNzt9jhaOz .error-icon{fill:#552222;}#mermaid-svg-WXvxuuaNzt9jhaOz .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-WXvxuuaNzt9jhaOz .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-WXvxuuaNzt9jhaOz .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-WXvxuuaNzt9jhaOz .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-WXvxuuaNzt9jhaOz .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-WXvxuuaNzt9jhaOz .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-WXvxuuaNzt9jhaOz .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-WXvxuuaNzt9jhaOz .marker{fill:#333333;stroke:#333333;}#mermaid-svg-WXvxuuaNzt9jhaOz .marker.cross{stroke:#333333;}#mermaid-svg-WXvxuuaNzt9jhaOz svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-WXvxuuaNzt9jhaOz p{margin:0;}#mermaid-svg-WXvxuuaNzt9jhaOz .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-WXvxuuaNzt9jhaOz .cluster-label text{fill:#333;}#mermaid-svg-WXvxuuaNzt9jhaOz .cluster-label span{color:#333;}#mermaid-svg-WXvxuuaNzt9jhaOz .cluster-label span p{background-color:transparent;}#mermaid-svg-WXvxuuaNzt9jhaOz .label text,#mermaid-svg-WXvxuuaNzt9jhaOz span{fill:#333;color:#333;}#mermaid-svg-WXvxuuaNzt9jhaOz .node rect,#mermaid-svg-WXvxuuaNzt9jhaOz .node circle,#mermaid-svg-WXvxuuaNzt9jhaOz .node ellipse,#mermaid-svg-WXvxuuaNzt9jhaOz .node polygon,#mermaid-svg-WXvxuuaNzt9jhaOz .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-WXvxuuaNzt9jhaOz .rough-node .label text,#mermaid-svg-WXvxuuaNzt9jhaOz .node .label text,#mermaid-svg-WXvxuuaNzt9jhaOz .image-shape .label,#mermaid-svg-WXvxuuaNzt9jhaOz .icon-shape .label{text-anchor:middle;}#mermaid-svg-WXvxuuaNzt9jhaOz .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-WXvxuuaNzt9jhaOz .rough-node .label,#mermaid-svg-WXvxuuaNzt9jhaOz .node .label,#mermaid-svg-WXvxuuaNzt9jhaOz .image-shape .label,#mermaid-svg-WXvxuuaNzt9jhaOz .icon-shape .label{text-align:center;}#mermaid-svg-WXvxuuaNzt9jhaOz .node.clickable{cursor:pointer;}#mermaid-svg-WXvxuuaNzt9jhaOz .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-WXvxuuaNzt9jhaOz .arrowheadPath{fill:#333333;}#mermaid-svg-WXvxuuaNzt9jhaOz .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-WXvxuuaNzt9jhaOz .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-WXvxuuaNzt9jhaOz .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-WXvxuuaNzt9jhaOz .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-WXvxuuaNzt9jhaOz .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-WXvxuuaNzt9jhaOz .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-WXvxuuaNzt9jhaOz .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-WXvxuuaNzt9jhaOz .cluster text{fill:#333;}#mermaid-svg-WXvxuuaNzt9jhaOz .cluster span{color:#333;}#mermaid-svg-WXvxuuaNzt9jhaOz div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-WXvxuuaNzt9jhaOz .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-WXvxuuaNzt9jhaOz rect.text{fill:none;stroke-width:0;}#mermaid-svg-WXvxuuaNzt9jhaOz .icon-shape,#mermaid-svg-WXvxuuaNzt9jhaOz .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-WXvxuuaNzt9jhaOz .icon-shape p,#mermaid-svg-WXvxuuaNzt9jhaOz .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-WXvxuuaNzt9jhaOz .icon-shape .label rect,#mermaid-svg-WXvxuuaNzt9jhaOz .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-WXvxuuaNzt9jhaOz .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-WXvxuuaNzt9jhaOz .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-WXvxuuaNzt9jhaOz :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 迭代优化循环（第 3~7 章）
预处理（第 2 章）
未收敛
收敛
输入

target 点云 + source 点云

初始 T（I 或里程计初值）
① VoxelGrid 降采样

减少点数，保留几何结构
② KD-tree 构建

O(logN) 最近邻加速
③ 特征分解估计协方差

每个点 → 法向量 + Σ(4×4)
④ 外点剔除（Rejector）

距离/法向量/特征过滤
⑤ 全局约束（GeneralFactor）

注入自由度/IMU 等先验
① 线性化：匹配 → e_i + J_i → (H_i, b_i)

ICP: e=p_s'-p_t | Plane: e=nᵀ(p_s'-p_t)

GICP: e=p_s'-p_t 权重Σ⁻¹ | VGICP: 体素化加速
② 归约：H=ΣH_i, b=Σb_i（并行/串行）
③ 求解：(H+λI)δ=-b → δ*（GN/LM + LDLT）
④ 更新：T ← T·se3_exp(δ)
⑤ 收敛判断：‖δ_rot‖<ε_r, ‖δ_trans‖<ε_t
输出

T_target_source：最优 4×4 变换

H：6×6 信息矩阵

converged / iterations / num_inliers / error

一句话总结 ：small_gicp 把点云配准拆成五个可替换的积木------误差怎么算 （Factor）、怎么并行 （Reduction）、怎么剔除误匹配 （Rejector）、怎么注入先验 （GeneralFactor）、怎么优化 （Optimizer）------你只需选好积木、塞进 Registration 模板，代码生成器（C++ 模板）自动为你编译出一套高效配准管线
四种 ICP 变体的选型指南：

场景	推荐算法	理由
初值很准、场景结构化	ICP	最简单，够用
平面多（墙面、地面）	Plane ICP	沿法向量投影，对平面结构更鲁棒
通用场景、追求精度	GICP	马氏距离加权，自动平衡点-点和点-面
实时性要求高、大量点云	VGICP	O(1) 体素查找，比 KD-tree 快一个量级
GPU 可用	VGICP-CUDA	5-10× 进一步加速

11 Gauss-Newton 可视化 Python 源码

本章提供第 3 章中 Gauss-Newton 一维和多维例子的完整 Python 代码，读者可直接运行生成配图

运行环境要求：pip install numpy matplotlib，脚本会自动检测系统中文字体

python 复制代码

"""
Gauss-Newton 可视化脚本
- 3-3: 一维例子（指数曲线拟合 y = exp(θ·x)）
- 3-4: 多维例子（3D 曲面拟合 z = a·x² + b·y²）
运行: python3 gauss_newton_vis.py
输出: gauss_newton_1d.png, gauss_newton_2d.png
"""
import sys, os

# ---- 修复 matplotlib 3.10.x 与系统 mpl_toolkits 的版本冲突 ----
for name in list(sys.modules.keys()):
    if name.startswith('mpl_toolkits'):
        del sys.modules[name]

import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import matplotlib.font_manager as fm

import mpl_toolkits
USER_MPL_TOOLKITS = os.path.expanduser('~/.local/lib/python3.10/site-packages/mpl_toolkits')
mpl_toolkits.__path__ = [USER_MPL_TOOLKITS]
for name in list(sys.modules.keys()):
    if 'mpl_toolkits' in name:
        del sys.modules[name]
import mpl_toolkits
mpl_toolkits.__path__ = [USER_MPL_TOOLKITS]

import numpy as np
import warnings
warnings.filterwarnings('ignore')


def setup_cjk_font():
    font_path = None; font_family = None
    for f in fm.fontManager.ttflist:
        if 'Noto Sans CJK SC' in f.name:
            font_path = f.fname; font_family = 'Noto Sans CJK SC'; break
    if not font_path:
        for f in fm.fontManager.ttflist:
            if 'Noto Sans CJK' in f.name:
                font_path = f.fname; font_family = f.name; break
    if font_path:
        fm.fontManager.addfont(font_path)
        plt.rcParams['font.sans-serif'] = [font_family, 'DejaVu Sans']
        plt.rcParams['font.family'] = 'sans-serif'
        print(f"CJK font: {font_family}")
    else:
        print("WARNING: no CJK font, Chinese may render as tofu")

setup_cjk_font()
plt.rcParams['font.size'] = 12
plt.rcParams['axes.unicode_minus'] = False


# ============================================================
# 3-3 一维 GN：指数曲线拟合 y = exp(θ·x)
# ============================================================
def plot_1d_gauss_newton():
    np.random.seed(42)
    theta_true = 0.7
    N = 20
    x_data = np.linspace(0.3, 4.0, N)
    y_true = np.exp(theta_true * x_data)
    y_data = y_true + np.random.normal(0, 0.6, N)

    # Gauss-Newton 迭代
    theta = 0.3
    thetas = [theta]; costs = []

    for it in range(15):
        exp_tx = np.exp(theta * x_data)
        e = exp_tx - y_data                  # 残差 e_i = exp(θ·x_i) - y_i
        J = x_data * exp_tx                   # Jacobian: d(exp(θx))/dθ = x·exp(θx)
        H = np.dot(J, J)                      # H = J^T J
        b = -np.dot(J, e)                     # b = -J^T e
        dtheta = b / H
        theta = theta + dtheta
        thetas.append(theta)
        costs.append(0.5 * np.sum(e**2))

    thetas = np.array(thetas); costs = np.array(costs)
    print(f"[3-3] θ_true={theta_true}, θ_0={thetas[0]:.3f}, final={thetas[-1]:.4f}")

    # 单张迭代图
    fig, ax = plt.subplots(figsize=(12, 7))
    x_fine = np.linspace(0.1, 4.3, 400)
    ax.plot(x_fine, np.exp(theta_true * x_fine), color='#27AE60', lw=2, ls='--',
            alpha=0.7, label=f'true: y=exp({theta_true}·x)')
    ax.scatter(x_data, y_data, c='#2C3E50', s=70, zorder=10, edgecolors='white', lw=0.8,
               label=f'{N} noisy points')

    cmap = plt.cm.plasma
    for j, idx in enumerate(range(len(thetas))):
        t = thetas[idx]; y_pred = np.exp(t * x_fine)
        frac = j/(len(thetas)-1); color = cmap(0.15+0.85*frac)
        lw = 0.6+1.8*frac; alpha = 0.25+0.75*frac
        if idx == 0:
            label = f'init (θ={t:.2f})'; ls = (0,(5,2))
        elif idx == len(thetas)-1:
            label = f'converged (θ={t:.4f})'; ls = '-'; lw = 2.5; alpha = 1.0
        elif idx <= 5:
            label = f'iter {idx}'; ls = '-'
        else:
            label = None; ls = '-'
        ax.plot(x_fine, y_pred, color=color, ls=ls, lw=lw, alpha=alpha, label=label)

    sm = plt.cm.ScalarMappable(cmap=cmap, norm=plt.Normalize(0, len(thetas)-1))
    cbar = plt.colorbar(sm, ax=ax, pad=0.02); cbar.set_label('Iteration', fontsize=11)

    inset = ax.inset_axes([0.58, 0.55, 0.38, 0.38])
    inset.semilogy(range(1, len(thetas)), costs, 'o-', color='#E74C3C', lw=2, ms=5,
                   markerfacecolor='white', markeredgewidth=1)
    inset.set_xlabel('Iter'); inset.set_ylabel('Cost (log)')
    inset.set_title('Cost convergence'); inset.grid(True, alpha=0.3, linestyle=':')
    inset.set_xticks(range(1, len(thetas), 3))

    ax.set_xlabel('x'); ax.set_ylabel('y')
    ax.set_title(f'Gauss-Newton: 1D Curve Fitting  y = exp(θ·x)', fontsize=14)
    ax.legend(fontsize=9, loc='upper left', ncol=2); ax.set_xlim(0.1, 4.3)
    plt.tight_layout()
    fig.savefig('gauss_newton_1d.png', dpi=150, bbox_inches='tight', facecolor='white')
    plt.close()
    print("[3-3] gauss_newton_1d.png saved")


# ============================================================
# 3-4 多维 GN：3D 曲面拟合 z = a·x² + b·y²
# ============================================================
def plot_2d_gauss_newton():
    np.random.seed(42)
    a_true, b_true = 2.0, 1.5          # true surface: z = 2x² + 1.5y²
    N = 50
    x_data = np.random.uniform(-2.2, 2.2, N)
    y_data = np.random.uniform(-2.2, 2.2, N)
    z_true = a_true*x_data**2 + b_true*y_data**2
    z_data = z_true + np.random.normal(0, 0.6, N)

    def cost(a, b):
        e = a*x_data**2 + b*y_data**2 - z_data
        return 0.5*np.sum(e**2)

    a, b = 0.3, 0.2                    # init far from (2.0, 1.5)
    path = [(a, b)]; cost_path = [cost(a, b)]

    for it in range(20):
        e = a*x_data**2 + b*y_data**2 - z_data
        J = np.column_stack([x_data**2, y_data**2])   # J_i = [x_i², y_i²]
        H = J.T @ J; rhs = -J.T @ e
        delta = np.linalg.solve(H, rhs)
        a += delta[0]; b += delta[1]
        path.append((a, b)); cost_path.append(cost(a, b))

    path = np.array(path); cost_path = np.array(cost_path)
    print(f"[3-4] (a,b)_true=({a_true},{b_true}), init=({path[0,0]:.1f},{path[0,1]:.1f}), final=({path[-1,0]:.3f},{path[-1,1]:.3f})")

    # 3D 曲面拟合图
    fig = plt.figure(figsize=(14, 9))
    ax = fig.add_subplot(111, projection='3d')

    xg, yg = np.meshgrid(np.linspace(-2.5, 2.5, 35), np.linspace(-2.5, 2.5, 35))
    zg_true = a_true*xg**2 + b_true*yg**2
    ax.plot_surface(xg, yg, zg_true, color='#27AE60', alpha=0.15, edgecolor='none')

    ax.scatter(x_data, y_data, z_data, c='#2C3E50', s=30, zorder=10,
               edgecolors='white', lw=0.2, alpha=0.9, label=f'{N} noisy 3D points')

    cmap = plt.cm.plasma
    show_iters = [0, 1, 2, 3, 5, 8, 12, len(path)-1]
    for j, idx in enumerate(show_iters):
        a_i, b_i = path[idx, 0], path[idx, 1]
        zg_fit = a_i*xg**2 + b_i*yg**2
        frac = j/(len(show_iters)-1)
        if idx == 0:
            label = f'init (a={a_i:.1f}, b={b_i:.1f})'
            color = cmap(0.2); alpha = 0.25; lw = 0.2
        elif idx == len(path)-1:
            label = f'converged (a={a_i:.3f}, b={b_i:.3f})'
            color = '#FFD700'; alpha = 1.0; lw = 0.8
        else:
            label = f'iter {idx}' if j <= 4 else None
            color = cmap(0.2+0.8*frac); alpha = 0.25+0.75*frac; lw = 0.2+0.7*frac
        ax.plot_wireframe(xg, yg, zg_fit, color=color, alpha=alpha, lw=lw,
                          rstride=2, cstride=2, label=label)

    ax.set_xlabel('x'); ax.set_ylabel('y'); ax.set_zlabel('z')
    ax.set_title(f'Gauss-Newton: 3D Surface Fitting  z = a·x² + b·y²', fontsize=15, pad=25)
    ax.legend(fontsize=8, loc='upper left')
    ax.view_init(elev=22, azim=-55)

    from mpl_toolkits.axes_grid1.inset_locator import inset_axes
    inset = inset_axes(ax, width='28%', height='22%', loc='upper right',
                       bbox_to_anchor=(0.02, -0.02, 1, 1), bbox_transform=ax.transAxes)
    inset.semilogy(range(1, len(cost_path)+1), cost_path, 'o-', color='#E74C3C', lw=2, ms=4,
                   markerfacecolor='white', markeredgewidth=1)
    inset.set_xlabel('Iter'); inset.set_ylabel('Cost (log)')
    inset.set_title('Cost convergence'); inset.grid(True, alpha=0.3, linestyle=':')
    inset.set_xticks(range(1, len(cost_path)+1, 5))

    plt.tight_layout()
    fig.savefig('gauss_newton_2d.png', dpi=150, bbox_inches='tight', facecolor='white')
    plt.close()
    print("[3-4] gauss_newton_2d.png saved")


if __name__ == '__main__':
    plot_1d_gauss_newton()
    plot_2d_gauss_newton()
    print("All done!")

这段代码的核心价值：把 3-2 节的四步迭代公式翻译成了可执行的代码 ------你可以逐行对照公式和代码，看到 H = J T J H=J^T J H=JTJ、 Δ θ = H − 1 b \Delta\theta=H^{-1}b Δθ=H−1b、 θ new = θ + Δ θ \theta_{\text{new}}=\theta+\Delta\theta θnew=θ+Δθ 是怎样变成 Python 语句的

总结

本文从源码出发，完整解析了 small_gicp 的点云配准算法体系------从 VoxelGrid 降采样、KD-tree 构建、法向量与协方差估计三个预处理模块，到 OpenMP/TBB 并行后端，再到 ICP、Point-to-Plane ICP、GICP、VGICP 四种 CPU 配准算法，以及 VGICP-CUDA 和 NDT 的对比补充，最后到 Gauss-Newton/LM 优化器和完整配准流水线
核心要点回顾：
- ICP：最基础的 Point-to-Point 距离最小化，Jacobian 只有平移和旋转两部分
- Plane ICP：将误差投影到法向量方向，利用局部平面结构加速收敛
- GICP ：通过协方差矩阵将误差加权，统一了 ICP 和 Plane ICP------协方差为 I I I 时退化到 ICP，协方差为各向异性时退化到 Plane ICP
- VGICP ：用高斯体素聚合 target 点云，将最近邻搜索从 O ( log ⁡ N ) O(\log N) O(logN) 降为 O ( 1 ) O(1) O(1)，大幅加速配准
- VGICP-CUDA ：VGICP 的 GPU 加速版（在 fast_gicp 中），将归约后端从 CPU 换到 CUDA 核心，速度再提升 5-10 倍
- NDT：另一种体素化+协方差的配准算法，优化目标为高斯混合似然最大化（而非加权最小二乘），对离群点更鲁棒
- OMP vs TBB：两种并行后端------OMP 开箱即用适合 4-16 线程，TBB 可定制适合 128+ 线程高扩展场景
- Traits 解耦：整个库通过 C++ Traits 机制将点云类型、因子类型、归约策略、优化器全部解耦，用户可自由组合
small_gicp 是 SLAM 后端的核心组件之一，配合上一期的 Scan Context 回环检测，就构成了一个完整的 SLAM 后端：回环检测 → 回环约束的 6-DOF 配准 → 图优化
如有错误，欢迎指出！
感谢观看！