Probabilistic Scan Matching:
Bayesian Pose Estimation from Point Clouds

Before introducing our measurement model, it is worth noting that all scan points are considered in homogeneous coordinates to cope with our pose representation, i.e., $\bm{d}^{(i)}_{\mathrm{H}}\triangleq[\bm{d}^{(i)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt1]^{\mathrm{T}}$, $\bm{n}^{(i)}_{\mathrm{H}}\triangleq[\bm{n}^{(i)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt0]^{\mathrm{T}},i=1,2,\ldots,N_{\mathrm{D}}$, and $\bm{s}^{(j)}_{\mathrm{H}}\triangleq[\bm{s}^{(j)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt1]^{\mathrm{T}},j=1,2,\ldots,N_{\mathrm{S}}$. For mathematical convenience, we introduce stacked vectors of source points, destination points and their normals as follows: $\bm{s}_{\mathrm{H}}=[\bm{s}_{\mathrm{H}}^{(1)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\bm{s}_{\mathrm{H}}^{(2)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\bm{s}_{\mathrm{H}}^{(N_{\mathrm{S}})\mathrm{T}}]^{\mathrm{T}}$, $\bm{d}_{\mathrm{H}}=[\bm{d}_{\mathrm{H}}^{(1)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\bm{d}_{\mathrm{H}}^{(2)\mathrm{T}}\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\bm{d}_{\mathrm{H}}^{(N_{\mathrm{D}})\mathrm{T}}]^{\mathrm{T}}$, and $\bm{n}_{\mathrm{H}}=[\bm{n}_{\mathrm{H}}^{(1)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\bm{n}_{\mathrm{H}}^{(2)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\bm{n}_{\mathrm{H}}^{(N_{\mathrm{D}})\mathrm{T}}]^{\mathrm{T}}$.

Fig. 1 graphically depicts the problem we attempt to solve. Our goal is to infer the relative pose $\Delta\bm{\mathsfbr{P}}$ of the sensor based on the source and destination point clouds by transforming the source point cloud onto the destination point cloud. Note that the associations between source scan points and destination surface points $\bm{\mathsfbr{a}}$ are unknown and need to be inferred jointly with the relative pose $\Delta\bm{\mathsfbr{P}}$ which is a random matrix on the manifold $\textit{SE}(N_{\text{dim}})$.

In the pursuit of solving this problem robustly, we resort to the probabilistic domain. Our goal is to obtain a Bayes-optimum estimate of the relative pose $\Delta\bm{\mathsfbr{P}}$ considering all valid data association hypotheses $\bm{\mathsfbr{a}}$. First, we marginalize the joint posterior distribution $f(\Delta\bm{P},\bm{a}|\bm{z})$ to obtain $f(\Delta\bm{P}|\bm{z})$ which factors in all valid data association hypotheses. Then, we derive an maximum a posteriori (MAP) estimate by maximizing the marginal posterior distribution, i.e.,

Note that determining $f(\Delta\bm{P}|\bm{z})$ by direct marginalization $\big{(}\text{i.e., }f(\Delta\bm{P}|\bm{z})=\sum_{\bm{a}}f(\Delta\bm{P},\bm{a}|\bm{z})\big{)}$ is infeasible due to the high-dimensionality of the summation space as discussed in Section III. We consider feasible approximate calculation based on the notion of message passing [21, 22, 23].

We attempt to associate $N_{\mathrm{S}}$ source points to $N_{\mathrm{D}}$ destination surface points. Recall, though, that not all points in the source and destination scan are associable. We assume the number of non-associable source points, $\mathsfbr{N}_{\mathrm{NA}}$, to be random and Poisson distributed with mean $\lambda_{\mathrm{NA}}$. Hence the total number of source points, $\mathsfbr{N}_{\mathrm{S}}$, is also random and given by

where ${\cal{D}}_{\bm{\mathsfbr{a}}}\triangleq\{i\in\{1,2,\ldots,N_{\mathrm{D}}\}:\mathsfbr{a}^{(i)}\neq 0\}$.

Valid associations are those in which each source point $\bm{s}_{\mathrm{H}}^{(j)}$ is associated to at most one destination surface point $\{\bm{d}_{\mathrm{H}}^{(i)},\bm{n}_{\mathrm{H}}^{(i)}\}$, and each destination surface point $\{\bm{d}_{\mathrm{H}}^{(i)},\bm{n}_{\mathrm{H}}^{(i)}\}$ is associated to at most one source point $\bm{s}_{\mathrm{H}}^{(j)}$. This assumptions imposes constraints on $\bm{\mathsfbr{a}}$. In particular, $\bm{\mathsfbr{a}}$ is valid if and only if $\mathsfbr{a}^{(i)}=j\in\{1,\ldots,N_{\mathrm{S}}\}$, $\mathsfbr{a}^{(i^{\prime})}\neq j,\forall i\neq i^{\prime}$. These constraints can be conveniently expressed by

Note that the cardinality of valid hypothesis is huge, rendering enumeration of all hypotheses intractable [12, 24]. To address this issue, we introduce a redundant formulation of $\bm{\mathsfbr{a}}$: the source-oriented data association vector $\bm{\mathsfbr{b}}=[\mathsfbr{b}^{(1)}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\mathsfbr{b}^{(2)}\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\mathsfbr{b}^{(N_{\mathrm{S}})}]^{\mathrm{T}}$, in which $\mathsfbr{b}^{(j)}\in\{1,\ldots,N_{\mathrm{D}}\}$ and $\mathsfbr{b}^{(j)}=i$ means that the $j^{\mathrm{th}}$ source point is associated to the $i^{\mathrm{th}}$ destination surface point. If $\mathsfbr{b}^{(j)}=0$, the $j^{\mathrm{th}}$ source point is non-associable. For source-oriented data association vectors $\bm{\mathsfbr{b}}$ we have $\mathsfbr{b}^{(j)}=i\in\{1,\ldots,N_{\mathrm{D}}\}$, $\mathsfbr{b}^{(j^{\prime})}\neq i,\forall j^{\prime}\neq j$. Note that the set of all valid vectors $\bm{\mathsfbr{b}}$ contains exactly the same information as that of all valid vectors $\bm{\mathsfbr{a}}$.

The reason for introducing this redundant data association representation is that validity of association hypotheses can now be described via pairs of $\mathsfbr{a}^{(i)}$ and $\mathsfbr{b}^{(j)}$ instead of the entire vector $\bm{\mathsfbr{a}}$, making evaluation tractable. This is described in more detail in [12, 24]. It can be easily verified that valid data associations can be described by

where

Note that we treat both $\bm{\mathsfbr{a}}$ and $\bm{\mathsfbr{b}}$ as random vectors that are inferred jointly with the relative pose $\Delta\bm{\mathsfbr{P}}$.

In what follows, we derive the joint posterior distribution $f(\Delta\bm{P},\bm{a},\bm{b}|\bm{z})$ and the corresponding factor graph. For $\bm{z}$ observed and thus fixed, we obtain

$f(\Delta\bm{P})$ is a random matrix pdf with support $\textit{SE}(N_{\text{dim}})$ that incorporates prior knowledge on the relative pose $\Delta\bm{\mathsfbr{P}}$. The conditional PDF $f(\bm{z},\bm{a},\bm{b},N_{\mathrm{S}}|\Delta\bm{P})$ is given by

where we introduced $v(\Delta\bm{P};\bm{s}_{\mathrm{H}})\triangleq\prod_{j=1}^{N_{\mathrm{S}}}f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(j)}|\Delta\bm{P})$ and

and used $\Psi(\bm{a},\bm{b})$ from (5). A detailed derivation of (8) together with the underlying assumptions is provided in Appendix A. By using (8) in (7), we obtain the joint posterior distribution $f(\Delta\bm{P},\bm{a},\bm{b}|\bm{z})$ shown in (9).

Let us discuss the expression in (9) to gain further insights into our model. Suppose that we have a destination surface point $i$, $\{\bm{\mathsfbr{d}}_{\mathrm{H}}^{(i)},\bm{\mathsfbr{n}}_{\mathrm{H}}^{(i)}\}$, that is associated with source point $\bm{\mathsfbr{s}}_{\mathrm{H}}^{(j)}$, i.e., $a^{(i)}=j$. Then, we have $q_{i}(\Delta\bm{P},j,\bm{z}^{(i)})=f_{\mathrm{A}}(\Delta\bm{P})f(\bm{z}^{(i,j)}|\Delta\bm{P})/(\lambda_{\mathrm{NA}}f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(j)}|\Delta\bm{P}))$, where $f_{\mathrm{A}}(\cdot)$ accounts for the fact that destination surface point $i$ is associable and contributes pseudo measurement $\bm{z}^{(i,j)}$ with likelihood $f(\bm{z}^{(i,j)}|\Delta\bm{P})$. Lastly, the term in the denominator, $f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(j)}|\Delta\bm{P})$, reflects the fact that source point $j$, $\bm{s}_{\mathrm{H}}^{(j)}$, is associable as well since it cancels the $j^{\text{th}}$ term in $v(\Delta\bm{P};\bm{s}_{\mathrm{H}})$. On the contrary, if the $i^{\mathrm{th}}$ destination surface point is not associated with any source point, we have $q_{i}(\Delta\bm{P},0,\bm{z}^{(i)})=1-f_{\mathrm{A}}(\Delta\bm{P})$. This term takes the likelihood of non-associability, $1-f_{\mathrm{A}}(\Delta\bm{P})$, of destination surface point $i$ into account. Finally, $\Psi(\bm{a},\bm{b})$ enforces validity of associations.

We consider a vehicle that drives in the urban environment depicted in Fig. 3. The vehicle moves with a constant velocity of $10$ m/s along the indicated trajectory (red line). It is equipped with a $360^{\circ}$-LiDAR sensor that generates scans every $\Delta T=80$ ms. We treat the current scan as the source scan and the previous scan as the destination scan to obtain pose estimates that are inline with the motion of the vehicle.

The sensor has an angular resolution of $1$ deg and a maximum sensing range of $100$ m. The scanner obtains range and bearing estimates that are corrupted by zero-mean Gaussian noise with standard deviation $\sigma_{\mathrm{range}}=0.05$ m and $\sigma_{\mathrm{bearing}}=0.5$ deg, respectively. Non-associable source points are assumed to be uniformly distributed in range and bearing, being independent of $\Delta\bm{\mathsfbr{P}}$. The number of non-associable source points is assumed Poisson distributed with mean $\lambda_{\mathrm{NA}}=1$. For surface normal computation, we use $d_{\mathrm{TH}}=2$ m. An exemplary scan is depicted in Fig. 3. We use the following parameters for our proposed algorithm. The probability of associability is constant $f_{\mathrm{A}}(\Delta\bm{P})=0.8$. For each $j\in\{1,2,\ldots,N_{\mathrm{S}}\}$, the noise statistics of $f(e^{(j)})$ are assumed zero-mean Gaussian distributed with standard deviation $\sigma_{e^{(j)}}=0.03$ m. Translation and rotation prior are considered uniform within the intervals $[-10,10]$ m and $[-90,90]$ ^∘. We performed $N_{\mathrm{MC}}=100$ Monte Carlo trials.

We compare our probabilistic scan matching approach to normal distributions transform (NDT) scan matching [3] and implicit moving least squares (IMLS) scan matching [6]. In our implementation of IMLS, we set the neighborhood parameter to $h=2$ and obtained estimates by minimizing [6, Eq. (1)]. We employ the following error metrics to evaluate the translation and rotation drift: $e_{\mathrm{trans}}=\|\Delta\hat{\bm{t}}_{\mathrm{MAP}}-\Delta\bm{t}\|/\|\Delta\bm{t}\|$ and $e_{\mathrm{rot}}=\arccos((\mathrm{Tr}(\Delta\hat{\bm{R}}_{\mathrm{MAP}}^{-1}\Delta\bm{R})-1)/2)/\|\Delta\bm{t}\|$, where $\Delta\hat{\bm{t}}_{\mathrm{MAP}}$ and $\Delta\hat{\bm{R}}_{\mathrm{MAP}}$ are the estimated translation and rotation extracted from $\Delta\hat{\bm{P}}_{\mathrm{MAP}}$, $\Delta\bm{t}$ and $\Delta\bm{R}$ are the ground truth translation and rotation, and $\mathrm{Tr}(\cdot)$ is the trace operator.

Results are depicted in Tab. I. Here, $e_{\mathrm{trans},q}$ refers to an error threshold such that $q\%$ of all relative observed translation errors $e_{\mathrm{trans}}$ are below this threshold. The same logic applies to $e_{\mathrm{rot},q}$. For example, $e_{\mathrm{trans},50}$ is the median relative translation error. From Tab. I we observe that median relative translation and rotation errors of NDT and our proposed approach both show low drifts. However, considering $e_{\mathrm{trans},q}$, NTD shows a significantly larger drift compared to our proposed probabilistic approach. IMLS scan matching shows large drifts, presumably due to the absence of a good initial guess provided to the optimizers. Exemplary estimated trajectories obtained via a concatenation of relative pose estimates are also shown in Fig. 3 where we can see how closely our approach attains ground truth.