python 检测直线 交点_python中的轨迹交点

I'm detecting persons and vehicles using tensorflow and python. I calculate the trajectories and predict them using Kalman filter and I fit a line for predicting the trajectory.

My problem is how would I find the intersection and time of collision between the two trajectories ?

I tried line to line intersection but the fitted line is not always a two point lines, it's a polyline.

Here is my attempt:

detections = tracker.update(np.array(z_box))

for trk in detections[0]:

trk = trk.astype(np.int32)

helpers.draw_box_label(img, trk, trk[4]) # Draw the bounding boxes on the

centerCoord = (((trk[1] +trk[3]) / 2), (trk[0] + trk[2]) / 2)

point_lists[trk[4]].append(centerCoord)

x = [i[0] for i in point_lists[trk[4]]]

y = [i[1] for i in point_lists[trk[4]]]

p = np.polyfit(x, y, deg=1)

y = p[1] + p[0] * np.array(x)

fitted = list(zip(x, y))

cv2.polylines(img, np.int32([fitted]), False, color=(255, 0, 0))

for other in detections[0]:

other = other.astype(np.int32)

if other[4] != trk[4]: # check for self ID

x2 = [i[0] for i in point_lists[other[4]]]

y2 = [i[1] for i in point_lists[other[4]]]

p2 = np.polyfit(x2, y2, deg=1)

y2 = p2[1] + p2[0] * np.array(x2)

other_fitted = list(zip(x2, y2))

if(line_intersection(fitted, other_fitted)):

print("intersection")

else:

print("not intersection")

解决方案

this is a bit broader topic so I will focus only on the math/physics part as I got the feeling the CV/DIP part is already handled by both of you askers (andre ahmed, and chris burgees).

For simplicity I am assuming linear movement with constant speeds So how to do this:

obtain 2D position of each object for 2 separate frames after known time dt

so obtain the 2D center (or corner or whatever) position on the image for each object in question.

convert them to 3D

so using known camera parameters or known bacground info about the scene you can un-project the 2D position on screen into 3D relative position to camera. This will get rid of the non linear interpolations otherwise need if handled just like a 2D case.

There are more option how to obtain 3D position depending on what you got at your disposal. For example like this:

obtaining actual speed of objects

the speed vector is simply:

vel = ( pos(t+dt) - pos(t) )/dt

so simply subbstract positions of the same object from 2 consequent frames and divide by the framerate period (or interval between the frames used).

test each 2 objects for collision

this is the funny stuff Yes you can solve a system of inequalities like:

| ( pos0 + vel0 * t ) - (pos1 + vel1 * t ) | <= threshold

but there is a simpler way I used in here

The idea is to compute t where the tested objects are closest together (if nearing towards eachother).

so we can extrapolate the future position of each object like this:

pos(t) = pos(t0) + vel*(t-t0)

where t is actual time and t0 is some start time (for example t0=0).

let assume we have 2 objects (pos0,vel0,pos1,vel1) we want to test so compute first 2 iterations of their distance so:

pos0(0) = pos0;

pos1(0) = pos1;

dis0 = | pos1(0) - pos0(0) |

pos0(dt) = pos0 + vel0*dt;

pos1(dt) = pos1 + vel1*dt;

dis1 = | pos1(dt) - pos0(dt) |

where dt is some small enough time (to avoid skipping through collision). Now if (dis0dis1) the objects are nearing to each other so we can estimate:

dis(t) = dis0 + (dis1-dis0)*t

and the collision expects that dis(t)=0 so we can extrapolate again:

0 = dis0 + (dis1-dis0)*t

(dis0-dis1)*t = dis0

t = dis0 / (dis0-dis1)

where t is the estimated time of collision. Of coarse all this handles all the movement as linear and extrapolates a lot so its not accurate but as you can do this for more consequent frames and the result will be more accurate with the time nearing to collision ... Also to be sure you should extrapolate the position of each object at the time of estimated collision to verify the result (if not colliding then the extrapolation was just numerical and the objects did not collide just was nearing to each for a time)

As mentioned before the conversion to 3D (bullet #2) is not necessary but it get rid of the nonlinearities so simple linear interpolation/extrapolation can be used later on greatly simplify things.