【三维点云配准】4PCS算法实现点云粗配准

算法原理

4PCS[1]配准算法使用的是RANSAC算法框架，通过构建与匹配全等四点对的方式来减少空间匹配运算，进而加速配准过程。 具体的在任意姿态的输入点云P和Q中构建共面四点集合，使用仿射不变性约束，在共面四点集合中匹配符合条件的对应点对，使用LCP（Largest Common Pointset）策略寻找配准后最大重叠度四点对，得到最优匹配结果，完成点云粗匹配。

1. 问题描述

给定两个处于任意初始位置的点集P 和Q ，找到两个点集之间的最佳刚性变换，使得P ，Q 中两点间距离小于δ 的点数最多。

1.1 RANSAC

• 分别在点集P和点集Q 中任意选取三个点来组成一个基础关联对
• 计算这个关联对的旋转矩阵Ti
• 计算点集P 中处在点集Q 中的点δ 距离内的点的个数ki
• 如果ki足够大，则认为kiki 是个好结果，否则重读以上步骤
• 这个过程将被重复L 次，选取最高的ki 作为最后的结果

1.2 Randomized Alignment

1. 在点集P 中随机选取一个base
2. 计算在点集Q 中所有有可能的bases，得到旋转矩阵
3. 验证配准
4. 先验证部分点集重合
5. 验证剩余点

如何有效的计算可能的bases，提出4点法，基于共麺点（planar congruent sets）

2 4PCS概述

使用从点集P 中选取的共面四点作为base B ，然后采用一定的算法在点集Q中找到所有的与B近似一致的共面4点对，近似一致指的是两个4点对可以经过刚性变换（rigid transformation）在允许范围（allowed tolerance）δ 内对齐

2.1 Overview

仿射变换遵循以下法则，4PCS算法构建与匹配点间空间拓扑关系过程如图1所示。 首先在基准点云P中寻找满足要求的长基线（基线的确定与输入参数中overlap有很大关系，overlap越大，基线选择越长，长基线能够保证匹配的鲁棒性，且匹配数量较少）共面四点基，如图1用B={a，b，c，d}表示，然后提取共面四点基的拓扑信息，按式（1）计算四点基间的两个比例因子r1、r2， 两比例因子在点云旋转和平移变化中具有仿射不变性。 按式（2）计算q1，q2∈Q四种可能存在的交点位置，进而计算所有Q中长基线点对交点坐标，比较交点坐标确定匹配集合，ei≈ej表示寻找到对应的一致全等四点，i，j分别表示第i个和第j个Q中长基线点对。 图1中B={a，b，c，d}的全等四点对为C={q1，q3，q4，q5}。 寻找点云中所有P的共面四点集合记为E={B1，B2,…,Bm}，m为P中四点集总数，重复上述步骤可得到全等四点集合D={C1，C2,…,Cn}，n为全等四点集合总数。

2.2 4点对的仿射不变性

在仿射变化中是不变的并且是唯一的。

现在给定一个具有仿射不变性n 个点的点集Q ，以及两个由点P 得到的仿射不变的比率r1r1 ,r2r2 ，对每一对点q1,q2⊂Qq1,q2⊂Q ，计算他们的中间点：
e1=q1+r1(q2−q1)
e1=q1+r1(q2 −q1)
e2=q1+r2(q2−q1)
e2=q1+r2(q2 −q1)

若任意两对这样的点，一对由r1 计算得到的中间点和另一对由r2 计算得到的中间点在允许范围内一致，那么可以认为这两对点可能是PP 中基础点的仿射对应点。
仿射对应点

在集合D={C1，C2,…,Cn}寻找最优全等四点对，4PCS使用LCP策略寻找最优全等四点匹配，即计算全等四点旋转和平移变化参数，将四点转化应用到全局点云转化，记录全局配准中包含最大的一致区域匹配记为最优匹配，至此完成4PCS算法局部粗配准工作。

4PCS算法官方介绍

C++实现代码

//PCL 4PCS算法实现点云粗配准
#include <pcl/registration/ia_fpcs.h> // 4PCS算法
void _4PCS(pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud)
{
    pcl::console::TicToc time;
    time.tic();
    //--------------初始化4PCS配准对象-------------------
    pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;
    fpcs.setInputSource(source_cloud);  // 源点云
    fpcs.setInputTarget(target_cloud);  // 目标点云
    fpcs.setApproxOverlap(0.7);         // 设置源和目标之间的近似重叠度。
    fpcs.setDelta(0.01);                // 设置配准后对应点之间的距离（以米为单位）。
    fpcs.setNumberOfSamples(100);       // 设置验证配准效果时要使用的采样点数量
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcs(new pcl::PointCloud<pcl::PointXYZ>);
    fpcs.align(*pcs);                   // 计算变换矩阵
    cout << "FPCS配准用时： " << time.toc() << " ms" << endl;
    cout << "变换矩阵：" << fpcs.getFinalTransformation() << endl;
    // 使用创建的变换对为输入点云进行变换
    pcl::transformPointCloud(*source_cloud, *pcs, fpcs.getFinalTransformation());
    visualizeCloud(target_cloud, source_cloud, pcs);
}

实验结果

无结果
初始位置差距稍微有点大，就无法配准。