Algorithm 5 three-point pose estimator + RANSAC
Input: corresponding 3D point set {pi}, {qi}; the maximum distance: dmax; the maximum number of iterations :max Iterations
Output: best pose estimation (R*, t*), Θ
1:	Θ = Φ, cmax = 0, Iterations = 0
2:	while Iterations <= maxIterations do
3:	Randomly select three pairs of corresponding points from {pi}and {qi}, use three-point method to compute R and t.
4:	Inliers=Φ
5:	ci = 0
6:	for pi , qi ∈ {pi}, {qi} do
7:	if ‖qi − (R · pi + t)‖ < dmax then
8:	Inliers = Inliers ∪ {i}
9:	ci = ci + 1
10:	end if
11:	end for
12:	$C=\text{∑}_{i}min\left(c_i,\varsigma \right)$
13:	if C > Cmax then
14:	Θ = Inliers
15:	Cmax = C
16:	end if
17:	end while
18:	$\left(R^{*},t^{*}\right)=\underset{R,t}{argmin}\text{∑}_{i\in \Theta }{\Vert q_i-\left(R\cdot p_i+t\right)\Vert }^2$
19:	return (R*, t*), Θ