## 2 The Algorithm
### Conditional Initial Value Modification  
As mentioned above, the optimization of a 3D point \(p\) is sensitive to its initial position and normal. When 3D points become dense enough after some iterations of expansion, it is possible to predicate the local variation of a surface by quadratic fitting over a number of neighboring 3D points. The fitted surface is used to modify the initial value. This is done following a so-called conditional-double-surface-fitting method, as described below. Before each fitting, an average distance \(\overline{d}\) from \(N_{1}\) neighbor points to point \(p\) is calculated to check whether the neighbor points are dense enough. Only on the condition that \(\overline{d}<th_{d}\) is a quadratic fitting performed, since at this time the fitted surface might approximate the local surface well. Then the center \(\overline{P}\) and average normal \(\overline{n}\) of its neighbor points are computed. The fitting will be carried out in a new coordinate system which take \(\overline{P}\) as the origin and \(\overline{n}\) as the \(Z\)-axis. The new coordinates of point \(p\) and its neighbor points can be computed by  
\[P_{n}=\left[ \begin{array}{cccc}1&0&0\\ 0&1&0\\ 0&0&1\end{array} \right.\quad-\quad\mathbf{R}^{{}^{\prime}}\times\overline{P} \left]\right.\times\left[ \begin{array}{cccc}&\mathbf{R}^{{}^{\prime}}\times P \\ 1&1&1&1\end{array} \right] \tag{11}\]  
where \(\mathbf{R}^{{}^{\prime}}\) is the transposition of the rotation matrix, which rotates the \(Z\)-axis of the world coordinate system to \(\overline{n}\). \(P_{n}\) and \(P\) are the coordinates of point \(p\) and its neighbor points in the new and old coordinate systems, respectively. Next, edge points are removed from neighbor points to find a more accurate center. To do this, the \(x\) and \(y\) components of \(P_{n}\) are sorted and the new center \(\overline{P_{n}}\) is computed by averaging over 80% of the central points, using Equation (12)  
\[\left\{ \begin{array}{l}\overline{P}_{nx}=\sum_{i=Ns}^{Ne}P_{snx}(i)/(Ne-Ns+1 )\\ \overline{P}_{nx}=\sum_{i=Ns}^{Ne}P_{sny}(i)/(Ne-Ns+1)\end{array} \right. \tag{12}\]  
where \(\overline{P_{nx}}\), \(\overline{P_{ny}}\) are the \(x\) and \(y\) components of \(\overline{P_{n}}\). \(Ns\approx 0.1\times(N_{1}+1)\). \(Ne\approx 0.9\times(N_{1}+1)\). \(P_{snx}\) and \(P_{sny}\) are the sorted \(x\) and \(y\) components of \(P_{n}\). Since the fitted surface usually matches well with the true surface only near the center, if point \(p\) is far away from the new center, the fitting and the modification procedure will be stopped. The point \(p\) is regarded as near the new center if it satisfies  
\[\left\{ \begin{array}{l}\left|p_{nx}-\overline{P}_{nx}\right|/\left|P_{snx}( Ne)-P_{snx}(Ns)\right|<th_{c}\\ \left|p_{ny}-\overline{P}_{ny}\right|/\left|P_{syv}(Ne)-P_{sny}(Ns)\right|<th_ {c}\end{array} \right. \tag{13}\]  
where \(p_{nx}\) and \(p_{ny}\) are the \(x\) and \(y\) components of points \(p\) in new coordinate system. \(th_{c}\) is a fixed threshold. If the above two conditions are satisfied, quadratic surface fitting will be conducted twice following Equation (14)  
\[P_{nz}=c_{1}P_{nx}^{2}+c_{2}P_{ny}^{2}+c_{3}P_{nx}P_{ny}+c_{4}P_{nx}+c_{5}P_{ny}+c _{6} \tag{14}\]  
where \(c_{1}\) to \(c_{6}\) are coefficients. As illustrated in Figure 3, the surface after the first fitting (dotted line) might lay a little away from the true surface (solid line) due to the existence of some irregular points \(p_{1-3}\). In our experiments, we sort the distances of point \(p\) and its neighbors to the fitted surface in descending order. Instead of identifying each irregular point using a threshold, we simply discarded the first 15% of points and used the rest of the points for the second fitting. The fitted surface after the second fitting (dashed line) might lay much closer to the true surface, in which case point \(p\) is projected onto the fitted surface and the position and normal of the foot \(p_{\rm s}\) are used to modify the initial value.