# 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
### Resilience to Occlusions and Outliers Rejection
#### 2.3.2 Considering Geometrical Constraints  
In the filter block (see Figure 7) we consider geometrical constraints related to the target in order to improve the performance of the filter. Since the target geometry is well-known, such as the distance between the three markers (see Figure 8) and the radius of each marker, we used this information to condition filter estimates.  
If the underwater scene contains other objects with a similar color to that of the markers, the colour cues became more prone to ambiguity. For that reason, it is necessary to use the target geometry constraints to allow the markers to be located with low ambiguity. When we take into account the geometric constraints, the procedure we implemented is as follow: considering the first instant \(t=0\) when the markers \(A\) and \(B\) are detected, the estimated distance \(d_{AB}\) at \(t=0\) is \(d_{AB}^{0}\). Supposing a time progression, at the instant \(t=1\), the distance \(d_{AB}^{1}\) will change according to the movement of the AUV, that is:  
\[d_{AB}^{1}=d_{AB}^{0}+u_{AUV}^{I}, \tag{23}\]  
where \(u_{AUV}^{I}\) is the movement of the AUV projected in the frame \(I\) of the camera. That movement depends on the proximity and pose of the AUV with regard to the target, and depend on the properties of the camera sensor and lens, namely the sensor size and the focal length:  
\[u_{AUV}^{I}=\hat{F}\left(x,y,z,\theta,\Psi,f,s_{x}\right)=d^{AB}, \tag{24}\]  
where \(\theta,\phi,\Psi\) are the angles of pitch, roll and yaw respectively and \(s_{x}\) is the physical dimension of the CCD sensor in the \(x\) axis.  
Knowing that,  
\[\left(x_{c},y_{c}\right)=\left(f\frac{x_{w}}{z_{w}},f\frac{y_{w}}{z_{w}}\right), \tag{25}\]  
where \(x_{w},y_{w},z_{w}\) are coordinates in the referential associated with the center of the camera lens \(W\), \(x_{c},y_{c}\) are the coordinates in the referential associated with the CCD of the camera \(C\), \(f\) is the focal length of the camera lens. We could say that,  
\[\left(d^{AB}\right)^{2}=\left(f\frac{x_{w}^{A}s_{x}}{z_{w}^{A}}-f\frac{x_{w}^ {B}s_{x}}{z_{w}^{B}}\right)^{2}+\left(f\frac{y_{w}^{A}s_{x}}{z_{w}{}^{A}}-f \frac{y_{w}^{B}s_{x}}{z_{w}{}^{B}}\right)^{2}. \tag{26}\]  
The \(u_{AUV}^{I}\) depends on the pose of the AUV, However, if the initialized distance is based on a wrong marker position extraction, since the system has three particle filters, one per marker, the system will automatically reset the wrong initialized particle filter.  
Figure 8: Estimated distances for the target.  
Considering that the movement uncertainty is described by a Gaussian function:  
\[G(x)=\frac{1}{\sigma\sqrt{2\pi}}e^{\frac{-\left(x-y\right)^{2}}{2\sigma^{2}}}. \tag{27}\]  
The probability of the particle \(pt_{i}^{A}\) is given by:  
\[P\left(pt_{i}^{A}\right)=\frac{e^{\frac{-\left(a_{pt_{i}}-\frac{d_{AB}^{0}-t_{ AU}^{A}}{2\sigma^{2}}\right)}{2\sigma^{2}}}}{\sigma\sqrt{2\pi}}, \tag{28}\]  
where \(d_{pt_{i}}\) is the distance of particle \(pt_{i}\) to the marker of reference (see Figure 9), and \(\sigma\) is the value of the standard deviation.  
When there is no AUV velocity feedback in the particle filter, then: