# Application of a Real-Time Visualization Method of AUVs in Underwater Visual Localization
## 2 Methodology
### Motion Estimation  
After extracting the ORB point features and LSD line segment features, feature matching between continuous frames is carried out by the traditional violent matching method. After the correspondences between two stereo frames are established, the key points and line segments of the first frame is projected to the next frame. In order to estimate the movements, robust Gauss-Newton minimization is used to reduce the error of the line and key-point projections [11].  
The difference between the transformed coordinates of the 3D point in the first frame image and in the second frame image is denoted as the re-projection error of the point. It can be solved by Equation (1):  
\[\Delta p_{i}(\xi)=\hat{p}_{i}(\xi)-p^{\prime}_{i} \tag{1}\]  
where \(p^{\prime}_{i}\) is the coordinates of the three dimensional points on the second frame, and \(p_{i}\) is the coordinates of the three dimensional points on the first frame. The traditional violent matching method indicates the information that features \(p_{i}\) in frame n and that corresponds to feature \(p^{\prime}_{i}\) in frame \(n+1\). \(\hat{p}_{i}(\xi)\) is the coordinates of the point projected onto the second frame from the first frame image after the transformation of \(\xi\). \(\xi\) is the motion transformation matrix between two frames, including rotation and displacement. \(i\) is the sequence number of the points feature. The first frame and the second frame are consecutive in time.  
Figure 1: Feature map of underwater extraction lines.  
The sum of the distances between the two transformed corresponding endpoints of the two frame images is the re-projection error of the line segment (see Figure 2). It can be solved by Equation (2):  
\[\Delta l_{j}(\xi)=\left[l_{j}^{\prime T}.\Big{[}\ \ \hat{p}_{j}[\xi]\ \ \ \hat{q}_{j}[\xi]\ \ \ \Big{]}\right]^{T} \tag{2}\]  
where \(l_{j}^{\prime}\) is the corresponding line segment feature on the second frame image, and \(l_{j}^{\prime}=p_{j}^{\prime}\times q_{j}^{\prime}\). \(p_{j}^{\prime}\) and \(q_{j}^{\prime}\) are the coordinates of two endpoints of a line segment feature transformed on the second frame image. \(\hat{p}_{j}(\xi)\) and \(\hat{q}_{j}(\xi)\) are the coordinates of two endpoints of a line segment feature transformed from the first frame image to the second frame image. \(l_{j}^{\prime T}.\hat{p}_{j}(\xi)\) is the distance from \(\hat{p}_{j}(\xi)\) to \(l_{j}^{\prime T}\). \(l_{j}^{\prime T}.\hat{q}_{j}(\xi)\) is the distance from \(\hat{q}_{j}(\xi)\) to \(l_{j}^{\prime T}\). j is the sequence number of the line segment feature.  
For the point-line-feature pairs on the two frames, the pose transformation of two frames can be obtained by minimizing the sum of the re-projection errors. It can be solved by Equation (3):  
\[\xi^{*}=argmin\left\{\sum_{i}^{N_{p}}\Delta p_{i}\{\xi\}^{T}\sum_{\Delta p_{i} }^{-1}\Delta p_{i}\{\xi\}+\sum_{j}^{N_{l}}\Delta l_{i}\{\xi\}^{T}\sum_{\Delta l _{i}}^{-1}\Delta l_{i}\{\xi\}\right\} \tag{3}\]  
where \(\sum_{\Delta p_{i}}^{-1}\Delta p_{i}(\xi)\) is the inverse of the covariance matrix of the point re-projection error. \(\sum_{\Delta l_{i}}^{-1}\Delta l_{i}(\xi)\) is the inverse of the covariance matrix of the re-projection error of a line.  
Although the motion transformation matrix \(\xi\) can be obtained according to Equations (1)-(3), it still has some errors because of the mistaken matching of point features and line segment features. Thus we called it an \"estimate\". These errors are eliminated in the closed-loop optimization in the following section.