Fast Global Registration Fast global registration

This article comes from CSDN Dianyunxia, ​​the reptile is self-respecting and treats itself as an individual.

1. Algorithm process

  The Fast Global Registration algorithm uses quaternary constraints to eliminate wrong relationship pairs in the matching relationship set, and finally realizes the registration of two sets of point clouds based on the optimization solution of the relationship set. The algorithm is mainly divided into three steps: the generation of relation set based on quaternary constraints, the construction of objective function based on relation set, and the optimization process of objective function.

2. Generation of quaternary constraint relationship sets

  The generation of relational sets based on quaternary constraints is mainly divided into four stages: the calculation of FPFH feature descriptors of point sets, the generation of initial relational sets, the mutual nearest neighbor constraints of relational sets, and the quaternary constraints of relational sets.

1. Calculation of the FPFH feature descriptor of the point set: use the fast feature histogram (FPFH) to describe the feature information of each point. The reason for choosing this feature descriptor is that it can be calculated quickly in a short time, and at the same time, more feature information in the field around the feature point can be obtained.
2. Generation of initial relation set: For point cloud PPFor each point p ∈ P p ∈ Pin$P$$p\in P$ , in the feature set$F\left(Q\right)$ finds$The\; nearest\; neighbor\; of\; F\left(p\right)$ , and for the point cloudQQEvery q in$Q$$q\in Q$ , considered inF ( P ) F( P)FindF ( q ) F( q )$in\; F$$($$P$$)$$The\; nearest\; neighbor\; of\; F\left(q\right)$ . Store the initial relationship pairs generated by the above steps into the set$K_1$middle. This relation set $K_1$can be used as input during optimization of the objective function. In fact, $K_1$has a very high number of false-matching pairs (i.e. has high outliers). Next, reduce the relation set K 1 K_1 by performing constrained optimization on the relation set$K_1$false relation pairs, reducing the number of outliers.
3. Mutual nearest neighbor bundles of relationship sets: For the relationship set $K_1$, randomly pick a set of relationship pairs $(p,q)$ if and only if in$F\left(P\right)$ ,$F\left(p\right)$ is$The\; nearest\; neighbor\; of\; F\left(q\right)$ , at the same time in$F(Q)$ 中，$F\left(q\right)$ is$The\; nearest\; neighbor\; of\; F\left(p\right)$ . For the relationship pair ( p , q ) ( p , q )that satisfy the mutual nearest neighbor constraint$(p,q)$ , store it into the set$K_2$middle.
4. Quaternary Constraints on Relational Sets: In order to reduce the relational set $K_2$The impact of the wrong matching point pair on the experiment, consider $K_2$Adding a further constraint, randomly from $K_2$Select 4 groups of relationship pairs $(p_1，q_1)，(p_2，q_2)，(p_3，q_3)，(p_4，q_4)$ , check the quadruple$(p_1，p_2，p_3，p_4)$ 和$(q_1，q_2，q_3，q_4)$ compatibility. Specifically, whether the four sets of relationship pairs meet the following conditions:
   

where $t=0.95$ . Intuitively, this constraint verifies whether the four sets of relation pairs are compatible. Store the tuple relationship pairs that meet the conditions in the set$K_3$middle. Figure 1 is a diagram of the quaternary constraints.

After the above two conditions are constrained, the relationship set $K=K_3$。

3. Construction of objective function based on relational set

  Point cloud $P$ and$For\; the\; registration\; of\; Q$ , the goal is to find an optimal rigid transformation matrix$T$ , the point cloud$Q$ and$P$ registration. algorithm optimized based on$P$ andQQA robust objective function for the correspondence between$Q.$First, relation sets are established by performing fast feature matching and relation constraints. During the optimization of the objective function, the relation set does not need to be recalculated, so the algorithm has a great advantage for the registration of dense and complex point clouds.
  Set$K=(p,q)$ is determined by$P$ andQQThe relationship set generated by the matching points in$Q\; ,\; the\; objective\; function\; is\; established\; based\; on\; the\; sum\; of\; the\; squares\; of\; the\; errors\; between\; the\; corresponding\; relationship\; pairs\; in\; the\; relationship\; set.$The specific form of the objective function is as follows:

  here$\rho (\cdot )$ is a robust penalty function. Proper use of a robust penalty function is very important, because many error terms in the objective function (2) are generated by wrong matching relations, which can reduce the impact of wrong relations on the objective function, thereby achieving better matching quasi-precision. At the same time, in order to ensure the calculation speed, it is not desirable to perform additional calculations such as downsampling and verification during the optimization process. In this paper, an estimator$\rho$ , which will automatically perform the verification without incurring additional computational cost. The specific expression is:

  Figure 2 shows the different μ μGeman-McClure estimator image of$\mu \; value.$As can be seen from Figure 2, the residuals are penalized in a least-squares manner, while the fast flattening of the estimator neutralizes the outliers of the relation set. Parameter$\mu$ controls the significant impact of residuals on the objective function.
  Because equation (2) is not easy to be directly optimized and solved, line processing is introduced. Specifically, let$L=lp,q,0<lp,q<1$ , the line processing indicates the relation pair$p$ andqqA discontinuity between$q$$lp，q\to 0$ discontinuity exists, or when$lp，q\to 1$ Discontinuity does not exist. Optimize about$T$ andLLThe joint objective function of$L\; ,\; the\; specific\; form\; is\; as\; follows:$

  Here $\Psi (lp,q)$ is a prior term, it is a penalty function, which means that for$p$ andqqThe penalty for discontinuity between$q$

is of the form: In order to make$E(T,L)$ is minimized for equation (5) with respect to$lp,q$ to find the partial derivative, let the derivative function be zero, and get the following formula:

Solve for $lp,q$ , the calculation results are as follows:

Finally, the $lp,q$ into$In\; E(T,L)$ , Equation (4) is transformed into Equation (2). Therefore, the transformation matrix TTproduced by optimizing the objective function (4)$T$ , is also the optimal solution for the original objective function (2).

  Equation (4) is non-convex, and its shape is determined by the parameter $\mu$ to control. In order to set$\mu$ and reduce the influence of the local minimum, using the gradient non-convexity method to solve. From equation (4), it can be seen that$\mu$ balances the prior and alignment terms. Larger$\mu$ makes the objective function smoother and allows some false relation pairs to participate in the optimization, even if they cannot be transformed$T$ is tightly aligned. The optimization of the objective function (4) of the algorithm in this paper begins with a very large$m=D^2$ values ​​where$D$ is the diameter of the largest surface. Parameter$\mu$ keeps decreasing during optimization until$m＜d^2$ stop optimization, where$\delta$ is the distance threshold of the ground truth relationship.

4. Optimization of the objective function

  Locally linearize the transformation matrix T into a 6-dimensional vector $X=(w,t)=(a,b,c,a,b,c)$ This vector contains the rotation component$w$ and the translation component$t$。$T$由$The\; linear\; function\; of\; \xi$ is approximated as:

  Here $T^k$ is the transformation matrix found in the previous iteration. Equation (4) becomes about$\xi$ is the least squares objective function. Consider using the Gauss-Newton method to compute$The\; value\; of\; \xi$ , get:

  where$r$ is the residual vector,$Jr$ is its Jacobian matrix. ξ ξobtained by using equation (9)$\xi$ and$T^{The\; value\; of\; k}$ , to update TTby equation (8)$T.$ _ The two steps optimize the same objective function (equation (4)), so the optimization process can ensure the convergence of the algorithm. The flow chart of the algorithm in this paper is shown in Table 1.