# A Collaborative Human-Robot Framework for Visual Topological Mapping of Coral Reefs
## 2 Method
### Graph Optimization and Scale Adjusting  
The process described so far is useful to obtain a topological representation of the environment. However, some spatial inconsistencies can arise from accumulation of small errors when concatenating the relative positions between nodes. This is more notorious when recognizing previously visited places.  
Figure 4: Calculation of the base lines. The Bresenham algorithm is applied to every pair of points defined by the central point (blue) and all the points in the grid’s perimeter (yellow).  
Figure 5: Given a similarity matrix \(\tilde{S}\), we look for the starting points with scores greater than the threshold \(\mu\) in the last row. Then, we evaluate the score for every predefined line in \(C\) translated to each starting point (\((i_{sp_{k}},j_{sp_{k}})\)). The winning sequence \(c^{*}\) is the one with highest score from all sequences. In this example is the matching sequence starts in point 2.  
solved by any proper method to obtain \(\Delta p_{i}^{*}\) for every node \(i\) in the graph. The adjusted position \(p_{i}^{*}\) for a node \(i\) is obtained from:  
\[p_{i}^{*}=\hat{p}_{i}+\Delta p_{i}^{*}. \tag{9}\]  
This procedure is executed only when a previous visited place is recognized. The presented process is useful to correct spatial inconsistencies due to cumulative errors in the concatenation of rigid transformations, however, another source of spatial inconsistencies is the scale difference between the images from the same place. Figure 7 contains an example of images taken from the same place but at different distances away from the scene.  
The issue of different scales between images from the same scene occurs when dealing with images from different explorations, as we have assumed that during the same exploration, the agent will remain at a constant distance from the coral reef. To tackle this issue, we calculate the relative scale between the images from the two explorations. We will denote the map obtained from a previous exploration as \(G_{0}\) and \(G_{1}\) as the one currently being created. To calculate the relative scales between \(G_{0}\) and \(G_{1}\), first, we need to recognize the same place in both maps. This is achieved by executing the procedure in Section 2.3. Once the same place in both graphs has been identified, the scale \(s_{a,b}\) between both images can be estimated by solving the same expression in (2) with \(A_{ab}\) defined as:  
\[A_{ab}=\begin{bmatrix}\alpha_{11}&\alpha_{12}&\alpha_{13}\\ -\alpha_{12}&\alpha_{11}&\alpha_{23}\\ 0&0&1\end{bmatrix}=\begin{bmatrix}s_{a,b}\cos\theta_{a,b}&s_{a,b}\sin\theta_{ a,b}&x_{a,b}\\ -s_{a,b}\sin\theta_{a,b}&s_{a,b}\cos\theta_{a,b}&y_{a,b}\\ 0&0&1\end{bmatrix}. \tag{10}\]  
As we assumed that the distance from the camera to the coral reef is kept constant during the same exploration, we only need to calculate the relative scale between \(G_{0}\) and \(G_{1}\) the first time that the same place is recognized. Once \(s_{a,b}\) and the transformation \(A_{ab}\) between a pair of images from \(G_{0}\) and \(G_{1}\) have been obtained, it is only necessary to rescale the relative positions \((x,y)\) from all the edges in \(G_{1}\) including the one that will be created when new images are processed. After that, every time the graph-based pose adjustment method is executed, \(G_{1}\) will be automatically aligned with respect to \(G_{0}\).