Open AccessArticle

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

Amirali Khodadadian Gostar

Chunyun Fu

^2,*

Weiqin Chuah

Mohammed Imran Hossain

Ruwan Tennakoon

Alireza Bab-Hadiashar

and

Reza Hoseinnezhad

School of Engineering, RMIT University, Melbourne VIC 3001, Australia

State Key Laboratory of Mechanical Transmissions, School of Automotive Engineering, Chongqing University, Chongqing 400044, China

Author to whom correspondence should be addressed.

Sensors 2019, 19(7), 1614; https://doi.org/10.3390/s19071614

Submission received: 11 February 2019 / Revised: 23 March 2019 / Accepted: 1 April 2019 / Published: 3 April 2019

(This article belongs to the Special Issue Multiple Object Tracking: Making Sense of the Sensors)

Download

Browse Figures

Figure 1
An example of the global and vehicle coordinate systems. "> Figure 2
Top view of the global and vehicle coordinate systems. "> Figure 3
Three pairs of predicted and segmented planes at time <math display="inline"><semantics> <mrow> <mi>k</mi> <mo>=</mo> <mn>125</mn> </mrow> </semantics></math>. "> Figure 4
Multi-angle comparison of the predicted and segmented planes at time <math display="inline"><semantics> <mrow> <mi>k</mi> <mo>=</mo> <mn>125</mn> </mrow> </semantics></math>. "> Figure 5
Three pairs of predicted and segmented planes at time <math display="inline"><semantics> <mrow> <mi>k</mi> <mo>=</mo> <mn>12</mn> </mrow> </semantics></math>. "> Figure 6
Multi-angle comparison of the predicted and segmented planes at time <math display="inline"><semantics> <mrow> <mi>k</mi> <mo>=</mo> <mn>12</mn> </mrow> </semantics></math>. "> Figure 6 Cont.
Multi-angle comparison of the predicted and segmented planes at time <math display="inline"><semantics> <mrow> <mi>k</mi> <mo>=</mo> <mn>12</mn> </mrow> </semantics></math>. ">

Versions Notes

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.

Keywords:

simultaneous localization and mapping; planar features; plane parameters; transition model; Bayesian filters

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,

x

and multi-object states by upper-case letters, e.g., X,

X

. In order to distinguish between labeled and unlabeled states and their distributions, the labeled one is shown by bolded letters, e.g.,

x

X

, etc., spaces by blackboard bold, e.g.,

X

L

C

, etc., and the class of finite subsets of a space

X

F (X)

. Following [20], throughout the paper, the standard inner product notation is used and denoted by

〈 f, g 〉 ≜ \int f (x) g (x) d x,

the generalized Kronecker delta is denoted by

δ_{Y} (X) ≜ \{\begin{matrix} 1, if X = Y \\ 0, otherwise \end{matrix},

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

1_{Y} (X) ≜ \{\begin{matrix} 1, if X \subseteq Y \\ 0, otherwise \end{matrix} .

The LMB RFS is completely described by its components

π = {(r^{(ℓ)}, p^{(ℓ)}) : ℓ \in L}

. The LMB RFS density is given by

π (X) = Δ (X) w (L (X)) {[p]}^{X},

where

p (x, ℓ) = p^{(ℓ)} (x)

and

w (L) = \prod_{i \in L} (1 - r^{(i)}) \prod_{ℓ \in L} \frac{1_{L} (ℓ) r^{(ℓ)}}{(1 - r^{(ℓ)})}

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{matrix} (1) & π (X) & = & Δ (X) w (L (X)) {[p]}^{X} \\ (2) & π_{_{B}} (X) & = & Δ (X) w_{_{B}} (L (X)) {[p_{_{B}}]}^{X} \end{matrix}

where

\begin{matrix} (3) & w (L) & = & \prod_{i \in L} (1 - r^{(i)}) \prod_{ℓ \in L} \frac{1_{L} (ℓ) r^{(ℓ)}}{1 - r^{(ℓ)}}, \\ (4) & w_{_{B}} (L) & = & \prod_{i \in B} (1 - r_{_{B}}^{(i)}) \prod_{ℓ \in L} \frac{1_{B} (ℓ) r_{_{B}}^{(ℓ)}}{1 - r_{_{B}}^{(ℓ)}}, \end{matrix}

\begin{matrix} (5) & p (x, ℓ) & = & p^{(ℓ)} (x) \\ (6) & p_{_{B}} (x, ℓ) & = & p_{_{B}}^{(ℓ)} (x) . \end{matrix}

with state space

X

and label space

L_{+} = B \cup L

and with the condition

B \cap L = \emptyset

. The predicted multi-object distribution is then a LMB RFS and given by

π_{+} (X) = Δ (X) w_{+} (L (X)) {[p_{+}]}^{X}

(7)

where

\begin{matrix} (8) & w_{+} (I_{+}) & = & w_{_{S}} (I_{+} \cap L) w_{_{B}} (I_{+} \cap B) \\ (9) & w_{_{S}} (L) & = & {(1 - r^{(\cdot)} η_{_{S}} (\cdot))}^{L} {(\frac{r^{(\cdot)} η_{_{S}} (\cdot)}{1 - r^{(\cdot)} η_{_{S}} (\cdot)})}^{L}, \\ (10) & η_{_{S}} (ℓ) & = & 〈p_{_{S}} (\cdot, ℓ), p (\cdot, ℓ)〉 \\ (11) & p_{+} (x, ℓ) & = & 1_{L} (ℓ) p_{+, S} (x, ℓ) + 1_{B} (ℓ) p_{_{B}} (x, ℓ) \\ (12) & p_{+, S} (x, ℓ) & = & \frac{〈p_{_{S}} (\cdot, ℓ) f (x | \cdot, ℓ), p (\cdot, ℓ)〉}{η_{_{S}} (ℓ)} \end{matrix}

where

p_{S} (\cdot | ℓ

) is the survival probability of an object and

f (x | \cdot, ℓ

) is the single-object transition model. Thus, if the multi-target posterior density is an LMB RFS with parameter set

π = {(r^{(ℓ)}, p^{(ℓ)}) : ℓ \in L}

with state space

X

and label space

L

and the birth model is also an LMB RFS with parameter set

π_{B} = {(r_{B}^{(ℓ)}, p_{B}^{(ℓ)}) : ℓ \in B}

with state space

X

and label space

B

then the predicted multi-target density is also an LMB RFS with state space

X

and label space

L_{+} = B \cup L (B \cap L = \emptyset)

and it is given by

π_{+} = {(r_{+, S}^{(ℓ)}, p_{+, S}^{(ℓ)}) : ℓ \in L} \cup {(r_{B}^{(ℓ)}, p_{B}^{(ℓ)}) : ℓ \in B}

(13)

where

\begin{matrix} (14) & r_{+, S}^{(ℓ)} & = & η_{_{S}} (ℓ) r^{(ℓ)}, \\ (15) & p_{+, S}^{(ℓ)} & = & \frac{〈p_{_{S}} (\cdot, ℓ) f (x | \cdot, ℓ), p (\cdot, ℓ)〉}{η_{_{S}} (ℓ)}, \end{matrix}

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

2.1.2. Update

If the predicted multi-target density is an LMB RFS with parameter set

π_{+} = {(r_{+}^{(ℓ)}, p_{+}^{(ℓ)}) : ℓ \in L_{+}}

, the multi-target posterior is then given by

π (\cdot | Z) = {(r^{(ℓ)}, p^{(ℓ)} (\cdot)) : ℓ \in L_{+}}

(16)

where

\begin{matrix} (17) & r^{(ℓ)} & = & \sum_{(I_{+}, θ) \in F (L_{+}) \times Θ_{I_{+}}} w^{(I_{+}, θ)} (Z) 1_{I_{+}} (ℓ), \\ (18) & p^{(ℓ)} (x) & = & \frac{1}{r^{(ℓ)}} \sum_{(I_{+}, θ) \in F (L_{+}) \times Θ_{I_{+}}} w^{(I_{+}, θ)} (Z) 1_{I_{+}} (ℓ) p^{(θ)} (x, ℓ), \end{matrix}

where

Θ_{I_{+}}

denotes the space of mapping

θ : I_{+} \to {0, 1, \dots, | Z |}

and,

\begin{matrix} (19) & w^{(I_{+}, θ)} (Z) & \propto & w_{+} (I_{+}) {[η_{Z}^{(θ)}]}^{I_{+}} \\ (20) & p^{(θ)} (x, ℓ | Z) & = & \frac{p_{+} (x, ℓ) ψ_{Z} (x, ℓ; θ)}{η_{Z}^{(θ)} (ℓ)}, \\ (21) & η_{Z}^{(θ)} (ℓ) & = & 〈 p_{+} (\cdot, ℓ), ψ_{Z} (\cdot, ℓ; θ) 〉, \\ ψ_{Z} (x, ℓ; θ) & = & δ_{0} (θ (ℓ)) q_{_{D}} (x, ℓ) \\ (22) & + & (1 - δ_{0} (θ (ℓ))) \frac{p_{D} (x, ℓ) g (z_{θ (ℓ)} | x, ℓ)}{κ (z_{θ (ℓ)})} \end{matrix}

where,

g (z | x)

is the single-sensor measurement likelihood,

p_{_{D}} (\cdot, ℓ)

denotes probability of detection,

q_{_{D}} (\cdot, ℓ) = 1 - p_{_{D}} (\cdot, ℓ)

is the probability of a missed detection, and

κ (\cdot)

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

O X Y Z

is fixed to the ground, and the vehicle coordinate system

o x y z

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.

r_{P}

and

r

are the position vectors of P in the global and vehicle coordinate systems, and

r_{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

r_{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:

a x + b y + c z + 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:

β^{T} r = 0,

(24)

where

β = {[a b c d]}^{T}

and

r = {[x y z 1]}^{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

r_{P} = {[X Y Z 1]}^{T}

, and the coordinates of point P in the vehicle coordinate system

r = {[x y z 1]}^{T}

, are related according to the following equations describing the kinematics of the vehicle [22,23]:

r_{P} = R r

(25)

and

R = [\begin{matrix} cos ϕ & - sin ϕ & 0 & X_{V} \\ sin ϕ & cos ϕ & 0 & Y_{V} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],

(26)

where

ϕ

represents the heading angle of the vehicle (with respect to the X axis), and

X_{V}

and

Y_{V}

denote the coordinates of the vehicle centre of mass (origin o) in the global coordinate system (namely

r_{V} = {[X_{V} Y_{V} 0 1]}^{T}

). The vehicle states

ϕ

X_{V}

and

Y_{V}

are graphically shown in Figure 2.

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

r_{P} = R_{k} r_{k}

(27)

and

R_{k} = [\begin{matrix} cos ϕ_{k} & - sin ϕ_{k} & 0 & X_{V, k} \\ sin ϕ_{k} & cos ϕ_{k} & 0 & Y_{V, k} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],

(28)

where

r_{k} = {[x_{k} y_{k} z_{k} 1]}^{T}

. Similarly, we have the following equations for time

k + 1

r_{P} = R_{k + 1} r_{k + 1}

(29)

and

R_{k + 1} = [\begin{matrix} cos ϕ_{k + 1} & - sin ϕ_{k + 1} & 0 & X_{V, k + 1} \\ sin ϕ_{k + 1} & cos ϕ_{k + 1} & 0 & Y_{V, k + 1} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}],

(30)

where

r_{k + 1} = {[x_{k + 1} y_{k + 1} z_{k + 1} 1]}^{T}

The vector

r_{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):

r_{k + 1} = R_{k + 1}^{- 1} R_{k} r_{k} .

(31)

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

β_{k}^{T} M_{k}^{- 1} M_{k} r_{k} = 0,

(32)

where

M_{k}

represents a 4-by-4 invertible matrix. We might as well let

M_{k} = R_{k + 1}^{- 1} R_{k}

and combine Equations (31) and (32), then we arrive at:

β_{k}^{T} M_{k}^{- 1} r_{k + 1} = 0 .

(33)

The plane Equation (24) for time

k + 1

can be expressed as follows:

β_{k + 1}^{T} r_{k + 1} = 0 .

(34)

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

β_{k + 1}^{T} = β_{k}^{T} M_{k}^{- 1},

(35)

where

M_{k}^{- 1} = {(R_{k + 1}^{- 1} R_{k})}^{- 1} = R_{k}^{- 1} 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

M_{k}^{- 1}

. Note that the computation of matrix

M_{k}^{- 1} = R_{k}^{- 1} R_{k + 1}

requires the inverse of

R_{k}

. It can be easily proven that the matrix

R_{k}

, expressed by Equation (28), has full rank and its inverse

R_{k}^{- 1}

exists at all times. Thus, the matrix

M_{k}^{- 1}

can be computed as long as

R_{k + 1}

is available. However, this matrix

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

R_{k + 1}

requires the unknown information from the future time

k + 1

. In order to obtain matrix

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:

x_{k + 1} = F_{k} x_{k} + G u,

(36)

with

x_{k + 1} = {[X_{V, k + 1} {\dot{X}}_{V, k + 1} Y_{V, k + 1} {\dot{Y}}_{V, k + 1} ϕ_{k + 1} {\dot{ϕ}}_{k + 1}]}^{T},

x_{k} = {[X_{V, k} {\dot{X}}_{V, k} Y_{V, k} {\dot{Y}}_{V, k} ϕ_{k} {\dot{ϕ}}_{k}]}^{T},

u = {[u_{X} u_{Y} u_{ϕ}]}^{T},

F_{k} = [\begin{matrix} 1 & \frac{sin ({\dot{ϕ}}_{k} T)}{{\dot{ϕ}}_{k}} & 0 & \frac{cos ({\dot{ϕ}}_{k} T) - 1}{{\dot{ϕ}}_{k}} & 0 & 0 \\ 0 & cos ({\dot{ϕ}}_{k} T) & 0 & - sin ({\dot{ϕ}}_{k} T) & 0 & 0 \\ 0 & \frac{1 - cos ({\dot{ϕ}}_{k} T)}{{\dot{ϕ}}_{k}} & 1 & \frac{sin ({\dot{ϕ}}_{k} T)}{{\dot{ϕ}}_{k}} & 0 & 0 \\ 0 & sin ({\dot{ϕ}}_{k} T) & 0 & cos ({\dot{ϕ}}_{k} T) & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & T \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}], and G = [\begin{matrix} \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{matrix}],

where

X_{V, k}

Y_{V, k}

X_{V, k + 1}

and

Y_{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

ϕ_{k}

and

ϕ_{k + 1}

represent the vehicle heading angles at time k and

k + 1

u_{X}

u_{Y}

and

u_{ω}

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

R_{k + 1}

and

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

π_{k}

at time k:

π_{k} = {\{(r_{k}^{(ℓ)}, p_{k}^{(ℓ)})\}}_{ℓ \in L},

(37)

where ℓ represents the label of a plane,

r_{k}^{(ℓ)}

denotes the existence probability of the plane ℓ,

p_{k}^{(ℓ)}

is the spatial distribution of its parameters, and

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}^{(ℓ)} = \sum_{i = 1}^{N^{(ℓ)}} ω_{i}^{(ℓ)} δ (a - a_{i, k}^{(ℓ)}) δ (b - b_{i, k}^{(ℓ)}) δ (c - c_{i, k}^{(ℓ)}) δ (d - d_{i, k}^{(ℓ)}),

(38)

where

N^{(ℓ)}

represents the number of particles,

ω_{i}^{(ℓ)}

denotes the weight of the i-th particle, and

δ

is the Dirac delta function.

For each particle

β_{i, k}^{(ℓ)} = {[a_{i, k}^{(ℓ)}, b_{i, k}^{(ℓ)}, c_{i, k}^{(ℓ)}, d_{i, k}^{(ℓ)}]}^{T}

at time k, we are able to calculate the corresponding predicted particle

β_{i, k + 1}^{(ℓ)} = {[a_{i, k + 1}^{(ℓ)}, b_{i, k + 1}^{(ℓ)}, c_{i, k + 1}^{(ℓ)}, d_{i, k + 1}^{(ℓ)}]}^{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:

{\{ω_{i, k + 1}^{(ℓ)}, β_{i, k + 1}^{(ℓ)}\}}_{i = 1}^{N^{(ℓ)}},

where

ω_{i, k + 1}^{(ℓ)} = ω_{i, k}^{(ℓ)}

. Then, based on the new predicted particles at time

k + 1

, the plane parameter estimate

{\hat{β}}_{k + 1}^{(ℓ)}

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_{V, k}$ , $Y_{V, k}$ , $ϕ_{k}$ , and particles ${\{ω_{i, k}^{(ℓ)}, β_{i, k}^{(ℓ)}\}}_{i = 1}^{N^{(ℓ)}}$ representing the distribution of each plane with label ℓ, for all labels $ℓ \in L$
1: compute matrix $R_{k}$	▹ use Equation (28)
2: generate a noise sample $u \sim N (0, Σ)$	▹ $Σ \sim$ diag(noise variances)
3: generate $X_{V, k + 1}$ , $Y_{V, k + 1}$ and $ϕ_{k + 1}$	▹ use Equation (36)
4: compute matrix $R_{k + 1}$	▹ use Equation (30)
5: $M_{k}^{- 1} \leftarrow R_{k}^{- 1} R_{k + 1}$
6: for $ℓ \in L$ do
7: for $i ⩽ N^{(ℓ)}$ do
8: ${β_{i, k + 1}^{(ℓ)}}^{T} \leftarrow {β_{i, k}^{(ℓ)}}^{T} \times M_{k}^{- 1}$
9: $ω_{i, k + 1}^{(ℓ)} \leftarrow ω_{i, k}^{(ℓ)}$
10: end for
11: ${\hat{β}}_{k + 1}^{(ℓ)} \leftarrow \sum_{i = 1}^{N^{(ℓ)}} ω_{i, k + 1}^{(ℓ)} β_{i, k + 1}^{(ℓ)}$	▹ 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 shows the areas of the predicted and segmented planes for time step

k = 125

(averaged from 100 MC runs), and Table 2 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 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, Table 5 and Table 6. 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.

Author Contributions

Conceptualization, R.H. and A.K.G.; Methodology, C.F., M.I.H. and R.T.; Software, C.F. and W.C.; Validation, C.F. and W.C.; Formal Analysis, C.F. and R.H.; Resources, A.K.G. and C.F.; Visualization, C.F. and R.T.; Supervision, R.H. and A.B.H.; Project Administration, C.F. and R.H.; Funding Acquisition, C.F. and R.H.

Funding

This research was funded by the National Natural Science Foundation of China (Grant No. 51805055), the National Key Research and Development Project (Grant No. 2018YFB0106100), and the Australian Research Council (the ARC Linkage Project grant LP160101081).

Conflicts of Interest

The authors declare no conflict of interest. The funding sponsors had no role in the design of the study; in the collection, analyses, or interpretation of data; in the writing of the manuscript, and in the decision to publish the results.

Abbreviations

The following abbreviations are used in this manuscript:

SLAM	Simultaneous Localization and Mapping
EKF	Extended Kalman Filter
RFS	Random Finite Set
LMB	Labeled Multi-Bernoulli
CT	Constant Turn
ECT	Extended Constant Turn
SMC	Sequential Monte Carlo
EAP	Expected A Posteriori
MAP	Maximum A Posteriori
MSSE	Modified Selective Statistical Estimator
MC	Monte Carlo

References

Cadena, C.; Carlone, L.; Carrillo, H.; Latif, Y.; Scaramuzza, D.; Neira, J.; Reid, I.D.; Leonard, J.J. Simultaneous Localization and Mapping: Present, Future, and the Robust-Perception Age. IEEE Trans. Robot. 2016, 32, 1309–1332. [Google Scholar] [CrossRef]
Mullane, J.; Vo, B.; Adams, M.D.; Vo, B. A Random-Finite-Set Approach to Bayesian SLAM. IEEE Trans. Robot. 2011, 27, 268–282. [Google Scholar] [CrossRef]
Davison, A.J. Real-Time Simultaneous Localisation and Mapping with a Single Camera. In Proceedings of the Ninth IEEE International Conference on Computer Vision, Nice, France, 13–16 October 2003; IEEE Computer Society: Washington, DC, USA, 2003; pp. 1403–1410. [Google Scholar]
Thrun, S.; Koller, D.; Ghahramani, Z.; Durrant-Whyte, H.; Ng, A.Y. Simultaneous Mapping and Localization with Sparse Extended Information Filters: Theory and Initial Results. In Algorithmic Foundations of Robotics V; Springer Tracts in Advanced Robotics; Springer: Berlin, Germany, 2002; Volume 7, pp. 363–380. [Google Scholar]
Montemerlo, M.; Thrun, S.; Koller, D.; Wegbreit, B. FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem; AAAI Press/The MIT Press: Cambridge, MA, USA, 2002; pp. 593–598. [Google Scholar]
Rozsa, Z.; Sziranyi, T. Obstacle prediction for automated guided vehicles based on point clouds measured by a tilted LIDAR sensor. IEEE Trans. Intell. Transp. Syst. 2018, 19, 2708–2720. [Google Scholar] [CrossRef]
Fossel, J.; Hennes, D.; Claes, D.; Alers, S.; Tuyls, K. OctoSLAM: A 3D mapping approach to situational awareness of unmanned aerial vehicles. In Proceedings of the 2013 International Conference on Unmanned Aircraft Systems (ICUAS), Atlanta, GA, USA, 28–31 May 2013; pp. 179–188. [Google Scholar]
Im, J.; Im, S.; Jee, G. Extended line map-based precise vehicle localization using 3D LIDAR. Sensors 2018, 18, 3179. [Google Scholar] [CrossRef] [PubMed]
Ahn, S.; Chung, W.K. Efficient SLAM algorithm with hybrid visual map in an indoor environment. In Proceedings of the 2007 International Conference on Control, Automation and Systems, Seoul, Korea, 17–20 October 2007; pp. 663–667. [Google Scholar]
Chen, Z.; Samarabandu, J.; Rodrigo, R. Recent advances in simultaneous localization and map-building using computer vision. Adv. Robot. 2007, 21, 233–265. [Google Scholar] [CrossRef]
Tomono, M. Robust 3D SLAM with a stereo camera based on an edge-point ICP algorithm. In Proceedings of the 2009 IEEE International Conference on Robotics and Automation, Kobe, Japan, 12–17 May 2009; pp. 4306–4311. [Google Scholar]
Sun, F.; Zhou, Y.; Li, C.; Huang, Y. Research on active SLAM with fusion of monocular vision and laser range data. In Proceedings of the 2010 8th World Congress on Intelligent Control and Automation, Jinan, China, 7–9 July 2010; pp. 6550–6554. [Google Scholar]
Henry, P.; Krainin, M.; Herbst, E.; Ren, X.; Fox, D. RGB-D mapping: Using Kinect-style depth cameras for dense 3D modeling of indoor environments. Int. J. Robot. Res. 2012, 31, 647–663. [Google Scholar] [CrossRef]
Endres, F.; Hess, J.; Engelhard, N.; Sturm, J.; Cremers, D.; Burgard, W. An evaluation of the RGB-D SLAM system. In Proceedings of the 2012 IEEE International Conference on Robotics and Automation, Saint Paul, MN, USA, 14–18 May 2012; pp. 1691–1696. [Google Scholar]
Fazli, S.; Kleeman, L. Simultaneous landmark classification, localization and map building for an advanced sonar ring. Robotica 2006, 25, 283–296. [Google Scholar] [CrossRef]
Kim, P.; Coltin, B.; Kim, H.J. Linear RGB-D SLAM for Planar Environments. In Lecture Notes in Computer Science (Including Subseries Lecture Notes in Artificial Intelligence and Lecture Notes in Bioinformatics); Springer: Heidelberg, Germany, 2018; Volume 11208 LNCS, pp. 350–366. [Google Scholar]
Grant, W.S.; Voorhies, R.C.; Itti, L. Efficient Velodyne SLAM with point and plane features. In Autonomous Robots; Springer: New York, NY, USA, 2018. [Google Scholar]
Pietzsch, T. Planar features for visual SLAM. In Lecture Notes in Computer Science (Including Subseries Lecture Notes in Artificial Intelligence and Lecture Notes in Bioinformatics); Springer: Heidelberg, Germany, 2008; Volume 5243 LNAI, pp. 119–126. [Google Scholar]
Geiger, A.; Lenz, P.; Stiller, C.; Urtasun, R. Vision meets robotics: The KITTI dataset. Int. J. Robot. Res. 2013, 32, 1231–1237. [Google Scholar] [CrossRef]
Reuter, S.; Vo, B.T.; Vo, B.N.; Dietmayer, K. The labeled multi-Bernoulli filter. IEEE Trans. Signal Process. 2014, 62, 3246–3260. [Google Scholar]
Hoseinnezhad, R.; Bab-Hadiashar, A. An M-estimator for high breakdown robust estimation in computer vision. Comput. Vis. Image Underst. 2011, 115, 1145–1156. [Google Scholar] [CrossRef]
Jazar, R.N. Vehicle Dynamics: Theory and Application; Springer: Berlin, Germany, 2014; Chapter 5; pp. 233–312. [Google Scholar]
Jazar, R.N. Advanced Dynamics: Rigid Body, Multibody, and Aerospace Applications; Wiley: Hoboken, NJ, USA, 2011; Chapter 4; pp. 357–415. [Google Scholar]
Li, X.R.; Jilkov, V.P. Survey of maneuvering target tracking. Part I. Dynamic models. IEEE Trans. Aerosp. Electron. Syst. 2003, 39, 1333–1364. [Google Scholar]
Gostar, A.K.; Hoseinnezhad, R.; Bab-Hadiashar, A.; Liu, W. Sensor-Management for Multitarget Filters via Minimization of Posterior Dispersion. IEEE Trans. Aerosp. Electron. Syst. 2017, 53, 2877–2884. [Google Scholar] [CrossRef]
Gostar, A.K.; Hoseinnezhad, R.; Bab-Hadiashar, A. Sensor control for multi-object tracking using labeled multi-Bernoulli filter. In Proceedings of the 17th International Conference on Information Fusion (FUSION), Salamanca, Spain, 7–10 July 2014; pp. 1–8. [Google Scholar]
Li, S.; Yi, W.; Hoseinnezhad, R.; Wang, B.; Kong, L. Multiobject Tracking for Generic Observation Model Using Labeled Random Finite Sets. IEEE Trans. Signal Process. 2018, 66, 368–383. [Google Scholar] [CrossRef]
Reuter, S. Multi-Object Tracking Using Random Finite Sets. Ph.D. Thesis, Ulm University, Ulm, Germany, 2014. [Google Scholar]
Deusch, H.; Reuter, S.; Dietmayer, K. The Labeled Multi-Bernoulli SLAM Filter. IEEE Signal Process. Lett. 2015, 22, 1561–1565. [Google Scholar] [CrossRef]
Hossain, M.I.; Gostar, A.K.; Bab-Hadiashar, A.; Hoseinnezhad, R. Visual Mitosis Detection and Cell Tracking Using Labeled Multi-Bernoulli Filter. In Proceedings of the 21th International Conference on Information Fusion (FUSION), Cambridge, UK, 10–13 July 2018; pp. 2468–2472. [Google Scholar]
Wang, X.; Hoseinnezhad, R.; Gostar, A.K.; Rathnayake, T.; Xu, B.; Bab-Hadiashar, A. Multi-sensor control for multi-object Bayes filters. Signal Process. 2018, 142, 260–270. [Google Scholar] [CrossRef]
Gostar, A.K.; Hoseinnezhad, R.; Rathnayake, T.; Wang, X.; Bab-Hadiashar, A. Constrained Sensor Control for Labeled Multi-Bernoulli Filter Using Cauchy-Schwarz Divergence. IEEE Signal Process. Lett. 2017, 24, 1313–1317. [Google Scholar] [CrossRef]
Bab-Hadiashar, A.; Suter, D. Robust segmentation of visual data using ranked unbiased scale estimate. Robotica 1999, 17, 649–660. [Google Scholar] [CrossRef]

Figure 1. An example of the global and vehicle coordinate systems.

Figure 2. Top view of the global and vehicle coordinate systems.

Figure 3. Three pairs of predicted and segmented planes at time

k = 125

Figure 3. Three pairs of predicted and segmented planes at time

k = 125

Figure 4. Multi-angle comparison of the predicted and segmented planes at time

k = 125

Figure 4. Multi-angle comparison of the predicted and segmented planes at time

k = 125

Figure 5. Three pairs of predicted and segmented planes at time

k = 12

Figure 5. Three pairs of predicted and segmented planes at time

k = 12

Figure 6. Multi-angle comparison of the predicted and segmented planes at time

k = 12

Figure 6. Multi-angle comparison of the predicted and segmented planes at time

k = 12

Table 1. Areas of the predicted and segmented planes for time step

k = 125

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 m^{2}$	$219.9937 m^{2}$	$22.9218 m^{2}$
Segmented Plane	$11.0470 m^{2}$	$226.3712 m^{2}$	$23.2926 m^{2}$

Table 2. Distances between each pair of plane vertexes for time step

k = 125

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 m$	$0.7089 m$	$0.0603 m$
Vertex Pair 2	$0.8519 m$	$0.8667 m$	$0.0294 m$
Vertex Pair 3	$0.1293 m$	$0.9060 m$	$0.0837 m$
Vertex Pair 4	$0.8107 m$	$1.0730 m$	$0.0677 m$

Table 3. Angles between each pair of normal vectors for time step

k = 125

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 rad$ / ${3.7458}^{\circ}$	$0.0129 rad$ / ${0.7382}^{\circ}$	$0.0037 rad$ / ${0.2115}^{\circ}$

Table 4. Areas of the predicted and segmented planes for time step

k = 12

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 m^{2}$	$22.7771 m^{2}$	$91.5893 m^{2}$
Segmented Plane	$33.7797 m^{2}$	$23.0555 m^{2}$	$90.5685 m^{2}$

Table 5. Distances between each pair of plane vertexes for time step

k = 12

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 m$	$0.8031 m$	$0.1053 m$
Vertex Pair 2	$0.1117 m$	$0.1600 m$	$0.1119 m$
Vertex Pair 3	$0.0602 m$	$0.2778 m$	$0.5599 m$
Vertex Pair 4	$0.0924 m$	$1.0056 m$	$0.5558 m$

Table 6. Angles between each pair of normal vectors for time step

k = 12

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 rad$ / ${0.1936}^{\circ}$	$0.1133 rad$ / ${6.4912}^{\circ}$	$0.0070 rad$ / ${0.3987}^{\circ}$

© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Share and Cite

MDPI and ACS Style

Gostar, A.K.; Fu, C.; Chuah, W.; Hossain, M.I.; Tennakoon, R.; Bab-Hadiashar, A.; Hoseinnezhad, R. State Transition for Statistical SLAM Using Planar Features in 3D Point Clouds. Sensors 2019, 19, 1614. https://doi.org/10.3390/s19071614

AMA Style

Gostar AK, Fu C, Chuah W, Hossain MI, Tennakoon R, Bab-Hadiashar A, Hoseinnezhad R. State Transition for Statistical SLAM Using Planar Features in 3D Point Clouds. Sensors. 2019; 19(7):1614. https://doi.org/10.3390/s19071614

Chicago/Turabian Style

Gostar, Amirali Khodadadian, Chunyun Fu, Weiqin Chuah, Mohammed Imran Hossain, Ruwan Tennakoon, Alireza Bab-Hadiashar, and Reza Hoseinnezhad. 2019. "State Transition for Statistical SLAM Using Planar Features in 3D Point Clouds" Sensors 19, no. 7: 1614. https://doi.org/10.3390/s19071614

APA Style

Gostar, A. K., Fu, C., Chuah, W., Hossain, M. I., Tennakoon, R., Bab-Hadiashar, A., & Hoseinnezhad, R. (2019). State Transition for Statistical SLAM Using Planar Features in 3D Point Clouds. Sensors, 19(7), 1614. https://doi.org/10.3390/s19071614

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Menu

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

Abstract

1. Introduction

2. Background

2.1. Labeled Multi-Bernoulli Filter

2.1.1. Prediction

2.1.2. Update

2.2. KITTI Dataset

3. Proposed Transition Model

3.1. Plane Transition Model

3.2. Vehicle Transition Model

4. Implementation

5. Simulation Results

5.1. Graphical Results

5.2. Numerical Results

6. Conclusions

Author Contributions

Funding

Conflicts of Interest

Abbreviations

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI