Acoustic-N-Point for Solving 2D Forward Looking Sonar Pose Estimation

2D forward looking sonar (FLS) has gained widespread application in underwater robotics research, primarily due to its capacity to produce high-resolution images in diverse aquatic environments. This study deals with pose estimation with given 3D positions and corresponding 2D pixels, which is a fundamental problem for computer vision, denoted as acoustic-n-point (AnP) problem. It is the key part for object pose estimation, extrinsic calibration, localization, and structure from motion (SfM). We propose two methods to acquire a closed-form solution of 5(degrees)-of-freedom (DoF) pose. The first utilizes a non-approximated model and eliminates the cosine terms. The singular value decomposition (SVD)-based method is proposed and the nullity is discussed. The second method approximates the projection model as a linear system and conducts null space analysis for more accurate solutions. After acquiring the initial 5DoF pose, the last DoF can be acquired by constrained nonlinear least square optimization. We take advantage of both methods by selecting solutions with the smallest reprojection error. Furthermore, we explore the potential for further refinement by utilizing this solution as the initial pose with 6DoF-constrained iterative optimization. Results are evaluated on both simulation and real data.