The relationship between point
\({\mathbf {P}}=[X~Y~Z]^T\), which exists in a 3D space in the camera coordinate, and point
\({\mathbf {p}}=[x~y]^T\), which is the projection of point
\({\mathbf {P}}\) onto a image plane, can be expressed by Eq. (
1), where
\({\mathbf {K}}\) is the intrinsic parameter of the camera and
s is the scale factor that makes the third line on the left side 1. In this study, the intrinsic parameter K is obtained by camera calibration.
$$\begin{aligned} s\begin{bmatrix}{\mathbf {p}}\\ 1\end{bmatrix}={\mathbf {K}}{\mathbf {P}} \end{aligned}$$
(1)
By transforming Eq. (
1), the relationship between a sphere in a 3D space whose center is
\({\mathbf {P}}_c=[X_c ~Y_c~Z_c]^T\) and a circle whose center is
\({\mathbf {p}}_c=[x_c ~x_c]^T\), which is a projection of the sphere onto a 2D image, can be expressed by Eq. (
2).
$$\begin{aligned} {\mathbf {P}}_c=s{\mathbf {K}}^{-1}\begin{bmatrix}{\mathbf {p}}_c\\ 1\end{bmatrix}=s\begin{bmatrix}k_{11} &{} 0 &{} k_{13} \\ 0 &{} k_{22} &{} k_{23} \\ 0 &{} 0 &{} 1\end{bmatrix}\begin{bmatrix}x_c \\ y_c \\ 1\end{bmatrix} \end{aligned}$$
(2)
Because the center
\({\mathbf {p}}_c\) of the circle was obtained in the previous section, the center
\({\mathbf {P}}_c\) of the sphere corresponding to the circle is determined only by
s when the intrinsic parameter
s of the camera is known. Conversely, the relationship between the radius
R of the sphere and the radius
r of the circle can be expressed by Eq. (
3).
$$\begin{aligned} {\mathbf {P}}_c+\begin{bmatrix}R\\ 0\\ 0\end{bmatrix}=s{\mathbf {K}}^{-1}\left( \begin{bmatrix}{\mathbf {p}}_c\\ 1\end{bmatrix}+\begin{bmatrix}r\\ 0\\ 0\end{bmatrix}\right) \end{aligned}$$
(3)
Eliminating the sphere and circle centers from Eqs. (
2) and (
3) yields Eq. (
4). Because the radius of the circle is known, the radius of the sphere is determined only by
s.
$$\begin{aligned} R=sk_{11}r \end{aligned}$$
(4)
Substituting Eqs. (
2) and (
4) into Eq. (
5), which is the equation of the sphere, yields Eq. (
6).
$$\begin{aligned} (X-X_c)^2+(Y-Y_c)^2+(Z-Z_c)^2&=R^2 \end{aligned}$$
(5)
$$\begin{aligned} (X-sS_x)^2+(Y-sS_y)^2+(Z-sS_z)^2&=(sS_r)^2 \end{aligned},$$
(6)
where each coefficient in Eq. (
6) is as follows.
$$\begin{aligned} S_x&=k_{11}x_c+k_{13} \end{aligned}$$
(7)
$$\begin{aligned} S_y&=k_{22}y_c+k_{23}\end{aligned}$$
(8)
$$\begin{aligned} S_z&=1\end{aligned}$$
(9)
$$\begin{aligned} S_r&=k_{11}r \end{aligned}$$
(10)
The objective function of the least squares method to fit a sphere from a 3D point cloud
\({\mathbf {P}}_i=[X_i ~Y_i~Z_i]^T\) can be expressed by Eq. (
11).
$$\begin{aligned} f(s)&=\sum _ie_i^2 \end{aligned}$$
(11)
where
\(e_i\) is the following equation.
$$\begin{aligned} e_i=(X_i-sS_x)^2+(Y_i-sS_y)^2+(Z_i-s)^2-(sS_r)^2 \end{aligned}$$
(12)
To organize Eq.
12), the coefficients are set as in the following equations.
$$\begin{aligned} A&=S_x^2+S_y^2+1-S_r^2 \end{aligned}$$
(13)
$$\begin{aligned} B_i&=S_xX_i+S_yY_i+Z_i \end{aligned}$$
(14)
$$\begin{aligned} C_i&=X_i^2+Y_i^2+Z_i^2 \end{aligned}$$
(15)
Using these coefficients, Eq. (
11) can be transformed into Eq. (
16).
$$\begin{aligned} f(s)&=\sum _i\left( As^2-2Bs+C\right) ^2 \end{aligned}$$
(16)
The
s that minimizes the objective function is the solution to Eq. (
17), which is a cubic equations, where RANSAC [
20] (Random sample consensus) is used to suppress the effect of a noisy point cloud.
$$\begin{aligned} \frac{\delta f(s)}{\delta n}=\sum _i\left( A^2s^3-3ABs^2+(AC+2B^2)s-BC\right) \end{aligned}$$
(17)
The 3D coordinates and radius of the center of the target sphere are obtained by substituting the obtained
s into Eqs. (
2) and (
4). By assuming that the sphere exists on the extension of the circle detected in the 2D image, instead of directly fitting the sphere from the point cloud, it is possible to fit the sphere even from an inaccurate point cloud.
Based on the calibration results between the robot arm and the camera, the last step is to transform the coordinates to the robot coordinate system.