上篇讲了传感器标定——相机内参、雷达-相机外参、IMU标定。标定做完了传感器之间的坐标系统一了数据才能放在一起用。激光雷达输出的数据就是点云——一堆三维空间中的点每个点有(x, y, z)坐标有的还有反射强度。原始点云本身信息量有限要做识别、匹配、建图得先从中提取特征。今天这篇讲点云处理的三件套法线估计、FPFH特征描述子、点云配准。这三步是激光SLAM和三维感知的基础面试中经常被问到。法线估计点云的朝向法线是垂直于表面的方向向量。对于平面法线就是平面的垂直方向。对于曲面每个点都有自己的法线方向。点云中的法线估计说白了就是对每个点找到它周围的邻居点用这些邻居点拟合一个局部平面平面的法线就是这个点的法线。具体怎么做用PCA主成分分析对目标点找K个最近邻或者半径r内的所有邻居计算邻居点的协方差矩阵3x3对协方差矩阵做特征值分解最小特征值对应的特征向量就是法线方向from open3d.geometry import PointCloud import open3d as o3d pcd o3d.io.read_point_cloud(scan.pcd) pcd.estimate_normals(search_paramo3d.geometry.KDTreeSearchParamKNN(knn20))法线方向有个模糊性——它可能朝上也可能朝下正负两个方向都垂直于平面。通常做法是让法线朝向传感器方向因为激光是从传感器发出来的打到物体表面后反射回来法线应该朝向传感器。工程经验K值的选择很关键。K太小邻居点太少法线估计对噪声敏感。K太大邻居点覆盖了太大的区域法线会模糊掉局部细节。对于室内环境的16线激光雷达K20到30通常比较合适。对于64线的高密度雷达K可以取到50甚至更大。FPFH特征给每个点一个身份证有了法线之后就可以计算特征描述子了。FPFHFast Point Feature Histograms是最经典的点云局部特征。FPFH的思路是对每个点统计它和邻居点之间的几何关系用一个直方图来描述。这个直方图就是该点的身份证——不同位置的点FPFH直方图不一样。具体来说FPFH计算三个角度特征alpha两个点的法线与连线之间的角度关系phi法线与参考方向之间的角度theta两个点的法线之间的夹角每个角度被量化成11个bin三个角度组成33维的特征向量。from open3d.pipelines.registration import compute_fpfh_feature fpfh compute_fpfh_feature(pcd, o3d.geometry.KDTreeSearchParamKNN(knn30)) # fpfh.data.shape (33, num_points)FPFH的特点是旋转不变不管点云怎么旋转同一点的FPFH不变、对噪声有一定鲁棒性、计算速度快。但它的区分能力有限——在几何结构相似的区域比如两面平行的墙FPFH可能很接近。点云配准把两片点云对齐点云配准就是找到两个点云之间的空间变换旋转平移使它们对齐。这是SLAM中帧间匹配、回环检测的核心操作。粗配准用特征匹配来找初始变换。流程是计算两片点云的FPFH特征对源点云中的每个点在目标点云中找FPFH最相似的点用匹配点对计算变换矩阵RANSACfrom open3d.pipelines.registration import registration_ransac result registration_ransac_based_on_fpfh( source, target, max_correspondence_distance0.05, estimation_methodo3d.pipelines.registration.TransformationEstimationPointToPoint(), ransac_n3, criteriao3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.9))RANSAC的作用是从大量匹配点对中找出正确的变换——随机选3对点算一个变换看有多少匹配点对支持这个变换反复迭代取支持最多的那个。精配准用ICPIterative Closest Point算法进一步优化。ICP的思路是对源点云每个点在目标点云中找最近点计算最优变换使源点云和目标点云的距离最小应用变换重复直到收敛from open3d.pipelines.registration import registration_icp result registration_icp(source, target, 0.02, result.transformation, o3d.pipelines.registration.TransformationEstimationPointToPoint())粗配准精配准是标准流程。粗配准提供一个还不错的初始变换精配准在这个基础上微调。如果直接上ICP不做粗配准初始偏差太大ICP会收敛到错误的匹配。面试中怎么聊面试官问点云处理按这个顺序回答先说法线估计PCA方法、K值选择再说FPFH特征三个角度、33维直方图、旋转不变性然后说配准流程RANSAC粗配准ICP精配准。如果面试官追问ICP的局限性你可以说ICP要求初始变换足够好否则收敛到局部最优要求两个点云有足够的重叠区域对噪声和外点比较敏感。改进版的ICP有很多变种——点到面ICP用目标点云的法线信息收敛更快、广义ICPGICP同时考虑两个点云的法线更鲁棒。在激光SLAM中NDT正态分布变换也是常用的配准方法它把点云分成体素每个体素用高斯分布建模然后优化源点云在高斯分布下的似然。分享一个我在项目中做点云配准的经验。当时做一台移动机器人的激光SLAM用ICP做帧间配准来估计运动。一开始配准效果很差经常出现跳帧——某一帧的配准结果突然偏离很大导致建图出现重影。排查后发现两个问题第一ICP的最近邻搜索用的是暴力搜索在点云密度不均匀的区域容易匹配错误。换成KD树搜索后匹配精度提升了不少。第二ICP对初始变换敏感当机器人转弯较快时帧间位姿差变大ICP容易收敛到错误的匹配。我们加了运动预测——用上一帧的速度预测当前帧的初始变换给ICP一个好的初始值。改完之后跳帧的问题基本消失了。如果面试官追问点云处理的加速方法你可以说体素化降采样是最基本的——把点云分成小体素每个体素只保留一个点通常是中心点大幅减少点数。KD树加速最近邻搜索是标配。对于FPFH计算可以用简化版SPFH先算简化特征再加权得到FPFH或者用深度学习的方法PointNet、PointNet提取全局特征比传统描述子更有区分力。实际项目中加速的关键是减少参与计算的点数——先降采样再配准配准完再把结果映射回原始密度。面试时候能讲出这种工程化的优化思路比只说算法原理更有说服力。另外在实际部署时建议用C而不是Python来跑点云处理——PCL的C接口配合OpenMP并行化处理速度能比Python快10倍以上这对实时系统来说至关重要。下一篇讲图像处理基础——滤波、边缘检测和形态学操作。从点云到图像感知系统处理的不仅是三维数据二维图像的处理同样重要。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第171篇 传感器标定实操——相机/雷达/IMU的标定流程总结下一篇预告第173篇 图像处理基础——滤波、边缘检测和形态学操作有任何问题欢迎评论区留言我会尽量回复。