Likelihood-field-model-based Dynamic Vehicle

Likelihood-field-model-based Dynamic Vehicle Detection with Velodyne Tongtong Chen, Bin Dai, Daxue Liu, Hao Fu College of Mechatronic Engineering and Automation National University of Defense Technology Changsha, China Email: [email protected]

Abstract—Dynamic vehicle detection is an important module for Autonomous Land Vehicle (ALV) navigation in outdoor environments. In this paper, we present a novel dynamic vehicle detection algorithm based on the likelihood field model for an ALV equipped with a Velodyne LIDAR. An improved 2D virtual scan is utilized to detect the dynamic objects with the scan differencing operation. For every dynamic object, a vehicle is fitted with the likelihood field model, and the motion evidence and motion consistence of the fitted vehicle are exploited to classify the dynamic object into the vehicle or not. The performance of the algorithm is validated on the data collected by our ALV in various environments. Keywords-dynamic vehicle detection; likelihood-field-based measurement model; ALV; Velodyne

I. I NTRODUCTION Self-driving in urban environments is a challenging task for an ALV. However, it also promises to bring plenty of benefits both to national defence and civilian traffic fields. In order to drive safely with respect to the urban environments, the ALV needs to thoroughly understand it, not only about the existence of static vehicles, but also the poses and velocities of the dynamic vehicles. Dynamic vehicle detection has become a hot research topic recently. A typical urban scene with six dynamic vehicles at a T-junction is shown in Fig. 1.

Jinze Song Graduate School National University of Defense Technology Changsha, China Email: [email protected]

For every Velodyne LIDAR scan, we assume that ground segmentation [1],[2] and object clustering[3] steps have been performed. In this paper, all the objects are projected into an improved 2D virtual scan, where dynamic objects can be obtained with the scan differencing operation between two consecutive scans with the motion compensation provided by the GPS-aid inertial navigation system. For each dynamic object, a vehicle is fitted with the likelihood-field-based vehicle measurement model, and the motion evidence proposed in [4] and motion consistence are utilized to classify the dynamic object into vehicle or not. Our algorithm is similar to reference [4], and our contributions are two folds. The most significant one is that a novel likelihood-field-based vehicle measurement model is proposed. The second one is that we improve the 2D virtual scan proposed in [4]. With the two contributions, our approach can detect the dynamic vehicles that are fully occluded in xy plane by other objects, compared with Petrovskaya’s method[4]. The rest of the paper is structured as follows. In the nest section, an outline of the related work is given. Section 3 describes the dynamic vehicle detection algorithm in detail. Some experimental results on the data collected in urban environments are shown in Section 4. Finally, Section 5 gives the conclusions. II. R ELATED W ORK

Figure 1. A typical urban scene at a T-junction. The yellow points are ground, and other colors represent different objects respectively. Two green cuboids represent six dynamic vehicles and the arrows are their orientations.

Much research about vehicle detection with the LIDAR sensors has been carried out over the last few years. In general, these methods can be roughly divided into two groups: feature-based methods[5], [6], [7], [8], [9], [10], [11] and vehicle measurement model-based methods [12], [4], [13], [14], [15]. Dube et al. [5] accumulated the radar data in an occupancy grid, where objects are detected. The candidate object is recognized as a parked vehicle according to 4 features with two random forest classifier. Cheng et al. [6] proposed three novel features for the finely segmented 3D object points to detect the vehicles with SVM. Zhang et al. [7] clustered the obstacle points based on a distance criterion and a cube bounding box is build for every object. The length, width and height are exploited to classify the object into a vehicle or not. Mori et al.[8] voted the sequential laser scan points

on a grid map to obtain the grid trajectories, based on which, a car can be recognized from the moving objects. Keat et al. [9] extracted line segments from raw laser data with the clustering and segmentation techniques, and then exploited bayesian programming to detect the vehicle from these line segments. Darms et al. [10] extracted edge targets which are interpreted as a part of the box model to detect the vehicle from the raw data of laser. Morris et al. [11] exploited the RANSAC algorithm to fit an edge or “L-shape”to detect the vehicle, and visibility constraints are used to make “L-shape ”fitting more efficient. Measurement model-based methods mainly utilize some cuboids or rectangles to represent the vehicles, and some constraints are exploited to eliminate false positives. Morris et al.[12] took view-dependent self-occlusion effects into account, and used 4 rectangles to represent the inside, outside, two visible sides of a vehicle, respectively. A gradient-based optimization technique is used to maximize the integrals of the segment points over the four rectangles to detect the vehicles. Petrovskaya et al.[4] detect the dynamic vehicle with importance sampling technique, and the scoring of each particle is performed with the pre-defined rectangular vehicle measurement model, and motion evidence is proposed to eliminate the false positives. Wojke et al. [13] also utilized the vehicle measurement model defined in reference [4]. However, temporal cues and geometric cues are brought in to reduce the sampling range to improve the efficiency and precision. Based on Petrovskaya’s vehicle measurement model, Liu et al. [15] proposed a modified measurement model for titled two dimensional laser range finder to detect the dynamic vehicles, and both temporal and motion cues are adopted to separate the foreground from background to get rid of the false positives. As Petrovskaya’s measurement model and its variants[4],[13],[14],[15] can only detect the nearest vehicles, it cannot be utilized to detect the vehicles that are fully occluded in xy plane by other vehicles in the crowd urban environments. This issue is addressed in this paper.

Figure 2. one (b).

Comparisons of the original virtual scan (a) and the improved

with the resolution of 0.2 meter, as shown in Fig. 2. All these objects are projected into the virtual scan. Instead of recording the range to the nearest object in each segment as [4] in Fig. 2a, we take the objects as the processing unit. For each object oi , we calculate the nearest and furthest bins bmin , bmax that oi covers. For each segment j that is covered i i by oi , we search the nearest bin bji , that is occupied by the , then the attribute of bin points of oi , starting from bin bmin i bji is the occupied bin, the attributes of the bins between bmin and bji are free ones, while the attributes between i j bi and bmax are occluded, as shown in Fig. 2b. In the i improved 2D virtual scan (Fig. 2b), the dynamic objects that are occluded by other objects can also be detected because of the additional information about free, occupied and occluded bins, as the object oj in Fig. 2b. In order to detect the dynamic objects, we should know what changes take place between two consecutive virtual scans. These changes can be easily detected by scan differencing operation proposed in [4] with the motion compensation provided by the GPS-aid inertial navigation system. Fig. 3 shows the results of scan differencing operation in an improved virtual scan.

III. DYNAMIC V EHICLE D ETECTION A LGORITHM In this section, we will employ the likelihood-fieldbased vehicle measurement model to detect the dynamic vehicles in the improved 2D virtual scan. Compared with Petrovskaya’s method [4], our algorithm can also detect the dynamic vehicles which are fully occluded in the xy plane. A. Improved 2D Virtual Scan In this section, we assume that ground segmentation and object clustering steps have be performed, and the input of the dynamic vehicle algorithm is a series of obtained objects. In order to improve the computational efficiency, A polar grid map termed as a virtual scan, which will be partitioned into 720 segments, is constructed. Every segment will be divided into 400 bins to discrete the range component

Figure 3. Results of scan differencing operation between two consecutive scans. The blue lines represent segments. The red points are new obstacles, green points are obstacle that disappeared. The yellow points represent the regions that are occluded in the last scan, but free in the current scan, while the cyan points are the regions that are free in the last scan, but occluded in the current scan.

B. Likelihood-field-based Vehicle Measurement Model

R0

yV

R1

yL

For every dynamic object, a vehicle is fitted according to its 3D points. In this paper, We utilize a rectangular shape to represent a vehicle and construct the following vehicle measurement model based on likelihood filed. Given a vehicle’s pose X = {x, y, θ} with width W , length L and a cluster of measurements Z = {q1 , q2 , · · · , qn }, where qi = (xi , yi , zi ) is represented by its Euclidean coordinates with respect to the LIDAR coordinate system. In this paper, we follow reference [12] and model each measurement qi = (xi , yi , zi ) as a two dimensional Gaussian distribute p(x, y) in xy plane, regardless of its zi coordinate.

{ (x − xi )2 } { (y − yi )2 } 1 p(x, y) = exp − exp − , (1) 2πσ 2 2σ 2 2σ 2

where σ 2 is the variance which represents the measurement noise. Our measurement model is illustrated in Fig. 4.L The gray and red rectangles represent the inside and outside of the vehicle respectively, while the green and blue ones are the visible length and width surfaces of the vehicle. For every edge of the rectangular shape, we define the direction pointing the original oL from its center as the corresponding orientation vector, and the orientation that is perpendicular to the edge and pointing outside the vehicle as its norm vector. If the angle θi between the orientation vector and norm vector is less than π/2, then the edge is visible. To begin with, a rectangular shape that represents the vehicle’s pose according to X is positioned in LIDAR coordinate system oL xL yL . Assume there is a real L vehicle in this configuration, we would expect that most of the measurements will fall in the visible surface rectangles (green and blue rectangles), and some ones will be inside of vehicle (gray rectangle), while few will fall outside the vehicle in the red rectangle. Because we believe that there are no objects in the vicinity of a moving vehicle. In this paper, we assume that each measurement is independent of other ones. Thus, the measurement model of the vehicle that contains n measurements in Z can be expressed as follows:

p(Z|X) =

n ∏

p(qi |X).

(2)

i=1

Each measurement qi ’s likelihood is model as the exp of the sum of the integrals of the measurement qiV over the four

T x, y ,

0

AD

R2 R3

!

yV

xV

qi

AD

x, y ! V i

q

3

1

W

oV

! 1m

xV

L

B C

BC

2

xL

oL

b!

a!

Figure 4. Measurement model computation. (a) shows the four geometric regions of the model. The green and blue rectangles are two visible surfaces of the model; the gray rectangle represents the inside region of the vehicle, while the red region is the outside. (b) shows the vehicle coordinate system oV xV yV with the origin at the geometric center of the rectangle. In this paper, the width and length of the model are fixed to constants, and the width of the visible surface of the model is set to ∆.

rectangles in theL vehicle coordinate system oV xV yV 1 L 4 ( ∑ ) p(qi |X) = ηi exp α cj I(qiV , Rj ) j=1

qiV

[ V ] z[ cos(θ) x = Vi = −sin(θ) yi

(3)

Tx,y,θ

}| ] [ ]{ sin(θ) xi − x cos(θ) yi − y

L

where qi is a measurement in the LIDAR coordinate system oL xL yL , and qiV is the corresponding point in the vehicle coordinate system oV xV yV obtained with the transformation Tx,y,θ as shown in Equation 3. ηi is a normalization xL x L constant. R0 ,L R1 , R2 , R3 are theL visible length rectangle, width rectangle, the inside and outside areas of the vehicle, respectively (the green, blue, gray and red areas in Fig 4); and c0 , c1 , c2 , c3 are the corresponding integral weights over these rectangles. As we expect most of the measurements would fall in the visible rectangle or inside of the vehicle, c0 , c1 , c2 are chosen to be greater than zero, while c3 is less √∑ ∫∫ V 3 than zero. α = 1/ c2 dxdy . I(qi , Rj ) represents k=1 Rk k V the integral of measurement qi over the rectangle Rj in the vehicle coordinate system oV xV yV . Let IA , IB , IC , ID represent the integrals of measurement qiV over quarter plane extending to positive infinity from the positions A = (xVa , yaV ), B = (xVb , ybV ), C = (xVc , ycV ), D = (xVd , ydV ), respectively, in the vehicle coordinate system. Here, we take the integral I(qiV , R0 ) of qiV 1 For every target vehicle, we construct a vehicle coordinate system oV xV yV at the geometric center of the rectangle, with x, y axes pointing forward and right respectively.

over the visible length rectangle R0 for instance, I(qiV , R0 ) = ID + IB − IA − IC ( xV − xV 1( IA = 1 + erf i √ a 4 2σ ( xVi − xVb 1( √ IB = 1 + erf 4 2σ ( xVi − xVc 1( √ IC = 1 + erf 4 2σ ( xVi − xVd 1( √ ID = 1 + erf 4 2σ

))(

( yiV − yaV √ 2σ ))( ( yiV − ybV √ 1 + erf 2σ ))( ( yiV − ycV √ 1 + erf 2σ ))( ( yiV − ydV √ 1 + erf 2σ 1 + erf

)) )) . )) )) (4)

The integrals I(qiV , R1 ), I(qiV , R2 ), I(qiV , R3 ) can be calculated in the same way as above. Not that the measurement model in this paper only has relationship with the original measurements. It can handle the pose estimation of the vehicle that have measurements but fully occluded by other objects in the xy plane, while Petrovskaya’s measurement model [4] cannot. C. Dynamic Vehicle Detection Algorithm In this paper, we use three consecutive scans to detect the dynamic vehicles. The first two scans are used to estimate the pose and speed of the candidate dynamic vehicle, and the third scan is exploited to validated the motion consistence of these candidate dynamic vehicles. The detailed algorithm contains three steps: First, a vehicle is fitted with Scaling Series importance sampling algorithm [16] for each dynamic object in the second scan, which is detected in the improved 2D virtual scan with scan differencing operation between the second and first scans. The weights of the particles are calculated with the likelihood-field-based vehicle measurement model in Subsection III-B. Second, the velocity of the vehicle is estimated with a particle filter between the second and first scans. In this stage, we assume that the velocities sample from uniform distribution U (−35m/s, 35m/s) from the second scan to the first one, and the particles are also weighted with the likelihood-field-based vehicle measurement model in Subsection III-B. We believe that the best particle with the maximum weight is the corresponding vehicle in the first scan, then the velocity of the vehicle is opposite to the one of the best particle. Once we have found the corresponding vehicle in the first scan, the motion evidence proposed in [4] is also employed to delete the false positive. Third, for the vehicle that meets the motion evidence, another particle filter update is performed against to the third scan. In this paper, we believe that the velocities of one vehicle between two consecutive scans would be similar ( termed as motion consistence). Only the vehicle that meets motion consistence will be output as a dynamic vehicle in this paper.

Figure 5. Our ALV used for the experiments in various scenes. It is equipped with a Velodyne HDL-64E S2 LIDAR, a NovAtel SPAN-CPT GPS-aid inertial navigation system and some cameras.

IV. E XPERIMENTAL E VALUATION AND A NALYSIS We have evaluated the performance of the dynamic vehicle detection algorithm proposed in this paper on the data collected by our own ALV in various urban environments. We implement the algorithm in C++ as well as the point cloud library (PCL)[17]. Our ALV is a modified Toyota Land Cruiser, as shown in Fig. 5, equipped with one Velodyne HDL-64E S2 LIDAR, one NovAtel SPAN-CPT GPS-aid inertial navigation system and some other sensors, such as cameras, laser ranges finders, etc. Fig. 6 illustrates some results of dynamic vehicle detection in urban, highway and ramp scenes from top to down, respectively. The top two images show the crowded urban environments where there are so many dynamic vehicles that they can easily be occluded by other objects. However, our algorithm can also detect all the dynamic vehicles except the buses in the red rectangles (in left top image) because of the fixed width and length of the vehicle measurement model in Subsection III-B. The middle two images are highway scenes. one can find that our algorithm can detect all the dynamic vehicles in the opposite lanes (in the green rectangles) that are occluded by the median strips, while Petrovskaya’s method [4] cannot, because it only records the range to the nearest obstacle in each segment of the 2D virtual scan, and their vehicle measurement model cannot evaluate the particles that are occluded by other objects with the range information. Because of the improved 2D virtual and our likelihood-field-based vehicle measurement model, our algorithm can address the issue successfully. The last two images in the bottom of Fig. 6 illustrate the ramp environments where there is a vehicle turning right (in the blue rectangles). Our algorithm can successfully detect these vehicles, which is very important for our ALV to merge into or out the traffic flow in the ramp environments. V. C ONCLUSION In this paper, We propose a novel dynamic vehicle detection based on likelihood-field-based vehicle measurement model in the improved 2D virtual scan. The dynamic objects,

Figure 6. Results of dynamic vehicle detection in urban, highway and ramp scenes from up to down. The yellow points are ground and the other colors represent different objects respectively. The green cuboids represent the poses of the dynamic vehicles, and the arrows on these cuboids are the their driving directions

which will be fitted as a vehicle with likelihood-field-based vehicle measurement model, can be obtained with scan differencing operation. The moving evidence and consistence are exploited to reduce the false positives. Because of the likelihood-field-based vehicle measurement model and the improved 2D virtual scan, our algorithm can detect the dynamic vehicles that are fully occluded by other objects in xy plane. The experimental results on the Velodyne validate that our algorithm could obtain promising performance. ACKNOWLEDGMENT This work is supported by National Nature Science Foundations under Grant 61375050, 61075043. R EFERENCES [1] T. Chen, B. Dai, R. Wang, and D. Liu, “Gaussian-processbased real-time ground segmentation for autonomous land vehicle,” Journal of Intelligent and Robotic systems, vol. 76, no. 3-4, pp. 563–582, 2014. [2] B. Douillard, J. Underwood, N. Kuntz, V. Vlaskine, A. Quadros, P. Morton, and A. Frenkel, “On the segmentation of 3d lidar point clouds,” in International Conference on Robotics and Automation, 2011, pp. 2798–2805. [3] K. Klasing, D. Wollherr, and M. Buss, “A clustering method for efficient segmentation of 3d laser data,” in International Conference on Robotics and Automation, 2008, pp. 4043– 4048. [4] A. Petrovskaya and S. Thrun, “Model based vehicle tracking for autonomous driving in urban environments,” in Robotics: Science and Systems(RSS), 2008, pp. 25–28. [5] R. Dube, M. Hahn, M. Schutz, J. Dickmann, and D. Gingras, “Detection of parked vehicles from a radar based occupancy grid,” in IEEE Intelligent Vehicles Symposium(IV), 2014, pp. 1415–1420. [6] J. Cheng, Z. Xiang, T. Cao, and J. Liu, “Robust vehicle detection using 3d lidar under complex urban environments,” in International Conference on Robotics and Automation(ICRA), 2014, pp. 691–696. [7] L. Zhang, Q. Li, M. Li, Q. Mao, and A. Nuchter, “Multiple vehicle-like target tracking based on thevelodyne lidar,” in Intelligent Autonomous Vehicles Symposium, 2013, pp. 126– 131. [8] T. Mori, T. Sato, H. Noguchi, M. Shimosaka, R. Fukui, and T. Sato, “Moving objects detection and classification based on trajectories of lrf scan data on a grid map,” in International Conference on Intelligent Robots and Systems, 2012, pp. 2602–2611. [9] C. T. M. Keat, C. Pradalier, and C. Laugier, “Vehicle detection and car park mapping using laser scanner,” in International Conference on Intelligent Robots and Systems(IROS), 2005, pp. 2054 – 2060.

[10] M. Darms, P. Rybski, and C. Urmson, “Classification and tracking of dynamic objects with multiple sensors for autonomous driving in urban environments,” in IEEE Intelligent Vehicles Symposium(IV), 2008, pp. 1197–1202. [11] D. D. Morris, P. Haley, W. Zachar, and S. McLean, “Ladarbased vehicle tracking and trajectory estimation for urban driving,” in Proceedings of Association for Unmanned Vehicle Systems International (AUVSI), 2008. [12] D. D. Morris, R. Hoffman, and P.Haley, “A view-dependent adaptive matched filter for ladar-based vehicle tracking,” in 14th Interational Conference on Robotics and Applications, 2009, pp. 1–9. [13] N. Wojke and M. Haselich, “Moving vehicle detection and tracking in unstructured environments,” in International Conference on Robotics and Automation(ICRA), 2012, pp. 3082– 3087. [14] T. D. Vu and O. Aycard, “Laser-based detection and tracking moving objects using data-driving markov chain monte carlo,” in International Conference on Robotics and Automation(ICRA), 2009, pp. 3800–3806. [15] Z. Liu, D. Liu, and T. Chen, “Vehicle detection and tracking with 2d laser range finders,” in 6th International Congress on Image and Signal Processing (CISP), 2013, pp. 1006 – 1013. [16] T. Chen, B. Dai, D. Liu, H. Fu, and J. Song, “Likelihoodfield-model-based vehicle pose estimation with velodyne,” in submitted to IEEE International Conference on Intelligent Transportation Systems (ITSC 2015), 2015. [17] R. Rusu and S. Cousins, “3d is here: point cloud library (pcl),” in International Conference on Robotics and Automation (ICRA), 2011, pp. 1–4.