A Convex and Global Solution for the P$n$P Problem in 2D Forward-Looking Sonar

TL;DR

The paper addresses the pose estimation challenge of the perspective-n-point problem for 2D forward-looking sonar by recasting it as a convex point-to-line 3D registration problem under an orthographic approximation. It develops a duality-based optimal solver, with a null-space analysis to resolve coplanar degeneracy, and provides a closed-form method for estimating the vertical translation $t_z$ while jointly estimating the rotation and horizontal translation. Extensive simulations show significant precision gains over non-reprojection-optimized baselines, with competitive or superior performance when combined with optimization refinements. The results demonstrate that accurate, globally optimal 2D FLS P$n$P estimation is feasible, supporting improved autonomous underwater perception, albeit with higher computation costs compared to some baselines. The work also outlines practical limitations and future directions, including faster closed-form solvers and robust handling of degenerate configurations.

Abstract

The perspective-$n$-point (P$n$P) problem is important for robotic pose estimation. It is well studied for optical cameras, but research is lacking for 2D forward-looking sonar (FLS) in underwater scenarios due to the vastly different imaging principles. In this paper, we demonstrate that, despite the nonlinearity inherent in sonar image formation, the P$n$P problem for 2D FLS can still be effectively addressed within a point-to-line (PtL) 3D registration paradigm through orthographic approximation. The registration is then resolved by a duality-based optimal solver, ensuring the global optimality. For coplanar cases, a null space analysis is conducted to retrieve the solutions from the dual formulation, enabling the methods to be applied to more general cases. Extensive simulations have been conducted to systematically evaluate the performance under different settings. Compared to non-reprojection-optimized state-of-the-art (SOTA) methods, the proposed approach achieves significantly higher precision. When both methods are optimized, ours demonstrates comparable or slightly superior precision.

Figures (6)

• Figure 1: The projection model of 2D FLS. The left part is a side view of one of the dashed sectors in the right part. The arc projection represents the true working mechanism of the sonar.
• Figure 2: The illustration of the point-to-line cost used in our method. 4 points of a rectangle are given in world coordinate, then transformed into sonar coordinate and projected as 4 pixels. Lines with direction $\mathbf{d} = [0 \ 0 \ 1]^T$ are shown as red lines passing through $\mathbf{m}/\mathbf{o}$. The transformed $\mathbf{p}^w$, which is $\mathbf{p}^s$, has its distance to the lines represented by purple line segments. The top-left green boxes are enlarged side/top-views.
• Figure 3: Results for the general cases with increasing noise level under 20 points. From left to right: angular error, $\mathbf{t}_{xy}$ error, and $t_z$ error.
• Figure 4: Results for the general cases with increasing point number under 0.025 m, 0.025 rad noise. From left to right: angular error, $\mathbf{t}_{xy}$ error, and $t_z$ error.
• Figure 5: Results for the coplanar cases with increasing noise level under 20 points. From left to right: angular error, $\mathbf{t}_{xy}$ error, and $t_z$ error.