- Email: [email protected]
Inter-Rays uncertainty and Fixed Size assumption for objects tracking using a Laser Scanner
Inter-Rays uncertainty and Fixed Size assumption for objects tracking using a Laser Scanner Pawel Kmiotek∗,∗∗ Yassine Ruichek∗ ∗
Systems and Transportation Laboratory, University of Technology of Belfort-Montbeliard, 13 Rue Thierry Mieg 90010 Belfort, France (e-mail: [email protected], [email protected]). ∗∗ Department of Computer Science, AGH - University of Science and Technology, al. Mickiewicza 30, 30-059 Krakow, Poland (e-mail: [email protected]). Abstract: This paper presents a geometric representation based method for objects tracking using laser sensory data. The representation model, based on an Oriented Bounding Box (OBB), is achieved by using a two-stage procedure. The first one concurs a LRF data based convex hull technique of object’s contour extraction. The second stage consists of contour analysis by a geometric method, called Rotating Calipers. In order to obtain good estimation for the geometric parameters of the object, two concepts are proposed. The first one, called Inter-Rays uncertainty, is introduced to consider the fact that the raw data points representing the extremities of the extracted OBB do not coincide with the real object’s extremities. The second concept, called Fixed Size assumption, is integrated to take into account that the size of the object dos not change during the tracking process. The tracking is ensured by the Extended Kalman Filter with Discrete White Noise Acceleration model. Experimental results are presented to show the robustness of the proposed method. Keywords: Tracking, Object representation, Laser range sensor, Intelligent vehicle, Signal processing 1. INTRODUCTION Representation of dynamic objects is crucial for tracking and trajectory planning. In the literature concerning tracking, points with elliptical uncertainty are used for representing objects position (see Bar-Shalom and Fortman (1988), Blanc et al. (2004)). This representation is good enough for obstacle detection, collision warning or driving assistance systems in well structured environments like highway (see Blanc et al. (2004), Hofmann et al. (2003)). In the urban areas, there are less constraints on the objects movements. Thus, for the task of autonomous navigation in demanding urban areas, these representation methods are not sufficient. Oriented Bounding Box (OBB) (see Toussaint (1983), Ericson (2004), Nguyen et al. (2005)) provides a good approximation of the size, shape and orientation angle of dynamic objects, with a good data compression ratio. In this paper, an OBB model, with an Inter-Rays (IR) uncertainty paradigm and a Fixed Size (FS) assumption, is proposed to represent dynamic objects. The IR uncertainty and FS assumption are introduced to increase the reliability of tracking in terms of object’s size and centre position. The IR uncertainty is developed to handle the fact that the raw data points representing the extremities of the extracted OBB do not coincide with the real object’s extremities. The idea of the FS assumption is to consider that objects’ size does not change during the tracking.
The tracking process is based on the Extended Kalman Filter (EKF) with Discrete White Noise Acceleration Model (DWNA) (see Bar-Shalom and Fortman (1988)) and ego odometry. The paper is organized as follows. Section II presents the OBB representation model. The IR uncertainty paradigm is described in section III. Section IV develops the FS assumption. The tracking process is explained in section V. Before concluding, experimental results are presented in section VI.
2. OBB BASED REPRESENTATION Urban environments are characterised by limited spaces available for navigation and there are little objects movement constraints. In these conditions, geometrical representation of dynamic objects is necessary. Oriented bounding box (OBB) is a way of representing objects geometry with sufficient approximation for the means of navigation. Most of the dynamic objects in urban environment are convex and as such can be effectively represented by OBB. The minority of the objects are concave, and in some cases, their concavity is not navigable, and thus, OBB is suitable. The mentioned objects can be called compact ones (see Figure 1). As a contrast, there are also complex objects whose concavity cannot be neglected as a navigable area. These can be represented as a combination of several OBB.
Fig. 1. (a) Compact object (b) complex object. The proposed method of the OBB construction consists of two main steps. The first step is to find a convex contour of the tracked objects. In the second step, a method of Rotating Calipers (see Toussaint (1983)) is used to construct an OBB, which is the best aligned to the object’s contour. In the first stage of the OBB construction method, semiconvex-hull is created for the visible part of the object. The methodology is based on sequencing characteristic of the raw scan points. To explain the method let us use two examples (see Figure 2). The example shows a convexhull, which is described by the points D, C, B and A. It is assumed that the point O represents the origin of the sensor’s coordinate system. When a new point N is considered, two cases can be distinguished. In the first one (see Figure 2 (a)), the point N can be added to the existing convex-hull by connecting it with the point A, without violating the convexity constraint. The second case takes place (see Figure 2 (b)), when by adding the point N , the convexity constraint is broken. In this case, connecting the points N and A produces a concavity represented by N AB. To recognise the two mentioned cases, the proposed algorithm computes and compares lengths of the two line segments OP and OA, where P is the intersection of the lines N B and OA. If the length OP is greater than OA, the point N is added to the convex-hull and the next iteration of the algorithm takes place. When the length OP is less or equal to OA, the point A is removed and the concavity test is repeated for the remaining points constructing the convex-hull (in the example: B, C, D). The repetition of the test is stopped when the convexity appears.
tour through it’s extreme points using four lines determining a rectangle. Because of the symmetry assumption, the algorithm uses only two perpendicular lines. In each step of the algorithm, at least one of these lines coincides with one of the edges of the contour. The lines are simultaneously rotated in one direction, about their supporting points (P 2 and P 4 in Figure 3) during each iteration of the algorithm. The rotation angle has a value which permits for one of the lines to coincide with the next edge of the contour (in Figure 3 the lines are rotated by the angle α). For each lines’ position, an area of an OBB, created by four lines (L1, L2 and their symmetrical lines) is computed. This is performed by computing the area of the rectangle defined by the line segments M M 1 and M M 1, where M is the middle of the line segment defined by the points P 1 and P 5, M 1 and M 2 are respectively the intersections of the lines L1 and L2 with theirs perpendicular lines passing by the point M . The process is repeated until the lines have been rotated by the right angle from the beginning of the algorithm. The smallest area over all iterations indicates the orientation angle of the minimum-area OBB.
Fig. 3. Rotating calipers. The OBB based representation is described by two vectors z (1) and σz2 (2). The first one represents the OBB geometry and includes the centre coordinations cx, cy, the orientation angle θ and the size dx, dy. The second vector represents uncertainties on the components of the vector z. z = [cx, cy, θ, dx, dy]T
(1)
2 2 2 2 T σz2 = [σcx , σcy , σθ2 , σdx , σdy ]
(2)
3. INTER RAYS Fig. 2. Convex-hull construction. The created convex-hull is used to create a minimum-area OBB. In the algorithm, it is assumed that the contour of visible and invisible parts of the object are symmetrical with respect to the point, which is the middle of the segment defined by the two extremities of the constructed contour (see Figure 3). The rotating calipers (RC) algorithm (see Toussaint (1983)) begins by bounding the con-
An important aspect of OBB extraction is the fact that the raw data points representing the extremities of the extracted OBB do not coincide with the real object’s extremities (see Figure 4). In the Figure 4, minX, minY , maxX, maxY are respectively the minimum x coordinate, the minimum y coordinate, the maximum x coordinate and the maximum y coordinate of the extracted OBB. The line Lr (respectively
For the following algorithm description, we consider the local OBB’s X axis. The same process is applied to the local OBB’s Y axis. Having the percepted OBB measurement with the IR line segment length zperc [dIRx ], we obtain the corrected IR line segment length z[dIRx ] associated with the OBB measurement: z[dIRx ] = min(zperc [dIRx ], xk−1 [dIRx ])
(5)
where xk−1 [dIRx ] is the IR line segment length associated with the track at time k-1. The quantity z[dIRx ] is then memorised in the track xk : xk [dIRx ] = z[dIRx ]
(6)
After using equations (3) and (4), the next step consists of the measurement’s size correction by using the following equation: z[dx] = max(z[dx], xk [dx])
(7)
Fig. 4. Inter-Rays uncertainty paradigm.
where xk [dx] is the track predicted size at the time k.
Lr + n) is crossing the point maxY (respectively minX) and is perpendicular to the OBB side to which maxY (respectively minX) belongs. The real object’s extremities are situated between the raw data points delimiting the OBB (maxY , minX) and the points P r and P r + n. P r (respecitvely P r + n) is the intersection point between the ray r (respecitvely r + n) with the line Lr (respectively Lr + n).
After correcting the percepted measurement’s size, the measurement’s center must be appropriately translated. The updating of the center position is achieved as follows.
Considering the OBB’s local X axis, the real object’s extremity position is uniformly distributed with the mean µIRx , which is equal to the half of the IR line segment length dIRx . The IR line segment is defined by the point maxY and P r. To fulfil Kalman Filter assumption, the distribution of the real object’s extremity position is approximated by a normal distribution with the mean 2 µIRx , and the variance σIRx , which is set to dIRx 6 . The 2 ] are used in each Inter-Rays values z[µIRx ] and z[σIRx iteration of the tracking algorithm to correct the size of the OBB measurement (see Kmiotek and Ruichek (2008)). The correction equations are expressed as follows: z[dx] = zperc [dx] + z[µIRx ]
(3)
2 2 2 z[σdx ] = zperc [σdx ] + z[σIRx ]
(4)
Firstly, a visibility factor V Fx is computed for the OBB’s local X axis: V Fx =
f f max(βminX , βmaxX ) f f + βmaxX βminX
(8)
where βminX and βmaxX correspond respectively to the angles between OBB’s sides minXside and maxXside normals and their radius vectors (see Figure 5). f is a parameter, which is set to 4.
where zperc is the percepred measurement, z is the corrected measurement used for tracking. The same process is applied for the OBB’s local Y axis. 4. FIXED SIZE ASSUMPTION The idea of the fixed size (FS) assumption is based on the fact that, in general cases, objects’ size does not change during the tracking. However, due to the LRF’s limited resolution and change of the relative distance and orientation of the observed object, measurements of the object’s size vary in time. The principle of the FS assumption is that the track representing the tracked object cannot reduce it’s size. The FS algorithm takes place in each iteration of the tracking after the track prediction and measurements extraction.
Fig. 5. Visibility factor associated to the OBB’s local X axis. In the second step, a direction factor DFx associated to the OBB’s local X axis is computed using the following equation:
DFx =
1, −1,
if βmaxX > βminX if βmaxX < βminX
(9a) (9b)
In the last step, the difference between the percepted size zperc [dx] and the corrected size z[ dx] is calculated: ∆dx = z[dx] − zperc [dx] (10) Finally, the measurement’s center translation is expressed as follows: 1 z[cx] = z[cx] + V Fx · DFx · ∆dx (11) 2 1 2 2 ] (12) z[σcx ] = z[σdx 2 5. TRACKING
6. RESULTS In the framework of the ”intelligent vehicles and their integration in the city of the future” project, a software platform is developed to simulate the sensors and the multiple objects tracking process. The simulator permits flexible changing of all sensors parameters and mounting position. This allows testing developed algorithms with different sensor configurations. In the simulator laser range finder (LRF), LIDAR, stereovision and odometry sensors are implemented. For the test of the proposed algorithms, a Laser Range Finder (LRF) is mounted in front of the vehicle. The step angle for the LRF is set to 1 with an angle range of 180. In the tests, the sensor range uncertainty is set to 0.05m.
The object’s state estimation is done by the means of Extended Kalman Filter (EKF). All values of the track’s state vector are expressed in the local ego-vehicle coordinate system. Tracks are represented by the augmented OBB state vector xk : ˙ dx, dy]T xk = [cx, cx, ˙ cy, cy, ˙ θ, θ, (13)
In this paper, the objective is to show the interest of the Inter-Rays (IR) uncertainty and the Fixed Size (FS) assumption when introduced in the objects tracking process. The results presented bellow are obtained with a scenario representing a vehicle, which moves in front of the instrumented vehicle according a circular trajectory.
When tracking is done from dynamic platform, odometry information is used to increase tracking accuracy. State change of the ego-vehicle is represented as differences of position ∆Ex , ∆Ey and angle ∆Eγ between consecutive instants. Thus, the input to the state transition equation is defined as: uk = [∆Ex , ∆Ey , ∆Eγ ] (14)
One can see in the Figures 6, 7, 8, 9 that using IR uncertainty and FS assumption allows estimating with a good accuracy the real object size (see Figures 8 and 9). The Difference between estimated and real values deceases in time, as the tracked vehicle moves. The same comment is noticed when considering the size uncertainty (see Figures 6 and 6). Thus, the better object’s size estimation increases the robustness of the object tracking system.
The Discrete White Noise Acceleration Model (DWNA) (see Bar-Shalom et al. (2001)) is used to describe objects kinematics and process noise. Thus, taking into account the odometry information, the track state transition is modelled as follows (see Kmiotek and Ruichek (2008)): xk|k−1 = A(∆Ex , ∆Ey , ∆Eγ )F xk−1 + Buk + Gvk−1 (15) where F is is the standard DWNA transition matrix, B is the odometry-input model, G represents the noise gain matrix. According to the chosen kinematics model, the process noise vk−1 is defined with Gaussian distribution: ¨σ vk−1 = [cx, ¨ cy, ¨ θ, ˆ ,σ ˆ ], vk−1 ∼ N (0, Qk ) (16) dx
dy
where Qk = Gvk−1 GT
(17)
with σ ˆdx and σ ˆdy are the process errors for OBB sizes dx and dy respectively. The predicted estimation covariance matrix is : ∂A ∂AT (xk−1 )F Pk−1 (xk−1 )F T + Qk Pk|k−1 = ∂x ∂x
(18)
where Pk−1 is the estimation covariance matrix. The observation equation can be written as follows: zk = Hxk|k−1 + wk (19) where H is the observation model, wk is defined with a Gaussian distribution: wk ∼ N (0, R)R = σz2 I5,5 (20) where I5,5 is the identity matrix.
Fig. 6. Comparison of the size uncertainty between the percepted measurement zperc and the track xk for the OBB’s local X axis. Figures 10 and 11 represent respectively the estimation of the orientation angle and the center’s position of the tracked vehicle. One can see that the good object’s size estimation leads to good center’s position estimation (see Figure 11). However, we can notice the presence of some errors in the center’s position estimation. These errors are not caused by the IR uncertainty and the FS assumption. They appear when the tracked vehicle is visible only by one side. Indeed, the tracking process, based on the IR uncertainty and FS assumption, depends directly on the orientation angle of the tracked vehicle. Furthermore, one can see that these errors appear at the same time
Fig. 7. Comparison of the size uncertainty between the percepted measurement zperc and the track xk for the OBB’s local Y axis.
Fig. 8. Comparison of the side length dx between the percepted measurement zperc and the track xk .
Fig. 9. Comparison of the side length dy between the percepted measurement zperc and the track xk .
Fig. 10. Real, measured and estimated orientation angle of the object.
in figures 10 and 11. These errors can be corrected by adjusting the Kalman filter parameters or by introducing a threshold procedure on the orientation angles, provided by the Rotating Calipers algorithm. 7. CONCLUSION A laser sensory data based method for objects tracking using a geometric representation is proposed. To obtain the model representation, called Oriented Bounding Box (OBB) representation, a procedure is used to extract the contour of the object, before determining the OBB thanks to the Rotating Calipers technique. Enriched by the InterRays uncertainty and the Fixed Size assumption, the model allows achieving tracking with good estimation for the geometric parameters of the tracked object. Experimental results are presented to show the robustness of the proposed method. In order to improve more the results, the authors are working on a gating procedure for the orientation angles with an adjustment of the Kalman filter parameters.
Fig. 11. Real, measured and estimated center’s position of the object. REFERENCES Bar-Shalom, Y. and Fortman, T.E. (1988). Tracking and data association. Academic Press Professional, Inc.
Bar-Shalom, Y., Li, X., and Kirubarajan, T. (2001). Estimation with applications to tracking and navigation. Wiley New York. Blanc, C., Trassoudaine, L., Guilloux, Y., and R.Moreira (2004). Track to track fusion method applied to road obstacle detection. In Proceedings of the Seventh International Conference on Information Fusion, volume 2, 775–782. Ericson, C. (2004). Real-Time Collision Detection. Morgan Kaufmann Publishers. Hofmann, U., Rieder, A., and Dickmanns, D. (2003). Radar and vision data fusion for hybrid adaptive cruise control on highways. Machine Vision and Applications, 14, 42–49. Kmiotek, P. and Ruichek, Y. (2008). Representing and tracking of dynamics objects using oriented bounding box and extended kalman filter. In Proceedings , 11th International IEEE Conference on Intelligent Transportation Systems, 322–328. Nguyen, V., Martinelli, A., Tomatis, N., and Siegwar, R. (2005). A comparison of line extraction algorithms using 2d laser rangefinder for indoor mobile robotics. Toussaint, G. (1983). Solving geometric problems with the rotating calipers. In Proc. MELECON, Athens, Greece.
Systems and Transportation Laboratory, University of Technology of Belfort-Montbeliard, 13 Rue Thierry Mieg 90010 Belfort, France (e-mail: [email protected], [email protected]). ∗∗ Department of Computer Science, AGH - University of Science and Technology, al. Mickiewicza 30, 30-059 Krakow, Poland (e-mail: [email protected]). Abstract: This paper presents a geometric representation based method for objects tracking using laser sensory data. The representation model, based on an Oriented Bounding Box (OBB), is achieved by using a two-stage procedure. The first one concurs a LRF data based convex hull technique of object’s contour extraction. The second stage consists of contour analysis by a geometric method, called Rotating Calipers. In order to obtain good estimation for the geometric parameters of the object, two concepts are proposed. The first one, called Inter-Rays uncertainty, is introduced to consider the fact that the raw data points representing the extremities of the extracted OBB do not coincide with the real object’s extremities. The second concept, called Fixed Size assumption, is integrated to take into account that the size of the object dos not change during the tracking process. The tracking is ensured by the Extended Kalman Filter with Discrete White Noise Acceleration model. Experimental results are presented to show the robustness of the proposed method. Keywords: Tracking, Object representation, Laser range sensor, Intelligent vehicle, Signal processing 1. INTRODUCTION Representation of dynamic objects is crucial for tracking and trajectory planning. In the literature concerning tracking, points with elliptical uncertainty are used for representing objects position (see Bar-Shalom and Fortman (1988), Blanc et al. (2004)). This representation is good enough for obstacle detection, collision warning or driving assistance systems in well structured environments like highway (see Blanc et al. (2004), Hofmann et al. (2003)). In the urban areas, there are less constraints on the objects movements. Thus, for the task of autonomous navigation in demanding urban areas, these representation methods are not sufficient. Oriented Bounding Box (OBB) (see Toussaint (1983), Ericson (2004), Nguyen et al. (2005)) provides a good approximation of the size, shape and orientation angle of dynamic objects, with a good data compression ratio. In this paper, an OBB model, with an Inter-Rays (IR) uncertainty paradigm and a Fixed Size (FS) assumption, is proposed to represent dynamic objects. The IR uncertainty and FS assumption are introduced to increase the reliability of tracking in terms of object’s size and centre position. The IR uncertainty is developed to handle the fact that the raw data points representing the extremities of the extracted OBB do not coincide with the real object’s extremities. The idea of the FS assumption is to consider that objects’ size does not change during the tracking.
The tracking process is based on the Extended Kalman Filter (EKF) with Discrete White Noise Acceleration Model (DWNA) (see Bar-Shalom and Fortman (1988)) and ego odometry. The paper is organized as follows. Section II presents the OBB representation model. The IR uncertainty paradigm is described in section III. Section IV develops the FS assumption. The tracking process is explained in section V. Before concluding, experimental results are presented in section VI.
2. OBB BASED REPRESENTATION Urban environments are characterised by limited spaces available for navigation and there are little objects movement constraints. In these conditions, geometrical representation of dynamic objects is necessary. Oriented bounding box (OBB) is a way of representing objects geometry with sufficient approximation for the means of navigation. Most of the dynamic objects in urban environment are convex and as such can be effectively represented by OBB. The minority of the objects are concave, and in some cases, their concavity is not navigable, and thus, OBB is suitable. The mentioned objects can be called compact ones (see Figure 1). As a contrast, there are also complex objects whose concavity cannot be neglected as a navigable area. These can be represented as a combination of several OBB.
Fig. 1. (a) Compact object (b) complex object. The proposed method of the OBB construction consists of two main steps. The first step is to find a convex contour of the tracked objects. In the second step, a method of Rotating Calipers (see Toussaint (1983)) is used to construct an OBB, which is the best aligned to the object’s contour. In the first stage of the OBB construction method, semiconvex-hull is created for the visible part of the object. The methodology is based on sequencing characteristic of the raw scan points. To explain the method let us use two examples (see Figure 2). The example shows a convexhull, which is described by the points D, C, B and A. It is assumed that the point O represents the origin of the sensor’s coordinate system. When a new point N is considered, two cases can be distinguished. In the first one (see Figure 2 (a)), the point N can be added to the existing convex-hull by connecting it with the point A, without violating the convexity constraint. The second case takes place (see Figure 2 (b)), when by adding the point N , the convexity constraint is broken. In this case, connecting the points N and A produces a concavity represented by N AB. To recognise the two mentioned cases, the proposed algorithm computes and compares lengths of the two line segments OP and OA, where P is the intersection of the lines N B and OA. If the length OP is greater than OA, the point N is added to the convex-hull and the next iteration of the algorithm takes place. When the length OP is less or equal to OA, the point A is removed and the concavity test is repeated for the remaining points constructing the convex-hull (in the example: B, C, D). The repetition of the test is stopped when the convexity appears.
tour through it’s extreme points using four lines determining a rectangle. Because of the symmetry assumption, the algorithm uses only two perpendicular lines. In each step of the algorithm, at least one of these lines coincides with one of the edges of the contour. The lines are simultaneously rotated in one direction, about their supporting points (P 2 and P 4 in Figure 3) during each iteration of the algorithm. The rotation angle has a value which permits for one of the lines to coincide with the next edge of the contour (in Figure 3 the lines are rotated by the angle α). For each lines’ position, an area of an OBB, created by four lines (L1, L2 and their symmetrical lines) is computed. This is performed by computing the area of the rectangle defined by the line segments M M 1 and M M 1, where M is the middle of the line segment defined by the points P 1 and P 5, M 1 and M 2 are respectively the intersections of the lines L1 and L2 with theirs perpendicular lines passing by the point M . The process is repeated until the lines have been rotated by the right angle from the beginning of the algorithm. The smallest area over all iterations indicates the orientation angle of the minimum-area OBB.
Fig. 3. Rotating calipers. The OBB based representation is described by two vectors z (1) and σz2 (2). The first one represents the OBB geometry and includes the centre coordinations cx, cy, the orientation angle θ and the size dx, dy. The second vector represents uncertainties on the components of the vector z. z = [cx, cy, θ, dx, dy]T
(1)
2 2 2 2 T σz2 = [σcx , σcy , σθ2 , σdx , σdy ]
(2)
3. INTER RAYS Fig. 2. Convex-hull construction. The created convex-hull is used to create a minimum-area OBB. In the algorithm, it is assumed that the contour of visible and invisible parts of the object are symmetrical with respect to the point, which is the middle of the segment defined by the two extremities of the constructed contour (see Figure 3). The rotating calipers (RC) algorithm (see Toussaint (1983)) begins by bounding the con-
An important aspect of OBB extraction is the fact that the raw data points representing the extremities of the extracted OBB do not coincide with the real object’s extremities (see Figure 4). In the Figure 4, minX, minY , maxX, maxY are respectively the minimum x coordinate, the minimum y coordinate, the maximum x coordinate and the maximum y coordinate of the extracted OBB. The line Lr (respectively
For the following algorithm description, we consider the local OBB’s X axis. The same process is applied to the local OBB’s Y axis. Having the percepted OBB measurement with the IR line segment length zperc [dIRx ], we obtain the corrected IR line segment length z[dIRx ] associated with the OBB measurement: z[dIRx ] = min(zperc [dIRx ], xk−1 [dIRx ])
(5)
where xk−1 [dIRx ] is the IR line segment length associated with the track at time k-1. The quantity z[dIRx ] is then memorised in the track xk : xk [dIRx ] = z[dIRx ]
(6)
After using equations (3) and (4), the next step consists of the measurement’s size correction by using the following equation: z[dx] = max(z[dx], xk [dx])
(7)
Fig. 4. Inter-Rays uncertainty paradigm.
where xk [dx] is the track predicted size at the time k.
Lr + n) is crossing the point maxY (respectively minX) and is perpendicular to the OBB side to which maxY (respectively minX) belongs. The real object’s extremities are situated between the raw data points delimiting the OBB (maxY , minX) and the points P r and P r + n. P r (respecitvely P r + n) is the intersection point between the ray r (respecitvely r + n) with the line Lr (respectively Lr + n).
After correcting the percepted measurement’s size, the measurement’s center must be appropriately translated. The updating of the center position is achieved as follows.
Considering the OBB’s local X axis, the real object’s extremity position is uniformly distributed with the mean µIRx , which is equal to the half of the IR line segment length dIRx . The IR line segment is defined by the point maxY and P r. To fulfil Kalman Filter assumption, the distribution of the real object’s extremity position is approximated by a normal distribution with the mean 2 µIRx , and the variance σIRx , which is set to dIRx 6 . The 2 ] are used in each Inter-Rays values z[µIRx ] and z[σIRx iteration of the tracking algorithm to correct the size of the OBB measurement (see Kmiotek and Ruichek (2008)). The correction equations are expressed as follows: z[dx] = zperc [dx] + z[µIRx ]
(3)
2 2 2 z[σdx ] = zperc [σdx ] + z[σIRx ]
(4)
Firstly, a visibility factor V Fx is computed for the OBB’s local X axis: V Fx =
f f max(βminX , βmaxX ) f f + βmaxX βminX
(8)
where βminX and βmaxX correspond respectively to the angles between OBB’s sides minXside and maxXside normals and their radius vectors (see Figure 5). f is a parameter, which is set to 4.
where zperc is the percepred measurement, z is the corrected measurement used for tracking. The same process is applied for the OBB’s local Y axis. 4. FIXED SIZE ASSUMPTION The idea of the fixed size (FS) assumption is based on the fact that, in general cases, objects’ size does not change during the tracking. However, due to the LRF’s limited resolution and change of the relative distance and orientation of the observed object, measurements of the object’s size vary in time. The principle of the FS assumption is that the track representing the tracked object cannot reduce it’s size. The FS algorithm takes place in each iteration of the tracking after the track prediction and measurements extraction.
Fig. 5. Visibility factor associated to the OBB’s local X axis. In the second step, a direction factor DFx associated to the OBB’s local X axis is computed using the following equation:
DFx =
1, −1,
if βmaxX > βminX if βmaxX < βminX
(9a) (9b)
In the last step, the difference between the percepted size zperc [dx] and the corrected size z[ dx] is calculated: ∆dx = z[dx] − zperc [dx] (10) Finally, the measurement’s center translation is expressed as follows: 1 z[cx] = z[cx] + V Fx · DFx · ∆dx (11) 2 1 2 2 ] (12) z[σcx ] = z[σdx 2 5. TRACKING
6. RESULTS In the framework of the ”intelligent vehicles and their integration in the city of the future” project, a software platform is developed to simulate the sensors and the multiple objects tracking process. The simulator permits flexible changing of all sensors parameters and mounting position. This allows testing developed algorithms with different sensor configurations. In the simulator laser range finder (LRF), LIDAR, stereovision and odometry sensors are implemented. For the test of the proposed algorithms, a Laser Range Finder (LRF) is mounted in front of the vehicle. The step angle for the LRF is set to 1 with an angle range of 180. In the tests, the sensor range uncertainty is set to 0.05m.
The object’s state estimation is done by the means of Extended Kalman Filter (EKF). All values of the track’s state vector are expressed in the local ego-vehicle coordinate system. Tracks are represented by the augmented OBB state vector xk : ˙ dx, dy]T xk = [cx, cx, ˙ cy, cy, ˙ θ, θ, (13)
In this paper, the objective is to show the interest of the Inter-Rays (IR) uncertainty and the Fixed Size (FS) assumption when introduced in the objects tracking process. The results presented bellow are obtained with a scenario representing a vehicle, which moves in front of the instrumented vehicle according a circular trajectory.
When tracking is done from dynamic platform, odometry information is used to increase tracking accuracy. State change of the ego-vehicle is represented as differences of position ∆Ex , ∆Ey and angle ∆Eγ between consecutive instants. Thus, the input to the state transition equation is defined as: uk = [∆Ex , ∆Ey , ∆Eγ ] (14)
One can see in the Figures 6, 7, 8, 9 that using IR uncertainty and FS assumption allows estimating with a good accuracy the real object size (see Figures 8 and 9). The Difference between estimated and real values deceases in time, as the tracked vehicle moves. The same comment is noticed when considering the size uncertainty (see Figures 6 and 6). Thus, the better object’s size estimation increases the robustness of the object tracking system.
The Discrete White Noise Acceleration Model (DWNA) (see Bar-Shalom et al. (2001)) is used to describe objects kinematics and process noise. Thus, taking into account the odometry information, the track state transition is modelled as follows (see Kmiotek and Ruichek (2008)): xk|k−1 = A(∆Ex , ∆Ey , ∆Eγ )F xk−1 + Buk + Gvk−1 (15) where F is is the standard DWNA transition matrix, B is the odometry-input model, G represents the noise gain matrix. According to the chosen kinematics model, the process noise vk−1 is defined with Gaussian distribution: ¨σ vk−1 = [cx, ¨ cy, ¨ θ, ˆ ,σ ˆ ], vk−1 ∼ N (0, Qk ) (16) dx
dy
where Qk = Gvk−1 GT
(17)
with σ ˆdx and σ ˆdy are the process errors for OBB sizes dx and dy respectively. The predicted estimation covariance matrix is : ∂A ∂AT (xk−1 )F Pk−1 (xk−1 )F T + Qk Pk|k−1 = ∂x ∂x
(18)
where Pk−1 is the estimation covariance matrix. The observation equation can be written as follows: zk = Hxk|k−1 + wk (19) where H is the observation model, wk is defined with a Gaussian distribution: wk ∼ N (0, R)R = σz2 I5,5 (20) where I5,5 is the identity matrix.
Fig. 6. Comparison of the size uncertainty between the percepted measurement zperc and the track xk for the OBB’s local X axis. Figures 10 and 11 represent respectively the estimation of the orientation angle and the center’s position of the tracked vehicle. One can see that the good object’s size estimation leads to good center’s position estimation (see Figure 11). However, we can notice the presence of some errors in the center’s position estimation. These errors are not caused by the IR uncertainty and the FS assumption. They appear when the tracked vehicle is visible only by one side. Indeed, the tracking process, based on the IR uncertainty and FS assumption, depends directly on the orientation angle of the tracked vehicle. Furthermore, one can see that these errors appear at the same time
Fig. 7. Comparison of the size uncertainty between the percepted measurement zperc and the track xk for the OBB’s local Y axis.
Fig. 8. Comparison of the side length dx between the percepted measurement zperc and the track xk .
Fig. 9. Comparison of the side length dy between the percepted measurement zperc and the track xk .
Fig. 10. Real, measured and estimated orientation angle of the object.
in figures 10 and 11. These errors can be corrected by adjusting the Kalman filter parameters or by introducing a threshold procedure on the orientation angles, provided by the Rotating Calipers algorithm. 7. CONCLUSION A laser sensory data based method for objects tracking using a geometric representation is proposed. To obtain the model representation, called Oriented Bounding Box (OBB) representation, a procedure is used to extract the contour of the object, before determining the OBB thanks to the Rotating Calipers technique. Enriched by the InterRays uncertainty and the Fixed Size assumption, the model allows achieving tracking with good estimation for the geometric parameters of the tracked object. Experimental results are presented to show the robustness of the proposed method. In order to improve more the results, the authors are working on a gating procedure for the orientation angles with an adjustment of the Kalman filter parameters.
Fig. 11. Real, measured and estimated center’s position of the object. REFERENCES Bar-Shalom, Y. and Fortman, T.E. (1988). Tracking and data association. Academic Press Professional, Inc.
Bar-Shalom, Y., Li, X., and Kirubarajan, T. (2001). Estimation with applications to tracking and navigation. Wiley New York. Blanc, C., Trassoudaine, L., Guilloux, Y., and R.Moreira (2004). Track to track fusion method applied to road obstacle detection. In Proceedings of the Seventh International Conference on Information Fusion, volume 2, 775–782. Ericson, C. (2004). Real-Time Collision Detection. Morgan Kaufmann Publishers. Hofmann, U., Rieder, A., and Dickmanns, D. (2003). Radar and vision data fusion for hybrid adaptive cruise control on highways. Machine Vision and Applications, 14, 42–49. Kmiotek, P. and Ruichek, Y. (2008). Representing and tracking of dynamics objects using oriented bounding box and extended kalman filter. In Proceedings , 11th International IEEE Conference on Intelligent Transportation Systems, 322–328. Nguyen, V., Martinelli, A., Tomatis, N., and Siegwar, R. (2005). A comparison of line extraction algorithms using 2d laser rangefinder for indoor mobile robotics. Toussaint, G. (1983). Solving geometric problems with the rotating calipers. In Proc. MELECON, Athens, Greece.