当前位置: 首页> 教育> 幼教 > 如何自己免费建网站_杭州十大互联网公司排名_seo服务外包公司_宣传软文

如何自己免费建网站_杭州十大互联网公司排名_seo服务外包公司_宣传软文

时间:2025/7/11 20:01:00来源:https://blog.csdn.net/qq_36812406/article/details/147474376 浏览次数:0次
如何自己免费建网站_杭州十大互联网公司排名_seo服务外包公司_宣传软文

一、算法概述

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=1nxj,Σi=n11j=1n(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)=xT(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=iJiTΣi1(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=iJiTΣi1Ji]
然后迭代更新:
[ θ k + 1 = θ k − H − 1 ∇ E ] [ \theta_{k+1} = \theta_k - H^{-1} \nabla \mathcal{E} ] [θk+1=θkH1E]
可使用李代数 ( 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=ξkH1E]
最终变换为 ( 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 o3dsource = 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 利用了体素网格 + 高斯概率分布建模,使得点云配准具备以下优点:

  • 对初始姿态误差鲁棒
  • 可导目标函数,利于快速优化
  • 支持稀疏点云、动态场景(配合滤波)
关键字:如何自己免费建网站_杭州十大互联网公司排名_seo服务外包公司_宣传软文

版权声明:

本网仅为发布的内容提供存储空间,不对发表、转载的内容提供任何形式的保证。凡本网注明“来源:XXX网络”的作品,均转载自其它媒体,著作权归作者所有,商业转载请联系作者获得授权,非商业转载请注明出处。

我们尊重并感谢每一位作者,均已注明文章来源和作者。如因作品内容、版权或其它问题,请及时与我们联系,联系邮箱:809451989@qq.com,投稿邮箱:809451989@qq.com

责任编辑: