Mercurial > hg > nsaunier > traffic-intelligence

annotate scripts/safety-analysis.py @ 398:3399bd48cb40

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Ajout d'une méthode pour obtenir le nombre de FPS Méthode de capture des trames vidéos plus résistante aux erreur Utilisation d'un dictionnaire pour les fichier de configuration afin de garder le nom des sections
author Jean-Philippe Jodoin <jpjodoin@gmail.com>
date Mon, 29 Jul 2013 13:46:07 -0400
parents 91679eb2ff2c
children 6551a3cf1750
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
328
5e43b7389c25 script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
1 #! /usr/bin/env python
5e43b7389c25 script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
2
357
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
3 import utils, storage, prediction, events, moving
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
4
337
dc2e68e936c7 work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
5 import sys, argparse
328
5e43b7389c25 script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
6
5e43b7389c25 script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
7 import matplotlib.pyplot as plt
5e43b7389c25 script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
8 import numpy as np
5e43b7389c25 script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
9
357
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
10 # todo: very slow if too many predicted trajectories
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
11 # add computation of probality of unsucessful evasive action
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
12
337
dc2e68e936c7 work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
13 parser = argparse.ArgumentParser(description='The program processes indicators for all pairs of road users in the scene')
350
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 348
diff changeset
14 parser.add_argument('--cfg', dest = 'configFilename', help = 'name of the configuration file')
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
15 parser.add_argument('--prediction-method', dest = 'predictionMethod', help = 'prediction method (constant velocity (vector computation), constant velocity, normal adaptation, point set prediction)', choices = ['cvd', 'cv', 'na', 'ps'])
357
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
16 parser.add_argument('--display-cp', dest = 'displayCollisionPoints', help = 'display collision points')
337
dc2e68e936c7 work in progress
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 335
diff changeset
17 args = parser.parse_args()
348
c64a4f889b97 added safety analysis options to feature tracking (with default values for backward compatibility)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 344
diff changeset
18
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
19 params = utils.TrackingParameters()
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
20 params.loadConfigFile(args.configFilename)
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
21
350
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 348
diff changeset
22 # parameters for prediction methods
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
23 if args.predictionMethod:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
24 predictionMethod = args.predictionMethod
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
25 else:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
26 predictionMethod = params.predictionMethod
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
27
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
28 if predictionMethod == 'cvd':
387
91679eb2ff2c cleaning up safety analysis and the new traditional constant velocity method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 359
diff changeset
29 predictionParameters = prediction.CVDirectPredictionParameters()
359
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 357
diff changeset
30 elif predictionMethod == 'cv':
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
31 predictionParameters = prediction.ConstantPredictionParameters(params.maxPredictedSpeed)
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
32 elif predictionMethod == 'na':
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
33 predictionParameters = prediction.NormalAdaptationPredictionParameters(params.maxPredictedSpeed,
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
34 params.nPredictedTrajectories,
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
35 params.maxAcceleration,
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
36 params.maxSteering,
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
37 params.useFeaturesForPrediction)
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
38 elif predictionMethod == 'ps':
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
39 predictionParameters = prediction.PointSetPredictionParameters(params.nPredictedTrajectories,
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
40 params.maxPredictedSpeed)
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
41 else:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
42 print('Prediction method {} is not valid. See help.'.format(predictionMethod))
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
43 sys.exit()
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
44
350
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 348
diff changeset
45 evasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(params.maxPredictedSpeed,
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 348
diff changeset
46 params.nPredictedTrajectories,
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 348
diff changeset
47 params.minAcceleration,
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 348
diff changeset
48 params.maxAcceleration,
7e9ad2d9d79c added new parameters in safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 348
diff changeset
49 params.maxSteering,
351
891858351bcb added new parameter (use features) for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 350
diff changeset
50 params.useFeaturesForPrediction)
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
51
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
52 objects = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'object')
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
53 if params.useFeaturesForPrediction:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
54 features = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'feature') # needed if normal adaptation
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
55 for obj in objects:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
56 obj.setFeatures(features)
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
57
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
58 interactions = events.createInteractions(objects)
357
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
59 for inter in interactions:
343
74e437ab5f11 first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 338
diff changeset
60 inter.computeIndicators()
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
61 inter.computeCrossingsCollisions(predictionParameters, params.collisionDistance, params.predictionTimeHorizon, params.crossingZones)
343
74e437ab5f11 first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 338
diff changeset
62
74e437ab5f11 first version of indicator loading code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 338
diff changeset
63 storage.saveIndicators(params.databaseFilename, interactions)
338
f3aceea2afbb first safety analysis script
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 337
diff changeset
64
357
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
65 if args.displayCollisionPoints:
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
66 plt.figure()
357
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
67 allCollisionPoints = []
352
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
68 for inter in interactions:
72aa44072093 safety analysis script with option for prediction method
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 351
diff changeset
69 for collisionPoints in inter.collisionPoints.values():
357
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
70 allCollisionPoints += collisionPoints
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
71 moving.Point.plotAll(allCollisionPoints)
e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 352
diff changeset
72 plt.axis('equal')
328
5e43b7389c25 script for safety analysis
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
73