Algorithm 1 Pre-processing to retrieve unreturned LiDAR beams. Inputs: $\mathbf{X}\in {\mathbb{R}}^{N\times 3}$ (3D Cartesian coordinates), $\mathbf{I}\in {\mathbb{R}}^N$ (intensity values), ${\mathbf{\Theta }}_{\mathrm{calib}}\in {\mathbb{R}}^V$ and ${\mathbf{\Phi }}_{\mathrm{calib}}\in {\mathbb{R}}^H$ (calibrated elevation and azimuth angles), $R_{max}\in \mathbb{R}$ (maximum sensor range).

1: function PreProcess($\mathbf{X},\mathbf{I},{\mathbf{\Theta }}_{\mathrm{calib}},{\mathbf{\Phi }}_{\mathrm{calib}},R_{max}$)   2:    Init: ${\mathbf{X}}_{\mathrm{pgm}}\in {\mathbb{R}}^{V\times H\times 3},{\mathbf{I}}_{\mathrm{pgm}},{\mathbf{R}}_{\mathrm{pgm}}\in {\mathbb{R}}^{V\times H}$ as zeros   3:    $(\mathbf{R},\mathbf{\Phi },\mathbf{\Theta })\leftarrow \mathtt{ToPolar}\left(\mathbf{X}\right)$   4:    ${\mathbf{\Theta }}_{\mathrm{idx}}\leftarrow {\mathtt{argmin}}_i|\mathtt{sort}\left({\mathbf{\Theta }}_{\mathrm{calib}}\right)\left[i\right]-\mathbf{\Theta }|$   5:    ${\mathbf{\Phi }}_{\mathrm{idx}}\leftarrow {\mathtt{argmin}}_j|\mathtt{sort}\left({\mathbf{\Phi }}_{\mathrm{calib}}\right)\left[j\right]-\mathbf{\Phi }|$   6:    ${\mathbf{X}}_{\mathrm{pgm}}[{\mathbf{\Theta }}_{\mathrm{idx}},{\mathbf{\Phi }}_{\mathrm{idx}}]\leftarrow \mathbf{X}$;   ${\mathbf{I}}_{\mathrm{pgm}}[·]\leftarrow \mathbf{I}$;   ${\mathbf{R}}_{\mathrm{pgm}}[·]\leftarrow \mathbf{R}$   7:    $M\leftarrow ({\mathbf{R}}_{\mathrm{pgm}}==0)$;   ${\mathbf{R}}_{\mathrm{pgm}}\left[M\right]\leftarrow R_{max}$   8:    ${\mathbf{\Phi }}_{\mathrm{full}},{\mathbf{\Theta }}_{\mathrm{full}}\leftarrow \mathtt{meshgrid}({\mathbf{\Phi }}_{\mathrm{calib}},{\mathbf{\Theta }}_{\mathrm{calib}})$   9:    ${\mathbf{X}}^{\prime }\leftarrow \mathtt{ToEuclidean}({\mathbf{R}}_{\mathrm{pgm}},{\mathbf{\Phi }}_{\mathrm{full}},{\mathbf{\Theta }}_{\mathrm{full}})$ 10:    ${\mathbf{X}}_{\mathrm{pgm}}\left[M\right]\leftarrow {\mathbf{X}}^{\prime }\left[M\right]$ 11:    return ${\mathbf{X}}_{\mathrm{pgm}}.\mathtt{reshape}(-1,3),{\mathbf{I}}_{\mathrm{pgm}}.\mathtt{reshape}(-1)$ 12:end function