上一篇聊了ICPICP精度高但计算量大特别是在大规模点云场景下逐点找最近邻的开销很可观。在自动驾驶和大型机器人项目中点云动辄几十万点ICP的速度就成了瓶颈。NDTNormal Distributions Transform正态分布变换提供了一种不需要找对应点的配准方案在大规模场景下效率比ICP高很多。NDT最早是Peter Biber在2003年提出的后来被Martin Magnusson在博士论文中系统化并开源到PCL库中。NDT的核心思想很直觉不逐点匹配而是把空间划分成体素每个体素用一个高斯分布来描述配准就变成了两个概率分布的匹配问题。这种从点对点到分布对分布的转变是NDT相比ICP的根本区别。NDT的数学原理NDT把目标点云所在的空间划分成等大小的体素格子。对于每个体素统计落在其中的所有点计算这些点的均值和协方差矩阵构成一个三维高斯分布。如果体素中的点太少比如少于3个协方差矩阵不可靠这个体素就会被忽略。# NDT体素化目标点云 def build_ndt_map(points, voxel_size1.0): voxels voxelize(points, voxel_size) ndt_map {} for key, pts in voxels.items(): if len(pts) 5: mean np.mean(pts, axis0) cov np.cov(pts.T) 1e-6 * np.eye(3) ndt_map[key] (mean, cov) return ndt_map配准时对于source点云中的每个点先经过当前估计的变换T找到它落在哪个体素中然后计算这个点在该体素高斯分布下的概率密度。所有点的概率密度之和就是总似然。优化目标就是找到一个变换T使得总似然最大。数学上NDT的优化目标函数是score(T) Σ exp(-0.5 * (Tp_i - μ_j)^T * Σ_j^{-1} * (Tp_i - μ_j))。其中p_i是source点μ_j和Σ_j是p_i所在体素的均值和协方差。这个目标函数可以用牛顿法或者高斯-牛顿法来优化。体素分辨率的选择体素分辨率是NDT最关键的一个参数直接决定了配准的精度和速度。分辨率太大比如5米一个体素覆盖了很大一片区域高斯分布把很多不同几何结构的点混在一起描述能力很差配准精度低。分辨率太小比如0.1米大部分体素中只有0个或1个点构不成高斯分布NDT退化为找不到足够的有效体素。经验值是这样的室内场景用0.5-1米室外场景用1-2米自动驾驶场景用2-5米。具体要看点云密度和场景特征。场景特征越丰富比如有很多角、边缘、平面变化分辨率可以设小一点场景越平坦比如大草原、空旷停车场分辨率要设大一点。# 不同场景的NDT分辨率经验值 scenarios { indoor_office: 0.5, # 室内办公室 indoor_warehouse: 1.0, # 室内仓库 outdoor_urban: 2.0, # 室外城市 outdoor_highway: 5.0, # 室外高速公路 }还有一种自适应分辨率的做法根据点云密度自动调整体素大小。密集区域用小体素稀疏区域用大体素。这种方法更灵活但实现复杂度也更高。PCL中的NDT是固定分辨率的Autoware中有自适应版本的实现。3D NDT和2D NDT3D NDT处理三维点云每个体素用一个三维高斯分布表示有3个均值参数和6个独立的协方差参数协方差矩阵是对称的。优化变量是6自由度的刚体变换3个平移3个旋转。2D NDT处理二维激光扫描数据每个格子用一个二维高斯分布表示。优化变量是3自由度2个平移1个旋转。2D NDT在移动机器人室内建图中用得很多比如ROS的slam_karto中就用了类似的思想。2D NDT的计算量比3D小得多因为体素格子数量少高斯分布的维度也低。但2D NDT只能处理平面场景对于有多层结构的3D场景就无能为力了。实际项目中2D激光雷达配准用2D NDT就够了3D激光雷达配准用3D NDT。如果3D点云数量太大也可以先投影到2D做粗配准再用3D NDT做精配准。多分辨率NDT是一种实用的加速策略。先在大体素比如4米下做粗配准快速收敛到大致正确的位姿然后逐步减小体素大小做精配准。这种策略既保证了收敛范围大分辨率的basin of attraction更大又保证了最终精度小分辨率的描述更精细。Autoware的ndt_matching模块就支持多分辨率设置。NDT在SLAM中的应用NDT在SLAM中有两个主要应用场景帧间配准和回环验证。帧间配准是用NDT对齐相邻帧的点云估计相对位姿。这个过程替代了ICP的位置作为前端里程计的核心。HDL-Graph-SLAM和Autoware的NDT Mapping都用这种方式。回环验证是在回环检测之后用NDT把当前帧和回环帧做精确配准计算精确的相对位姿约束。回环检测给出的是粗位姿比如从Scan Context的列偏移估计NDT在此基础上做精化。# NDT在SLAM中的典型用法 def ndt_odometry(current_scan, previous_scan, init_guess): ndt NDT(resolution2.0) ndt.set_target(previous_scan) T ndt.align(current_scan, init_guess) return TNDT做帧间配准的好处是不需要找对应点对噪声和离群点鲁棒。坏处是精度受体素分辨率限制通常比点到面ICP差一些。很多系统的做法是NDT做粗配准ICP做精配准两者结合效果最好。面试追问环节面试官NDT为什么不需要找对应点因为NDT把目标点云表示成了连续的概率分布高斯混合模型source中的每个点都可以在这个分布中直接计算概率密度不需要显式地找一个对应点。ICP需要找最近点建立对应关系NDT直接通过概率分布建立联系。这种soft correspondence的思路让NDT避免了ICP中对应关系不准确的问题。面试官NDT的目标函数为什么用牛顿法优化NDT的目标函数是多个高斯函数的和它的Hessian矩阵可以解析计算出来不需要数值近似。牛顿法利用了二阶导数信息收敛速度比梯度下降快很多二阶收敛 vs 一阶收敛。但牛顿法要求Hessian矩阵正定如果初始位姿偏差太大Hessian可能不正定这时候要加正则化或者改用其他优化方法。面试官NDT和ICP哪个更适合自动驾驶各有适用场景。NDT在远距离、大场景下效率更高因为体素数量远小于点数量。ICP在近距离、精细结构下精度更高。很多自动驾驶系统两个都用NDT做帧间配准和回环配准速度快ICP做精细对齐和地图更新精度高。Autoware的ndt_mapping模块就是一个典型例子。面试官NDT对动态物体敏感吗NDT对动态物体有一定的鲁棒性但不是说完全不受影响。因为NDT用体素统计信息少量动态物体比如几辆车改变的只是少数体素的高斯分布参数大部分体素不受影响。但如果动态物体很多比如拥堵的交通大量体素被干扰NDT的效果也会下降。处理方法是在NDT配准前先做动态物体滤除。面试官NDT的协方差矩阵为什么要加正则化如果一个体素中的点分布很集中比如几乎共线或共面协方差矩阵会接近奇异逆矩阵计算不稳定。加一个小的正则化项比如1e-6乘以单位矩阵可以确保协方差矩阵可逆数值稳定性更好。这是一个工程细节但在实际项目中很重要不加正则化经常出现NaN或inf导致配准崩溃。NDT用概率分布替代了点对应关系这种思路的转变让它在大规模点云配准中表现出色。不需要找最近邻、对噪声鲁棒、计算效率高这些优点让NDT成为自动驾驶和机器人领域最常用的配准算法之一。理解NDT和ICP的关系很关键——它们不是替代关系而是互补关系。粗配准用NDT精配准用ICP这种组合在实际项目中非常常见。上一篇第255篇 ICP点云配准——精确对齐的迭代最近点算法下一篇我们聊全局一致性优化这是SLAM后端的核心问题。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力
第256篇 NDT正态分布变换——大规模点云配准的高效方案
上一篇聊了ICPICP精度高但计算量大特别是在大规模点云场景下逐点找最近邻的开销很可观。在自动驾驶和大型机器人项目中点云动辄几十万点ICP的速度就成了瓶颈。NDTNormal Distributions Transform正态分布变换提供了一种不需要找对应点的配准方案在大规模场景下效率比ICP高很多。NDT最早是Peter Biber在2003年提出的后来被Martin Magnusson在博士论文中系统化并开源到PCL库中。NDT的核心思想很直觉不逐点匹配而是把空间划分成体素每个体素用一个高斯分布来描述配准就变成了两个概率分布的匹配问题。这种从点对点到分布对分布的转变是NDT相比ICP的根本区别。NDT的数学原理NDT把目标点云所在的空间划分成等大小的体素格子。对于每个体素统计落在其中的所有点计算这些点的均值和协方差矩阵构成一个三维高斯分布。如果体素中的点太少比如少于3个协方差矩阵不可靠这个体素就会被忽略。# NDT体素化目标点云 def build_ndt_map(points, voxel_size1.0): voxels voxelize(points, voxel_size) ndt_map {} for key, pts in voxels.items(): if len(pts) 5: mean np.mean(pts, axis0) cov np.cov(pts.T) 1e-6 * np.eye(3) ndt_map[key] (mean, cov) return ndt_map配准时对于source点云中的每个点先经过当前估计的变换T找到它落在哪个体素中然后计算这个点在该体素高斯分布下的概率密度。所有点的概率密度之和就是总似然。优化目标就是找到一个变换T使得总似然最大。数学上NDT的优化目标函数是score(T) Σ exp(-0.5 * (Tp_i - μ_j)^T * Σ_j^{-1} * (Tp_i - μ_j))。其中p_i是source点μ_j和Σ_j是p_i所在体素的均值和协方差。这个目标函数可以用牛顿法或者高斯-牛顿法来优化。体素分辨率的选择体素分辨率是NDT最关键的一个参数直接决定了配准的精度和速度。分辨率太大比如5米一个体素覆盖了很大一片区域高斯分布把很多不同几何结构的点混在一起描述能力很差配准精度低。分辨率太小比如0.1米大部分体素中只有0个或1个点构不成高斯分布NDT退化为找不到足够的有效体素。经验值是这样的室内场景用0.5-1米室外场景用1-2米自动驾驶场景用2-5米。具体要看点云密度和场景特征。场景特征越丰富比如有很多角、边缘、平面变化分辨率可以设小一点场景越平坦比如大草原、空旷停车场分辨率要设大一点。# 不同场景的NDT分辨率经验值 scenarios { indoor_office: 0.5, # 室内办公室 indoor_warehouse: 1.0, # 室内仓库 outdoor_urban: 2.0, # 室外城市 outdoor_highway: 5.0, # 室外高速公路 }还有一种自适应分辨率的做法根据点云密度自动调整体素大小。密集区域用小体素稀疏区域用大体素。这种方法更灵活但实现复杂度也更高。PCL中的NDT是固定分辨率的Autoware中有自适应版本的实现。3D NDT和2D NDT3D NDT处理三维点云每个体素用一个三维高斯分布表示有3个均值参数和6个独立的协方差参数协方差矩阵是对称的。优化变量是6自由度的刚体变换3个平移3个旋转。2D NDT处理二维激光扫描数据每个格子用一个二维高斯分布表示。优化变量是3自由度2个平移1个旋转。2D NDT在移动机器人室内建图中用得很多比如ROS的slam_karto中就用了类似的思想。2D NDT的计算量比3D小得多因为体素格子数量少高斯分布的维度也低。但2D NDT只能处理平面场景对于有多层结构的3D场景就无能为力了。实际项目中2D激光雷达配准用2D NDT就够了3D激光雷达配准用3D NDT。如果3D点云数量太大也可以先投影到2D做粗配准再用3D NDT做精配准。多分辨率NDT是一种实用的加速策略。先在大体素比如4米下做粗配准快速收敛到大致正确的位姿然后逐步减小体素大小做精配准。这种策略既保证了收敛范围大分辨率的basin of attraction更大又保证了最终精度小分辨率的描述更精细。Autoware的ndt_matching模块就支持多分辨率设置。NDT在SLAM中的应用NDT在SLAM中有两个主要应用场景帧间配准和回环验证。帧间配准是用NDT对齐相邻帧的点云估计相对位姿。这个过程替代了ICP的位置作为前端里程计的核心。HDL-Graph-SLAM和Autoware的NDT Mapping都用这种方式。回环验证是在回环检测之后用NDT把当前帧和回环帧做精确配准计算精确的相对位姿约束。回环检测给出的是粗位姿比如从Scan Context的列偏移估计NDT在此基础上做精化。# NDT在SLAM中的典型用法 def ndt_odometry(current_scan, previous_scan, init_guess): ndt NDT(resolution2.0) ndt.set_target(previous_scan) T ndt.align(current_scan, init_guess) return TNDT做帧间配准的好处是不需要找对应点对噪声和离群点鲁棒。坏处是精度受体素分辨率限制通常比点到面ICP差一些。很多系统的做法是NDT做粗配准ICP做精配准两者结合效果最好。面试追问环节面试官NDT为什么不需要找对应点因为NDT把目标点云表示成了连续的概率分布高斯混合模型source中的每个点都可以在这个分布中直接计算概率密度不需要显式地找一个对应点。ICP需要找最近点建立对应关系NDT直接通过概率分布建立联系。这种soft correspondence的思路让NDT避免了ICP中对应关系不准确的问题。面试官NDT的目标函数为什么用牛顿法优化NDT的目标函数是多个高斯函数的和它的Hessian矩阵可以解析计算出来不需要数值近似。牛顿法利用了二阶导数信息收敛速度比梯度下降快很多二阶收敛 vs 一阶收敛。但牛顿法要求Hessian矩阵正定如果初始位姿偏差太大Hessian可能不正定这时候要加正则化或者改用其他优化方法。面试官NDT和ICP哪个更适合自动驾驶各有适用场景。NDT在远距离、大场景下效率更高因为体素数量远小于点数量。ICP在近距离、精细结构下精度更高。很多自动驾驶系统两个都用NDT做帧间配准和回环配准速度快ICP做精细对齐和地图更新精度高。Autoware的ndt_mapping模块就是一个典型例子。面试官NDT对动态物体敏感吗NDT对动态物体有一定的鲁棒性但不是说完全不受影响。因为NDT用体素统计信息少量动态物体比如几辆车改变的只是少数体素的高斯分布参数大部分体素不受影响。但如果动态物体很多比如拥堵的交通大量体素被干扰NDT的效果也会下降。处理方法是在NDT配准前先做动态物体滤除。面试官NDT的协方差矩阵为什么要加正则化如果一个体素中的点分布很集中比如几乎共线或共面协方差矩阵会接近奇异逆矩阵计算不稳定。加一个小的正则化项比如1e-6乘以单位矩阵可以确保协方差矩阵可逆数值稳定性更好。这是一个工程细节但在实际项目中很重要不加正则化经常出现NaN或inf导致配准崩溃。NDT用概率分布替代了点对应关系这种思路的转变让它在大规模点云配准中表现出色。不需要找最近邻、对噪声鲁棒、计算效率高这些优点让NDT成为自动驾驶和机器人领域最常用的配准算法之一。理解NDT和ICP的关系很关键——它们不是替代关系而是互补关系。粗配准用NDT精配准用ICP这种组合在实际项目中非常常见。上一篇第255篇 ICP点云配准——精确对齐的迭代最近点算法下一篇我们聊全局一致性优化这是SLAM后端的核心问题。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力
![[光学原理与应用-521]：对光的错误理解与纠偏](http://pic.xiahunao.cn/yaotu/[光学原理与应用-521]：对光的错误理解与纠偏)
