Point Cloud Structural Similarity-based
Underwater Sonar Loop Detection

In the case of a downward-facing MBES, the beams spread widely in the lateral direction, while they are relatively narrow in the forward and backward directions of the vehicle. Therefore, when reconstructing point clouds from data of a single sequence obtained by MBES, the resulting point cloud, when viewed vertically, resembles single-channel 2D LiDAR data with a limited FoV. In underwater autonomous navigation scenarios, when data is collected downward, there is hardly any overlap when approaching the same location from different directions due to the single-channel nature. Thus, this single-channel point cloud cannot be used to find underwater terrain loops. To overcome this, we utilized IMU sensors to acquire linear acceleration, angular velocity, and magnetic field data, which were then input into an IMU filter to calculate orientation. The gyro-corrected orientation of the ego-vehicle is used to transform the local coordinate-based speed obtained from the Doppler Velocity Log (DVL) sensor into speed based on reference coordinates. Then, using velocity-time integration, the relative transformation in the reference coordinates is calculated. The process is expressed mathematically as follows:

where $\mathbf{v}^{dvl}$ represents the local velocity, which corresponds to the velocity values measured by the DVL. $\mathbf{R}^{g}$ is the orientation of the ego vehicle and $\mathbf{v}^{g}$ represents the global velocity, which is the local velocity $\mathbf{v}^{dvl}$ transformed into global coordinates by multiplying it by the orientation of the ego vehicle. $\Delta t$ denotes the time interval between timestep $t$ and $t+1$. $\mathbf{p}^{g}$ refers to the position of the ego vehicle in global coordinates. $\mathbf{R}^{g}_{0}$ and $\mathbf{p}^{g}_{0}$ respectively signify the orientation and position of the ego vehicle at the reference point in time. $\mathbf{R}^{l}$ and $\mathbf{p}^{l}$ respectively refer to the local orientation and position, where the local coordinate system is based on the pose of the ego vehicle at the starting point.

For the data acquired by MBES, each beam has values for distance and angle. When these values are converted into points with $x,y,z$ coordinates, they form a point cloud that can be obtained from a single sequence [13]. The point cloud obtained from a single sequence of MBES data resembles single-channel data. In such single-channel data, there is minimal overlap when data is acquired from different directions, making loop detection difficult. Therefore, it is necessary to merge consecutive data to augment the information obtained from a specific location. During this process, dead reckoning using DVL and IMU sensors is employed to calculate transformations and accumulate consecutive point clouds. The accumulated point clouds are then cropped in a square shape around the $z$ axis centered on the current $x,y$ position of the vehicle, based on a predefined distance. This point cloud processing procedure is as follows:

where $x,y,z$ represent the positions of points in the point cloud obtained from the 3D transformation of the raw data from the MBES. $P$ denotes the point cloud composed of these points $\left(x,y,z\right)$. $\Delta\mathbf{R}$ and $\Delta\mathbf{t}$ are the current relative rotation and translation transformations calculated in Sec. III-A. $n$ is a predefined value that corresponds to half of the time sequence to accumulate the point cloud. $\bar{\mathbf{P}}$ refers to the accumulated point cloud created by transforming the point clouds based on the current pose using $\Delta\mathbf{R}$ and $\Delta\mathbf{t}$. $\|\cdot\|$ indicates the Manhattan distance. $c$ is the reference point of $\bar{\mathbf{P}}$, corresponding to the current pose of the vehicle. $\left(\cdot\right)_{x}$ and $\left(\cdot\right)_{y}$ represent the coordinates $x$ and $y$ of the respective point. $d$ is a predefined value used for square cropping $\bar{\mathbf{P}}$. $\mathbf{P}$ is the point cloud obtained as a result of the square cropping.

To calculate the similarity between point clouds, we created feature maps using the structural features of the point cloud in this paper. Initially, we utilized three types of features: geometry, normals, and curvature. Geometry is represented by the Euclidean distance between each point in the point cloud and its neighboring points. For normals, it involves the angle between the normal vectors of each point and its neighbors. The reason for not directly using the position information of the points and their normal vectors, which are commonly used for geometry and normal features, is that these values are rotation-variant. Since our research also addresses loop detection when passing the same place with different orientations, we use rotation-invariant geometry and normal features. For curvature, it represents the mean curvature for each point. The calculations for the feature maps and the curvature are referenced from [11] and [14], respectively. The process of creating the quantity, which is the prior stage of the feature map from a square-shaped cropped point cloud is as follows:

where $G_{p}$, $N_{p}$, and $C_{p}$ represent the geometry, normal, and curvature quantities at point $p$, respectively. Additionally, $\mathbf{G}$, $\mathbf{N}$, and $\mathbf{C}$ are the sets of $G_{p}$, $N_{p}$, and $C_{p}$ in Eq. (7). Furthermore, $\|\cdot\|_{2}$, $|\cdot|$, and $|\,\vec{\cdot}\,|$ denote the Euclidean distance, the cardinality of the set, and the size of the vector, respectively. $p_{i}$ denotes the $i$-th nearest neighbor point of $p$. $M$ refers to the number of the nearest neighbors. $\vec{n}$ in Eq. (9) represents the normal vector calculated at $p$ by plane fitting and $\vec{n}_{i}$ indicates the normal vector at $p_{i}$. In Eq. (11), $a$, $b$, $c$, $d$, and $e$ represent the coefficients of the quadric surface $Q_{p}(x,y)=ax^{2}+by^{2}+cxy+dx+ey+f$, fitted using $p$ and the nearest-neighbor points. This quadric surface fitting is based on the method described in [14].

As shown in Fig. 2, for these calculated quantities, we create a feature map by calculating the mean and variance of the values of each quantity for the neighborhood point by point. By using representative values such as mean and variance, we reduce the dimension of the data used to calculate point cloud similarity, thereby shortening the computation time.

where the subscripts $\mu$ and $\sigma^{2}$ under $\mathbf{G},\mathbf{N},\mathbf{C}$ denote the methods used to calculate the feature maps, corresponding to mean and variance, respectively. $\mathbf{F}$ refers to any element of the entire set of feature maps possessed by a single point cloud. $\mathds{F}$ indicates the set of feature maps $\mathbf{F}$. In addition, $F_{p}$ denotes the feature map $F$ of point $p$. $F_{\mu_{p}}$ and $F_{\sigma_{p}^{2}}$ represent the mean and variance of the feature $F$ at point $p$, respectively, and correspond to elements of $\mathbf{F}_{\mu_{p}}$ and $\mathbf{F}_{\sigma_{p}^{2}}$.

We calculate similarity by comparing the point cloud feature map, computed in Sec. III-C, with the feature maps of previously stored point clouds. The sum of similarities obtained in this manner is identified and, if it exceeds a predefined threshold, it is considered a loop. This process can be mathematically expressed as follows:

where $S(\cdot,\cdot,\cdot)$ is the structural similarity between the first two elements calculated based on the third element. Moreover, $abs\left(\cdot\right)$ indicates the absolute value. $\epsilon$ is a constant that is used to prevent the denominator in $S$ from being zero. $\Gamma$ indicates similarity between two point clouds corresponding to the average of similarities of the feature maps. $\gamma$ represents the threshold for each feature map used to determine whether it constitutes a loop. Lastly, $f$ is the indicator function to determine whether two sequences are a loop pair or not.

The two research questions we aim to verify in this paper are as follows: 1) Does the proposed method demonstrate good loop detection performance in various environments without preprocessing tasks such as model training or creating a vocabulary? 2) Does the method perform well even in monotonous underwater environments by utilizing the point-wise information of the 3D point cloud? To answer the first research question, we compared the proposed method’s loop detection performance against the method by Tan et al.[7], which trains a neural network model for each dataset, and Shape BoW [8], which generates a vocabulary for each dataset. To address the second question, we verified whether the proposed method can successfully detect loops even in environments where few appropriate keypoints are detected for loop detection. Additionally, for comparative validation, we compared the loop detection performance on the same datasets with a keypoint-based method, such as Hammond et al. [6].

In this experiment, we evaluated the performance of the proposed algorithm using the Antarctica dataset, obtained using the Hugin 3000 AUV [7], and the Seaward dataset, acquired from banks via boats [15]. To compare the performance of the proposed method, we used studies that employed sonar sensors for bathymetric purposes [6, 7, 8] as baselines. Since the code for [6, 8] was not publicly available, we implemented the algorithms based on the descriptions in the respective papers, except for [7]. Moreover, to test the method by Tan et al., we trained a neural network model using each dataset. Similarly, we created a vocabulary for each dataset and validated the performance of Shape BoW. For the Antarctica dataset, where the depth was greater and the lateral spread was wider, the distance parameter $d$ was set to $100\ m$ based on the criteria in [7].

In contrast, for the Seaward dataset, due to its shallow depth, the lateral spread of the vehicle-based point cloud was narrower, thus $d$ was set to $10\ m$ for square cropping, as in Eq. (6). The determination of positive and negative pairs between two sequences was based on the Euclidean distance of the ego-vehicle pose. If the distance was smaller than $0.5\,d$, the pair was considered a positive pair, while distances greater than $2\,d$ were considered negative pairs. For the Seaward dataset, experiments were conducted across four different environments: Wiggles Bank, North River, Dutch Island, and Beach Pond.