Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

BESTAnP: Bi-Step Efficient and Statistically Optimal Estimator for Acoustic-n-Point Problem

Wenliang Sheng, Hongxu Zhao, Lingpeng Chen, Guangyang Zeng, Yunling Shao, Yuze Hong, Chao Yang, Ziyang Hong, Junfeng Wu

TL;DR

BESTAnP is the first AnP algorithm that gives a closed-form solution for the full 6-of-freedom (DoF) pose and is compared with the state-of-the-art (SOTA) methods, which is over ten times faster and features real-time capacity in resource-constrained platforms while exhibiting comparable accuracy.

Abstract

We consider the acoustic-n-point (AnP) problem, which estimates the pose of a 2D forward-looking sonar (FLS) according to n 3D-2D point correspondences. We explore the nature of the measured partial spherical coordinates and reveal their inherent relationships to translation and orientation. Based on this, we propose a bi-step efficient and statistically optimal AnP (BESTAnP) algorithm that decouples the estimation of translation and orientation. Specifically, in the first step, the translation estimation is formulated as the range-based localization problem based on distance-only measurements. In the second step, the rotation is estimated via eigendecomposition based on azimuth-only measurements and the estimated translation. BESTAnP is the first AnP algorithm that gives a closed-form solution for the full six-degree pose. In addition, we conduct bias elimination for BESTAnP such that it owns the statistical property of consistency. Through simulation and real-world experiments, we demonstrate that compared with the state-of-the-art (SOTA) methods, BESTAnP is over ten times faster and features real-time capacity in resource-constrained platforms while exhibiting comparable accuracy. Moreover, for the first time, we embed BESTAnP into a sonar-based odometry which shows its effectiveness for trajectory estimation.

BESTAnP: Bi-Step Efficient and Statistically Optimal Estimator for Acoustic-n-Point Problem

TL;DR

BESTAnP is the first AnP algorithm that gives a closed-form solution for the full 6-of-freedom (DoF) pose and is compared with the state-of-the-art (SOTA) methods, which is over ten times faster and features real-time capacity in resource-constrained platforms while exhibiting comparable accuracy.

Abstract

We consider the acoustic-n-point (AnP) problem, which estimates the pose of a 2D forward-looking sonar (FLS) according to n 3D-2D point correspondences. We explore the nature of the measured partial spherical coordinates and reveal their inherent relationships to translation and orientation. Based on this, we propose a bi-step efficient and statistically optimal AnP (BESTAnP) algorithm that decouples the estimation of translation and orientation. Specifically, in the first step, the translation estimation is formulated as the range-based localization problem based on distance-only measurements. In the second step, the rotation is estimated via eigendecomposition based on azimuth-only measurements and the estimated translation. BESTAnP is the first AnP algorithm that gives a closed-form solution for the full six-degree pose. In addition, we conduct bias elimination for BESTAnP such that it owns the statistical property of consistency. Through simulation and real-world experiments, we demonstrate that compared with the state-of-the-art (SOTA) methods, BESTAnP is over ten times faster and features real-time capacity in resource-constrained platforms while exhibiting comparable accuracy. Moreover, for the first time, we embed BESTAnP into a sonar-based odometry which shows its effectiveness for trajectory estimation.

Paper Structure

This paper contains 27 sections, 6 theorems, 27 equations, 10 figures, 4 tables, 1 algorithm.

Table of Contents

  1. Introduction
  2. Related Works
  3. PnP for Camera Pose Estimation
  4. AnP for Sonar Pose Estimation
  5. Problem Formulation
  6. Ideal Measurement Model
  7. Problem Formulation
  8. BESTAnP algorithm
  9. Consistent Estimator of the Translation Vector
  10. Consistent Estimator of the Rotation Matrix
  11. Gauss-Newton Refinement
  12. Experiments on synthetic data
  13. Absolute Pose Estimation with Known 3D Points
  14. Rationality of considering noise on $\bm{\tan\theta^*}$
  15. Rationality of using a single GN iteration
  16. ...and 12 more sections

Key Result

Theorem 1

(9855392, Theorem 4) The estimator consistent_estimate_t is $\sqrt{n}$-consistent for both $\bm t^*$ and $\sigma_d^2$.

Figures (10)

  • Figure 1: Illustration of the AnP problem. Given $n$ 3D points in the world frame and their corresponding 2D measurements in the sonar image, the pose of the FLS with respect to the world frame needs to be estimated.
  • Figure 2: Effect of different noise-adding schemes.
  • Figure 3: Effect of different GN iteration numbers.
  • Figure 4: RMSE comparison under varied noise intensities.
  • Figure 5: RMSE comparison under varied point numbers.
  • ...and 5 more figures

Theorems & Definitions (8)

  • Definition 1: Big $O_p$
  • Theorem 1
  • Lemma 1
  • Theorem 2
  • Definition 2: Small $o_p$
  • Theorem 3
  • Lemma 2: zeng2024optimal
  • Lemma 3