Self Calibration of a Sonar–Vision System for Underwater Vehicles: A New Method and a Dataset

. 2023 Feb 3;23(3):1700. doi: 10.3390/s23031700

Algorithm 1 Research of the calibration algorithm on one set of camera and sonar image pairs.

$I_{s_{i}} \leftarrow g e t N e x t S o n a r I m a g e ()$
$I_{o_{i}} \leftarrow g e t N e x t C a m e r a I m a g e ()$
$I_{s_{i + 1}} \leftarrow g e t N e x t S o n a r I m a g e ()$
$I_{o_{i + 1}} \leftarrow g e t N e x t C a m e r a I m a g e ()$
$p_{s_{i}} \leftarrow s e l e c t F e a t u r e P o i n t s (I_{s_{i}})$
$p_{s_{i + 1}} \leftarrow L u c a s K a n n a d e (I_{s_{i}}, I_{s_{i + 1}}, p_{s_{i}})$
$s c o r e M i n \leftarrow + \infty$

for all $R_{s}^{o}$ , $T_{s}^{o}$ and f do
[ $p_{o_{i}}$ , $p_{o_{i + 1}}$ ] $\leftarrow p r o j e c t P o i n t s S o n a r T o C a m e r a (R_{s}^{o}, T_{s}^{o}, f, p_{s_{i}}, p_{s_{i + 1}})$
${\hat{p}}_{o_{i + 1}} \leftarrow L u c a s K a n n a d e (I_{o_{i}}, I_{o_{i + 1}}, p_{o_{i}})$
$p r o j e c t i o n S c o r e \leftarrow c o m p u t e S c o r e (p_{o_{i}}, p_{o_{i + 1}}, {\hat{p}}_{o_{i + 1}})$
if $s c o r e < s c o r e M i n$ then
$s c o r e M i n \leftarrow s c o r e$
[ $T_{m i n}$ , $R_{m i n}$ , $f_{m i n}$ ] ← [ $R_{s}^{o}$ , $T_{s}^{o}$ , f]
end if
end for
Return: [ $T_{m i n}$ , $R_{m i n}$ , $f_{m i n}$ ]