Algorithm 3 Rigid Transformation Recovery

Input: $P_s=\left\{P^i|1\le i\le N\right\}$: scaled 3D points from RGB images; $P_D=\left\{P^{\prime i}|1\le i\le N\right\}$: 3D points from depth images

Output: best transformation estimation ($R,\mathrm{t}$)

1. $nInlie{r}_{max}=0$, Iterations = 0, $\mathrm{F}=\varnothing$

2. While Iterations <= MaxIterations do

3.  $\mathrm{nInliers}=0,\mathrm{Inliers}=\varnothing$

4.  Randomly select 5 pairs of corresponding points from $\left\{P_s\right\}$ and $\left\{P_D\right\}$, use Besl’s method to compute the rigid transformation R’, t’

5.  For $P_i,P{\prime }_i\in \left\{P_s\right\},\left\{P_D\right\}$ do

6.   If $\parallel P{\prime }_i-\left({\mathrm{R}}^{\prime }{P}_s^i+{\mathrm{t}}^{\prime }\right)\parallel <Threshold$ then

7.    nInlier = nInlier + 1

8.    $\mathrm{Inliers}=\mathrm{Inliers}{\displaystyle \cup }\left\{i\right\}$

9.   End if

10.  End for

11.  If nInlier > $nInlie{r}_{max}$ then

12.   $nInlie{r}_{max}=$nInlier

13.   $\mathrm{F}=\mathrm{Inliers}$

14  End if

15.  Iterations = Iterations + 1

16. End while

17. $\left(R,t\right)=\underset{R,t}{\min }{{\displaystyle \sum }}_{i\in \mathrm{F}}{\parallel \mathrm{R}{P}_s^i+\mathrm{t}-P_D^i\parallel }^2$

18 Return $\left(R,t\right)$