Algorithm 1 3D point cloud registration based on SVD algorithm

Input: Two-point cloud sets: $P=\left\{p_i\right\}$, $Q=\left\{q_i\right\}$, $i=1,2,3,\cdots ,N$ Output: $f(R,T)=min{\displaystyle \sum _{i=1}^n}{(q_i-R{p}_i-T)}^2$ 1:Calculate the centroid of two 3D point cloud sets $P=\left\{p_i\right\}$, $Q=\left\{q_i\right\}$ according to Equations (4) and (5); 2:Translating the two point cloud sets $P=\left\{p_i\right\}$, $Q=\left\{q_i\right\}$ to get the two new 3D point cloud sets $\left\{m_i\right\}\left\{m_i^{\prime }\right\}$ according to Equations (6) and (7); 3:Computing 3 × 3 matrix $C={\displaystyle \sum _{i=1}^N}{m}_i{{m^{\prime }}_i}^T$; 4:Decomposition of singular values for matrix C, $C=U\mathsf{\Lambda }{V}^T$, where $\mathsf{\Lambda }=diag\left(d_i\right),d_1\ge d_2\ge d_3\ge 0$ and the rotation matrix $R=V{U}^T$; 5:Computing the translation matrix $T=q-Rp$.