马上注册，结交更多好友，享用更多功能，让你轻松玩转社区。
您需要 登录 才可以下载或查看，没有账号？立即注册
x
一、算法概述
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
对比项NDTICP模子方式概率分布（高斯）最近点对优化目标最大似然估计点对隔断最小收敛半径较大，鲁棒性强对初始值敏感可导性连续、可微须要最近邻离散搜刮实现复杂度中高低
七、体现图（可视理解）
- 将目标点云转换为栅格地图（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 利用了体素网格 + 高斯概率分布建模，使得点云配准具备以下长处：
- 对初始姿态误差鲁棒
- 可导目标函数，利于快速优化
- 支持稀疏点云、动态场景（共同滤波）
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。 |
