1预备知识

1.0 wide base

4pcs算法的关键是选取一个宽基集（wide-base），如图。（就是选取的点的距离相对大些，算法才更稳定，明显上面的配准效果比下面的好，因为上面的点隔得远，但是最远距离要受重叠度的限制）。1

1.1 LCP(largest common pointset)

即通过一个刚体变换，使得重叠区域内所有点对之间的距离小于δ，这个重叠区域就是要求的最大公共点集.
简单来说就是，给定两个处于任意初始位置的点集P和Q，找到两个点集之间的最佳刚性变换，使得P，Q中两点间距离小于δ的点数最多.
δ\deltaδ为常数，TTT变换矩阵，Pmax⁡⊆P，P_{\max } \subseteq P ， \quadPmax⊆P，满足 ∀Pi∈Pmax⁡,∥T(pi)→−qi→∥≤δ\forall P_{i} \in P_{\max },\left\|\overrightarrow{T\left(p_{i}\right)}-\overrightarrow{q_{i}}\right\| \leq \delta∀Pi∈Pmax,∥∥∥∥T(pi)−qi∥∥∥∥≤δ都成立，则满足最多数量的那个PMAXP_{MAX}PMAX称为最大公共集 (largest common pointset LCP)2
即可以利用LCP做为代价函数，求解变换矩阵,4pcs就是利用LCP做为求解变换矩阵的度量。
wide-base和LCP度量的组合使得4PCS配准方法能应对噪点和异常值

1.2 RANSAC配准过程

基于RANSAC的对齐过程很简单：从P中随机选择三个不同的点，从Q中随机选择三个不同的点，形成对应的一对基，计算对齐基对的候选变换Ti，然后计算距离Q中点δ-距离内的P中的点ki的数量。如果ki足够大，则接受Ti作为一个好的解决方案。否则，通过随机选择另一个三个点来重复该过程，从而导出可能改进当前最佳拟合的不同候选变换。

1.3 Randomized Alignment

该配准方法是RANSAC算法的一种变体。该过程从P中随机选取一个基，计算将该基与Q中所有可能的基对齐的变换，并验证配准结果。如在RANSAC中一样，为了获得一定的成功概率，该方法重复从P中选择L个不同的基。此外，验证阶段也是随机化的：首先，只验证P中数量恒定的随机点，并且仅当该子集的显著部分匹配良好时，才测试其余点。
pgp_{g}pg是从点云PPP中随机选择一点，该点恰好也出现在点云QQQ中的概率，（即该点在重叠区域内）。用于配准的基（base）的数量为NNN。pfp_{f}pf是在尝试L次后找不到存在的良好拟合时，算法退出的概率。因为我们从PPP中随机选择基，假设： pf=(1−pgN)Lp_{f}=\left(1-p_{g}^{N}\right)^{L}pf=(1−pgN)L，设psp_{s}ps为配准成功的概率，则有迭代次数LLL：

L>log⁡(1−ps)/log⁡(1−pgN)(1)L>\log \left(1-p_{s}\right) / \log \left(1-p_{g}^{N}\right)\tag{1}L>log(1−ps)/log(1−pgN)(1)

For rigid transforms, it is sufficient to have N=3N=3N=3

4PCS算法流程是基于Randomized Alignment方法。然而，文中引入了平面全等集的思想，从Q中只选择一小部分可能与P中给定的基相匹配的基，而不是从Q中测试所有可能的基.

2 Approximate Congruent 4-Points近似共面四点（4PCS特征）

仿射变换具有以下性质：给定三个共线点{a，b，c}，比率‖a− ∥a−b∥/∥a−c∥\|\mathbf{a}-\mathbf{b}\| /\|\mathbf{a}-\mathbf{c}\|∥a−b∥/∥a−c∥是不变的。Huttenlocher[1991]使用该不变量提取平面上4个点的所有二维仿射不变量集，这些不变量在仿射变换下是等价的。我们在三维空间中采用类似的方法。给定一个共面4点基，在另外一个点云数据中寻找其仿射等价（全等）4点集。所有仿射不变4点集是3维空间中4点全等点的超集。随后，我们验证这样的4点集是否（近似）与所选的基集一致。首先简要介绍了二维仿射不变四点集的提取方法，然后详细介绍了三维仿射不变四点集的提取过程。

2.1 4点对的仿射不变性

Huttenlocher（1991）介绍了一种提取二维仿射不变的4点集的方法。设一组共面点X≡ {a，b，c，d}，非所有点共线，定义了三个共线点的两个独立比率。设ab和cd为相交于点e的两条线。这两个比率r1,r2r_1,r_2r1,r2：
r1=∥a−e∥/∥a−b∥r2=∥c−e∥/∥c−d∥r_{1}=\|\mathbf{a}-\mathbf{e}\| /\|\mathbf{a}-\mathbf{b}\|\\ r_{2}=\|\mathbf{c}-\mathbf{e}\| /\|\mathbf{c}-\mathbf{d}\|r1=∥a−e∥/∥a−b∥r2=∥c−e∥/∥c−d∥
这两个比率r1,r2r_1,r_2r1,r2,具有仿射变换不变性，并且能唯一定义四个点。
如下图：

现在，给定一个由n个点组成的集Q，以及两个仿射不变比r1和r2，我们可以有效地提取由这两个不变量定义的所有4个点集，其中k是4个点集的数目，如下所示：对于每对点q1，q2∈ Q、计算两个中间点：
e1=q1+r1(q2−q1)\mathbf{e}_{1}=\mathbf{q}_{1}+r_{1}\left(\mathbf{q}_{2}-\mathbf{q}_{1}\right) e1=q1+r1(q2−q1)
e2=q1+r2(q2−q1)\mathbf{e}_{2}=\mathbf{q}_{1}+r_{2}\left(\mathbf{q}_{2}-\mathbf{q}_{1}\right) e2=q1+r2(q2−q1)
如下图的中间图所示：对于基准点云任意4个点中得到的r1,r2r_1,r_2r1,r2,在待配准点云任意两点q1，q2{q}_{1}，{q}_{2}q1，q2,由上式可以得到2个e1e_1e1(由 r1计算得到)，2个e2e_2e2点(由 r2计算得到)。如下图中的右边图所示：对于两条线段（4个点）而言，若第一个线段中的任意一个e1e_1e1（一个线段两个e1e_1e1）与第二条线段中任意一个e2e_2e2（一个线段两个e2e_2e2）重合或者在很小距离范围内，那么可以认为这两个线段对应的四个端点，可能是基准点云中4个点的对应点。
如下图：

2.2在3维空间中寻找4个共面点集对

给定从点集P和另一点集Q中选择的（近似）共面点的4点基B∈ R3，我们的目标是从Q中提取与B近似相等的所有4个点的集合。首先给出B，我们计算它在这个平面上的两个仿射不变量比，如前所述。
然后，使用第3.2节中描述的方法，从点Q中，通过仿射变换提取与B相关的所有点集。虽然此方法能够获得所需4点的共面点集，但是会存在一定数量的虚假匹配。
为了去除非全等基，我们查看它们在点云中的原始位置，并验证相应的集是否在某个距离阈值内与基集B一致。然后，使用基B和Q中的每个基，通过最小二乘计算最佳对齐刚性变换。

上述程序需要大量内存，对于大量点云是不合适的。做出以下改进：刚性变换保持点间欧氏距离。给定基数B≡ {a，b，c，d}，我们首先计算距离d1=‖a− b‖和d2=‖c− d‖。现在我们只考虑Q中的的距离为d1或d2的点对，最大误差为δ。

3 4PCS算法

我们给出了两个点集P和Q，这些点的位置精度的不确定性度量（δ>0），以及P中可与Q重叠区域分数f的估计方法。我们的目标是找到一种刚性变换，使P中的最大点数与Q中某个点之间的距离小于δ。我们提出了一种算法（见算法1），其运行时间取决于给定点集之间匹配点数的最大值，并将其随机化，以很高的概率发现正确的刚体变换。

我们首先选择一个由由4个共面点组成的基B⊆ P。实际上，我们允许一些非平面性，因为不太可能存在4个共面点。我们随机选取三个点，并选择剩余点，使四个点一起形成一个widebase，该base（近似）共面。通过选择彼此相距较远的点来创建widebase，从而产生更稳定的共面点。然而，如果我们选择的点相距太远，所选的点可能不会全部位于重叠区域（用于部分匹配）。我们使用重叠分数f来估计这个最大距离。如果没有提供f的估计值，我们将在f=1，0.5，0.25，…的情况下以减少猜测的方式运行该算法。直到我们达到所需的误差容限。在f已知的情况下，我们首先选择一组可能位于重叠区域的三个点，然后如前所述选择第四个点。

对于从算法的SELECTCOPLANARBASE阶段获得的平面基B，我们可以使用基点定义仿射不变比率。然而，对于近似平面基底，我们使用位于连接点对的的最近点来定义不变比率。现在，我们应用第2.1和2.2节中描述的方法来提取集合U≡ Q中的4个点的所有子集的{U1，U2，…，Us}，它们可能与B一致。对于每个Ui，使用B和Ui之间的对应信息，我们计算使B在最小二乘意义上接近Ui的最佳对齐刚性变换Ti【Horn 1987年】。为了验证Ti，我们计算Ti*（P），并计算Ti*（P）中有多少点比δ更接近Q中的某个点。为Ti评分（参见[Irani和Raghavan 1996]）。选取评分最高的Ti，为最终的变换T。

给定一个基Bi，我们描述了如何计算与其对应的最佳变换Ti。根据对齐的点数，为每个对齐变换Ti分配一个分数。使用该程序，我们现在执行Randomized Alignment过程，并根据重叠分数f的估计值，测试出L个不同的基（见公式1）。在所有这些试验中，我们选择得分最高的变换矩阵Topt。

4 一些加速技术

配合局部特征描述子和局部特征（曲率，法线），加速4点共面点集的选择过程。

5 4pcs在pcl中应用


#pragma once
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <pcl/registration/ia_fpcs.h>
#include <pcl/registration/ia_kfpcs.h>
#include <time.h>
#include <boost/thread/thread.hpp>using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;int fourpcs(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source,const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target,pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_source)
{//四点法配准PointCloud::Ptr pcs(new PointCloud);pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;fpcs.setInputSource(cloud_source);//输入待配准点云fpcs.setInputTarget(cloud_target);//输入目标点云//参数设置fpcs.setApproxOverlap(0.6);//两点云重叠度fpcs.setDelta(0.5);//Bunny//fpcs.setDelta(0.5);//hippofpcs.setMaxComputationTime(50);fpcs.setNumberOfSamples(int(cloud_source->size()/20));Eigen::Matrix4f tras;clock_t start = clock();fpcs.align(*pcs);clock_t end = clock();cout << "time:" << (double)(end - start) / (double)CLOCKS_PER_SEC << endl;cout << "score:" << fpcs.getFitnessScore() << endl;tras = fpcs.getFinalTransformation();cout << "matrix:" << endl << tras << endl << endl << endl;//PointCloud::Ptr cloud_end(new PointCloud);pcl::transformPointCloud(*cloud_source, *transformed_source, tras);//visualize_pcd(cloud_source, cloud_target, cloud_end);return (0);
}

参考：
《4-Points Congruent Sets for Robust Pairwise Surface Registration》
https://blog.csdn.net/qq_41102371/article/details/111293664
https://blog.csdn.net/Ha_ku/article/details/79480613
https://blog.csdn.net/renyuanxingxing/article/details/84662986
https://blog.csdn.net/qq_41102371/article/details/111715115

点云配准--4PCS原理与应用相关推荐

【点云配准-4PCS（2008）】4-Points Congruent Sets for Robust Pairwise Surface Registration
文章目录 1 背景 1.1 问题描述 1.2 相似性测度(Similarity Measure) 1.3 随机采样一致性(Random Sample Consensus,RANSAC) 1.4 Ran ...
点云配准方法原理（NDT、ICP）
配准是点云处理中的一个基础问题,众多学者此问题进行了广泛而深入的研究,也出现了一系列优秀成熟的算法,在三维建模.自动驾驶等领域发挥着重要的作用. 本文主要介绍粗配准NDT (Normal Distri ...
点云配准5：4pcs算法在pcl上的实现
目录配准结果点云配准系列准备完整项目文件参数设定及说明数据参数代码结果 Bunny hippo 算法缺点参考及感谢完配准结果偶尔效果比较好,白色是目标点云0°的Bunny,紫 ...
点云配准论文阅读笔记--(4PCS)4-Points Congruent Sets for Robust Pairwise Surface Registration
目录点云配准系列写在前面 Abstract摘要 1 Introduction引言 2 Background研究背景 RANSAC Randomized Alignment 3 Approximat ...
3D点云配准算法-4PCS（4点全等集配准算法）
简介 4PCS算法是一种快速的.鲁棒的三维点云配准方案,该方案使用宽基底,对噪声和异常值具有良好的适应性,该算法允许对有噪声的原始数据进行配准,而无需对数据进行预过滤或去噪.此外,该方法大大减少了在噪 ...
点云粗配准4PCS及相应变种+在R稠密三维重建场景中的应用效果
一.4PCS简介: 4PCS(4 point congruent sets)算法是一种点云粗配准的算法. 适用于重叠区域较小的两帧点云图(深度图)进行粗配准.传统的ICP算法需要两帧点云有较大的重叠区 ...
干货 | 三维点云配准：ICP 算法原理及推导
编者荐语点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步.粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主 ...
描述点云配准的正太分布变换和著名的ICP点云配准原理
正太分布变换(Normal Distribution Transform, NDT):正太分布变换是一种基于特征点的点云配准算法,能够有效地处理大规模点云数据.它通过将点云数据转换为网格形式,并计算每 ...
点云配准各种方法总结[不定时更新]
本文主要用来记录我在学习过程中接触到的一些或新或旧的点云配准的方法,有些方法我可能还不怎么理解,如果有错误欢迎大家指出. 2018/04/15 添加LORAX 2018/04/08 添加DO法和结合法 ...

点云配准--4PCS原理与应用

文章目录