Mercurial Hosting > traffic-intelligence
changeset 1214:01c24c1cdb70
implemented direct method
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 04 May 2023 22:30:32 -0400 |
parents | 3f2214125164 |
children | 1b472cddf9b1 |
files | trafficintelligence/moving.py trafficintelligence/prediction.py |
diffstat | 2 files changed, 26 insertions(+), 14 deletions(-) [+] |
line wrap: on
line diff
--- a/trafficintelligence/moving.py Wed May 03 14:58:26 2023 -0400 +++ b/trafficintelligence/moving.py Thu May 04 22:30:32 2023 -0400 @@ -398,23 +398,24 @@ return None @staticmethod - def ttc_calculate(bbox_info_total_1, bbox_info_total_2): + def timeToCollisionPoly(corners1, v1, corners2, v2): """ :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 + from sympy.abc import t 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() + v_1_x, v_1_y = v1.x, v1.y + v_2_x, v_2_y = v2.x, v2.y + x_bbox_1 = [p.x for p in corners1] + y_bbox_1 = [p.y for p in corners1] bbox_1 = [x_bbox_1, y_bbox_1] - x_bbox_2 = bbox_2[:, 0].tolist() - y_bbox_2 = bbox_2[:, 1].tolist() + x_bbox_2 = [p.x for p in corners2] + y_bbox_2 = [p.y for p in corners2] bbox_2 = [x_bbox_2, y_bbox_2] t_total = [] line = [[0, 1], [1, 2], [2, 3], [3, 0]] @@ -484,7 +485,7 @@ # print(f'collision_time:') # print(collision_t_min) else: - collision_t_min = 1e7 + collision_t_min = None # print(f'No collision') return collision_t_min
--- a/trafficintelligence/prediction.py Wed May 03 14:58:26 2023 -0400 +++ b/trafficintelligence/prediction.py Thu May 04 22:30:32 2023 -0400 @@ -605,24 +605,35 @@ pass # compute pPET return collisionPoints, crossingZones -class CVRectPredictionParameters(PredictionParameters): +class CVExactPolyPredictionParameters(PredictionParameters): '''Prediction parameters of prediction at constant velocity for objects represented by boxes (bird eye view boxes) Warning: the computed time to collision may be higher than timeHorizon (not used)''' def __init__(self): - PredictionParameters.__init__(self, 'constant velocity for rectangles', None) + PredictionParameters.__init__(self, 'constant velocity for polygon representation (direct exact)', None) def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): 'TODO compute pPET' collisionPoints = [] crossingZones = [] - # first test if there is a collision (test if movement is parallel to sides of vehicle) - # test if vehicle aligned with y axis, otherwise, compute slope - + if self.useCurvilinear: + pass # Lionel + else: + poly1 = obj1.getBoundingPolygon(currentInstant) + poly2 = obj2.getBoundingPolygon(currentInstant) + v1 = obj1.getVelocityAtInstant(currentInstant) + v2 = obj2.getVelocityAtInstant(currentInstant) - # compute approximate time of arrival + if not moving.Point.parallel(v1, v2): + ttc = moving.Point.timeToCollisionPoly(poly1, v1, poly2, v2) + if ttc is not None: + collisionPoints = [SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc)] + else: + pass # compute pPET + return collisionPoints, crossingZones + class PrototypePredictionParameters(PredictionParameters): def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True):