pcl点云配准

最新推荐文章于 2025-11-13 08:17:30 发布

原创最新推荐文章于 2025-11-13 08:17:30 发布 · 3k 阅读

31 ·

CC 4.0 BY-SA版权

文章标签：

#算法

点云同时被 2 个专栏收录

3 篇文章

订阅专栏

pcl

2 篇文章

订阅专栏

1. 配准引出

在这里插入图片描述

在激光SLAM系统中,通常会需要对当前获取的激光点云数据与地图的点云数据进行匹配操作,也叫做配准问题.
配准的方法有很多,根据是否使用特征可分为2类:

直接法
特征法

本文将借助PCL库介绍如何使用非特征法的ICP和NDT,以及特征法的FPFH算法.

2. 直接法

回想视觉SLAM系统中,一般是先提取特征,然后再基于特征对两帧进行匹配,最后执行优化得到相对位姿.
而直接法就是简单粗暴地直接配准

2.1 ICP

若对ICP再进行细分,还可以分为:

点到点
点到线
点到面

2.1.1 算法流程

ICP是一边执行匹配,一边执行优化
具体算法流程:

设初始的位姿估计为R0,t0;
执行位姿优化:从初始位姿开始迭代,得到使误差最小的最优位姿Rk,tk;
在Rk,tk位姿下,按照最近邻方式寻找匹配点;
计算该位姿和该匹配下的误差;
判断误差是否收敛,若不收敛返回步骤3,收敛则退出.

这里的优化用到的雅可比矩阵和平常的使用一样,详见<自动驾驶与机器人中的SLAM-ch7>

2.1.2 PCL应用ICP

// 1. 对待配准的点云进行下采样(非必须)
pcl::PointCloud<pcl::PointXYZ>::Ptr srcFiltered(new pcl::PointCloud<pcl::PointXYZ>);

pcl::ApproximateVoxelGrid<pcl::PointXYZ> filter;
// 设置每个体素的大小, leaf_size没别为lx ly lz的长 宽 高
filter.setLeafSize(0.01, 0.01, 0.01);
filter.setInputCloud(src);
filter.filter(*srcFiltered);

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

//kdTree 加速紧邻搜索
pcl::search::KdTree<pcl::PointXYZ>::Ptr treeSource(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr treeTarget(new pcl::search::KdTree<pcl::PointXYZ>);

treeSource->setInputCloud(srcFiltered);
treeTarget->setInputCloud(target);

icp.setSearchMethodSource(treeSource);
icp.setSearchMethodTarget(treeTarget);

icp.setInputSource(srcFiltered);
icp.setInputTarget(target);

icp.setMaxCorrespondenceDistance(0.5);//当两个点云相距较远时候，距离值要变大，所以一开始需要粗配准。
icp.setTransformationEpsilon(1e-10);//svd奇异值分解，对icp时间影响不大
icp.setEuclideanFitnessEpsilon(1e-10);//前后两次误差大小，当误差值小于这个值停止迭代
icp.setMaximumIterations(100);//最大迭代次数

pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
icp.align(*output);

2.2 NDT

2.2.1 NDT算法流程

NDT全称 Normal Distribution Transform, 它的核心思想是提取点云的局部特征信息,对局部的特征采用正态分布去描述,因此它需要对点云按体素分开,每个体素计算均值和方差,具体而言:

把目标点云按一定分辨率分成若干体素;
计算每个体素的高斯分布;
配准时,先判断每个点所落在的体素上,并根据该体素的均值和方差构造残差;
利用G-N或L-M方法对位姿进行优化,更新位姿

2.2.2 NDT的本质

换个角度去想:NDT实则是计算了每个体素的质心,待匹配的点云的每个点落在对应的体素后,计算其与质心的距离,并且根据该体素的方差(分散程度)设置权重,因此NDT其实就是一个带权重的最小二乘问题(也可以叫最大似然问题,详见笔记<最大似然与最小二乘.md>).

2.2.3 PCL应用NDT

// 同样可以先降采样,这里不再累赘

//初始化正态分布变换（NDT）
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

//为终止条件设置最小转换差异
ndt.setTransformationEpsilon(1e-6);
//为More-Thuente线搜索设置最大步长
ndt.setStepSize(0.001);
//设置NDT网格结构的分辨率（VoxelGridCovariance
ndt.setResolution(0.03);
//设置匹配迭代的最大次数
ndt.setMaximumIterations(1000);
// 设置要配准的点云
ndt.setInputSource(srcFiltered);
//设置点云配准目标
ndt.setInputTarget(target);

pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);

Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
ndt.align(*output, T);

在此强调:NDT对体素的划分分辨率十分敏感,因为不同大小分辨率所描述的局部不一致,另外实测发现StepSize这个参数也很重要

3. 特征法

特征法的思想很简单,参照二维图片一样,希望提取一些特征点(3D),并使用经过设计的描述子去表达某些特征,特征法有很多种…小弟不才,目前仅了解到PFH和FPFH,而FPFH是由PFH发展而来.

3.1 PFH和FPFH

pcl关于PFH的介绍

PFH的核心思想就是对一个点的邻域(可以以半径r为范围,也可以以最近邻k个点)作描述,关键步骤有:

计算每个点的法向量;
搜索每个点邻域,并计算其PFH特征三要素;
归一化及特征分配

在这里插入图片描述

然而,PFH有个致命的缺点,就是计算量大,无法保证实时,为此有了FPFH(未深入了解)

3.2 使用PCL计算FPFH特征

点云提取FPFH特征

typedef pcl::PointCloud<pcl::Normal> PointNormals;
typedef pcl::PointCloud<pcl::FPFHSignature33> FPFHfeatures;

FPFHfeatures::Ptr ComputeFpfh(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

    // 计算法向量
    PointNormals::Ptr normals(new PointNormals);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> estNormal;
    estNormal.setInputCloud(cloud);
    estNormal.setSearchMethod(tree);
    estNormal.setKSearch(8);
    estNormal.compute(*normals);

    // 计算FPFH特征
    FPFHfeatures::Ptr feature(new FPFHfeatures);
    pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> estFPFH;
    estFPFH.setNumberOfThreads(4);
    estFPFH.setInputCloud(cloud);
    estFPFH.setInputNormals(normals);
    estFPFH.setSearchMethod(tree);
    estFPFH.setKSearch(8);
    estFPFH.compute(*feature);

    return feature;
}

3.3 采样一致性算法结合特征实现配准

现在有了点云中各个点的特征,就可以通过特征的相似性(计算特征的距离)进行配准,一般是采用采样一致性算法,即我们熟知的RANSAC

PCL的ia_ransac.hpp实现了相应的算法:

从source点云中随机采样n个采样点,n个采样点有距离限制;
从target点云中找到与上面采样点特征相似的点,组成n组点对:
- 实际中使用了KDtree加速找到相似的特征,PCL的做法是找K个近邻点(这里的近邻至特征距离,非尺寸空间的距离),再随机选取其中1个
根据点对求解转换矩阵;
计算两幅点云的误差,记录最小误差和最佳变换矩阵;
重复以上步骤,直至迭代次数达到设定值.

实际使用方法:

// 同样可以先降采样,这里不再累赘

FPFHfeatures::Ptr srcFPFH = ComputeFpfh(srcFiltered);
FPFHfeatures::Ptr targetFPFH = ComputeFpfh(target);

//对齐(占用了大部分运行时间)
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac;
sac.setInputSource(srcFiltered);
sac.setSourceFeatures(srcFPFH);
sac.setInputTarget(target);
sac.setTargetFeatures(targetFPFH);

sac.setMinSampleDistance(0.08);
// sac.setMaximumIterations(200);

pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
sac.align(*output);