Algorithm 1 Progressive refinement of 6DoF poses ${\left\{{\mathit{P}}_i\right\}}_{i=1}^N$ for sequence of frames ${\left\{{\mathit{f}}_i\right\}}_{i=1}^N$ by optimizing pose graph $\mathit{G}$.

1: for $d=2$ to $\frac{N}{2}$ do 2:  for $i=1$ to N do 3:   $j:=(i+d)modN$ 4:   ${\mathit{T}}_{i\to j}:={\mathit{P}}_j^{-1}·{\mathit{P}}_i$ 5:   $o_{ij}:=\mathrm{OVERLAP}({\mathit{f}}_i,{\mathit{f}}_j,{\mathit{T}}_{i\to j})$ 6:   if $o_{ij}>t_o$ then 7:    ${\mathit{T}}_{i\to j},e:=\mathrm{CLSREGISTARTION}({\mathit{f}}_i,{\mathit{f}}_j,{\mathit{T}}_{i\to j},o_{ij})$ 8:    if $e\le \mathrm{MEDIANRANGE}\left({\mathit{f}}_i\right)·t_r+t_a$ then 9:     $\mathit{G}:=\mathit{G}\cup \left\{\mathrm{EDGE}(i,j,{\mathit{T}}_{i\to j})\right\}$ 10:    end if 11:   end if 12:  end for 13:  ${\mathit{P}}_1,{\mathit{P}}_2,\dots ,{\mathit{P}}_N=\mathrm{OPTIMIZE}\left(G\right)$ 14: end for 15: return ${\mathit{P}}_1,{\mathit{P}}_2,\dots ,{\mathit{P}}_N$