Mercurial Hosting > traffic-intelligence
changeset 1213:3f2214125164
work in progress
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 03 May 2023 14:58:26 -0400 |
parents | af329f3330ba |
children | 01c24c1cdb70 |
files | trafficintelligence/moving.py trafficintelligence/storage.py |
diffstat | 2 files changed, 94 insertions(+), 4 deletions(-) [+] |
line wrap: on
line diff
--- a/trafficintelligence/moving.py Tue May 02 17:11:24 2023 -0400 +++ b/trafficintelligence/moving.py Wed May 03 14:58:26 2023 -0400 @@ -398,6 +398,97 @@ return None @staticmethod + def ttc_calculate(bbox_info_total_1, bbox_info_total_2): + """ + :param bbox_1: list: [bbox_1 array 4*2 , v_1_x (float), v_1_y (float)] + :param bbox_2: [bbox_2_x array 4*2, v_2_x (float), v_2_y (float)] + :return: ttc + """ + from sympy import solve + def NewFourPoints(bbox, col_time, v_x, v_y): + return [i + col_time * v_x for i in bbox[0]], [j + col_time * v_y for j in bbox[1]] + + bbox_1, v_1_x, v_1_y = bbox_info_total_1[0], bbox_info_total_1[1], bbox_info_total_1[2] + bbox_2, v_2_x, v_2_y = bbox_info_total_2[0], bbox_info_total_2[1], bbox_info_total_2[2] + x_bbox_1 = bbox_1[:, 0].tolist() + y_bbox_1 = bbox_1[:, 1].tolist() + bbox_1 = [x_bbox_1, y_bbox_1] + x_bbox_2 = bbox_2[:, 0].tolist() + y_bbox_2 = bbox_2[:, 1].tolist() + bbox_2 = [x_bbox_2, y_bbox_2] + t_total = [] + line = [[0, 1], [1, 2], [2, 3], [3, 0]] + for i in range(4): + for j in range(4): + p0_x, p0_y = x_bbox_2[i], y_bbox_2[i] + p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]] + p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]] + f = (p2_x - p1_x) * (p0_y - p1_y) - (p0_x - p1_x) * (p2_y - p1_y) + collision_t = solve(f, t, set=True, dict=True) + if collision_t and collision_t[0][t] >= 0: + collision_t = collision_t[0][t] + print(f'collision_t: {collision_t} >>>>>>>>>') + x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t, v_1_x, v_1_y) + x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t, v_2_x, v_2_y) + p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i] + p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]] + p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]] + min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x) + min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y) + if min_x == max_x: + if min_y <= p0_y <= max_y: + t_total.append(collision_t) + + elif min_y == max_y: + if min_x <= p0_x <= max_x: + t_total.append(collision_t) + + else: + if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y: + t_total.append(collision_t) + x_temps = x_bbox_2 + x_bbox_2 = x_bbox_1 + x_bbox_1 = x_temps + # print(f'Change bounding box1 and box2 >>>>>>>>>>>>>>>>>>>\n\n\n') + for i in range(4): + for j in range(4): + p0_x, p0_y = x_bbox_2[i], y_bbox_2[i] + p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]] + p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]] + f = (p2_x - p1_x) * (p0_y - p1_y) - (p0_x - p1_x) * (p2_y - p1_y) + collision_t = solve(f, t, set=True, dict=True) + if collision_t and collision_t[0][t] >= 0: + collision_t = collision_t[0][t] + # print(f'collision_t: {collision_t} >>>>>>>>>') + x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t) + x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t) + p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i] + p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]] + p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]] + min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x) + min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y) + if min_x == max_x: + if min_y <= p0_y <= max_y: + t_total.append(collision_t) + + elif min_y == max_y: + if min_x <= p0_x <= max_x: + t_total.append(collision_t) + + else: + if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y: + t_total.append(collision_t) + + if t_total: + collision_t_min = min(t_total) + # print(f'collision_time:') + # print(collision_t_min) + else: + collision_t_min = 1e7 + # print(f'No collision') + return collision_t_min + + @staticmethod def midPoint(p1, p2): 'Returns the middle of the segment [p1, p2]' return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y)
--- a/trafficintelligence/storage.py Tue May 02 17:11:24 2023 -0400 +++ b/trafficintelligence/storage.py Wed May 03 14:58:26 2023 -0400 @@ -1389,11 +1389,10 @@ # ego vehicle t = moving.Trajectory() featureTrajectories = [moving.Trajectory() for i in range(4)] - interval = moving.TimeInterval(int(data.frame.min()), int(data.frame.max())) + interval = moving.TimeInterval(0, len(oxts)-1)#int(data.frame.min()), int(data.frame.max())) R = roty(pi/2) - for frame in interval: - _, Tr_imu_to_world = oxts[frame] - + #for frame in interval: + for _, Tr_imu_to_world in oxts: l = 4.5 # m w = 1.8 # m h = 0.