Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/prediction.py @ 1214:01c24c1cdb70

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
implemented direct method
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 04 May 2023 22:30:32 -0400
parents af329f3330ba
children 1b472cddf9b1
comparison
equal deleted inserted replaced
1213:3f2214125164 1214:01c24c1cdb70
603 collisionPoints = [SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc)] 603 collisionPoints = [SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc)]
604 else: 604 else:
605 pass # compute pPET 605 pass # compute pPET
606 return collisionPoints, crossingZones 606 return collisionPoints, crossingZones
607 607
608 class CVRectPredictionParameters(PredictionParameters): 608 class CVExactPolyPredictionParameters(PredictionParameters):
609 '''Prediction parameters of prediction at constant velocity 609 '''Prediction parameters of prediction at constant velocity
610 for objects represented by boxes (bird eye view boxes) 610 for objects represented by boxes (bird eye view boxes)
611 Warning: the computed time to collision may be higher than timeHorizon (not used)''' 611 Warning: the computed time to collision may be higher than timeHorizon (not used)'''
612 612
613 def __init__(self): 613 def __init__(self):
614 PredictionParameters.__init__(self, 'constant velocity for rectangles', None) 614 PredictionParameters.__init__(self, 'constant velocity for polygon representation (direct exact)', None)
615 615
616 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): 616 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
617 'TODO compute pPET' 617 'TODO compute pPET'
618 collisionPoints = [] 618 collisionPoints = []
619 crossingZones = [] 619 crossingZones = []
620 620
621 # first test if there is a collision (test if movement is parallel to sides of vehicle) 621 if self.useCurvilinear:
622 # test if vehicle aligned with y axis, otherwise, compute slope 622 pass # Lionel
623 623 else:
624 624 poly1 = obj1.getBoundingPolygon(currentInstant)
625 # compute approximate time of arrival 625 poly2 = obj2.getBoundingPolygon(currentInstant)
626 v1 = obj1.getVelocityAtInstant(currentInstant)
627 v2 = obj2.getVelocityAtInstant(currentInstant)
628
629 if not moving.Point.parallel(v1, v2):
630 ttc = moving.Point.timeToCollisionPoly(poly1, v1, poly2, v2)
631 if ttc is not None:
632 collisionPoints = [SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc)]
633 else:
634 pass # compute pPET
635 return collisionPoints, crossingZones
636
626 637
627 class PrototypePredictionParameters(PredictionParameters): 638 class PrototypePredictionParameters(PredictionParameters):
628 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): 639 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True):
629 PredictionParameters.__init__(self, 'prototypes', None) 640 PredictionParameters.__init__(self, 'prototypes', None)
630 self.prototypes = prototypes 641 self.prototypes = prototypes