# MViDO: A High Performance Monocular Vision-Based System for Docking A Hovering AUV
## 2 Mvido: A Monocular Vision-Based System for Docking a Hovering Auv
### Attitude Estimator  
In our previous work [3], we develop an algorithm to visually detect the target formed by the three-color spherical markers rigidly connected. The detection algorithm is based on an image color segmentation which detects the center of mass of each of the markers after an iterative process where, for each captured frame, a threshold is made for the range of the reference colors of the markers. When the three points in the image are identified, we proceed to relative pose estimation. For the 3D pose estimation, the developed algorithm is based on a \(u,v\) image that determines the target position and orientation with regard to the camera frame-mounted onboard AUV. For estimate the relative position of the AUV MARES with regard of the target (see Figure 4), we use the algorithm presented on [23] and we consider the pinhole camera model presented on Figure 5. A relative position vector is defined as represented bellow:  
\[\mathbf{d}= \begin{bmatrix}x_{r}&y_{r}&z_{r}\end{bmatrix} ^{T} \tag{1}\]  
where \(x_{r},y_{r},z_{r}\) are the estimated relative distances of the centre of the color markers from the camera along \(x_{c},y_{c},z_{c}\) directions. The estimate of \(z_{r}\) can be obtained from the following equation in \(z_{r}^{2}\):  
\[z_{r}^{4}\left(k_{2}k_{3}-k_{1}k_{4}\right)^{2}-z_{r}^{2}\left(k_{1}^{2}+k_{2} ^{2}+k_{3}^{2}+k_{4}^{2}\right)+1=0, \tag{2}\]  
where, \(k_{1}\), \(k_{2}\), \(k_{3}\) and \(k_{4}\) are known constants, determined in function of the coordinates of the markers in the image, the focal length of the camera and the fixed real distances between the markers on the target. That constants are given by:  
\[k_{1}=\frac{u_{C}-u_{A}}{2*a*f} \tag{3}\]  
\[k_{2}=\frac{v_{C}-v_{A}}{2*a*f} \tag{4}\]  
\[k_{3}=\frac{u_{A}+u_{C}-2*u_{B}}{2*b*f} \tag{5}\]  
\[k_{4}=\frac{v_{A}+v_{C}-2*v_{B}}{2*b*f}, \tag{6}\]  
where \((u_{A},v_{A});(u_{B},v_{B});(u_{C},v_{C})\) are the image coordinates of the markers, \(a\) is the distance from the marker \(A\) and \(C\) to the center of the target reference frame and \(b\) is the distance from the marker \(B\) to  
Figure 3: Docking station. the center of target reference frame,\(f\) is the focal length of the lens, More details of the algorithm can be found in [23]. In terms of the relative orientation estimation, the algorithm presented in [23] has some assumptions and simplifications that do not fit our problem. There are two ambiguities when using a target with only three markers: an ambiguity in the signal when the target is rotated around the \(x\) axis and another ambiguity when the target is rotated around the \(y\) axis. For that reasons, we decided to develop a new algorithm for attitude estimation. For that, we considered the Figure 6. In first place, we rotate the 'plane \(r\)' (the plane of the target) in order to place it parallel to the 'plane \(c\)' (a mirror plane of the CCD sensor of the camera). Then, a scale factor is applied in order to obtain the same limits for both planes. Finally, a translation is applied to match the two planes. Figure 4: Definitions of the camera and target reference frames for relative localization. Figure 5: Pinhole camera model. This will allow us to transform any point from one referential frame into the other:  
\[\mathbf{p_{c}}=\alpha\mathbf{R}\mathbf{p_{r}}+\mathbf{t_{r}} \tag{7}\]  
In this perspective projection \(\alpha\) is a scale factor (which depends on the focal length), \(\mathbf{R}\) is the rotation matrix, \(\mathbf{t_{r}}\) is the translation term, \(\mathbf{p_{c}}\) and \(\mathbf{p_{r}}\) are a point in the plane \(c\) (a mirror plane of the sensor of the camera) and in the plane \(r\) (plane of the target) respectively. With reference to Figure 4 and according to the Figure 6 the vectors \(\mathbf{\bar{p}_{c}}\) and \(\mathbf{\bar{p}_{r}}\) are defined as:  
\[\mathbf{\bar{p}_{c}}\in\mathbb{R}^{3}:\mathbf{\bar{p}_{c}}= \begin{bmatrix}p_{ c_{x}}\\ p_{c_{y}}\\ 0\end{bmatrix}  \tag{8}\]  
\[\mathbf{\bar{p}_{r}}\in\mathbb{R}^{3}:\mathbf{\bar{p}_{r}}=