Mercurial Hosting > traffic-intelligence
changeset 258:d90be3c02267
reasonably efficient computation of collision points and crossing zones
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 24 Jul 2012 12:37:47 -0400 |
parents | 9281878ff19e |
children | 8ab76b95ee72 |
files | python/extrapolation.py python/moving.py |
diffstat | 2 files changed, 66 insertions(+), 34 deletions(-) [+] |
line wrap: on
line diff
--- a/python/extrapolation.py Tue Jul 24 03:27:29 2012 -0400 +++ b/python/extrapolation.py Tue Jul 24 12:37:47 2012 -0400 @@ -10,6 +10,13 @@ it should also have a probability''' + def __init__(self): + self.probability = 0. + self.predictedPositions = {} + self.predictedSpeedOrientations = {} + self.collisionPoints = {} + self.crossingZones = {} + def predictPosition(self, nTimeSteps): if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): self.predictPosition(nTimeSteps-1) @@ -63,41 +70,60 @@ def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed): '''extrapolationParameters specific to each method (in name field) ''' - if extrapolationHypothesis.name == 'constant velocity': + if extrapolationParameters.name == 'constant velocity': return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)] else: print('Unknown extrapolation hypothesis') return [] +class CollisionPoint(moving.Point): + def __init__(self, p, probability = 1., TTC = -1): + self.x = p.x + self.y = p.y + self.probability = probability + self.TTC = TTC + +class CrossingZone(moving.Point): + def __init__(self, p, probability = 1., pPET = -1): + self.x = p.x + self.y = p.y + self.probability = probability + self.pPET = pPET + def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): '''returns the lists of collision points and crossing zones ''' collisionPoints = [] - TTCs = [] crossingZones = [] - pPETs = [] for et1 in extrapolatedTrajectories1: for et2 in extrapolatedTrajectories2: t = 1 p1 = et1.predictPosition(t) p2 = et2.predictPosition(t) - while t <= timeHorizon and (p1-p2).norm() > collisionDistanceThreshold: + while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: p1 = et1.predictPosition(t) p2 = et2.predictPosition(t) t += 1 if t <= timeHorizon: - TTCs.append(t) # todo store probability - collisionPoints.append((p1+p2).multiply(0.5)) + collisionPoints.append(CollisionPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) else: # check if there is a crossing zone - for t1 in xrange(timeHorizon-1): - for t2 in xrange(timeHorizon-1): - cz = moving.segmentIntersection (et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) + # TODO? zone should be around the points at which the traj are the closest + # look for CZ at different times, otherwise it would be a collision + # an approximation would be to look for close points at different times, ie the complementary of collision points + cz = None + t1 = 0 + while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 + t2 = 0 + while not cz and t2 < timeHorizon: + #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: + # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) + cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) if cz: - crossingZones.append(cz) - pPETs.append(abs(t1-t2)) - break - return collisionPoints, crossingZones, TTCs, pPETs + crossingZones.append(CrossingZone(cz, et1.probability*et2.probability, abs(t1-t2))) + t2 += 1 + t1 += 1 + return collisionPoints, crossingZones
--- a/python/moving.py Tue Jul 24 03:27:29 2012 -0400 +++ b/python/moving.py Tue Jul 24 12:37:47 2012 -0400 @@ -1,10 +1,10 @@ #! /usr/bin/env python '''Libraries for moving objects, trajectories...''' -import utils; -import cvutils; +import utils +import cvutils -from math import sqrt; +from math import sqrt #from shapely.geometry import Polygon @@ -295,25 +295,31 @@ from numpy import matrix from numpy.linalg import linalg, det - dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]] - dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]] - - A = matrix([[dp1.y, -dp1.x], - [dp2.y, -dp2.x]]) - B = matrix([[dp1.y*p1.x-dp1.x*p1.y], - [dp2.y*p3.x-dp2.x*p3.y]]) - - if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0: + if (Interval(p1.x,p2.x, True).intersection(Interval(p3.x,p4.x, True)).empty() + or Interval(p1.y,p2.y, True).intersection(Interval(p3.y,p4.y, True)).empty()): return None else: - intersection = linalg.solve(A,B) - if (utils.inBetween(p1.x, p2.x, intersection[0,0]) - and utils.inBetween(p3.x, p4.x, intersection[0,0]) - and utils.inBetween(p1.y, p2.y, intersection[1,0]) - and utils.inBetween(p3.y, p4.y, intersection[1,0])): - return Point(intersection[0,0], intersection[1,0]) + dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]] + dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]] + + A = matrix([[dp1.y, -dp1.x], + [dp2.y, -dp2.x]]) + B = matrix([[dp1.y*p1.x-dp1.x*p1.y], + [dp2.y*p3.x-dp2.x*p3.y]]) + + if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0: + return None else: - return None + intersection = linalg.solve(A,B) + if (utils.inBetween(p1.x, p2.x, intersection[0,0]) + and utils.inBetween(p3.x, p4.x, intersection[0,0]) + and utils.inBetween(p1.y, p2.y, intersection[1,0]) + and utils.inBetween(p3.y, p4.y, intersection[1,0])): + return Point(intersection[0,0], intersection[1,0]) + else: + return None + +# TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection class Trajectory: '''Class for trajectories @@ -408,11 +414,11 @@ def xBounds(self): # look for function that does min and max in one pass - return [min(self.getXCoordinates()), max(self.getXCoordinates())] + return Interval(min(self.getXCoordinates()), max(self.getXCoordinates())) def yBounds(self): # look for function that does min and max in one pass - return [min(self.getYCoordinates()), max(self.getYCoordinates())] + return Interval(min(self.getYCoordinates()), max(self.getYCoordinates())) def add(self, traj2): '''Returns a new trajectory of the same length'''