A Novel High-Precision Railway Obstacle Detection Algorithm Based on 3D LiDAR

. 2024 May 15;24(10):3148. doi: 10.3390/s24103148

Algorithm 1: Railway scanline division and calibration, SFRE process

Input: point cloud set

P

, the number of pulses of a scanning line

m

;

Output: Rail point cloud

P_{r a i l}

k = 0

;

for i = 1; i < n; i + +

L i n e c l u s t e r I (k) \leftarrow m p o i n t s

k + +

5: end for
6:

I (g) \leftarrow g r o u n d s c a n n i n g l i n e

p \cdot z = 0 (p \in I (g))

α_{x o y} \leftarrow I (g) [50, m - 50]

RANSAC line fitting
9:

R_{1} \leftarrow α_{x o y}

10:

p \cdot x = 0 (p \in I (g))

11:

α_{y o z} \leftarrow I (g) [50, m - 50]

RANSAC line fitting
12:

R_{2} \leftarrow α_{y o z}

13:

P' = R_{1} R_{2} P

14:

b u c k e t < k e y, v a l u e >

15: for

i = 0; i < P' \cdot s i z e (); i + +

do
16: if

b u c k e t \cdot c o u n t (P' \cdot x / A) > 0

then
17:

b u c k e t [P' \cdot x / A] + +

18: else

b u c k e t [P' \cdot x / A] = 1

19: end if
20: end for
21: //Get the the two keys with the maximum values
22:

k e y 1, k e y 2 \leftarrow M a x (b u c k e t [k e y 1]), M a x (b u c k e t [k e y 2])

23:

P_{r a i l} \cdot x 1 = k e y 1 * A

24:

P_{r a i l} \cdot x 2 = k e y 2 * A

25:

/ / f (P, x 1, x 2)

filtering in the x direction with set P

26:

P r a i l \leftarrow f (P', P_{r a i l} \cdot x 1, P_{r a i l} \cdot x 2)

27:

return P r a i l