4PCS用于点云粗配准,ICP用于粗配准之后的精配准。
本文算法原理参考于:3D点云配准算法-4PCS(4点全等集配准算法)
1.4PCS算法原理
核心思想:使用四个近似共面点来描述两个三维点云之间的刚性变换关系,因此由称”四点匹配算法“。即不停迭代计算源点云Q的四点集Qa,与目标点云P中与Qa重叠度最高的四点集Pb,二者之间的最优变换矩阵T,得到粗配准的变换矩阵。
如下图:
1.点云四点集的确定
(1)选择三个共面点
1)在源点云中随机选择三个点,要求这三点所构成的三角形面积尽量的大(三点确定一个平面,向量叉积可以计算面积),但是三点间的距离不能超过一定的阈值上限,该上限由两片点云的给定重叠率 f 确定。
2)因为三点之间距离越大,配准的鲁棒性越高;但距离过大,三点均在两点云的重叠区域之外了,配准效果又不好。因此需要在满足上限的基础之上,尽可能保证大的三级形面积。
3)若没有给定点云重叠率f,则可以进行f=1.0,0.75,0.5...重叠率递减测试,选择最优变换。
(2)选择第四个共面点
1)确定三点后,遍历源点云中所有的点,对每一个点进行计算验证选择最优的第四点。
2)第四点需要与其他三点组成的平面尽可能的共面(即不强制要求四点共面,但第四点到其他三点平面的距离尽可能小),并且第四点与其他三点的距离也要满足距离阈值范围(不能太近不能太远)。
2.4PCS计算
计算其四点构成的两线段交点的变换不变比,根据不变比在目标点云中遍历搜索对应的满足该约束所有四点集,得到的四点集,即近似全等四点集。
具体流程:
(1)选取源点云四点集
1)在源点云P中,使用上述的四点集的选择方法随机选择一个四点集B={b1,b2,b3,b4},其中(b1,b2)确定线段1,(b3,b4)确定线段2。
2)计算不变量d1=|b1-b2|,d2=|b3-b4|(约束1),不变比r1=|b1-e| / |b1-b2|,r2=|b3-e| / |b3-b4|(约束2)。注意因为四点不一定共面,两条线段也不一定相交,所以可以使用连接两个线段的最近点的中心点作为“交点”。
(2)遍历目标点云点对,筛选得到目标点云中的点对集合。
1)在目标点云Q中,遍历所有的点对,筛选满足约束1(允许有一定的误差)的点对集合R1,R2。其表示为:
计算R1 ={ (pi, pj) | pi, pj ∈ Q) },使|| pi - pj ||∈ [ d1- δ,d1+δ ].
计算R2 ={ (pi, pj) | pi, pj ∈ Q) },使|| pi - pj ||∈ [ d2- δ,d2+δ ].
(3)构建映射
1)遍历点对集合R1中的所有点对元素,计算其线段上满足不变比r1的目标交点
。
2)所有计算结果e存入搜索树ANN Tree(近似最邻近搜索树,最常见的是K-D Tree算法),并构建相应的映射
(4)求解近似全等四点集
1)遍历点对集合R2中的所有点对元素,计算其线段上满足不变比r2的目标交点
,并构建相应的映射
。
2)遍历所有的点,在之前构建的ANN Tree中搜索可接受误差范围内的重合点
,若可找到则说明能在Q中找到一个对应的近似全等四点集
。最终求得所有的近似全等四点集
(5)得到本次迭代最优变换矩阵T
1)遍历所有的近似全等四点集,对每一个
,通过最小二乘法计算其与B的对应变换矩阵
。
2)使用该变换矩阵对源点云P进行变换得到P',统计P'与Q中的最大公共点集(LCP),记max(LCP)的变换矩阵为本次迭代的最优变换矩阵T并保留。
(6)不断迭代这个过程,记录最优的变换。迭代结束后得到的变换矩阵即为最优变换矩阵。
2.关键函数
(1)设置源和目标之间的近似重叠度
registration.setApproxOverlap(0.5)
(2)设置常数因子delta,用于对内部计算的参数进行加权
registration.setDelta(0.01)
(3)设置验证配准效果时要使用的采样点数量
registration.setNumberOfSamples(100)
3.完整代码
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ia_fpcs.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
int main()
{
/****************FPCS配准********************/
pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>); // 源点云
pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>); // 目标点云
pcl::io::loadPCDFile("D:/code/csdn/data/B30.pcd", *source);
pcl::io::loadPCDFile("D:/code/csdn/data/B31.pcd", *target);
pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>); // 目标点云转换后的结果
// FPCS配准
pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> registration;
registration.setInputSource(source); // 源点云
registration.setInputTarget(target); // 目标点云
registration.setApproxOverlap(0.5); // 设置源和目标之间的近似重叠度。
registration.setDelta(0.01); // 设置常数因子delta,用于对内部计算的参数进行加权
registration.setNumberOfSamples(100); // 设置验证配准效果时要使用的采样点数量
registration.align(*result);
/****************展示********************/
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("FPCS"));
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target, 255, 0, 0); // 目标点云
viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>result_color(result, 0, 255, 0); // 配准结果点云
viewer->addPointCloud<pcl::PointXYZ>(result, result_color, "result cloud");
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return (0);
}