Dynamic vehicle pose estimation and tracking based on motion feedback for LiDARs

Abstract

This paper presents a novel dynamic vehicle tracking framework, achieving accurate pose estimation and tracking in urban environments. For vehicle tracking with laser scanners, pose estimation extracts geometric information of the target from a point cloud clustering unit, which plays an essential role in tracking tasks. However, the point cloud acquired from laser scanners only provides distance measurements to the object surface facing the sensor, leading to nonnegligible pose estimation errors. To address this issue, we take the motion information of targets as feedback to assist vehicle detection and pose estimation. In addition, the heading normalization vehicle model and a robust target size estimation method are introduced to deduce the pose of a vehicle with 2D matched filtering. Furthermore, considering the mobility of vehicles, we utilize the interactive multitude model (IMM) to capture multiple motion patterns. Compared to existing methods in the literature, our method can be applied to spatially sparse or incomplete point cloud observations. Experimental results demonstrate that our vehicle tracking framework achieves promising performance, and its real-time capability is also validated in real traffic scenarios.

Appendices

Appendix 1: Derivation of the double integral u ₍ p _i, p _r)

For a pair of points p_i and p_r

$$ \begin{aligned} u_(p_{i},p_{r})&={\int}_{x_{r}}^{+\infty}{\int}_{y_{r}}^{+\infty}\frac{1}{2\pi\sigma^{2}}e^{-\frac{{(x-x_{i})}^{2}}{2\sigma^{2}}}e^{-\frac{{(y-y_{i})}^{2}}{2\sigma^{2}}}dxdy\\ &={\int}_{x_{r}}^{+\infty}\frac{1}{\sqrt{2\pi}\sigma}e^{-\frac{{(x-x_{i})}^{2}}{2\sigma^{2}}}dx{\int}_{y_{r}}^{+\infty}\frac{1}{\sqrt{2\pi}\sigma}e^{-\frac{{(y-y_{i})}^{2}}{2\sigma^{2}}}dy\\ &={\int}_{\frac{x_{r}-x_{i}}{\sqrt{2}\sigma}}^{+\infty}\frac{1}{\sqrt{2\pi}\sigma}e^{-{z_{x}^{2}}}\sqrt{2}\sigma dz_{x} {\int}_{\frac{{y_{r}-y_{i}}}{\sqrt{2}\sigma}}^{+\infty}\frac{1}{\sqrt{2\pi}\sigma}e^{-{z_{y}^{2}}}\sqrt{2}\sigma dz_{y}\\ &={\int}_{\frac{x_{r}-x_{i}}{\sqrt{2}\sigma}}^{+\infty}\frac{1}{\sqrt{\pi}}e^{-{z_{x}^{2}}} dz_{x} {\int}_{\frac{{y_{r}-y_{i}}}{\sqrt{2}\sigma}}^{+\infty}\frac{1}{\sqrt{\pi}}e^{-{z_{y}^{2}}} dz_{y} \end{aligned} $$

(31)

where

$$ \begin{aligned} &z_{x}=\frac{x-x_{i}}{\sqrt{2}\sigma} \quad \text{and} \quad z_{y}=\frac{y-x_{i}}{\sqrt{2}\sigma} \end{aligned} $$

(32)

the derivative of (32) is as follows:

$$ \begin{aligned} dz_{x}=\frac{1}{\sqrt{2}\sigma}dx \quad \text{and} \quad dz_{y}=\frac{1}{\sqrt{2}\sigma}dy \end{aligned} $$

(33)

Given the Gaussian error function, we have

$$ \begin{aligned} &{\int}_{\frac{x_{r}-x_{i}}{\sqrt{2}\sigma}}^{+\infty}\frac{1}{\sqrt{\pi}}e^{-{z_{x}^{2}}} dz_{x}\\ =&{\int}_{0}^{+\infty}\frac{1}{\sqrt{\pi}}e^{-{z_{x}^{2}}} dz_{x}-{\int}_{0}^{\frac{x_{r}-x_{i}}{\sqrt{2}\sigma}}\frac{1}{\sqrt{\pi}}e^{-{z_{x}^{2}}} dz_{x}\\ =&\frac{1}{\sqrt{\pi}}(\frac{\sqrt{\pi}}{2}\text{erf}(+\infty)-\frac{\sqrt{\pi}}{2}\text{erf}(\frac{{x_{r}-x_{i}}}{\sqrt{2}\sigma}))\\ =&\frac{1}{2}(1-\text{erf}(\frac{{x_{r}-x_{i}}}{\sqrt{2}\sigma})) \end{aligned} $$

(34)

Similarly,

$$ \begin{aligned} {\int}_{\frac{y_{r}-y_{i}}{\sqrt{2}\sigma}}^{+\infty}\frac{1}{\sqrt{\pi}}e^{-{z_{y}^{2}}} dz_{y} =\frac{1}{2}(1-\text{erf}(\frac{{y_{r}-y_{i}}}{\sqrt{2}\sigma})) \end{aligned} $$

(35)

As a result, u₍p_i, p_r) can be calculated by:

$$ \begin{aligned} u_(p_{i},p_{r})=\frac{1}{4}\{1-\text{erf}(\frac{x_{r}-x_{i}}{\sqrt{2}\sigma})\}\{1-\text{erf}(\frac{y_{r}-y_{i}}{\sqrt{2}\sigma})\} \end{aligned} $$

(36)

Appendix 2: The gradient and Hessian of the cost function

$$ \begin{aligned} F_{\text{cost}}(x,y) &=\sum\limits_{i=0}^{I-1}M(x,y,p_{i})\\ &=\sum\limits_{i=0}^{I-1}\sum\limits_{j=0}^{J-1}c_{j}I(p_{i},R_{c_{p},j}) \end{aligned} $$

(37)

where I is the total number of points in a clustering unit, c_p = (x, y) denotes the center point, and J represents the total number of rectangular areas. Therefore, J = 5 in this paper.

(1)The gradient of F_cost(x, y) is:

$$ \begin{aligned} &\nabla F_{\text{cost}}(x,y)=\left[\frac{\partial F_{\text{cost}}}{\partial x},\frac{\partial F_{\text{cost}}}{\partial y}\right]^{T} \end{aligned} $$

(38)

where

$$ \begin{aligned} &\frac{\partial F_{\text{cost}}}{\partial x}=\sum\limits_{i=0}^{I-1}\sum\limits_{j=0}^{J-1}c_{j} \frac{\partial I(p_{i},R_{c_{p},j})}{\partial x} \\ &\frac{\partial F_{\text{cost}}}{\partial y}=\sum\limits_{i=0}^{I-1}\sum\limits_{j=0}^{J-1}c_{j}\frac{\partial I(p_{i},R_{c_{p},j})}{\partial y} \end{aligned} $$

(39)

(2)The Hessian of F_cost(x, y) is :

$$ \nabla^{2} F_{\text{cost}}(x,y)=\left[ \begin{array}{ll} \frac{\partial^{2} F_{\text{cost}}}{\partial^{2} x} & \frac{\partial^{2} F_{\text{cost}}}{\partial x \partial y} \\ \frac{\partial^{2} F_{\text{cost}}}{\partial y\partial x} & \frac{\partial^{2} F_{\text{cost}}}{\partial^{2} y} \end{array} \right] $$

(40)

where

$$ \begin{aligned} &\frac{\partial^{2} F_{\text{cost}}}{\partial^{2} x}=\sum\limits_{i=0}^{I-1}\sum\limits_{j=0}^{J-1}c_{j} \frac{\partial^{2} I(p_{i},R_{c_{p},j})}{\partial^{2} x}\\ &\frac{\partial^{2} F_{\text{cost}}}{\partial x \partial y}=\sum\limits_{i=0}^{I-1}\sum\limits_{j=0}^{J-1}c_{j}\frac{\partial^{2} I(p_{i},R_{c_{p},j})}{\partial x \partial y}\\ &\frac{\partial^{2} F_{\text{cost}}}{\partial y\partial x} =\sum\limits_{i=0}^{I-1}\sum\limits_{j=0}^{J-1}c_{j} \frac{\partial^{2} I(p_{i},R_{c_{p},j})}{\partial y\partial x}\\ &\frac{\partial^{2} F_{\text{cost}}}{\partial^{2} y}=\sum\limits_{i=0}^{I-1}\sum\limits_{j=0}^{J-1}c_{j}\frac{\partial^{2} I(p_{i},R_{c_{p},j})}{\partial^{2} y} \end{aligned} $$

(41)

From the definition of $I(p_{i}, R_{c_{p},j})$ in (16), $I(p_{i}, R_{c_{p},j})$ is a linear combination of the double integral u(p_i, p_r). Therefore, it is necessary to compute the first and second derivatives of u(p_i, p_r). For notation simplicity, let

$$ \begin{aligned} &e_{x}=\text{erf}(\frac{x+x_{\text{offset}}-x_{i}}{\sqrt{2}\sigma}) \\ &e_{y}=\text{erf}(\frac{y+y_{\text{offset}}-y_{i}}{\sqrt{2}\sigma}) \end{aligned} $$

(42)

where x_offset and y_offset represent the offset from the center to a rectangular vertex in the X and Y directions, respectively, as shown in Table 3. Hence, (36) can be rewritten as:

$$ \begin{aligned} u_(p_{i},p_{r})&=\frac{1}{4}(1-e_{x})(1-e_{y}) \end{aligned} $$

(43)

Furthermore, the Gaussian error function can be differentiated to the first- and second-order as follows:

$$ \begin{aligned} &\frac{\partial}{\partial x}\text{erf}(x)=\frac{2}{\sqrt{\pi}}e^{-x^{2}}\\ &\frac{\partial^{2}}{\partial x^{2}}\text{erf}(x)=-\frac{4}{\sqrt{\pi}}xe^{-x^{2}} \end{aligned} $$

(44)

By combining (42), (43) and (44), we have:

$$ \begin{array}{@{}rcl@{}} \begin{aligned} &\frac{\partial u}{\partial x}=\frac{1}{4}\frac{\partial e_x}{\partial x}(e_y-1)+\frac{1}{4}\frac{\partial e_y}{\partial x}(e_x-1)\\ &\frac{\partial u}{\partial y}=\frac{1}{4}\frac{\partial e_x}{\partial y}(e_y-1)+\frac{1}{4}\frac{\partial e_y}{\partial y}(e_x-1)\\ &\frac{\partial^2 u}{\partial^2 x}=\frac{1}{4}\left[\frac{\partial^2 e_x}{\partial^2 x}(e_y-1)+\frac{\partial^2 e_y}{\partial^2 x}(e_x-1)+2\frac{\partial e_y}{\partial x}\frac{\partial e_x}{\partial x}\right]\\ &\frac{\partial^2 u}{\partial x \partial y}=\frac{1}{4}\left[\frac{\partial^2 e_x}{\partial x \partial y}(e_y-1)+\frac{\partial e_y}{\partial y}\frac{\partial e_x}{\partial x}\right.\\&\qquad\qquad\quad\left.+\frac{\partial^2 e_y}{\partial x \partial y}(e_x-1)+\frac{\partial e_y}{\partial x}\frac{\partial e_x}{\partial y}\right]\\ &\frac{\partial^2 u}{\partial^2 y}=\frac{1}{4}\left[\frac{\partial^2 e_x}{\partial^2 y}(e_y-1)+\frac{\partial^2 e_y}{\partial^2 y}(e_x-1)+2\frac{\partial e_y}{\partial y}\frac{\partial e_x}{\partial y}\right]\\ &\frac{\partial^2 u}{\partial y \partial x}=\frac{1}{4}\left[\frac{\partial^2 e_x}{\partial y \partial x}(e_y-1)+\frac{\partial e_x}{\partial y}\frac{\partial e_y}{\partial x}\right.\\&\qquad\qquad\quad\left.+\frac{\partial^2 e_y}{\partial y \partial x}(e_x-1)+\frac{\partial e_x}{\partial x}\frac{\partial e_y}{\partial y}\right] \end{aligned} \end{array} $$

Then, the gradient and Hessian of the cost function can be calculated using (45).

Appendix 3: Motion model equation

The state vector X of a moving target contains its pose information and motion information as follows:

$$ X=[x,y,\theta,v,\omega]^{T} \in \mathbb{R}^{5} \\ $$

(45)

where (x, y) is the center position in a global coordinate system, 𝜃 denotes the target heading, and v and ω represent the velocity and yaw rate of the target, respectively. Assuming the time interval is δ_t, the system functions of the three motion models are given as follows:

a) CV Model

$$ X^{\text{CV}}_{k}=\left[ \begin{array}{cc} & x_{k-1}+ v_{k-1}*cos(\theta_{k-1})*\delta_{t}\\ & y_{k-1}+ v_{k-1}*sin(\theta_{k-1})*\delta_{t} \\ & \theta_{k-1} \\ & v_{k-1} \\ & 0 \end{array} \right] $$

(46)

b) CTRV Model

$$ X^{\text{CTRV}}_{k}=\left[ \begin{array}{cc} & x_{k-1}+ \frac{v_{k-1}}{\omega_{k-1}}(sin(\theta_{k-1}+\omega_{k-1}*\delta_{t})-sin(\theta_{k-1}))\\ & y_{k-1}+ \frac{v_{k-1}}{\omega_{k-1}}(cos(\theta_{k-1})-cos(\theta_{k-1}+\omega_{k-1}*\delta_{t})) \\ & \theta_{k-1}+ \omega_{k-1}*\delta_{t} \\ & v_{k-1} \\ & \omega_{k-1} \end{array} \right] $$

(47)

c) RM Model

$$ X^{\text{RM}}_{k}=\left[ \begin{array}{cc} & x_{k-1} \\ & y_{k-1} \\ & \theta_{k-1} \\ & v_{k-1} \\ & \omega_{k-1} \end{array} \right] $$

(48)

Appendix: 4: Parameter table

Parameter	Description	Value
Xsize/Ysize	the size of the elevation map	400/600
d grid	the resolution of the elevation map	20cm
xcar/ ycar	the location of ego-car in the elevation map	200/200
D th	the segmentation distance threshold	0.7m
d th	the merging distance threshold	0.3m
N th	the frame count threshold for dynamic target	10
d cell	the length of cells	0.3m
X th	the X-axis span threshold	2.5m
v	the width of the rectangle edges	0.5m
σx / σy	the variances of measurement noise	0.212/0.212
N itmax	the maximum number of iteration	10
δ th	permission error	0.05m
Lini/Wini	the initial value of length and width	4m/2m
$S_{\max \limits }$	maximum size	15m
n th	the element count threshold for size estimation	15