# Color Point Cloud Registration Algorithm Based on [PERSON]
## 3 Algorithm Implementation
### Error Function  
In general, the traditional ICP can get accurate transformation relations on the basis of coarse registration. ICP belongs to the greedy algorithm, and because of the corresponding relationship determined by the nearest point principle, it is easy to fall into the local optimal solution in iteration, which usually occurs in the parts with similar geometric structure. For example, when the source and target point cloud are two planes, there are many solutions if they are on the same plane. Traditional error function only contains the geometric information and may lead to the local optimal solution in the mentioned condition previously. The error function we used includes not only geometric information but also color information. In the previous chapter, we introduce the robustness of hue and it proves that hue has great robustness about the lighting condition which is usually different. Hence, we use the hue as the color information to constrain the error function. Let \(w\in R^{3\times 1}\) be the rotation angle of the current transformation and \(t\in R^{3\times 1}\) be the translation vector. The error function is about the transformation \((w,t)\) and the expression of it is as follows:  
\[E(w,t)=E_{H}(w,t)+\sigma E_{G}(w,t) \tag{7}\]  
where \(E_{H}(w,t)\) represents the hue error at the transformation \((w,t)\) while \(E_{G}(w,t)\) is the geometric error, and \(\sigma\) is a scalar factor, which is aimed to adjust them to the same order of magnitude to balance their contribution to the optimization. According to the statistics of the point cloud, the range of hue is [0, 1] and that of geometry is [0, 0.03], and adding them directly will result in enhancing the hue error and ignoring the influence of geometric error, considering their range, where \(\sigma=30\) is an appropriate value. Let \(P\) and \(Q\) be the target and source point cloud and our goal is to find the transformation that transforms \(Q\) to \(P\). \(\mathcal{K}=(p,q)\) be the correspondence set according to the nearest point principle [1], \(p\in P\) and \(q\in\mathcal{Q}\). Those pairs with the distance beyond the preset threshold are removed. \(q^{\prime}\) is the point \(q\in Q\) through the transformation (\(\omega\),\(t\)), and their relationship is as follows:  
\[q^{\prime}=s(q,w,t)=R(w)q+t \tag{8}\]  
where \(R(w)\in R^{3\times 3}\) is the rotation matrix of \(w\), and \(s\) represents the transformation with rotation angle \(w\) and translation vector \(t\). After the transformation, \(q^{\prime}\) is transferred to the target point cloud \(P\) from source point cloud \(Q\) and can find its correspondence point \(p\in P\). Moreover, if the transformation is correct, the target point cloud \(P\)'s hue at \(q^{\prime}\) would be very similar to that at \(p\). For the common condition, \(q^{\prime}\) would not fall at \(p\) but in its neighborhood \(\mathcal{N}_{p}\), so we should find the hue information at \(q^{\prime}\) in the neighborhood of \(p\). According to Equations (3) and (5), we can approximate the hue distribution in \(\mathcal{N}_{p}\) with that on the tangent plane of \(p\), \(H_{p}(u)\), which can be estimated through Equation (4), so we should find the projection of \(q^{\prime}\) at the tangent plane. Let \(q^{\prime\prime}\) be the projection of \(q^{\prime}\) on the tangent plane of \(p\):  
\[q^{\prime\prime}=f(q^{\prime}) \tag{9}\]  
The space relationship of point \(p_{A}q^{\prime}\) and \(q^{\prime\prime}\) is shown as Figure 4. Point \(q\) is in the source point cloud \(Q\), after transformation \((w,t)\), and \(q^{\prime}\) is in the target point cloud \(P\). Point \(q^{\prime\prime}\) is the projection of \(q^{\prime}\) and is on the tangent plane of \(p\).