def computeIndicators(self):
'''Computes the collision course cosine only if the cosine is positive'''
collisionCourseDotProducts = {} #[0]*int(self.timeInterval.length())
collisionCourseAngles = {}
velocityAngles = {}
distances = {} #[0]*int(self.timeInterval.length())
speedDifferentials = {}
interactionInstants = []
for instant in self.timeInterval:
deltap = self.roadUser1.getPositionAtInstant(
instant) - self.roadUser2.getPositionAtInstant(instant)
v1 = self.roadUser1.getVelocityAtInstant(instant)
v2 = self.roadUser2.getVelocityAtInstant(instant)
deltav = v2 - v1
velocityAngles[instant] = np.arccos(
moving.Point.dot(v1, v2) / (v1.norm2() * v2.norm2()))
collisionCourseDotProducts[instant] = moving.Point.dot(
deltap, deltav)
distances[instant] = deltap.norm2()
speedDifferentials[instant] = deltav.norm2()
if collisionCourseDotProducts[instant] > 0:
interactionInstants.append(instant)
if distances[instant] != 0 and speedDifferentials[instant] != 0:
collisionCourseAngles[instant] = np.arccos(
collisionCourseDotProducts[instant] /
(distances[instant] * speedDifferentials[instant]))
if len(interactionInstants) >= 2:
self.interactionInterval = moving.TimeInterval(
interactionInstants[0], interactionInstants[-1])
else:
self.interactionInterval = moving.TimeInterval()
self.addIndicator(
indicators.SeverityIndicator(Interaction.indicatorNames[0],
collisionCourseDotProducts))
self.addIndicator(
indicators.SeverityIndicator(Interaction.indicatorNames[1],
collisionCourseAngles))
self.addIndicator(
indicators.SeverityIndicator(Interaction.indicatorNames[2],
distances,
mostSevereIsMax=False))
self.addIndicator(
indicators.SeverityIndicator(Interaction.indicatorNames[4],
velocityAngles))
self.addIndicator(
indicators.SeverityIndicator(Interaction.indicatorNames[5],
speedDifferentials))
# if we have features, compute other indicators
if self.roadUser1.hasFeatures() and self.roadUser2.hasFeatures():
minDistances = {}
for instant in self.timeInterval:
minDistances[instant] = moving.MovingObject.minDistance(
self.roadUser1, self.roadUser2, instant)
self.addIndicator(
indicators.SeverityIndicator(Interaction.indicatorNames[3],
minDistances,
mostSevereIsMax=False))
def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
collisionPoints = []
crossingZones = []
p1 = obj1.getPositionAtInstant(currentInstant)
p2 = obj2.getPositionAtInstant(currentInstant)
if (p1-p2).norm2() <= collisionDistanceThreshold:
collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)]
else:
v1 = obj1.getVelocityAtInstant(currentInstant)
v2 = obj2.getVelocityAtInstant(currentInstant)
intersection = moving.intersection(p1, p1+v1, p2, p2+v2)
if intersection is not None:
dp1 = intersection-p1
dp2 = intersection-p2
dot1 = moving.Point.dot(dp1, v1)
dot2 = moving.Point.dot(dp2, v2)
#print dot1, dot2
# (computeCZ and (dot1 > 0 or dot2 > 0)) or (
if (computeCZ and (dot1 > 0 or dot2 > 0)) or (dot1 > 0 and dot2 > 0): # if the road users are moving towards the intersection or if computing pPET
dist1 = dp1.norm2()
dist2 = dp2.norm2()
s1 = math.copysign(v1.norm2(), dot1)
s2 = math.copysign(v2.norm2(), dot2)
halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
timeInterval1 = moving.TimeInterval(max(0,dist1-halfCollisionDistanceThreshold)/s1, (dist1+halfCollisionDistanceThreshold)/s1)
timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2)
collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2)
if collisionTimeInterval.empty():
if computeCZ:
crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
else:
collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
if debug and intersection is not None:
from matplotlib.pyplot import plot, figure, axis, title
figure()
plot([p1.x, intersection.x], [p1.y, intersection.y], 'r')
plot([p2.x, intersection.x], [p2.y, intersection.y], 'b')
intersection.plot()
obj1.plot('r')
obj2.plot('b')
title('instant {0}'.format(currentInstant))
axis('equal')
return currentInstant, collisionPoints, crossingZones
def __init__(self, name, values, timeInterval = None, maxValue = None):
self.name = name
if timeInterval is None:
self.values = values
instants = sorted(self.values.keys())
if len(instants) > 0:
self.timeInterval = moving.TimeInterval(instants[0], instants[-1])
else:
self.timeInterval = moving.TimeInterval()
else:
assert len(values) == timeInterval.length()
self.timeInterval = timeInterval
self.values = {}
for i in xrange(int(round(self.timeInterval.length()))):
self.values[self.timeInterval[i]] = values[i]
self.maxValue = maxValue
def smoothObject(obj,
newNum,
minLengthParam=0.7,
smoothing=False,
plotResults=True,
halfWidth=3,
_computeVelocities=True,
optimize=True,
create=False):
'''Computes a smoother trajectory for the object
and optionnally smoother velocities
The object should have its features in obj.features
TODO: check whether features are necessary'''
if not obj.hasFeatures():
print(
'Object {} has an empty list of features: please load and add them using obj.setFeatures(features)'
.format(obj.getNum()))
from sys import exit
exit()
featureList = [
i for i, f in enumerate(obj.getFeatures())
if f.length() >= minLengthParam * obj.length()
]
if featureList == []:
featureList.append(
utils.argmaxDict(
{i: f.length()
for i, f in enumerate(obj.getFeatures())}))
create = True
newObjects = []
for featureID in featureList: # featureID should be the index in the list of obj.features
newObjects.append(
smoothObjectTrajectory(obj,
featureID,
newNum,
smoothing=smoothing,
halfWidth=halfWidth,
create=create))
newTranslated = moving.Trajectory()
newInterval = []
for t in obj.getTimeInterval():
xCoord = []
yCoord = []
for i in newObjects:
if i.existsAtInstant(t):
p1 = i.getPositionAtInstant(t)
xCoord.append(p1.x)
yCoord.append(p1.y)
if xCoord != []:
tmp = moving.Point(median(xCoord), median(yCoord))
newInterval.append(t)
newTranslated.addPosition(tmp)
newObj = moving.MovingObject(newNum,
timeInterval=moving.TimeInterval(
min(newInterval), max(newInterval)),
positions=newTranslated)
if _computeVelocities:
tmpTraj = moving.Trajectory()
velocities = computeVelocities(newObj, True, 5)
for i in sorted(velocities.keys()):
tmpTraj.addPosition(velocities[i])
newObj.velocities = tmpTraj
else:
newObj.velocities = obj.velocities
if optimize:
csj1 = sumSquaredJerk(obj, fromPosition=True)
csj2 = sumSquaredJerk(newObj, fromPosition=True)
if csj1 < csj2:
newObj = obj
newObj.velocities = obj.velocities
if _computeVelocities and csj1 >= csj2:
csj3 = sumSquaredJerk(obj, fromPosition=False)
csj4 = sumSquaredJerk(newObj, fromPosition=False)
if csj4 <= csj3:
newObj.velocities = obj.velocities
newObj.featureNumbers = obj.featureNumbers
newObj.features = obj.getFeatures()
newObj.userType = obj.userType
if plotResults:
plt.figure()
plt.title('objects_id = {}'.format(obj.num))
for i in featureList:
obj.getFeature(i).plot('cx-')
obj.plot('rx-')
newObj.plot('gx-')
return newObj