Underwater Matching Correction Navigation Based on Geometric Features Using Sonar Point Cloud Data

Algorithm 1

The map matching correction based on geometric features.

;

to get the four corners of the water tank using Hough /

HoughTransform (all points);

Least square method to fit lines of the most data with adjacent /

 ,

 set the line with most data is set to the baseline , the adjacent two

lines to be and , the fourth

line to be /

if

 then one of the lines is perpendicular to axis

  the rectangle can be easily /

else

  while    &&  , do

     and remains unchanged, modify /

   ;

   ;

  end while

;

end if