State Transition for Statistical SLAM Using Planar Features in 3D Point Clouds

Abstract

There is a large body of literature on solving the SLAM problem for various autonomous vehicle applications. A substantial part of the solutions is formulated based on using statistical (mainly Bayesian) filters such as Kalman filter and its extended version. In such solutions, the measurements are commonly some point features or detections collected by the sensor(s) on board the autonomous vehicle. With the increasing utilization of scanners with common autonomous cars, and availability of 3D point clouds in real-time and at fast rates, it is now possible to use more sophisticated features extracted from the point clouds for filtering. This paper presents the idea of using planar features with multi-object Bayesian filters for SLAM. With Bayesian filters, the first step is prediction, where the object states are propagated to the next time based on a stochastic transition model. We first present how such a transition model can be developed, and then propose a solution for state prediction. In the simulation studies, using a dataset of measurements acquired from real vehicle sensors, we apply the proposed model to predict the next planar features and vehicle states. The results show reasonable accuracy and efficiency for statistical filtering-based SLAM applications.

1. Introduction

Simultaneous Localization and Mapping (SLAM) is often considered as one of the main challenges in the field of robotics and autonomous vehicles [1,2]. The aim of SLAM is to build a map of an unknown environment while simultaneously determining the location of the vehicle within this map. Neither the map nor the vehicle position are known in advance. However, a kinematic model of the vehicle motion is assumed to be known a priori, and the unknown environment is populated with artificial or natural landmarks.

There is a large body of literature on solving the SLAM problem for various autonomous vehicle applications. Many of the solutions are formulated based on using a Bayesian filter that recursively propagates the distribution of the vehicle’s dynamic states (and sometimes the map features). Davison [3] proposed a solution based on using Extended Kalman Filters (EKFs) to track the location of each map feature which was extracted from the sensor measurements. To improve the accuracy of this method, Thrun et al. [4] proposed the Sparse Extended Information Filter. The idea is to optimize the calculations required by the EKF, via taking advantage of sparse matrices. Montemerlo et al. [5] proposed a Rao-Blackwellised particle filter-based method called FastSLAM. In this method, the odometry uncertainties are rectified by adding random offsets to the odometry data for each particle.

In such solutions, to simultaneously estimate both vehicle and landmark locations, the Bayesian filter needs to employ two models: an observation model, and a motion model. With regards to the observations, the vehicle must be equipped with a sensor or set of sensors that produce measurements related to the surrounding landmark locations. The most common examples are LIDAR [6,7,8], RGB camera [9,10,11,12], RGB-D camera [13,14] and sonar [15] sensors. The sensory measurements usually have range limitations, and contain measurement noise. The raw measurements are normally processed further to extract point features. An on-board sensory system on the vehicle is usually in place to collect the features in real-time.

With the advancement of sensor technology and decreasing cost of LIDAR scanners, the generation of an adequate 3D point cloud is now possible. A LIDAR works by sending a laser pulse towards an object and measuring the time it takes to reflect from the object and return to the sensor. Because of its high accuracy and ease of use, a LIDAR sensor has become a common choice for SLAM purposes. Having the generated point cloud, it is possible to extract features that are more complex (and convey more information) than point features. Employing more information-rich features as observations in a Bayesian filter within a SLAM algorithm can increase the algorithm efficiency and reduce the computational cost. For example employing planar features instead of 3D points could considerably enhance the algorithm efficiency, as the number of planes is significantly smaller than the number of points in a typical 3D point cloud. However, in order to properly use these features in the Bayesian framework, an appropriate motion model needs to be devised.

In human-inhabited environments, buildings and large facilities are currently almost ubiquitous and their geometric profiles comprise a large number of plane shapes. Out of all these shapes, the major ones naturally create “obstacles” or define “edges” beyond which the vehicles cannot protrude, and these planes can be extracted and employed as features or landmarks in SLAM applications. Indeed, due to the geometric simplicity of the plane shapes and their abundance in human-inhabited environments, planar features have attracted increasing attention from both the computer graphics and robotics community in recent years [16,17,18]. As for the smaller plane shapes, they may be from the profiles of small objects or even moving objects such as vehicles. These small planes can be readily excluded from the set of landmarks by applying a threshold to the dimension of planes.

This paper presents for the first time the idea of using planar features extracted from a 3D point cloud, instead of point features, for Bayesian SLAM filters. The major contribution of this study lies in proposing a novel transition model that predicts the parameters of planar features extracted from a 3D point cloud. With Bayesian filters, the first step is prediction, where the object state densities are propagated to the next time step based on a stochastic transition model (motion model). We present how such a transition model can be developed, and propose a solution for state prediction. In the simulation studies, using a dataset of measurements acquired from real vehicle sensors, we apply the model to predict the planar features at the next time. The results show reasonable accuracy and efficiency for statistical filtering-based SLAM applications.

The proposed transition model consists of two sub-models: the plane transition model and the vehicle transition model. The latter one is associated with localization; namely, it predicts the vehicle states at time $k+1$ using the state information at time k. The former one is associated with mapping, which predicts the feature parameters at time $k+1$ based on the feature information at time k as well as the predicted vehicle states resulting from the latter model. The accuracy and effectiveness of the proposed transition model are verified using real-world point cloud measurements from the KITTI dataset [19]. The graphical and numerical simulation results show that the predicted planar features resulting from the proposed transition model closely match the measured features (i.e., segmented 3D planes) at time $k+1$, in terms of the plane dimensions (plane area and vertex distance) and the plane orientation (normal vector).

The rest of the paper is organized as follows. Section 2 provides relevant theoretical background. Section 3 introduces the details of the proposed transition model which comprises two sub-models: the plane transition model and the vehicle transition model. Section 4 explains the detailed procedure for implementation of the proposed transition model. Section 5 provides verification of the proposed transition model by means of graphical and numerical simulation results. Section 6 concludes the paper.

2. Background

The proposed transition model is mainly designed to be used with a recently developed Random Finite Set (RFS) based Bayesian filter—the Labeled Multi-Bernoulli (LMB) filter [20]. In this section, we provide the theoretical background necessary for understanding this filter, followed by a brief introduction to the KITTI dataset [19] which was used for simulation studies in this work.

2.1. Labeled Multi-Bernoulli Filter

In this section, we introduce the Labeled Multi-Bernoulli (LMB) filter [20]. We adopt the same notation used in [20] where the single-object states are denoted by lower-case letters, e.g., x, $\mathbf{x}$ and multi-object states by upper-case letters, e.g., X, $\mathbf{X}$. In order to distinguish between labeled and unlabeled states and their distributions, the labeled one is shown by bolded letters, e.g., $\mathbf{x}$, $\mathbf{X}$, etc., spaces by blackboard bold, e.g., $\mathbb{X}$, $\mathbb{L}$, $\mathbb{C}$, etc., and the class of finite subsets of a space $\mathbb{X}$ by $\mathcal{F}\left(\mathbb{X}\right)$. Following [20], throughout the paper, the standard inner product notation is used and denoted by $\langle f,g\rangle \triangleq \int f\left(x\right)g\left(x\right)dx,$ the generalized Kronecker delta is denoted by

$${\delta }_Y\left(X\right)\triangleq \left\{\begin{array}{c}1,\mathrm{if}X=Y\hfill \\ 0,\mathrm{otherwise}\hfill \end{array}\right.,$$

and the inclusion function, a generalization of the indicator function, by

$$1_Y\left(X\right)\triangleq \left\{\begin{array}{c}1,\mathrm{if}X\subseteq Y\hfill \\ 0,\mathrm{otherwise}\hfill \end{array}\right..$$

The LMB RFS is completely described by its components $\pi =\{(r^{(\ell )},p^{(\ell )}):\ell \in \mathbb{L}\}$. The LMB RFS density is given by $\pi \left(\mathbf{X}\right)=\Delta \left(\mathbf{X}\right)w\left(\mathcal{L}\left(\mathbf{X}\right)\right){\left[p\right]}^{\mathbf{X}},$ where $p(x,\ell )=p^{(\ell )}\left(x\right)$ and $w\left(L\right)={\displaystyle \prod _{i\in \mathbb{L}}\left(1-r^{\left(i\right)}\right)\prod _{\ell \in L}}\frac{1_{\mathbb{L}}(\ell )r^{(\ell )}}{(1-r^{(\ell )})}$ comprising a single component [20]. The LMB multi-target Bayes recursion propagates multi-target posterior density at each time step according to the Chapman–Kolmogorov and the Bayes rule.

2.1.1. Prediction

Assume that the prior and birth labeled multi-Bernoulli sets are modeled as follows:

$$\begin{array}{}\mathrm{(1)}& \hfill \pi \left(\mathbf{X}\right)& =& \Delta \left(\mathbf{X}\right)w\left(\mathcal{L}\left(\mathbf{X}\right)\right){\left[p\right]}^{\mathbf{X}}\hfill \mathrm{(2)}& \hfill {\pi }_{{}_B}\left(\mathbf{X}\right)& =& \Delta \left(\mathbf{X}\right)w_{{}_B}\left(\mathcal{L}\left(\mathbf{X}\right)\right){\left[p_{{}_B}\right]}^{\mathbf{X}}\hfill \end{array}$$

where

$$\begin{array}{}\mathrm{(3)}& \hfill w\left(L\right)& =& \prod _{i\in \mathbb{L}}\left(1-r^{\left(i\right)}\right)\prod _{\ell \in L}\frac{1_{\mathbb{L}}(\ell )r^{(\ell )}}{1-r^{(\ell )}},\hfill \mathrm{(4)}& \hfill w_{{}_B}\left(L\right)& =& \prod _{i\in \mathbb{B}}\left(1-r_{{}_B}^{\left(i\right)}\right)\prod _{\ell \in L}\frac{1_{\mathbb{B}}(\ell )r_{{}_B}^{(\ell )}}{1-r_{{}_B}^{(\ell )}},\hfill \end{array}$$

$$\begin{array}{}\mathrm{(5)}& \hfill p(x,\ell )& =& p^{(\ell )}\left(x\right)\hfill \mathrm{(6)}& \hfill p_{{}_B}(x,\ell )& =& p_{{}_B}^{(\ell )}\left(x\right).\hfill \end{array}$$

with state space $\mathbb{X}$ and label space ${\mathbb{L}}_{+}=\mathbb{B}\cup \mathbb{L}$ and with the condition $\mathbb{B}\cap \mathbb{L}=\varnothing$. The predicted multi-object distribution is then a LMB RFS and given by

$${\pi }_{+}\left(\mathbf{X}\right)=\Delta \left(\mathbf{X}\right)w_{+}\left(\mathcal{L}\left(\mathbf{X}\right)\right){\left[p_{+}\right]}^{\mathbf{X}}$$

(7)

where

$$\begin{array}{}\mathrm{(8)}& \hfill w_{+}\left(I_{+}\right)& =& w_{{}_S}(I_{+}\cap \mathbb{L})w_{{}_B}(I_{+}\cap \mathbb{B})\hfill \mathrm{(9)}& \hfill w_{{}_S}\left(L\right)& =& {\left(1-r^{(·)}{\eta }_{{}_S}(·)\right)}^{\mathbb{L}}{\left(\frac{r^{(·)}{\eta }_{{}_S}(·)}{1-r^{(·)}{\eta }_{{}_S}(·)}\right)}^L,\hfill \mathrm{(10)}& \hfill {\eta }_{{}_S}(\ell )& =& 〈p_{{}_S}(·,\ell ),p(·,\ell )〉\hfill \mathrm{(11)}& \hfill p_{+}(x,\ell )& =& 1_{\mathbb{L}}(\ell )p_{+,S}(x,\ell )+1_{\mathbb{B}}(\ell )p_{{}_B}(x,\ell )\hfill \mathrm{(12)}& \hfill p_{+,S}(x,\ell )& =& \frac{〈p_{{}_S}(·,\ell )f\left(x\right|·,\ell ),p(·,\ell )〉}{{\eta }_{{}_S}(\ell )}\hfill \end{array}$$

where $p_S(·|\ell$) is the survival probability of an object and $f\left(x\right|·,\ell$) is the single-object transition model. Thus, if the multi-target posterior density is an LMB RFS with parameter set $\pi =\{(r^{(\ell )},p^{(\ell )}):\ell \in \mathbb{L}\}$ with state space $\mathbb{X}$ and label space $\mathbb{L}$ and the birth model is also an LMB RFS with parameter set ${\pi }_{\mathbf{B}}=\{(r_B^{(\ell )},p_B^{(\ell )}):\ell \in \mathbb{B}\}$ with state space $\mathbb{X}$ and label space $\mathbb{B}$ then the predicted multi-target density is also an LMB RFS with state space $\mathbb{X}$ and label space ${\mathbb{L}}_{+}=\mathbb{B}\cup \mathbb{L}(\mathbb{B}\cap \mathbb{L}=\varnothing )$ and it is given by

$${\pi }_{+}=\{(r_{+,S}^{(\ell )},p_{+,S}^{(\ell )}):\ell \in \mathbb{L}\}\cup \{(r_B^{(\ell )},p_B^{(\ell )}):\ell \in \mathbb{B}\}$$

(13)

where

$$\begin{array}{}\mathrm{(14)}& \hfill r_{+,S}^{(\ell )}& =& {\eta }_{{}_S}(\ell )r^{(\ell )},\hfill \mathrm{(15)}& \hfill p_{+,S}^{(\ell )}& =& \frac{〈p_{{}_S}(·,\ell )f\left(x\right|·,\ell ),p(·,\ell )〉}{{\eta }_{{}_S}(\ell )},\hfill \end{array}$$

for more details see— [20]—proposition 2.

2.1.2. Update

If the predicted multi-target density is an LMB RFS with parameter set ${\pi }_{+}=\{(r_{+}^{(\ell )},p_{+}^{(\ell )}):\ell \in {\mathbb{L}}_{+}\}$, the multi-target posterior is then given by

$$\pi (·|Z)=\{(r^{(\ell )},p^{(\ell )}(·)):\ell \in {\mathbb{L}}_{+}\}$$

(16)

where

$$\begin{array}{}\mathrm{(17)}& \hfill r^{(\ell )}& =& \sum _{(I_{+},\theta )\in \mathcal{F}\left({\mathbb{L}}_{+}\right)\times {\Theta }_{I_{+}}}{w}^{(I_{+},\theta )}\left(Z\right)1_{I_{+}}(\ell ),\hfill \mathrm{(18)}& \hfill p^{(\ell )}\left(x\right)& =& \frac{1}{r^{(\ell )}}\sum _{(I_{+},\theta )\in \mathcal{F}\left({\mathbb{L}}_{+}\right)\times {\Theta }_{I_{+}}}{w}^{(I_{+},\theta )}\left(Z\right)1_{I_{+}}(\ell )p^{\left(\theta \right)}(x,\ell ),\hfill \end{array}$$

where ${\Theta }_{I_{+}}$ denotes the space of mapping $\theta :I_{+}\to \{0,1,\cdots ,|Z\left|\right\}$ and,

$$\begin{array}{}\mathrm{(19)}& \hfill w^{(I_{+},\theta )}\left(Z\right)& \propto & w_{+}\left(I_{+}\right){\left[{\eta }_Z^{\left(\theta \right)}\right]}^{I_{+}}\hfill \mathrm{(20)}& \hfill p^{\left(\theta \right)}(x,\ell |Z)& =& \frac{p_{+}(x,\ell ){\psi }_Z(x,\ell ;\theta )}{{\eta }_Z^{\left(\theta \right)}(\ell )},\hfill \mathrm{(21)}& \hfill {\eta }_Z^{\left(\theta \right)}(\ell )& =& \langle p_{+}(·,\ell ),{\psi }_Z(·,\ell ;\theta )\rangle ,\hfill & \hfill {\psi }_Z(x,\ell ;\theta )& =& {\delta }_0\left(\theta (\ell )\right)q_{{}_D}(x,\ell )\hfill \mathrm{(22)}& & +& (1-{\delta }_0\left(\theta (\ell )\right))\frac{p_D(x,\ell )g\left(z_{\theta (\ell )}\right|x,\ell )}{\kappa \left(z_{\theta (\ell )}\right)}\hfill \end{array}$$

where, $g\left(z\right|x)$ is the single-sensor measurement likelihood, $p_{{}_D}(·,\ell )$ denotes probability of detection, $q_{{}_D}(·,\ell )=1-p_{{}_D}(·,\ell )$ is the probability of a missed detection, and $\kappa (·)$ intensity function of the Poisson distributed clutter process.

2.2. KITTI Dataset

The KITTI dataset [19] was recorded in and around Karlsruhe, Germany using a VW wagon equipped with various types of sensors including a 3D laser scanner, four video cameras and a GPS/IMU navigation system. The data was collected in both urban and suburban areas (city, rural, and highway) and information was gathered over several days. In this dataset, many of the scenes are dominated by large buildings with planar surfaces. The KITTI dataset has been widely used in mobile robotics and autonomous driving research.

3. Proposed Transition Model

As explained in Section 1, a transition model is required in SLAM filters to predict the states of the features at the next time step, based on the state information at the current time step. Specifically, in this study, we are interested in constructing a transition model for predicting the plane parameters at the next time step, using the current plane parameters estimated by a SLAM filter. Note that the planes involved in the transition model are expressed in the vehicle coordinate system (fixed to the vehicle), as the point cloud measurements are acquired by a laser scanner mounted on top of the experimental vehicle. Thus the plane parameters vary with time, as long as the experimental vehicle is in motion. On the other hand, if seen from the global coordinate system (fixed to the ground), the plane parameters are invariant since the planes are all static in this coordinate system.

We emphasize that the vehicle is assumed to move in a static environment. If this is not the case, we make the practical assumption that planar surfaces on other moving objects are small enough to be excluded from the set “plane observations” extracted from the 3D point cloud. That set is expected to include only relatively large planar surfaces such as walls of buildings along the road.

3.1. Plane Transition Model

To facilitate the design of the transition model, the two aforementioned coordinate systems are established, as shown in Figure 1. The global coordinate system $OXYZ$ is fixed to the ground, and the vehicle coordinate system $oxyz$ is attached to the vehicle Center of Mass and moves along with the vehicle. P is a point on a plane segmented from the 3D point cloud obtained from the on-board laser scanner. ${\mathit{r}}_{\mathrm{P}}$ and $\mathit{r}$ are the position vectors of P in the global and vehicle coordinate systems, and ${\mathit{r}}_{\mathrm{V}}$ is the position vector of the vehicle centre of mann (i.e., origin o) in the global coordinate system. The global coordinates of P are invariant since ${\mathit{r}}_{\mathrm{P}}$ is static as seen from the global coordinate system. However, the local coordinates of P, expressed in the vehicle coordinate system, vary with time due to the motion of the vehicle. As a result, the plane parameters expressed in the vehicle coordinate system also evolve with time, and a transition model is thus needed to predict the change of plane parameters for the purpose of accurate and effective SLAM.

The plane equation, expressed in the vehicle coordinate system, is assumed to take the following form in this study:

$$ax+by+cz+d=0,$$

(23)

where x, y and z are the coordinates of a point on the plane, and a, b, c and d are the plane parameters. Note that other forms of plane equations are also available in the literature [21], but all forms of plane equations can eventually lead to Equation (23) after simple algebraic manipulation. We may rewrite Equation (23) in the following vector form:

$${\mathbf{\beta }}^{\mathrm{T}}\mathit{r}=0,$$

(24)

where $\mathbf{\beta }={\left[abcd\right]}^{\mathrm{T}}$ and $\mathit{r}={\left[xyz1\right]}^{\mathrm{T}}$.

Assuming that the vehicle is only performing a planar motion (which is normally the case in common flat-road urban driving scenarios), the coordinates of point P in the global coordinate system ${\mathit{r}}_{\mathrm{P}}={\left[XYZ1\right]}^{\mathrm{T}}$, and the coordinates of point P in the vehicle coordinate system $\mathit{r}={\left[xyz1\right]}^{\mathrm{T}}$, are related according to the following equations describing the kinematics of the vehicle [22,23]:

$${\mathit{r}}_{\mathrm{P}}=\mathit{R}\mathit{r}$$

(25)

and

$$\mathit{R}=\left[\begin{array}{cccc}cos\varphi & -sin\varphi & 0& X_{\mathrm{V}}\\ sin\varphi & cos\varphi & 0& Y_{\mathrm{V}}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$$

(26)

where $\varphi$ represents the heading angle of the vehicle (with respect to the X axis), and $X_{\mathrm{V}}$ and $Y_{\mathrm{V}}$ denote the coordinates of the vehicle centre of mass (origin o) in the global coordinate system (namely ${\mathit{r}}_{\mathrm{V}}={\left[X_{\mathrm{V}}{Y}_{\mathrm{V}}01\right]}^{\mathrm{T}}$). The vehicle states $\varphi$, $X_{\mathrm{V}}$ and $Y_{\mathrm{V}}$ are graphically shown in Figure 2.

Equations (25) and (26) can be rewritten as follows, for time k:

$${\mathit{r}}_{\mathrm{P}}={\mathit{R}}_k{\mathit{r}}_k$$

(27)

and

$${\mathit{R}}_k=\left[\begin{array}{cccc}cos{\varphi }_k& -sin{\varphi }_k& 0& X_{\mathrm{V},k}\\ sin{\varphi }_k& cos{\varphi }_k& 0& Y_{\mathrm{V},k}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$$

(28)

where ${\mathit{r}}_k={\left[x_k{y}_k{z}_{k}1\right]}^{\mathrm{T}}$. Similarly, we have the following equations for time $k+1$:

$${\mathit{r}}_{\mathrm{P}}={\mathit{R}}_{k+1}{\mathit{r}}_{k+1}$$

(29)

and

$${\mathit{R}}_{k+1}=\left[\begin{array}{cccc}cos{\varphi }_{k+1}& -sin{\varphi }_{k+1}& 0& X_{\mathrm{V},k+1}\\ sin{\varphi }_{k+1}& cos{\varphi }_{k+1}& 0& Y_{\mathrm{V},k+1}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$$

(30)

where ${\mathit{r}}_{k+1}={\left[x_{k+1}{y}_{k+1}{z}_{k+1}1\right]}^{\mathrm{T}}$.

The vector ${\mathit{r}}_{\mathrm{P}}$ is invariant with time, since it is the position vector of P in the global coordinate system. As a result, one can achieve the following equation by combining Equations (27) and (29):

$${\mathit{r}}_{k+1}={\mathit{R}}_{k+1}^{-1}{\mathit{R}}_k{\mathit{r}}_k.$$

(31)

Note that Equation (24) can be rewritten as follows, for time k:

$${\mathbf{\beta }}_k^{\mathrm{T}}{\mathit{M}}_k^{-1}{\mathit{M}}_k{\mathit{r}}_k=0,$$

(32)

where ${\mathit{M}}_k$ represents a 4-by-4 invertible matrix. We might as well let ${\mathit{M}}_k={\mathit{R}}_{k+1}^{-1}{\mathit{R}}_k$ and combine Equations (31) and (32), then we arrive at:

$${\mathbf{\beta }}_k^{\mathrm{T}}{\mathit{M}}_k^{-1}{\mathit{r}}_{k+1}=0.$$

(33)

The plane Equation (24) for time $k+1$ can be expressed as follows:

$${\mathbf{\beta }}_{k+1}^{\mathrm{T}}{\mathit{r}}_{k+1}=0.$$

(34)

Combining Equations (33) and (34) leads to the following transition model for plane parameters:

$${\mathbf{\beta }}_{k+1}^{\mathrm{T}}={\mathbf{\beta }}_k^{\mathrm{T}}{\mathit{M}}_k^{-1},$$

(35)

where ${\mathit{M}}_k^{-1}={\left({\mathit{R}}_{k+1}^{-1}{\mathit{R}}_k\right)}^{-1}={\mathit{R}}_k^{-1}{\mathit{R}}_{k+1}$.

This plane transition model indicates that the plane parameters at time $k+1$ can be calculated based on the plane parameter information at time k and a vehicle-motion-dependent 4-by-4 matrix ${\mathit{M}}_k^{-1}$. Note that the computation of matrix ${\mathit{M}}_k^{-1}={\mathit{R}}_k^{-1}{\mathit{R}}_{k+1}$ requires the inverse of ${\mathit{R}}_k$. It can be easily proven that the matrix ${\mathit{R}}_k$, expressed by Equation (28), has full rank and its inverse ${\mathit{R}}_k^{-1}$ exists at all times. Thus, the matrix ${\mathit{M}}_k^{-1}$ can be computed as long as ${\mathit{R}}_{k+1}$ is available. However, this matrix ${\mathit{R}}_{k+1}$ (as indicated by Equation (30)) requires the vehicle state information at the future time $k+1$, which is not available in real-time SLAM applications. To tackle this issue, in the following section we will introduce a new vehicle transition model and demonstrate how the vehicle states at time $k+1$ can be predicted.

3.2. Vehicle Transition Model

As mentioned above, the transformation matrix ${\mathit{R}}_{k+1}$ requires the unknown information from the future time $k+1$. In order to obtain matrix ${\mathit{R}}_{k+1}$, in this section a vehicle transition model is introduced to predict the future vehicle states at time $k+1$, using the available information at time k.

In Bayesian filtering, the Constant Turn (CT) model [24,25,26] which describes the maneuver of the vehicle motion is commonly used. This model is formulated based on the assumption that the vehicle maneuvers with a (nearly) constant speed and a (nearly) constant angular rate [24]. Based on the CT model, an Extended Constant Turn (ECT) model is proposed in this work, which can be mathematically expressed as follows:

$${\mathit{x}}_{k+1}={\mathit{F}}_k{\mathit{x}}_k+\mathit{G}\mathit{u},$$

(36)

with

$${\mathit{x}}_{k+1}={\left[X_{\mathrm{V},k+1}{\dot{X}}_{\mathrm{V},k+1}{Y}_{\mathrm{V},k+1}{\dot{Y}}_{\mathrm{V},k+1}{\varphi }_{k+1}{\dot{\varphi }}_{k+1}\right]}^{\mathrm{T}},$$

$${\mathit{x}}_k={\left[X_{\mathrm{V},k}{\dot{X}}_{\mathrm{V},k}{Y}_{\mathrm{V},k}{\dot{Y}}_{\mathrm{V},k}{\varphi }_k{\dot{\varphi }}_k\right]}^{\mathrm{T}},$$

$$\mathit{u}={\left[u_X{u}_Y{u}_{\varphi }\right]}^{\mathrm{T}},$$

$${\mathit{F}}_k=\left[\begin{array}{cccccc}1& \frac{sin\left({\dot{\varphi }}_{k}T\right)}{{\dot{\varphi }}_k}& 0& \frac{cos\left({\dot{\varphi }}_{k}T\right)-1}{{\dot{\varphi }}_k}& 0& 0\\ 0& cos\left({\dot{\varphi }}_{k}T\right)& 0& -sin\left({\dot{\varphi }}_{k}T\right)& 0& 0\\ 0& \frac{1-cos\left({\dot{\varphi }}_{k}T\right)}{{\dot{\varphi }}_k}& 1& \frac{sin\left({\dot{\varphi }}_{k}T\right)}{{\dot{\varphi }}_k}& 0& 0\\ 0& sin\left({\dot{\varphi }}_{k}T\right)& 0& cos\left({\dot{\varphi }}_{k}T\right)& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right],\mathrm{and}\mathit{G}=\left[\begin{array}{ccc}\frac{T^2}{2}& 0& 0\\ T& 0& 0\\ 0& \frac{T^2}{2}& 0\\ 0& T& 0\\ 0& 0& \frac{T^2}{2}\\ 0& 0& T\end{array}\right],$$

where $X_{\mathrm{V},k}$, $Y_{\mathrm{V},k}$, $X_{\mathrm{V},k+1}$ and $Y_{\mathrm{V},k+1}$ denote the vehicle longitudinal and lateral displacements (i.e., the coordinates of the vehicle centre of mass as introduced in Section 3.1) in the global coordinate system at time k and $k+1$, ${\varphi }_k$ and ${\varphi }_{k+1}$ represent the vehicle heading angles at time k and $k+1$, $u_X$, $u_Y$ and $u_{\omega }$ are the vehicle velocity and yaw rate noise components, and T stands for the sampling period. By means of this vehicle transition model (36), the required vehicle state information at time $k+1$ can be predicted and in turn the matrices ${\mathit{R}}_{k+1}$ and ${\mathit{M}}_k^{-1}$ can be calculated.

Note that the plane transition model proposed in Section 3.1 (i.e., Equation (35)), along with the above ECT vehicle transition model, constitutes a complete state transition model that propagates the state densities of the planar features to the next time. This state transition model plays a key role in the prediction step for prospective planar-feature-based SLAM filters.

4. Implementation

In this section, we introduce the Sequential Monte Carlo (SMC) implementation of the proposed transition model, for the recently developed LMB filter. The LMB filter has been used in many applications such as target tracking [27,28], SLAM [29], visual tracking [30], and sensor control [31,32]. In the SMC implementation, the LMB posterior for the map features (planes) at each time step are represented by a set of particles. These particles are propagated using the proposed transition model to obtain the LMB posterior at the next time step.

Assuming the following LMB posterior ${\mathbf{\pi }}_k$ at time k:

$${\mathbf{\pi }}_k={\left\{\left(r_k^{(\ell )},p_k^{(\ell )}\right)\right\}}_{\ell \in \mathbb{L}},$$

(37)

where ℓ represents the label of a plane, $r_k^{(\ell )}$ denotes the existence probability of the plane ℓ, $p_k^{(\ell )}$ is the spatial distribution of its parameters, and $\mathbb{L}$ stands for the label space. In an SMC implementation, the spatial distribution is represented by a set of weighted samples [28], namely:

$$p_k^{(\ell )}=\sum _{i=1}^{N^{(\ell )}}{\omega }_i^{(\ell )}\delta \left(a-a_{i,k}^{(\ell )}\right)\delta \left(b-b_{i,k}^{(\ell )}\right)\delta \left(c-c_{i,k}^{(\ell )}\right)\delta \left(d-d_{i,k}^{(\ell )}\right),$$

(38)

where $N^{(\ell )}$ represents the number of particles, ${\omega }_i^{(\ell )}$ denotes the weight of the i-th particle, and $\delta$ is the Dirac delta function.

For each particle ${\mathbf{\beta }}_{i,k}^{(\ell )}={\left[a_{i,k}^{(\ell )},b_{i,k}^{(\ell )},c_{i,k}^{(\ell )},d_{i,k}^{(\ell )}\right]}^{\mathrm{T}}$ at time k, we are able to calculate the corresponding predicted particle ${\mathbf{\beta }}_{i,k+1}^{(\ell )}={\left[a_{i,k+1}^{(\ell )},b_{i,k+1}^{(\ell )},c_{i,k+1}^{(\ell )},d_{i,k+1}^{(\ell )}\right]}^{\mathrm{T}}$ at time $k+1$ based on the transition model proposed in Section 3. As a result, after the prediction stage, we will end up with a new set of particles with the same weights, namely:

$${\left\{{\omega }_{i,k+1}^{(\ell )},{\mathbf{\beta }}_{i,k+1}^{(\ell )}\right\}}_{i=1}^{N^{(\ell )}},$$

where ${\omega }_{i,k+1}^{(\ell )}={\omega }_{i,k}^{(\ell )}$. Then, based on the new predicted particles at time $k+1$, the plane parameter estimate ${\widehat{\mathbf{\beta }}}_{k+1}^{(\ell )}$ can be achieved from the distribution particles as the Expected A Posteriori (EAP) estimate (i.e., weighted average), or the Maximum A Posteriori (MAP) estimate (i.e., considering the particle with the largest weight as the estimate).

The above implementation procedure is illustrated step by step in Algorithm 1, in the form of a pseudo-code.

Algorithm 1 Step-by-Step Implementation of the Proposed Transition Model
Require: vehicle states $X_{\mathrm{V},k}$, $Y_{\mathrm{V},k}$, ${\varphi }_k$, and particles ${\left\{{\omega }_{i,k}^{(\ell )},{\mathbf{\beta }}_{i,k}^{(\ell )}\right\}}_{i=1}^{N^{(\ell )}}$ representing the distribution of each plane with label ℓ, for all labels $\ell \in \mathbb{L}$
1:  compute matrix ${\mathit{R}}_k$	▹ use Equation (28)
2:  generate a noise sample $\mathit{u}\sim \mathcal{N}(0,\Sigma )$	▹$\Sigma \sim$ diag(noise variances)
3:  generate $X_{\mathrm{V},k+1}$, $Y_{\mathrm{V},k+1}$ and ${\varphi }_{k+1}$	▹ use Equation (36)
4:  compute matrix ${\mathit{R}}_{k+1}$	▹ use Equation (30)
5:  ${\mathit{M}}_k^{-1}\leftarrow {\mathit{R}}_k^{-1}{\mathit{R}}_{k+1}$
6:  for $\ell \in \mathbb{L}$ do
7:  for $i⩽N^{(\ell )}$ do
8:     ${{\mathbf{\beta }}_{i,k+1}^{(\ell )}}^{\mathrm{T}}\leftarrow {{\mathbf{\beta }}_{i,k}^{(\ell )}}^{\mathrm{T}}\times {\mathit{M}}_k^{-1}$
9:     ${\omega }_{i,k+1}^{(\ell )}\leftarrow {\omega }_{i,k}^{(\ell )}$
10:  end for
11:  ${\displaystyle {\widehat{\mathbf{\beta }}}_{k+1}^{(\ell )}\leftarrow \sum _{i=1}^{N^{(\ell )}}{\omega }_{i,k+1}^{(\ell )}{\mathbf{\beta }}_{i,k+1}^{(\ell )}}$	▹ compute the EAP estimate
12:  end for

5. Simulation Results

In this section, we demonstrate how well the proposed transition model performs, compared with the actually measured results from the KITTI dataset. Specifically, we generate the predicted planes at time $k+1$ using the plane information at time k, by means of the proposed transition model, and we segment planes from the point cloud data in the KITTI dataset at time $k+1$, by means of the Modified Selective Statistical Estimator (MSSE) [33]. Then, the predicted planes at time $k+1$ are compared with the segmented planes at time $k+1$, and both graphical and numerical results are demonstrated to show the closeness of the predicted planes and the segmented planes.

The point cloud data from the folder “2011_09_26_drive_0005_sync” in the KITTI dataset are employed in the simulation studies for plane segmentation. The details for accessing and using the KITTI dataset are available in [19]. The folder “2011_09_26_drive_0005_sync” includes 154 time steps (i.e., $k\in [0,153]$), and the points obtained at $k=11$, $k=12$, $k=124$ and $k=125$ are used in the simulation. The number of points at $k=11$, $k=12$, $k=124$ and $k=125$ are 123,334, 123,316, 118,950 and 118,572 respectively.

5.1. Graphical Results

Figure 3 and Figure 4 demonstrate both the predicted planes and the segmented planes at time $k=125$ for one Monte Carlo (MC) run. The predicted planes are in orange color, and are generated by the proposed transition model using the LIDAR measurements and vehicle states at time $k=124$. The segmented planes are in blue color, and are plotted based on the point cloud measurements at time $k=125$. Apart from the planes themselves, the predicted and measured point clouds at time $k=125$ are also plotted in Figure 4, on top of the corresponding planes.

Figure 3 shows that by means of the transition model and the available information at time $k=124$, three planes are predicted to exist at time $k=125$. Also, three segmented planes are present in this figure based on the LIDAR measurements at time $k=125$. It is clearly observed in Figure 3 that each predicted plane is associated with one segmented plane. Namely, these six planes constitute three pairs of planes, with each pair composed of one predicted plane and one segmented plane. Besides, we see that for each pair of orange and blue planes, the areas of planes are fairly close and the distances between each pair of plane vertexes are rather small (‘Each pair of plane vertexes’ is referred to one vertex on the predicted plane and its closest counterpart on the segmented plane).

Figure 4 demonstrates the three pairs of planes from different angles. Again, we observe the closeness of the predicted and segmented planes in terms of plane areas and vertex distances, when viewed from different angles. Figure 4c shows a ‘side view’ of the three pairs of planes, from which we clearly see that the predicted and segmented planes (and their normal vectors) almost coincide. Similar results are also observed in Figure 4d where a ‘top view’ of the planes is shown.

In addition to $k=125$, our simulation studies have included a large range of other time steps. Figure 5 and Figure 6 provide the results of another time step $k=12$. The rest of time steps present similar results and are not shown in this paper for the purpose of brevity. In the following section, the above graphical results will be further supplemented and clarified, by means of detailed numerical results.

5.2. Numerical Results

In Figure 3, Figure 4, Figure 5 and Figure 6, we have graphically demonstrated the comparison results between the predicted and segmented planes, in terms of three important indicators – plane area, vertex distance and normal vector. In this section, we provide the results of our numerical simulation for 100 MC runs, in order to demonstrate ‘how close’ exactly these planes are.

The first two indicators, plane area and vertex distance, are mainly used to quantify the closeness in terms of plane dimensions (sizes).

Table 1. Areas of the predicted and segmented planes for time step $k=125$.

	Plane Pair 1	Plane Pair 2	Plane Pair 3
Predicted Plane	$13.7147{\mathrm{m}}^2$	$219.9937{\mathrm{m}}^2$	$22.9218{\mathrm{m}}^2$
Segmented Plane	$11.0470{\mathrm{m}}^2$	$226.3712{\mathrm{m}}^2$	$23.2926{\mathrm{m}}^2$

shows the areas of the predicted and segmented planes for time step $k=125$ (averaged from 100 MC runs), and

Table 2. Distances between each pair of plane vertexes for time step $k=125$.

	Plane Pair 1	Plane Pair 2	Plane Pair 3
Vertex Pair 1	$0.2503\mathrm{m}$	$0.7089\mathrm{m}$	$0.0603\mathrm{m}$
Vertex Pair 2	$0.8519\mathrm{m}$	$0.8667\mathrm{m}$	$0.0294\mathrm{m}$
Vertex Pair 3	$0.1293\mathrm{m}$	$0.9060\mathrm{m}$	$0.0837\mathrm{m}$
Vertex Pair 4	$0.8107\mathrm{m}$	$1.0730\mathrm{m}$	$0.0677\mathrm{m}$

gives the distances between each pair of plane vertexes for time step $k=125$ (averaged from 100 MC runs). We see in these tables that the areas of the predicted and segmented planes are close, and the distances between each pair of vertexes are small. These numerical results are consistent with the graphical results shown in Section 5.1.

The third indicator, normal vector, is employed to evaluate the closeness in terms of plane orientation.

Table 3. Angles between each pair of normal vectors for time step $k=125$.

	Plane Pair 1	Plane Pair 2	Plane Pair 3
Angle Between Normal Vectors	$0.0654\mathrm{rad}$/${3.7458}^{\circ }$	$0.0129\mathrm{rad}$/${0.7382}^{\circ }$	$0.0037\mathrm{rad}$/${0.2115}^{\circ }$

shows the angles between each pair of normal vectors for time step $k=125$ (averaged from 100 MC runs) (‘Each pair of normal vectors’ is referred to the normal vector on the predicted plane and its counterpart on the segmented plane). The small magnitudes of these angles imply that the predicted and segmented planes are very close in terms of orientation. This explains the coincidence of the predicted and segmented planes seen in Figure 4c,d.

Besides the above three indicators for closeness evaluation, the execution time of the program is also recorded. The proposed transition model is implemented based on Algorithm 1, and the program is executed on a laptop equipped with an Intel i7-5600U CPU and an 8G RAM. For time step $k=125$, the execution time is 0.0375 s (averaged from 100 MC runs).

Apart from $k=125$, the numerical results for another time step $k=12$ are also presented in

Table 4. Areas of the predicted and segmented planes for time step $k=12$.

	Plane Pair 1	Plane Pair 2	Plane Pair 3
Predicted Plane	$34.5121{\mathrm{m}}^2$	$22.7771{\mathrm{m}}^2$	$91.5893{\mathrm{m}}^2$
Segmented Plane	$33.7797{\mathrm{m}}^2$	$23.0555{\mathrm{m}}^2$	$90.5685{\mathrm{m}}^2$

,

Table 5. Distances between each pair of plane vertexes for time step $k=12$.

	Plane Pair 1	Plane Pair 2	Plane Pair 3
Vertex Pair 1	$0.1311\mathrm{m}$	$0.8031\mathrm{m}$	$0.1053\mathrm{m}$
Vertex Pair 2	$0.1117\mathrm{m}$	$0.1600\mathrm{m}$	$0.1119\mathrm{m}$
Vertex Pair 3	$0.0602\mathrm{m}$	$0.2778\mathrm{m}$	$0.5599\mathrm{m}$
Vertex Pair 4	$0.0924\mathrm{m}$	$1.0056\mathrm{m}$	$0.5558\mathrm{m}$

and

Table 6. Angles between each pair of normal vectors for time step $k=12$.

	Plane Pair 1	Plane Pair 2	Plane Pair 3
Angle Between Normal Vectors	$0.0034\mathrm{rad}$/${0.1936}^{\circ }$	$0.1133\mathrm{rad}$/${6.4912}^{\circ }$	$0.0070\mathrm{rad}$/${0.3987}^{\circ }$

. Similarly, these results show that the predicted planes (obtained using the information at $k=11$) are fairly close to the segmented planes at $k=12$, in terms of plane areas, vertex distances and normal vectors. Besides, the execution time of the program for $k=12$ is 0.0369 s (averaged from 100 MC runs). For the purpose of brevity, the numerical results for other time steps are omitted in this paper.

6. Conclusions

The majority of the current statistical SLAM solutions are based on using point features such as the representation of landmarks. The fast advancement of sensory technology makes it possible to utilize more sophisticated features in SLAM applications to achieve superior results while lowering computational cost. This paper presented the idea of using planar features for statistical SLAM, and proposed a stochastic transition model to propagate the plane parameters to the next time. A large range of simulation studies using real-world measurements have been conducted to evaluate the proposed transition model. Both graphical and numerical results show that the predicted planes generated by the proposed transition model closely match the segmented planes resulting from real-world point cloud measurements, in terms of three important indicators; plane area, vertex distance and normal vector. In the next step, we will look into other planar-feature-based stochastic transition models, and investigate the application of such transition models in statistical SLAM tasks.