# Fast Point Cloud Registration Method with Incorporation of RGB Image Information
## 2 Methods
### Accurate Registration of Point Cloud with FR-ICP  
Given two sets of point cloud data \(P=\{p_{1},\ldots,p_{M}\}\) and \(Q=\{q_{1},\ldots,q_{N}\}\), ICP aligns \(P\) with \(Q\) by optimizing the rigid transformations (rotation matrix \(R\) and translation matrix \(t\)) on \(P\):  
\[\begin{array}{l}\underset{R,t}{min}\sum_{i=1}^{M}(D_{i}(R,t))^{2}\end{array} \tag{10}\]  
where \(D_{i}(R,t)=min_{qQ}\parallel Rp_{i}+t-q\parallel\) is expressed as the distance from the transformed point \(Rp_{i}+t\) to the point cloud \(Q\). ICP solves the point cloud registration issue by alternating iterations in two steps as follows:  
Figure 6: Results of transition down with voxel grid.  
1. Correspondence step: seek the nearest corresponding point \(\hat{q_{i}}^{(k)}\) of \(p_{i}\in P\) in \(Q\) based on the transformation \(\left(R^{(k)},t^{(k)}\right)\): \[\hat{q_{i}}^{(k)}=\underset{q\in Q}{min}\parallel R^{(k)}p_{i}+t^{(k)}-q\parallel\] (11) 2. Alignment step: update the transformation by minimizing the Euclidean distance between the corresponding points: \[\left(R^{(k+1)},t^{(k+1)}\right)=\underset{R,t}{min}\sum_{i=1}^{M}\parallel Rp _{i}+t-\hat{q_{i}}^{(k)}\parallel^{2}\] (12)  
The FR-ICP algorithm uses a Welsch-based function to measure the error based on ICP, and [PERSON]'s accelerated majorization-minimization (MM) [30] algorithm is applied to accelerate the iteration speed of the objective function to make the error minimized. Specifically, the registration is denoted as  
\[\underset{R,t}{min}\sum_{i=1}^{M}\varphi_{u}(D_{i}(R,t)) \tag{13}\]  
where \(\varphi_{u}\) is the Welsch function:  
\[\varphi_{u}(x)=1-exp(-\frac{x^{2}}{2u^{2}}) \tag{14}\]  
\(u>0\) is a user-specified parameter. The Welsch function is used as the metric error, and its quadratic proxy function is minimized to obtain the optimal solution, and the final registration is reported as  
\[\left(R^{(k+1)},t^{(k+1)}\right)=\underset{R,t}{min}\sum_{i=1}^{M}w_{i} \parallel Rp_{i}+t-\hat{q_{i}}^{(k)}\parallel^{2} \tag{15}\]  
where \(\omega_{i}=exp(-\parallel R^{(k)}p_{i}+t^{(k)}-\hat{q_{i}}^{(k)}\parallel^{2}/ \left(2u^{2}\right))\). This paper limits the parameter range of \(u\) with \(u_{min}=\overline{E}_{Q}/3\sqrt{3}\) and \(u_{max}=3\overline{D}^{(0)}\), where \(\overline{E}_{Q}\) is computed as the median distances from each point \(q_{i}\in\ Q\) to its six nearest points on \(Q\) and \(\overline{D}^{(0)}\) is the median of all initial point-wise distances \(\left\{(R^{(0)},t^{(0)})\right\}\).  
The FR-ICP algorithm is based on minimizing the Euclidean distance between corresponding points in two sets of point sets to achieve the optimal rigid transformation between two point clouds, so there is also the problem of sensitivity to the initial position. In this paper, initial alignment and FR-ICP were used to complete the whole registration process to avoid registration failure caused by falling into the local optimum, and also to improve the efficiency of the accurate registration stage. In the accurate registration stage, the results of the initial alignment of the point cloud were treated as the input of FR-ICP. The default parameter settings of FR-ICP were used to register the two sets of points to ensure the ultimate accuracy.