Point cloud registration -- 4pcs principle and Application

summary ：4PCS differ icp and ndt, It is based on local features （ Feature descriptors ） The global registration method , For noise and outliers 、 Low overlap has robustness , Insensitive to the initial value of registration .

1 Preliminary knowledge

1.0 wide base

4pcs The key of the algorithm is to select a wide base set （wide-base）, Pictured .（ The distance between the selected points is relatively large , The algorithm is more stable , Obviously, the registration effect of the above is better than that of the following , Because the points above are far apart , But the longest distance is limited by the degree of overlap ）.1

1.1 LCP(largest common pointset)

That is, through a rigid body transformation , Make the distance between all pairs of points in the overlapping area less than δ, This overlapping area is the maximum common point set required .
In a nutshell , Given two sets of points at any initial position P and Q, Find the best rigid transformation between two point sets , bring P,Q The distance between two points is less than δ Has the most points .
$\delta$ Constant ,$T$ Transformation matrix ,$P_{\max }\subseteq P,$ Satisfy $\forall P_i\in P_{\max },\Vert \overrightarrow{T\left(p_i\right)}-\overrightarrow{q_i}\Vert \le \delta$ All set up , The one that satisfies the maximum number $P_{MAX}$ It is called the maximum common set (largest common pointset LCP)2
That is to use LCP As a cost function , Solve the transformation matrix ,4pcs Is the use LCP As a measure of the transformation matrix .
wide-base and LCP The combination of metrics makes 4PCS The registration method can deal with noise and outliers

1.2 RANSAC Registration process

be based on RANSAC The alignment process for is simple ： from P Randomly select three different points in the , from Q Randomly select three different points in the , Form a pair of corresponding bases , Calculate the candidate transformation of the alignment base pair Ti, And then calculate the distance Q Midpoint δ- Within a distance P In the point ki The number of . If ki Large enough , received Ti As a good solution . otherwise , Repeat the process by randomly selecting another three points , Thus, different candidate transformations that may improve the current best fit are derived .

1.3 Randomized Alignment

The registration method is RANSAC A variant of the algorithm . The process starts from P Select a base randomly in , The calculation combines the base with Q All possible base aligned transformations in , And verify the registration results . If in RANSAC In the same , In order to achieve a certain probability of success , The method repeats from P Choose from L Different bases . Besides , The validation phase is also randomized ： First , Just verify P A constant number of random points in , And only when the significant parts of the subset match well , To test the rest .
$p_g$ It's from the point cloud $P$ Select a point at random in , This point also happens to appear in the point cloud $Q$ The probability of ,（ That is, the point is in the overlapping area ）. Basis for registration （base） The quantity of is $N$.$p_f$ Is trying L When a good fit cannot be found after times , Probability of algorithm exit . Because we get $P$ Random selection basis in , hypothesis ： $p_f={\left(1-p_g^N\right)}^L$, set up $p_s$ Is the probability of successful registration , There are iterations $L$：

$$L>\log \left(1-p_s\right)/\log \left(1-p_g^N\right)\text{\hspace{1em}(1)}$$

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

4PCS The algorithm flow is based on Randomized Alignment Method . However , The idea of plane congruent set is introduced , from Q Only a small part of P The basis that matches the basis given in , Not from Q Test all possible bases in .

2 Approximate Congruent 4-Points Approximately coplanar four points （4PCS features ）

Affine transformation has the following properties ： Given three collinear points {a,b,c}, ratio ‖a− $\Vert \mathbf{a}-\mathbf{b}\Vert /\Vert \mathbf{a}-\mathbf{c}\Vert$ It is the same. .Huttenlocher[1991] The invariant is used to extract 4 All two-dimensional affine invariant sets of points , These invariants are equivalent under affine transformation . We use a similar approach in three-dimensional space . Given a coplanar 4 Point base , Find its affine equivalence in another point cloud data （ Congruence ）4 Point set . All affine invariants 4 Point set is 3 In the dimension 4 A superset of congruent points . And then , We verify this 4 Whether the point set （ The approximate ） Consistent with the selected base set . Firstly, the extraction method of two-dimensional affine invariant four point set is briefly introduced , Then the extraction process of three-dimensional affine invariant four point set is introduced in detail .

2.1 4 Affine invariance of point pairs

Huttenlocher（1991） This paper introduces a method of extracting two-dimensional affine invariant 4 Point set method . Set up a group Coplanar points X≡ {a,b,c,d}, Not all points are collinear , Two independent ratios of three collinear points are defined . set up ab and cd To intersect at a point e Two lines of . These two ratios $r_1,r_2$：
$$\begin{gathered} r_1=\Vert \mathbf{a}-\mathbf{e}\Vert /\Vert \mathbf{a}-\mathbf{b}\Vert \\ {r}_2=\Vert \mathbf{c}-\mathbf{e}\Vert /\Vert \mathbf{c}-\mathbf{d}\Vert \end{gathered}$$
These two ratios $r_1,r_2$, It has affine transformation invariance , And can uniquely define four points .
Here's the picture ：

Now? , Given a by n A set of points Q, And two affine invariant ratios r1 and r2, We can effectively extract all the 4 A point set , among k yes 4 Number of point sets , As shown below ： For each pair of points q1,q2∈ Q、 Calculate the two intermediate points ：
$${\mathbf{e}}_1={\mathbf{q}}_1+r_1\left({\mathbf{q}}_2-{\mathbf{q}}_1\right)$$
$${\mathbf{e}}_2={\mathbf{q}}_1+r_2\left({\mathbf{q}}_2-{\mathbf{q}}_1\right)$$
As shown in the middle of the figure below ： For the reference point cloud, any 4 From points $r_1,r_2$, At any two points of the point cloud to be registered $q_1,q_2$, From the above formula we can get 2 individual $e_1$( from r1 To calculate the ),2 individual $e_2$ spot ( from r2 To calculate the ). As shown on the right in the following figure ： For two segments （4 A little bit ） for , If any one of the first line segments $e_1$（ One line segment has two $e_1$） And any one of the second line segment $e_2$（ One line segment has two $e_2$） Coincident or within a small distance , Then we can think of the four endpoints corresponding to these two line segments , It may be in the benchmark cloud 4 The corresponding points of points .
Here's the picture ：

2.2 stay 3 Search in dimensional space 4 Coplanar point set pairs

Given the set of slave points P And another point set Q Selected in （ The approximate ） Coplanar point 4 Point base B∈ R3, Our goal is to start from Q Extract and B Nearly equal all 4 The set of points . First of all give B, We calculate the ratio of its two affine invariants on this plane , As mentioned earlier .
then , Use the 3.2 The methods described in Section , From the point of Q in , By affine transformation to extract and B All relevant point sets . Although this method can obtain the required 4 Coplanar point set of points , But there will be a certain number of false matches .
To remove non congruent bases , Let's look at their original location in the point cloud , And verify whether the corresponding set is within a certain distance threshold with the base set B Agreement . then , Use base B and Q Each base in , The best alignment stiffness transformation is calculated by the least square method .

The above program requires a lot of memory , It is not appropriate for a large number of point clouds . Make the following improvements ： Rigid transformation maintains the Euclidean distance between points . Given cardinality B≡ {a,b,c,d}, We first calculate the distance d1=‖a− b‖ and d2=‖c− d‖. Now we only consider Q The distance in is d1 or d2 That's right , The maximum error is δ.

3 4PCS Algorithm

We give two point sets P and Q, The uncertainty measure of the position accuracy of these points （δ>0）, as well as P Zhongke and Q Overlap area score f Estimation method of . Our goal is to find a rigid transformation , send P The maximum number of points in is the same as Q The distance between some points in is less than δ. We propose an algorithm （ See algorithm 1）, Its running time depends on the maximum number of matching points between a given set of points , And randomize it , Find the correct rigid body transformation with high probability .

We first choose a cause 4 A base of coplanar points B⊆ P. actually , We allow some non planarity , Because it is unlikely to exist 4 Coplanar points . We randomly select three points , And select the remaining points , Make the four points together to form a widebase, The base（ The approximate ） Coplanar . Create by selecting points far from each other widebase, This produces more stable coplanar points . However , If the points we choose are too far apart , The selected points may not all be in the overlap area （ For partial matching ）. We use overlapping scores f To estimate the maximum distance . If not provided f The estimate of , We will be in f=1,0.5,0.25,… The algorithm is run in a way that reduces guesswork . Until we reach the required error tolerance . stay f When known , We first select a set of three points that may be located in the overlapping area , Then select the fourth point as described above .

For the slave algorithm SELECTCOPLANARBASE The plane base obtained in the stage B, We can use base points to define affine invariant ratios . However , For approximately planar substrates , We use The nearest point of a pair of connection points To define constant ratios . Now? , We apply the second 2.1 and 2.2 Section to extract a collection U≡ Q Medium 4 Of all subsets of points {U1,U2,…,Us}, They may be associated with B Agreement . For each Ui, Use B and Ui Corresponding information between , We calculate to make B Approach... In the sense of least squares Ui The best alignment stiffness transformation of Ti【Horn 1987 year 】. In order to verify Ti, We calculated Ti*（P）, And calculate Ti*（P） How many points in δ Closer to the Q A point in . by Ti score （ See [Irani and Raghavan 1996]）. Select the one with the highest score Ti, For the final transformation T.

Given a base Bi, We described how to calculate the optimal transformation corresponding to it Ti. According to the number of points aligned , Transform for each alignment Ti Assign a score . Use this program , We are now executing Randomized Alignment The process , And according to the overlap score f The estimate of , Test out L Different bases （ See formula 1）. In all these tests , We choose the transformation matrix with the highest score Topt.

4 Some acceleration techniques

Cooperate with local feature descriptor and local feature （ Curvature , normal ）, Speed up 4 The selection process of coplanar point set .

5 4pcs stay pcl Application in

#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)
{
    




	// Four point registration 
	PointCloud::Ptr pcs(new PointCloud);
	pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;

	fpcs.setInputSource(cloud_source);// Enter the point cloud to be registered 
	fpcs.setInputTarget(cloud_target);// Enter the target point cloud 

	// Parameter setting 
	fpcs.setApproxOverlap(0.6);// Two point cloud overlap 
	fpcs.setDelta(0.5);//Bunny
	//fpcs.setDelta(0.5);//hippo
	fpcs.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);
}

Reference resources ：
《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