点云配准算法之NDT算法原理详解

一、算法概述

NDT（Normal Distributions Transform）最初用于2D激光雷达地图构建（Biber & Straßer, 2003），后扩展为3D点云配准。它将点云数据空间划分为网格单元（Voxel），在每个体素中拟合一个高斯分布，用此概率模型对点进行匹配优化。

与 ICP 不同，NDT 是一个概率模型配准算法，具有更强的鲁棒性，适合处理稀疏/局部不一致的点云。

---

二、核心思想

• 目标点云（target map）中的每个体素内，使用包含的点拟合高斯分布。
• 源点云 （source cloud）中的点通过变换 ( T ) ( T ) (T) 后，落入目标空间中对应体素。
• 对每个变换后点 ( x ) ( x ) (x)，在该体素对应的高斯模型上计算似然概率。
• 使用最大似然估计或最小负对数似然，优化变换参数。

---

三、数学建模与公式推导

1️高斯分布建模（每个体素）

每个体素 ( V i ) ( V_i ) (Vi) 中的点 ( { x j } ) ( \{x_j\} ) ({xj}) 用一个多维高斯分布拟合：

μ i = 1 n ∑ j = 1 n x j , Σ i = 1 n − 1 ∑ j = 1 n ( x j − μ i ) ( x j − μ i ) T \] \[ \\mu_i = \\frac{1}{n} \\sum_{j=1}\^{n} x_j,\\quad \\Sigma_i = \\frac{1}{n-1} \\sum_{j=1}\^{n} (x_j - \\mu_i)(x_j - \\mu_i)\^T \] \[μi=n1j=1∑nxj,Σi=n−11j=1∑n(xj−μi)(xj−μi)T

其中：

• ( μ i ) ( \mu_i ) (μi)：均值向量
• ( Σ i ) ( \Sigma_i ) (Σi)：协方差矩阵

---

2️目标函数构建（最大似然）

给定一个源点 ( p ) ( p ) (p)，变换后为 ( x = T ( p ) ) ( x = T(p) ) (x=T(p))，其在某个体素内，拟合的高斯分布概率密度为：

P ( x ) = 1 ( 2 π ) d / 2 ∣ Σ ∣ 1 / 2 exp ⁡ ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) \] \[ P(x) = \\frac{1}{(2\\pi)\^{d/2} \|\\Sigma\|\^{1/2}} \\exp\\left( -\\frac{1}{2}(x - \\mu)\^T \\Sigma\^{-1}(x - \\mu) \\right) \] \[P(x)=(2π)d/2∣Σ∣1/21exp(−21(x−μ)TΣ−1(x−μ))

我们选择最大化所有点的概率密度（最大似然），等价于最小化负对数似然 ：

E ( T ) = ∑ x ∈ T ( P ) 1 2 ( x − μ ) T Σ − 1 ( x − μ ) \] \[ \\mathcal{E}(T) = \\sum_{x \\in T(P)} \\frac{1}{2}(x - \\mu)\^T \\Sigma\^{-1}(x - \\mu) \] \[E(T)=x∈T(P)∑21(x−μ)TΣ−1(x−μ)

（去掉常数项后的负对数似然）

---

四、优化过程

我们要最小化目标函数 ( E ( T ) ) ( \mathcal{E}(T) ) (E(T))，对变换参数（旋转和平移）进行优化。

记：

• 源点 ( p ) ( p ) (p)
• 当前变换 ( x = T ( p ; θ ) ) ( x = T(p; \theta) ) (x=T(p;θ))
• ( θ ) ( \theta ) (θ) 是 6 维参数向量（3 平移 + 3 欧拉角/李代数）

采用高斯牛顿法或牛顿法迭代优化：

梯度：

∇ E = ∑ i J i T Σ i − 1 ( x i − μ i ) \] \[ \\nabla \\mathcal{E} = \\sum_{i} J_i\^T \\Sigma_i\^{-1}(x_i - \\mu_i) \] \[∇E=i∑JiTΣi−1(xi−μi)

其中 ( J i = ∂ x i ∂ θ ) ( J_i = \frac{\partial x_i}{\partial \theta} ) (Ji=∂θ∂xi)：变换后的点对参数的雅可比。

海塞矩阵（近似）：

H = ∑ i J i T Σ i − 1 J i \] \[ H = \\sum_{i} J_i\^T \\Sigma_i\^{-1} J_i \] \[H=i∑JiTΣi−1Ji

然后迭代更新：

θ k + 1 = θ k − H − 1 ∇ E \] \[ \\theta_{k+1} = \\theta_k - H\^{-1} \\nabla \\mathcal{E} \] \[θk+1=θk−H−1∇E

可使用李代数 ( s e ( 3 ) ) (\mathfrak{se}(3)) (se(3)) 表达变换更稳定。

---

五、变换模型：SE(3)

为了数值稳定和避免欧拉角奇异性，NDT 实际实现常用李代数参数化变换：

T ( ξ ) = exp ⁡ ( ξ ∧ ) \] \[ T(\\xi) = \\exp(\\xi\^\\wedge) \] \[T(ξ)=exp(ξ∧)

其中：

• ( ξ ∈ R 6 ) ( \xi \in \mathbb{R}^6 ) (ξ∈R6)：李代数（3 旋转 + 3 平移）
• ( ξ ∧ ) ( \xi^\wedge ) (ξ∧)：向量到矩阵的帽运算
• ( exp ⁡ ) ( \exp ) (exp)：李群指数映射

更新形式：

ξ k + 1 = ξ k − H − 1 ∇ E \] \[ \\xi_{k+1} = \\xi_k - H\^{-1} \\nabla \\mathcal{E} \] \[ξk+1=ξk−H−1∇E

最终变换为 ( T = exp ⁡ ( ξ ∧ ) ) ( T = \exp(\xi^\wedge) ) (T=exp(ξ∧))

---

六、NDT vs ICP

对比项	NDT	ICP
模型方式	概率分布（高斯）	最近点对
优化目标	最大似然估计	点对距离最小
收敛半径	较大，鲁棒性强	对初始值敏感
可导性	连续、可微	需要最近邻离散搜索
实现复杂度	中高	低

---

七、示意图（可视理解）

• 将目标点云转换为栅格地图（VoxelGrid）
• 每个体素内构建高斯模型
• 源点经过变换后投影到高斯模型空间，优化拟合度

---

八、变体与优化

• 3D NDT（PCL 中的实现）
• NDT-OM（Occupancy Mapping）
• Fast NDT：稀疏点、高效矩阵更新
• Robust NDT：加入鲁棒核函数（Huber/Loss）

---

九、PCL 中的 NDT 使用（C++）

#include <pcl/registration/ndt.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("target.pcd", *target);
    pcl::io::loadPCDFile("source.pcd", *source);

    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setTransformationEpsilon(0.01);
    ndt.setStepSize(0.1);
    ndt.setResolution(1.0);
    ndt.setMaximumIterations(35);
    ndt.setInputSource(source);
    ndt.setInputTarget(target);

    pcl::PointCloud<pcl::PointXYZ> aligned;
    ndt.align(aligned);

    std::cout << "Has converged: " << ndt.hasConverged()
              << " score: " << ndt.getFitnessScore() << std::endl;
}

---

十、Open3D 中的 NDT 使用（Python）

import open3d as o3d

source = o3d.io.read_point_cloud("source.pcd")
target = o3d.io.read_point_cloud("target.pcd")

trans_init = np.eye(4)
result = o3d.pipelines.registration.registration_ndt(
    source, target, max_distance=1.0, init=trans_init,
    criteria=o3d.pipelines.registration.RegistrationCriterion()
)

print("Transformation:\n", result.transformation)

十一、 总结

NDT 利用了体素网格 + 高斯概率分布建模，使得点云配准具备以下优点：

• 对初始姿态误差鲁棒
• 可导目标函数，利于快速优化
• 支持稀疏点云、动态场景（配合滤波）