# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1467084425 14400
# Node ID 8b74a5176549faa3b0a2a8b8f5f822a0e9eb506a
# Parent  28526917a5837967d9bb14df557df3784ce4cb73
explicitly computed the probabilities for predicted trajectories

diff -r 28526917a583 -r 8b74a5176549 python/prediction.py
--- a/python/prediction.py	Mon Jun 27 16:19:34 2016 -0400
+++ b/python/prediction.py	Mon Jun 27 23:27:05 2016 -0400
@@ -391,8 +391,7 @@
         PredictionParameters.__init__(self, 'constant velocity', maxSpeed)
 
     def generatePredictedTrajectories(self, obj, instant):
-        return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), 
-                                               maxSpeed = self.maxSpeed)]
+        return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), maxSpeed = self.maxSpeed)]
 
 class NormalAdaptationPredictionParameters(PredictionParameters):
     def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
@@ -423,12 +422,14 @@
         else:
             positions = [obj.getPositionAtInstant(instant)]
             velocities = [obj.getVelocityAtInstant(instant)]
+        probability = 1./float(len(positions)*self.nPredictedTrajectories)
         for i in xrange(self.nPredictedTrajectories):
             for initialPosition,initialVelocity in zip(positions, velocities):
                 predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition, 
                                                                               initialVelocity, 
                                                                               self.accelerationDistribution, 
                                                                               self.steeringDistribution, 
+                                                                              probability, 
                                                                               maxSpeed = self.maxSpeed))
         return predictedTrajectories
 
@@ -444,10 +445,9 @@
             features = [f for f in obj.getFeatures() if f.existsAtInstant(instant)]
             positions = [f.getPositionAtInstant(instant) for f in features]
             velocities = [f.getVelocityAtInstant(instant) for f in features]
-            #for i in xrange(self.nPredictedTrajectories):
+            probability = 1./float(len(positions))
             for initialPosition,initialVelocity in zip(positions, velocities):
-                predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, 
-                                                                         maxSpeed = self.maxSpeed))
+                predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, probability = probability, maxSpeed = self.maxSpeed))
             return predictedTrajectories
         else:
             print('Object {} has no features'.format(obj.getNum()))
@@ -480,13 +480,15 @@
         else:
             positions = [obj.getPositionAtInstant(instant)]
             velocities = [obj.getVelocityAtInstant(instant)]
+        probability = 1./float(self.nPredictedTrajectories)
         for i in xrange(self.nPredictedTrajectories):
             for initialPosition,initialVelocity in zip(positions, velocities):
                 predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, 
                                                                          initialVelocity, 
                                                                          moving.NormAngle(self.accelerationDistribution(), 
                                                                                           self.steeringDistribution()), 
-                                                                         maxSpeed = self.maxSpeed))
+                                                                         probability, 
+                                                                         self.maxSpeed))
         return predictedTrajectories