## 2 6-DOF Displacement Estimation Using Vision Sensor  
The 6-DOF relative displacements that include translational and rotational displacements in three axes can be estimated by using positions of feature points in world coordinates and the intrinsic parameters of a vision sensor. The intrinsic parameters determine the optical properties of the camera lens, including the focal lengths, principal points, and distortion coefficients. Figure 2 represents the geometry view of the feature points in both the world and image planes. In the figure, \(Q_{i}\) and \(q_{i}\) (\(i\) = 1,..., \(N\)) denote the corresponding points of the world and image planes, respectively, where \(N\) is the number of feature points. The points in the world plane, \(Q_{i}\), defined as \(Q_{i}=[X\ Y\ Z\ 1]^{T}\), are represented in the three-dimensional coordinate system. The corresponding points, \(q_{i}\), defined as \(q_{i}=[u\ v\ 1]^{T}\), are represented in two-dimensional space. The relationship between the two planes can be expressed in terms of matrix multiplication, as follows:  
\[\left[ \begin{array}{c}u\\ v\\ 1\end{array} \right]=\left[ \begin{array}{ccc}f_{u}&0&c_{u}\\ 0&f_{v}&c_{v}\\ 0&0&1\end{array} \right]\left[ \begin{array}{c}x_{d}\\ y_{d}\\ 1\end{array} \right], \tag{1}\]  
\[\left[ \begin{array}{c}x_{d}\\ y_{d}\end{array} \right]=\left[ \begin{array}{c}x(1+K_{1}r^{2}+K_{2}r^{4})+2K _{3}xy+K_{4}(r^{2}+2x^{2})\\ y(1+K_{1}r^{2}+K_{2}r^{4})+2K_{3}(r^{2}+2 xy^{2})+K_{4}xy\end{array} \right] \tag{2}\]  
\[\left[ \begin{array}{c}X_{c}\\ Y_{c}\\ Z_{c}\end{array} \right]=\left[ \begin{array}{ccc}r_{11}&r_{12}&r_{13}&t_{x}\\ r_{21}&r_{22}&r_{23}&t_{y}\\ r_{31}&r_{32}&r_{33}&t_{z}\end{array} \right]\left[ \begin{array}{c}X\\ Y\\ Z\\ 1\end{array} \right] \tag{3}\]  
where \(f_{u}\) and \(f_{v}\) are the focal length, \(c_{u}\) and \(c_{v}\) represent the principal point where the focal axis of the camera intersects the image plane; \(K_{1}\) and \(K_{2}\) are the radial distortion coefficients, \(K_{3}\) and \(K_{4}\) are the tangential distortion coefficients; \(r\) and \(t\) are parameters of the rotation matrix and translation vector. In Equation (2), \(x=X_{c}/Z_{c}\), \(y=Y_{c}/Z_{c}\), and \(r^{2}=x^{2}+y^{2}\), where \(X_{c}\), \(Y_{c}\), and \(Z_{c}\) are defined in Equation (3). The homography matrix composed of intrinsic and extrinsic camera parameters explains how to map pixels on a 2D image to the corresponding real-world coordinates in 3D scenes, as shown in Equations (1)-(3) [17,18]. By using the feature points on the same level, 3 \(\times\) 3 sized homography matrix can be used with the given 2D-to-2D point correspondences. Since the degree of freedom of the homography matrix is equal to eight, at least four point-to-point correspondences are required. In other words, the rotation matrix and the translation vector can be obtained with the known positions of more than four feature points (\(N\geq 4\)) [18]. By calculating rotational and translational displacements from the vision sensor to the target and the fixed structures, the relative 6-DOF displacement of the target structure can be estimated. Figure 3 shows the entire process of the relative displacement estimation between the two structures using image processing techniques. The figure shows that the camera captures the image of the structures, then the camera lens distortion is corrected by using the previously calculated intrinsic parameters. From the undistorted image, the feature points of the structures, including corners, are detected by using various image processing techniques, such as binarization, and corner detection at the sub-pixel level. By calculating at least four feature point positions, 6-DOF displacement between the camera and the structures can be estimated. The relative displacement can be estimated by using the previously calculated displacements on each structure. The relative displacement between two structures, the fixed and target structures, can be estimated using the following equations:  
\[