Algorithm A1. Optimize Point Cloud Registration

Input: $P_{\mathrm{q}\mathrm{u}\mathrm{e}\mathrm{r}\mathrm{y}}$: Query point cloud with multiple points $P_{\mathrm{m}\mathrm{o}\mathrm{d}\mathrm{e}\mathrm{l}}$: Model point cloud with multiple points $f_p$: the percentage value to determine the capacity of the min heap Output: H: The min heap H with capacity $⌊P_{\mathrm{q}\mathrm{u}\mathrm{e}\mathrm{r}\mathrm{y}}\times f_p⌋$ is used to store distances and indexes ·For each point $D_q$ in $P_{\mathrm{q}\mathrm{u}\mathrm{e}\mathrm{r}\mathrm{y}}$ do      dis = $\mathrm{C}\mathrm{a}\mathrm{l}\mathrm{c}\mathrm{u}\mathrm{l}\mathrm{a}\mathrm{t}\mathrm{e}\mathrm{N}\mathrm{e}\mathrm{a}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{t}\mathrm{D}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}(D_q,P_{\mathrm{m}\mathrm{o}\mathrm{d}\mathrm{e}\mathrm{l}})$      $neg\_dis$ = $-dis$         If $\mathrm{H}.\mathrm{s}\mathrm{i}\mathrm{z}\mathrm{e}\mathrm{s}$() < $⌊P_{\mathrm{q}\mathrm{u}\mathrm{e}\mathrm{r}\mathrm{y}}\times f_p⌋$      $\mathrm{H}.\mathrm{a}\mathrm{d}\mathrm{d}(neg\_dis$, ${index\_of\_D}_q$)      · ElseIf $ne{g}_{dis}$ > $\mathrm{H}.\mathrm{p}\mathrm{e}\mathrm{e}\mathrm{k}$()      $\mathrm{H}.\mathrm{r}\mathrm{e}\mathrm{m}\mathrm{o}\mathrm{v}\mathrm{e}\mathrm{M}\mathrm{i}\mathrm{n}$()      $\mathrm{H}.\mathrm{a}\mathrm{d}\mathrm{d}(neg\_dis$, ${index\_of\_D}_q$) End