Algorithm 2: Improved pose estimation algorithm

Input: a set of $\mathcal{N}$ corresponding points selected manually from two cameras.

Output: improved transformation matrix based on all groups of points.

Procedure:

Find the nearest feature points to the corresponding points. Obtain the map points that represent the feature points determined in Step 1. Randomly select three points out of $\mathcal{N}$ points. Calculate quaternions. If there is a group of points that have not been selected before, go to Step 3. Else, continue. Calculate median of quaternions and set range using (1) and (2). Create new quaternion (mean within the range). Calculate the rotation matrix using the new quaternion calculated in Step 7. Calculate the centroid of the $\mathcal{N}$ points and relative coordinates, then calculate the scale and translation using Horn’s method [19]. Calculate the final improved transformation matrix.