Mercurial Hosting > traffic-intelligence
changeset 617:e7f6ca76b7db
Merge
| author | MohamedGomaa |
|---|---|
| date | Wed, 10 Dec 2014 14:27:03 -0500 |
| parents | 0791b3b55b8f (diff) 8ba4b8ad4c86 (current diff) |
| children | 1a92d28e2d05 |
| files | |
| diffstat | 24 files changed, 1501 insertions(+), 346 deletions(-) [+] |
line wrap: on
line diff
--- a/.hgignore Tue Jul 15 13:25:15 2014 -0400 +++ b/.hgignore Wed Dec 10 14:27:03 2014 -0500 @@ -22,7 +22,6 @@ CMakeCache.txt *.cmake -Makefile install_manifest.txt latex
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Makefile Wed Dec 10 14:27:03 2014 -0500 @@ -0,0 +1,25 @@ + +cexe: + @cd c && make feature-based-tracking + +doc: + doxygen + +clean: + @cd c && make clean + @cd python && rm *.pyc + +install: cexe + @echo "=========================================" + @echo "Installing for Linux" + @echo "=========================================" + @echo "Copying feature-based tracking executable" + @cp bin/feature-based-tracking /usr/local/bin + @echo "=========================================" + @echo "Copying Python scripts" + @cp scripts/* /usr/local/bin + +uninstall: + @echo "Uninstalling for Linux" + rm /usr/local/bin/feature-based-tracking + @cd scripts && ./uninstall-scripts.sh \ No newline at end of file
--- a/c/Motion.cpp Tue Jul 15 13:25:15 2014 -0400 +++ b/c/Motion.cpp Wed Dec 10 14:27:03 2014 -0500 @@ -51,7 +51,7 @@ float disp = 0; for (unsigned int i=0; i<nDisplacements; i++) disp += displacementDistances[nPositions-2-i]; - result = disp < minTotalFeatureDisplacement; + result = disp <= minTotalFeatureDisplacement; } return result; }
--- a/c/feature-based-tracking.cpp Tue Jul 15 13:25:15 2014 -0400 +++ b/c/feature-based-tracking.cpp Wed Dec 10 14:27:03 2014 -0500 @@ -374,6 +374,8 @@ } else if (params.groupFeatures) { cout << "The program groups features" << endl; groupFeatures(params); + } else { + cout << "Main option missing or misspelt" << endl; } return 0;
--- a/python/cvutils.py Tue Jul 15 13:25:15 2014 -0400 +++ b/python/cvutils.py Wed Dec 10 14:27:03 2014 -0500 @@ -121,6 +121,8 @@ def playVideo(filename, firstFrameNum = 0, frameRate = -1, interactive = False, printFrames = True, text = None, rescale = 1.): '''Plays the video''' + windowName = 'frame' + cv2.namedWindow(windowName, cv2.WINDOW_NORMAL) wait = 5 if frameRate > 0: wait = int(round(1000./frameRate)) @@ -140,7 +142,7 @@ frameNum+=1 if text != None: cv2.putText(img, text, (10,50), cv2.cv.CV_FONT_HERSHEY_PLAIN, 1, cvRed) - cvImshow('frame', img, rescale) + cvImshow(windowName, img, rescale) key = cv2.waitKey(wait) cv2.destroyAllWindows() else: @@ -220,6 +222,9 @@ width = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH)) height = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT)) + windowName = 'frame' + #cv2.namedWindow(windowName, cv2.WINDOW_NORMAL) + if undistort: # setup undistortion [map1, map2] = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients) if capture.isOpened(): @@ -259,7 +264,7 @@ objDescription += userTypeNames[obj.userType][0].upper() cv2.putText(img, objDescription, obj.projectedPositions[frameNum-obj.getFirstInstant()].asint().astuple(), cv2.cv.CV_FONT_HERSHEY_PLAIN, 1, cvRed) if not saveAllImages: - cvImshow('frame', img, rescale) + cvImshow(windowName, img, rescale) key = cv2.waitKey() if saveAllImages or saveKey(key): cv2.imwrite('image-{{:0{}}}.png'.format(nZerosFilename).format(frameNum), img) @@ -325,6 +330,78 @@ invMap2[y,x] = res[1] return invMap1, invMap2 + def cameraIntrinsicCalibration(path, checkerBoardSize=[6,7], secondPassSearch=False, display=False): + ''' Camera calibration searches through all the images (jpg or png) located + in _path_ for matches to a checkerboard pattern of size checkboardSize. + These images should all be of the same camera with the same resolution. + + For best results, use an asymetric board and ensure that the image has + very high contrast, including the background. Suitable checkerboard: + http://ftp.isr.ist.utl.pt/pub/roswiki/attachments/camera_calibration(2f)Tutorials(2f)StereoCalibration/check-108.png + + The code below is based off of: + https://opencv-python-tutroals.readthedocs.org/en/latest/py_tutorials/py_calib3d/py_calibration/py_calibration.html + Modified by Paul St-Aubin + ''' + from numpy import zeros, mgrid, float32, savetxt + import glob, os + + # termination criteria + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) + + # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) + objp = zeros((checkerBoardSize[0]*checkerBoardSize[1],3), float32) + objp[:,:2] = mgrid[0:checkerBoardSize[1],0:checkerBoardSize[0]].T.reshape(-1,2) + + # Arrays to store object points and image points from all the images. + objpoints = [] # 3d point in real world space + imgpoints = [] # 2d points in image plane. + + ## Loop throuhg all images in _path_ + images = glob.glob(os.path.join(path,'*.[jJ][pP][gG]'))+glob.glob(os.path.join(path,'*.[jJ][pP][eE][gG]'))+glob.glob(os.path.join(path,'*.[pP][nN][gG]')) + for fname in images: + img = cv2.imread(fname) + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + + # Find the chess board corners + ret, corners = cv2.findChessboardCorners(gray, (checkerBoardSize[1],checkerBoardSize[0]), None) + + # If found, add object points, image points (after refining them) + if ret: + print 'Found pattern in '+fname + + if(secondPassSearch): + corners = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria) + + objpoints.append(objp) + imgpoints.append(corners) + + # Draw and display the corners + if(display): + img = cv2.drawChessboardCorners(img, (checkerBoardSize[1],checkerBoardSize[0]), corners, ret) + if(img): + cv2.imshow('img',img) + cv2.waitKey(0) + + ## Close up image loading and calibrate + cv2.destroyAllWindows() + if len(objpoints) == 0 or len(imgpoints) == 0: + return False + try: + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None) + except NameError: + return False + savetxt('intrinsic-camera.txt', camera_matrix) + return camera_matrix, dist_coeffs + + def undistortImage(img, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1., interpolation=cv2.INTER_LINEAR): + '''Undistorts the image passed in argument''' + width = img.shape[1] + height = img.shape[0] + [map1, map2] = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients) + return cv2.remap(img, map1, map2, interpolation=interpolation) + + def printCvMat(cvmat, out = stdout): '''Prints the cvmat to out''' print('Deprecated, use new interface')
--- a/python/events.py Tue Jul 15 13:25:15 2014 -0400 +++ b/python/events.py Wed Dec 10 14:27:03 2014 -0500 @@ -1,7 +1,7 @@ #! /usr/bin/env python '''Libraries for events Interactions, pedestrian crossing...''' -import sys + import numpy as np from numpy import arccos @@ -9,8 +9,37 @@ import itertools import moving, prediction, indicators, utils +__metaclass__ = type -__metaclass__ = type +def findRoute(prototypes,objects,i,j,noiseEntryNums,noiseExitNums,minSimilarity= 0.3, spatialThreshold=1.0, delta=180): + if i[0] not in noiseEntryNums: + prototypesRoutes= [ x for x in sorted(prototypes.keys()) if i[0]==x[0]] + elif i[1] not in noiseExitNums: + prototypesRoutes=[ x for x in sorted(prototypes.keys()) if i[1]==x[1]] + else: + prototypesRoutes=[x for x in sorted(prototypes.keys())] + routeSim={} + lcss = utils.LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta) + for y in prototypesRoutes: + if y in prototypes.keys(): + prototypesIDs=prototypes[y] + similarity=[] + for x in prototypesIDs: + s=lcss.computeNormalized(objects[j].positions, objects[x].positions) + similarity.append(s) + routeSim[y]=max(similarity) + route=max(routeSim, key=routeSim.get) + if routeSim[route]>=minSimilarity: + return route + else: + return i + +def getRoute(obj,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination=True): + route=(obj.startRouteID,obj.endRouteID) + if useDestination: + if route not in prototypes.keys(): + route= findRoute(prototypes,objects,route,obj.getNum(),noiseEntryNums,noiseExitNums) + return route class Interaction(moving.STObject): '''Class for an interaction between two road users @@ -62,9 +91,16 @@ def __init__(self, num = None, timeInterval = None, roaduserNum1 = None, roaduserNum2 = None, roadUser1 = None, roadUser2 = None, categoryNum = None): moving.STObject.__init__(self, num, timeInterval) - self.roadUserNumbers = set([roaduserNum1, roaduserNum2]) + if timeInterval == None and roadUser1 != None and roadUser2 != None: + self.timeInterval = roadUser1.commonTimeInterval(roadUser2) self.roadUser1 = roadUser1 self.roadUser2 = roadUser2 + if roaduserNum1 != None and roaduserNum2 != None: + self.roadUserNumbers = set([roaduserNum1, roaduserNum2]) + elif roadUser1 != None and roadUser2 != None: + self.roadUserNumbers = set(roadUser1.getNum(), roadUser2.getNum()) + else: + self.roadUserNumbers = None self.categoryNum = categoryNum self.indicators = {} self.interactionInterval = None @@ -111,76 +147,37 @@ self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[5], speedDifferentials)) # if we have features, compute other indicators - if self.roadUser1.features != None and self.roadUser2.features != None: + if len(self.roadUser1.features) != 0 and len(self.roadUser2.features) != 0: minDistance={} for instant in self.timeInterval: minDistance[instant] = moving.MovingObject.minDistance(self.roadUser1, self.roadUser2, instant) self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[3], minDistance)) - def computeCrossingsCollisions(self, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None): + def computeCrossingsCollisions(self, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): '''Computes all crossing and collision points at each common instant for two road users. ''' self.collisionPoints={} self.crossingZones={} TTCs = {} + if usePrototypes: + route1= getRoute(self.roadUser1,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination) + route2= getRoute(self.roadUser2,prototypes,objects,noiseEntryNums,noiseExitNums,useDestination) if timeInterval: commonTimeInterval = timeInterval else: commonTimeInterval = self.timeInterval - for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors - self.collisionPoints[i], self.crossingZones[i] = predictionParameters.computeCrossingsCollisionsAtInstant(i, self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) - if len(self.collisionPoints[i]) > 0: - TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.collisionPoints[i]) + self.collisionPoints, self.crossingZones = prediction.computeCrossingsCollisions(predictionParameters, self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, commonTimeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step) + for i, cp in self.collisionPoints.iteritems(): + TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(cp) # add probability of collision, and probability of successful evasive action self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[7], TTCs)) if computeCZ: pPETs = {} - for i in list(commonTimeInterval)[:-1]: - if len(self.crossingZones[i]) > 0: - pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.crossingZones[i]) + for i, cz in self.crossingZones.iteritems(): + pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(cz) self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[9], pPETs)) - - def computeCrossingsCollisionsPrototype(self, predictionParameters, collisionDistanceThreshold, timeHorizon, prototypes,nMatching,objects,minSimilarity=0.5,index=None,mostMatched=None, computeCZ = False, debug = False, timeInterval = None,acceptPartialLength=30): - '''Computes all crossing and collision points at each common instant for two road users. ''' - self.collisionPoints={} - self.crossingZones={} - TTCs = {} - route1=(self.roadUser1.startRouteID,self.roadUser1.endRouteID) - route2=(self.roadUser2.startRouteID,self.roadUser2.endRouteID) - - if timeInterval: - commonTimeInterval = timeInterval - else: - commonTimeInterval = self.timeInterval - for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors - sys.path.append("D:/behaviourAnalysis/libs") - import trajLearning - if i-self.roadUser1.timeInterval.first >= acceptPartialLength and i-self.roadUser2.timeInterval.first >= acceptPartialLength: - inter1=moving.Interval(self.roadUser1.timeInterval.first,i) - inter2=moving.Interval(self.roadUser2.timeInterval.first,i) - partialObjPositions1= self.roadUser1.getObjectInTimeInterval(inter1).positions - partialObjPositions2= self.roadUser2.getObjectInTimeInterval(inter2).positions - prototypeTrajectories1=trajLearning.findPrototypes(prototypes,nMatching,objects,route1,partialObjPositions1,minSimilarity,index,mostMatched) - prototypeTrajectories2=trajLearning.findPrototypes(prototypes,nMatching,objects,route2,partialObjPositions2,minSimilarity,index,mostMatched) - if prototypeTrajectories1=={}: - print self.roadUser1.num, 'is abnormal at instant', str(i) - elif prototypeTrajectories2=={}: - print self.roadUser2.num, 'is abnormal at instant', str(i) - else: - self.collisionPoints[i], self.crossingZones[i] = predictionParameters.computeCrossingsCollisionsAtInstant(i, self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2) - if len(self.collisionPoints[i]) > 0: - TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.collisionPoints[i]) - # add probability of collision, and probability of successful evasive action - self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[7], TTCs)) - - if computeCZ: - pPETs = {} - for i in list(commonTimeInterval)[:-1]: - if i in self.crossingZones.keys() and len(self.crossingZones[i]) > 0: - pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.crossingZones[i]) - self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[9], pPETs)) - + def addVideoFilename(self,videoFilename): self.videoFilename= videoFilename
--- a/python/metadata.py Tue Jul 15 13:25:15 2014 -0400 +++ b/python/metadata.py Wed Dec 10 14:27:03 2014 -0500 @@ -13,7 +13,7 @@ class Site(Base): __tablename__ = 'sites' - id = Column(Integer, primary_key=True) + idx = Column(Integer, primary_key=True) name = Column(String) # same as path, relative to the database position description = Column(String) # longer names, eg intersection of road1 and road2 xcoordinate = Column(Float) # ideally moving.Point, but needs to be @@ -34,13 +34,13 @@ * changing road configuration, geometry, signalization, etc. ex: sunny, rainy, before counter-measure, after counter-measure''' __tablename__ = 'environmental_factors' - id = Column(Integer, primary_key=True) + idx = Column(Integer, primary_key=True) startTime = Column(DateTime) endTime = Column(DateTime) description = Column(String) # eg sunny, before, after - siteId = Column(Integer, ForeignKey('sites.id')) + siteIdx = Column(Integer, ForeignKey('sites.idx')) - site = relationship("Site", backref=backref('environmental_factors', order_by = id)) + site = relationship("Site", backref=backref('environmental_factors', order_by = idx)) def __init__(self, startTime, endTime, description, site): 'startTime is passed as string in utils.datetimeFormat, eg 2011-06-22 10:00:39' @@ -51,15 +51,15 @@ class CameraView(Base): __tablename__ = 'camera_views' - id = Column(Integer, primary_key=True) + idx = Column(Integer, primary_key=True) frameRate = Column(Float) homographyFilename = Column(String) # path to homograph filename, relative to the site name cameraCalibrationFilename = Column(String) # path to full camera calibration, relative to the site name - siteId = Column(Integer, ForeignKey('sites.id')) + siteIdx = Column(Integer, ForeignKey('sites.idx')) homographyDistanceUnit = Column(String, default = 'm') # make sure it is default in the database configurationFilename = Column(String) # path to configuration .cfg file, relative to site name - site = relationship("Site", backref=backref('camera_views', order_by = id)) + site = relationship("Site", backref=backref('camera_views', order_by = idx)) def __init__(self, frameRate, homographyFilename, cameraCalibrationFilename, site, configurationFilename): self.frameRate = frameRate @@ -73,19 +73,44 @@ else: return self.homographyFilename +class Alignment(Base): + __tablename__ = 'alignments' + idx = Column(Integer, primary_key=True) + cameraViewIdx = Column(Integer, ForeignKey('camera_views.idx')) + + cameraView = relationship("CameraView", backref=backref('alignments', order_by = idx)) + + def __init__(self, cameraView): + self.cameraView = cameraView + +class Point(Base): + __tablename__ = 'points' + alignmentIdx = Column(Integer, ForeignKey('alignments.idx'), primary_key=True) + index = Column(Integer, primary_key=True) # order of points in this alignment + x = Column(Float) + y = Column(Float) + + alignment = relationship("Alignment", backref=backref('points', order_by = index)) + + def __init__(self, alignmentIdx, index, x, y): + self.alignmentIdx = alignmentIdx + self.index = index + self.x = x + self.y = y + class VideoSequence(Base): __tablename__ = 'video_sequences' - id = Column(Integer, primary_key=True) + idx = Column(Integer, primary_key=True) name = Column(String) # path relative to the the site name startTime = Column(DateTime) duration = Column(Float) # video sequence duration durationUnit = Column(String, default = 's') - siteId = Column(Integer, ForeignKey('sites.id')) - cameraViewId = Column(Integer, ForeignKey('camera_views.id')) + siteIdx = Column(Integer, ForeignKey('sites.idx')) + cameraViewIdx = Column(Integer, ForeignKey('camera_views.idx')) configurationFilename = Column(String) - site = relationship("Site", backref=backref('video_sequences', order_by = id)) - cameraView = relationship("CameraView", backref=backref('video_sequences', order_by = id)) + site = relationship("Site", backref=backref('video_sequences', order_by = idx)) + cameraView = relationship("CameraView", backref=backref('video_sequences', order_by = idx)) def __init__(self, name, startTime, duration, site, cameraView, configurationFilename = None): 'startTime is passed as string in utils.datetimeFormat, eg 2011-06-22 10:00:39'
--- a/python/ml.py Tue Jul 15 13:25:15 2014 -0400 +++ b/python/ml.py Wed Dec 10 14:27:03 2014 -0500 @@ -107,3 +107,14 @@ centroids,distortion = kmeans(features,k, iter) code,distance = vq(features,centroids) # code starting from 0 (represent first cluster) to k-1 (last cluster) return code,sigma + +def motionPatterLearning(objects, maxDistance): + ''' + Option to use only the (n?) longest features per object instead of all for speed up + TODO''' + pass + +def prototypeCluster(): + ''' + TODO''' + pass
--- a/python/moving.py Tue Jul 15 13:25:15 2014 -0400 +++ b/python/moving.py Wed Dec 10 14:27:03 2014 -0500 @@ -180,7 +180,19 @@ def __neg__(self): return Point(-self.x, -self.y) + def __getitem__(self, i): + if i == 0: + return self.x + elif i == 1: + return self.y + else: + raise IndexError() + + def orthogonal(self): + return Point(self.y, -self.x) + def multiply(self, alpha): + 'Warning, returns a new Point' return Point(self.x*alpha, self.y*alpha) def plot(self, options = 'o', **kwargs): @@ -189,7 +201,7 @@ def norm2Squared(self): '''2-norm distance (Euclidean distance)''' - return self.x*self.x+self.y*self.y + return self.x**2+self.y**2 def norm2(self): '''2-norm distance (Euclidean distance)''' @@ -244,6 +256,10 @@ return (counter%2 == 1); @staticmethod + def fromList(p): + return Point(p[0], p[1]) + + @staticmethod def dot(p1, p2): 'Scalar product' return p1.x*p2.x+p1.y*p2.y @@ -300,6 +316,10 @@ ttc = None return ttc + @staticmethod + def midPoint(p1, p2): + 'Returns the middle of the segment [p1, p2]' + return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y) if shapelyAvailable: def pointsInPolygon(points, polygon): @@ -307,6 +327,144 @@ prepared_polygon = prep(polygon) return filter(prepared_polygon.contains, points) +# Functions for coordinate transformation +# From Paul St-Aubin's PVA tools +def subsec_spline_dist(splines): + ''' Prepare list of spline subsegments from a spline list. + + Output: + ======= + ss_spline_d[spline #][mode][station] + + where: + mode=0: incremental distance + mode=1: cumulative distance + mode=2: cumulative distance with trailing distance + ''' + + from numpy import zeros + ss_spline_d = [] + #Prepare subsegment distances + for spline in range(len(splines)): + ss_spline_d.append([[],[],[]]) + ss_spline_d[spline][0] = zeros(len(splines[spline])-1) #Incremental distance + ss_spline_d[spline][1] = zeros(len(splines[spline])-1) #Cumulative distance + ss_spline_d[spline][2] = zeros(len(splines[spline])) #Cumulative distance with trailing distance + for spline_p in range(len(splines[spline])): + if spline_p > (len(splines[spline]) - 2): + break + ss_spline_d[spline][0][spline_p] = utils.pointDistanceL2(splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][(spline_p+1)][0],splines[spline][(spline_p+1)][1]) + ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) + ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p]) + + ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1] + + return ss_spline_d + +def ppldb2p(qx,qy, p0x,p0y, p1x,p1y): + ''' Point-projection (Q) on line defined by 2 points (P0,P1). + http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf + ''' + if(p0x == p1x and p0y == p1y): + return None + try: + #Approximate slope singularity by giving some slope roundoff; account for roundoff error + if(round(p0x, 10) == round(p1x, 10)): + p1x += 0.0000000001 + if(round(p0y, 10) == round(p1y, 10)): + p1y += 0.0000000001 + #make the calculation + Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x)) + X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x) + except ZeroDivisionError: + print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') + print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) + import pdb; pdb.set_trace() + return Point(X,Y) + +def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): + ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). A spline is a list of points (class Point), most likely a trajectory. + + Output: + ======= + [spline index, + subsegment leading point index, + snapped point, + subsegment distance, + spline distance, + orthogonal point offset] + ''' + minOffsetY = float('inf') + #For each spline + for spline in range(len(splines)): + #For each spline point index + for spline_p in range(len(splines[spline])-1): + #Get closest point on spline + closestPoint = ppldb2p(p.x,p.y,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1]) + if closestPoint == None: + print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) + return None + # check if the + if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], closestPoint.y): + offsetY = Point.distanceNorm2(closestPoint, p) + if offsetY < minOffsetY: + minOffsetY = offsetY + snappedSpline = spline + snappedSplineLeadingPoint = spline_p + snappedPoint = Point(closestPoint.x, closestPoint.y) + #Jump loop if significantly close + if offsetY < goodEnoughSplineDistance: + break + #Get sub-segment distance + if minOffsetY != float('inf'): + subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint]) + #Get cumulative alignment distance (total segment distance) + splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance + orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal() + offsetVector = p-snappedPoint + if Point.dot(orthogonalSplineVector, offsetVector) < 0: + minOffsetY = -minOffsetY + return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] + else: + return None + +def getXYfromSY(s, y, splineNum, splines, mode = 0): + ''' Find X,Y coordinate from S,Y data. + if mode = 0 : return Snapped X,Y + if mode !=0 : return Real X,Y + ''' + + #(buckle in, it gets ugly from here on out) + ss_spline_d = subsec_spline_dist(splines) + + #Find subsegment + snapped_x = None + snapped_y = None + for spline_ss_index in range(len(ss_spline_d[splineNum][1])): + if(s < ss_spline_d[splineNum][1][spline_ss_index]): + ss_value = s - ss_spline_d[splineNum][1][spline_ss_index-1] + #Get normal vector and then snap + vector_l_x = (splines[splineNum][spline_ss_index][0] - splines[splineNum][spline_ss_index-1][0]) + vector_l_y = (splines[splineNum][spline_ss_index][1] - splines[splineNum][spline_ss_index-1][1]) + magnitude = sqrt(vector_l_x**2 + vector_l_y**2) + n_vector_x = vector_l_x/magnitude + n_vector_y = vector_l_y/magnitude + snapped_x = splines[splineNum][spline_ss_index-1][0] + ss_value*n_vector_x + snapped_y = splines[splineNum][spline_ss_index-1][1] + ss_value*n_vector_y + + #Real values (including orthogonal projection of y)) + real_x = snapped_x - y*n_vector_y + real_y = snapped_y + y*n_vector_x + break + + if mode == 0 or (not snapped_x): + if(not snapped_x): + snapped_x = splines[splineNum][-1][0] + snapped_y = splines[splineNum][-1][1] + return [snapped_x,snapped_y] + else: + return [real_x,real_y] + class NormAngle(object): '''Alternate encoding of a point, by its norm and orientation''' @@ -372,21 +530,34 @@ def similar(f1, f2, maxDistance2, maxDeltavelocity2): return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 -def intersection(p1, p2, dp1, dp2): - '''Returns the intersection point between the two lines - defined by the respective vectors (dp) and origin points (p)''' - from numpy import matrix - from numpy.linalg import linalg - A = matrix([[dp1.y, -dp1.x], - [dp2.y, -dp2.x]]) - B = matrix([[dp1.y*p1.x-dp1.x*p1.y], - [dp2.y*p2.x-dp2.x*p2.y]]) - - if linalg.det(A) == 0: +def intersection(p1, p2, p3, p4): + ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4 + http://paulbourke.net/geometry/pointlineplane/''' + dp12 = p2-p1 + dp34 = p4-p3 + #det = (p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y) + det = dp34.y*dp12.x-dp34.x*dp12.y + if det == 0: return None else: - intersection = linalg.solve(A,B) - return Point(intersection[0,0], intersection[1,0]) + ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det + return p1+dp12.multiply(ua) + +# def intersection(p1, p2, dp1, dp2): +# '''Returns the intersection point between the two lines +# defined by the respective vectors (dp) and origin points (p)''' +# from numpy import matrix +# from numpy.linalg import linalg +# A = matrix([[dp1.y, -dp1.x], +# [dp2.y, -dp2.x]]) +# B = matrix([[dp1.y*p1.x-dp1.x*p1.y], +# [dp2.y*p2.x-dp2.x*p2.y]]) + +# if linalg.det(A) == 0: +# return None +# else: +# intersection = linalg.solve(A,B) +# return Point(intersection[0,0], intersection[1,0]) def segmentIntersection(p1, p2, p3, p4): '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' @@ -394,9 +565,7 @@ if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()): return None else: - dp1 = p2-p1 - dp3 = p4-p3 - inter = intersection(p1, p3, dp1, dp3) + inter = intersection(p1, p2, p3, p4) if (inter != None and utils.inBetween(p1.x, p2.x, inter.x) and utils.inBetween(p3.x, p4.x, inter.x) @@ -406,8 +575,6 @@ else: return None -# TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection - class Trajectory(object): '''Class for trajectories: temporal sequence of positions @@ -420,6 +587,16 @@ self.positions = [[],[]] @staticmethod + def generate(p, v, nPoints): + t = Trajectory() + p0 = Point(p.x, p.y) + t.addPosition(p0) + for i in xrange(nPoints-1): + p0 += v + t.addPosition(p0) + return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints]) + + @staticmethod def load(line1, line2): return Trajectory([[float(n) for n in line1.split(' ')], [float(n) for n in line2.split(' ')]]) @@ -427,8 +604,12 @@ @staticmethod def fromPointList(points): t = Trajectory() - for p in points: - t.addPosition(p) + if isinstance(points[0], list) or isinstance(points[0], tuple): + for p in points: + t.addPositionXY(p[0],p[1]) + else: + for p in points: + t.addPosition(p) return t def __len__(self): @@ -437,6 +618,9 @@ def length(self): return self.__len__() + def empty(self): + return self.__len__() == 0 + def __getitem__(self, i): if isinstance(i, int): return Point(self.positions[0][i], self.positions[1][i]) @@ -542,10 +726,12 @@ return Trajectory([[a-b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())], [a-b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]]) - def differentiate(self): + def differentiate(self, doubleLastPosition = False): diff = Trajectory() for i in xrange(1, self.length()): diff.addPosition(self[i]-self[i-1]) + if doubleLastPosition: + diff.addPosition(diff[-1]) return diff def norm(self): @@ -556,12 +742,40 @@ from numpy import hypot return hypot(self.positions[0], self.positions[1]) - def cumulatedDisplacement(self): - 'Returns the sum of the distances between each successive point' - displacement = 0 + # def cumulatedDisplacement(self): + # 'Returns the sum of the distances between each successive point' + # displacement = 0 + # for i in xrange(self.length()-1): + # displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1)) + # return displacement + + def computeCumulativeDistances(self): + '''Computes the distance from each point to the next and the cumulative distance up to the point + Can be accessed through getDistance(idx) and getCumulativeDistance(idx)''' + self.distances = [] + self.cumulativeDistances = [0.] + p1 = self[0] + cumulativeDistance = 0. for i in xrange(self.length()-1): - displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1)) - return displacement + p2 = self[i+1] + self.distances.append(Point.distanceNorm2(p1,p2)) + cumulativeDistance += self.distances[-1] + self.cumulativeDistances.append(cumulativeDistance) + p1 = p2 + + def getDistance(self,i): + '''Return the distance between points i and i+1''' + if i < self.length()-1: + return self.distances[i] + else: + print('Index {} beyond trajectory length {}-1'.format(i, self.length())) + + def getCumulativeDistance(self, i): + '''Return the cumulative distance between the beginning and point i''' + if i < self.length(): + return self.cumulativeDistances[i] + else: + print('Index {} beyond trajectory length {}'.format(i, self.length())) def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity) @@ -576,7 +790,7 @@ return False def wiggliness(self): - return self.cumulatedDisplacement()/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))) + return self.getCumulativeDistance(self.length()-1)/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))) def getIntersections(self, p1, p2): '''Returns a list of the indices at which the trajectory @@ -633,11 +847,13 @@ lateral coordiante is stored as second coordinate''' def __init__(self, S = None, Y = None, lanes = None): - if S == None or Y == None: + if S == None or Y == None or len(S) != len(Y): self.positions = [[],[]] + if S != None and Y != None and len(S) != len(Y): + print("S and Y coordinates of different lengths\nInitializing to empty lists") else: self.positions = [S,Y] - if lanes == None: + if lanes == None or len(lanes) != self.length(): self.lanes = [] else: self.lanes = lanes @@ -655,15 +871,30 @@ def getLanes(self): return self.lanes - def addPosition(self, s, y, lane): + def addPositionSYL(self, s, y, lane): self.addPositionXY(s,y) self.lanes.append(lane) + def addPosition(self, p): + 'Adds position in the point format for curvilinear of list with 3 values' + self.addPositionSYL(p[0], p[1], p[2]) + def setPosition(self, i, s, y, lane): self.setPositionXY(i, s, y) if i < self.__len__(): self.lanes[i] = lane + def differentiate(self, doubleLastPosition = False): + diff = CurvilinearTrajectory() + p1 = self[0] + for i in xrange(1, self.length()): + p2 = self[i] + diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], p1[2]) + p1=p2 + if doubleLastPosition and self.length() > 1: + diff.addPosition(diff[-1]) + return diff + def getIntersections(self, S1, lane = None): '''Returns a list of the indices at which the trajectory goes past the curvilinear coordinate S1 @@ -693,7 +924,8 @@ class MovingObject(STObject): '''Class for moving objects: a spatio-temporal object - with a trajectory and a geometry (constant volume over time) and a usertype (e.g. road user) coded as a number (see + with a trajectory and a geometry (constant volume over time) + and a usertype (e.g. road user) coded as a number (see userTypeNames) ''' def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown']): @@ -705,6 +937,11 @@ self.features = [] # compute bounding polygon from trajectory + @staticmethod + def generate(p, v, timeInterval): + positions, velocities = Trajectory.generate(p, v, int(timeInterval.length())) + return MovingObject(timeInterval = timeInterval, positions = positions, velocities = velocities) + def getObjectInTimeInterval(self, inter): '''Returns a new object extracted from self, restricted to time interval inter''' @@ -793,30 +1030,23 @@ plot(list(self.getTimeInterval()), speeds) @staticmethod - def distances(obj1, obj2, instant): + def distances(obj1, obj2, instant1, _instant2 = None): from scipy.spatial.distance import cdist - positions1 = [f.getPositionAtInstant(instant).astuple() for f in obj1.features if f.existsAtInstant(instant)] - positions2 = [f.getPositionAtInstant(instant).astuple() for f in obj2.features if f.existsAtInstant(instant)] - return cdist(positions1, positions2, metric = 'euclidean') - - @staticmethod - def minDistance(obj1, obj2, instant): - return MovingObject.distances(obj1, obj2, instant).min() - - @staticmethod - def distances2(obj1, obj2, instant1,instant2): - from scipy.spatial.distance import cdist + if _instant2 == None: + instant2 = instant1 + else: + instant2 = _instant2 positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)] positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)] return cdist(positions1, positions2, metric = 'euclidean') @staticmethod - def minDistance2(obj1, obj2, instant1,instant2): - return MovingObject.distances2(obj1, obj2, instant1,instant2).min() + def minDistance(obj1, obj2, instant1, instant2 = None): + return MovingObject.distances(obj1, obj2, instant1, instant2).min() @staticmethod - def maxDistance(obj1, obj2, instant): - return MovingObject.distances(obj1, obj2, instant).max() + def maxDistance(obj1, obj2, instant, instant2 = None): + return MovingObject.distances(obj1, obj2, instant1, instant2).max() def maxSize(self): '''Returns the max distance between features @@ -834,9 +1064,9 @@ print('Load features to compute a maximum size') return None - def setRoutes(self,startCode,endCode): - self.startRouteID=startCode - self.endRouteID=endCode + def setRoutes(self, startRouteID, endRouteID): + self.startRouteID = startRouteID + self.endRouteID = endRouteID def getInstantsCrossingLane(self, p1, p2): '''Returns the instant(s) @@ -850,13 +1080,90 @@ at constant speed''' return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) + def projectCurvilinear(self, alignments, ln_mv_av_win=3): + ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()' + (curvilinearPositions instance) which holds information about the + curvilinear coordinates using alignment metadata. + From Paul St-Aubin's PVA tools + ====== + + Input: + ====== + alignments = a list of alignments, where each alignment is a list of + points (class Point). + ln_mv_av_win = moving average window (in points) in which to smooth + lane changes. As per tools_math.cat_mvgavg(), this term + is a search *radius* around the center of the window. + + ''' + + self.curvilinearPositions = CurvilinearTrajectory() + + #For each point + for i in xrange(int(self.length())): + result = getSYfromXY(self.getPositionAt(i), alignments) + + # Error handling + if(result == None): + print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i))) + else: + [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result + self.curvilinearPositions.addPositionSYL(S, Y, align) + + ## Go back through points and correct lane + #Run through objects looking for outlier point + smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win) + ## Recalculate projected point to new lane + lanes = self.curvilinearPositions.getLanes() + if(lanes != smoothed_lanes): + for i in xrange(len(lanes)): + if(lanes[i] != smoothed_lanes[i]): + result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]]) + + # Error handling + if(result == None): + ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment. + print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i))) + else: + [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result + self.curvilinearPositions.setPosition(i, S, Y, align) + + def computeSmoothTrajectory(self, minCommonIntervalLength): + '''Computes the trajectory as the mean of all features + if a feature exists, its position is + + Warning work in progress + TODO? not use the first/last 1-.. positions''' + from numpy import array, median + nFeatures = len(self.features) + if nFeatures == 0: + print('Empty object features\nCannot compute smooth trajectory') + else: + # compute the relative position vectors + relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i + for i in xrange(nFeatures): + for j in xrange(i): + fi = self.features[i] + fj = self.features[j] + inter = fi.commonTimeInterval(fj) + if inter.length() >= minCommonIntervalLength: + xi = array(fi.getXCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)]) + yi = array(fi.getYCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)]) + xj = array(fj.getXCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)]) + yj = array(fj.getYCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)]) + relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi)) + relativePositions[(j,i)] = -relativePositions[(i,j)] + ### # User Type Classification ### def classifyUserTypeSpeedMotorized(self, threshold, aggregationFunc = median, ignoreNInstantsAtEnds = 0): '''Classifies slow and fast road users slow: non-motorized -> pedestrians - fast: motorized -> cars''' + fast: motorized -> cars + + aggregationFunc can be any function that can be applied to a vector of speeds, including percentile: + aggregationFunc = lambda x: percentile(x, percentileFactor) # where percentileFactor is 85 for 85th percentile''' if ignoreNInstantsAtEnds > 0: speeds = self.getSpeeds()[ignoreNInstantsAtEnds:-ignoreNInstantsAtEnds] else: @@ -880,7 +1187,9 @@ return x''' if not hasattr(self, aggregatedSpeed): self.aggregatedSpeed = aggregationFunc(self.getSpeeds()) - userTypeProbabilities = {userType2Num[userTypename]: speedProbabilities[userTypename](self.aggregatedSpeed) for userTypename in speedProbabilities} + userTypeProbabilities = {} + for userTypename in speedProbabilities: + userTypeProbabilities[userType2Num[userTypename]] = speedProbabilities[userTypename](self.aggregatedSpeed) self.setUserType(utils.argmaxDict(userTypeProbabilities)) return userTypeProbabilities @@ -958,7 +1267,6 @@ # what to do: threshold for most common type? self.setUserType() return possibleUserTypes - @staticmethod def collisionCourseDotProduct(movingObject1, movingObject2, instant): 'A positive result indicates that the road users are getting closer' @@ -972,6 +1280,24 @@ return Point.cosine(movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant), #deltap movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant)) #deltav + +################## +# Annotations +################## + +class BBAnnotation(MovingObject): + '''Class for : a series of ground truth annotations using bounding boxes + Its center is the center of the containing shape + ''' + + def __init__(self, num = None, timeInterval = None, topPositions = None, bottomPositions = None, userType = userType2Num['unknown']): + super(BBAnnotation, self).__init__(num, timeInterval, Trajectory(), userType = userType) + self.topPositions = topPositions.getPositions() + self.bottomPositions = bottomPositions.getPositions() + for i in xrange(int(topPositions.length())): + self.positions.addPosition((topPositions.getPositionAt(i) + bottomPositions.getPositionAt(i)).multiply(0.5)) + + def plotRoadUsers(objects, colors): '''Colors is a PlottingPropertyValues instance''' from matplotlib.pyplot import figure, axis
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/python/objectsmoothing.py Wed Dec 10 14:27:03 2014 -0500 @@ -0,0 +1,234 @@ +import storage, moving, utils +from math import * #atan2,asin,degrees,sin,cos,pi +import numpy as np + +import matplotlib.pyplot as plt + +def findNearest(feat, featureSet,t,reverse=True): + dist={} + for f in featureSet: + if reverse: + dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t+1),f.getPositionAtInstant(t)) + else: + dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t-1),f.getPositionAtInstant(t)) + return min(dist, key=dist.get) # = utils.argmaxDict(dist) + +def getFeatures(obj,features,featureID): + #longestFeature = utils.argmaxDict({f:f.length() for i,f in enumerate(obj.features)}) + t1,t3 = features[featureID].getFirstInstant(), features[featureID].getLastInstant() + listFeatures=[[features[featureID],t1,t3,moving.Point(0,0)]] + # find the features to fill in the beginning of the object existence + currentFeature = features[featureID] + while t1!=obj.getFirstInstant(): + delta=listFeatures[-1][3] + featureSet = [f for f in obj.features if f.existsAtInstant(t1-1)] + feat = findNearest(currentFeature,featureSet,t1-1,reverse=True) + if feat.existsAtInstant(t1): + listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1))+delta]) + else: + listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1-1))+delta]) + currentFeature = feat + t1= feat.getFirstInstant() + # find the features to fill in the end of the object existence + delta=moving.Point(0,0) + currentFeature = features[featureID] + while t3!= obj.getLastInstant(): + featureSet = [f for f in obj.features if f.existsAtInstant(t3+1)] + feat = findNearest(currentFeature,featureSet,t3+1,reverse=False) + if feat.existsAtInstant(t3): + listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3))+delta]) + else: + listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3+1))+delta]) + currentFeature = feat + t3= feat.getLastInstant() + delta=listFeatures[-1][3] + return listFeatures + +def buildFeature(obj,features,featureID,num=1): + listFeatures= getFeatures(obj,features,featureID) + tmp={} + delta={} + for i in listFeatures: + for t in xrange(i[1],i[2]+1): + tmp[t]=[i[0],i[3]] + newTraj = moving.Trajectory() + + for instant in obj.getTimeInterval(): + newTraj.addPosition(tmp[instant][0].getPositionAtInstant(instant)+tmp[instant][1]) + newFeature= moving.MovingObject(num,timeInterval=obj.getTimeInterval(),positions=newTraj) + return newFeature + +def getBearing(p1,p2,p3): + angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x)) + bearing1 = (90 - angle) % 360 + angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x)) + bearing2 = (90 - angle2) % 360 + dist= moving.Point.distanceNorm2(p1, p2) + return [dist,bearing1,bearing2,bearing2-bearing1] + +#Quantitative analysis "CSJ" functions +def computeVelocities (object,smoothing=True,halfWidth=3): #compute velocities from positions + velocities={} + for i in list(object.timeInterval)[:-1]: + p1= object.getPositionAtInstant(i) + p2= object.getPositionAtInstant(i+1) + velocities[i]=p2-p1 + velocities[object.getLastInstant()]= velocities[object.getLastInstant()-1] # duplicate last point + if smoothing: + velX= [velocities[y].aslist()[0] for y in sorted(velocities.keys())] + velY= [velocities[y].aslist()[1] for y in sorted(velocities.keys())] + v1= list(utils.filterMovingWindow(velX, halfWidth)) + v2= list(utils.filterMovingWindow(velY, halfWidth)) + smoothedVelocity={} + for t,i in enumerate(sorted(velocities.keys())): + smoothedVelocity[i]=moving.Point(v1[t], v2[t]) + velocities=smoothedVelocity + return velocities + +def computeAcceleration (object,fromPosition=True): + acceleration={} + if fromPosition: + velocities=computeVelocities(object,False,1) + for i in sorted (velocities.keys()): + if i != sorted (velocities.keys())[-1]: + acceleration[i]= velocities[i+1]-velocities[i] + else: + for i in list(object.timeInterval)[:-1]: + v1= object.getVelocityAtInstant(i) + v2= object.getVelocityAtInstant(i+1) + acceleration[i]= v2-v1 + return acceleration + +def computeJerk (object,fromPosition=True): + jerk={} + acceleration=computeAcceleration (object,fromPosition=fromPosition) + for i in sorted (acceleration.keys()): + if i != sorted (acceleration.keys())[-1]: + jerk[i]= (acceleration[i+1]-acceleration[i]).norm2() + return jerk + +def sumSquaredJerk (object,fromPosition=True): + jerk= computeJerk (object,fromPosition=fromPosition) + t=0 + for i in sorted(jerk.keys()): + t+= jerk[i]* jerk[i] + return t + +def smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=False,halfWidth=3,create=False): + results=[] + bearing={} + if create: + feature= buildFeature(obj,features,featureID,num=1) + else: + feature=features[featureID] + for t in feature.getTimeInterval(): + p1= feature.getPositionAtInstant(t) + p2= obj.getPositionAtInstant(t) + if t!=feature.getLastInstant(): + p3= feature.getPositionAtInstant(t+1) + else: + p1= feature.getPositionAtInstant(t-1) + p3= feature.getPositionAtInstant(t) + bearing[t]= getBearing(p1,p2,p3)[1] + results.append(getBearing(p1,p2,p3)) + + + medianResults=np.median(results,0) + dist= medianResults[0] + angle= medianResults[3] + + for i in sorted(bearing.keys()): + bearing[i]= bearing[i]+angle + + if smoothing: + bearingInput=[] + for i in sorted(bearing.keys()): + bearingInput.append(bearing[i]) + import utils + bearingOut=utils.filterMovingWindow(bearingInput, halfWidth) + for t,i in enumerate(sorted(bearing.keys())): + bearing[i]=bearingOut[t] + + #solve a smoothing problem in case of big drop in computing bearing (0,360) + for t,i in enumerate(sorted(bearing.keys())): + if i!= max(bearing.keys()) and abs(bearingInput[t] - bearingInput[t+1])>=340: + for x in xrange(max(i-halfWidth,min(bearing.keys())),min(i+halfWidth,max(bearing.keys()))+1): + bearing[x]=bearingInput[t-i+x] + + translated = moving.Trajectory() + for t in feature.getTimeInterval(): + p1= feature.getPositionAtInstant(t) + p1.x = p1.x + dist*sin(bearing[t]*pi/180) + p1.y = p1.y + dist*cos(bearing[t]*pi/180) + translated.addPosition(p1) + + #modify first and last un-smoothed positions (half width) + if smoothing: + d1= translated[halfWidth]- feature.positions[halfWidth] + d2= translated[-halfWidth-1]- feature.positions[-halfWidth-1] + for i in xrange(halfWidth): + p1= feature.getPositionAt(i)+d1 + p2= feature.getPositionAt(-i-1)+d2 + translated.setPosition(i,p1) + translated.setPosition(-i-1,p2) + + newObj= moving.MovingObject(newNum,timeInterval=feature.getTimeInterval(),positions=translated) + return newObj + +def smoothObjectTrajectory(obj,features,newNum,minLengthParam=0.7,smoothing=False,plotResults=True,halfWidth=3,computeVelocities=True,optimize=True,create=False): + featureList=[i.num for i in obj.features if i.length() >= minLengthParam*obj.length()] + if featureList==[]: + featureList.append(longestFeature(obj)) + create=True + objs=[] + for featureID in featureList: + objTMP=smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=smoothing,halfWidth=halfWidth,create=create) + objs.append(objTMP) + newTranslated = moving.Trajectory() + newInterval=[] + for t in obj.timeInterval: + xCoord=[] + yCoord=[] + for i in objs: + if i.existsAtInstant(t): + p1= i.getPositionAtInstant(t) + xCoord.append(p1.x) + yCoord.append(p1.y) + if xCoord!=[]: + tmp= moving.Point(np.median(xCoord),np.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.features + newObj.userType=obj.userType + if plotResults: + plt.figure() + plt.title('objects_id = {}'.format(obj.num)) + for i in featureList: + features[i].plot('cx-') + obj.plot('rx-') + newObj.plot('gx-') + return newObj \ No newline at end of file
--- a/python/poly-utils.py Tue Jul 15 13:25:15 2014 -0400 +++ b/python/poly-utils.py Wed Dec 10 14:27:03 2014 -0500 @@ -6,26 +6,18 @@ import numpy as np __metaclass__ = type +from indicators import SeverityIndicator -# inputs variables -#dirname= 'G:/0-phdstart/Code/trial/indicatorsNew/' -#extension= '-indicatorsNew.csv' -#indicatorsNames= {1:'Distance',2:'Cosine',3:'collision Course Angle',4:'Velocity Cosine',5:'Velocity Angle',6:'Speed Differential',7:'Collision Probability',8:'Severity Index',9:'TTC'} -''' min Distance case''' -dirname= 'G:/0-phdstart/Code/trial/minDistanceIndicator/' -extension= '-minDistanceInd.csv' -indicatorsNames= {1:'minDistance'} -def loadNewInteractions(videoFilename,interactionType, roaduserNum1,roaduserNum2, selectedIndicators=[]): +def loadNewInteractions(videoFilename,interactionType,dirname, extension, indicatorsNames, roaduserNum1,roaduserNum2, selectedIndicators=[]): '''Loads interactions from the POLY traffic event format''' from events import Interaction - from indicators import SeverityIndicator - #filename= dirname + videoFilename + extension - filename= dirname + interactionType+ '-' + videoFilename + extension # case of min distance todo: change the saving format to be matched with all outputs + filename= dirname + videoFilename + extension + #filename= dirname + interactionType+ '-' + videoFilename + extension # case of min distance todo: change the saving format to be matched with all outputs file = utils.openCheck(filename) if (not file): return [] - interactions = [] + #interactions = [] interactionNum = 0 data= np.loadtxt(filename) indicatorFrameNums= data[:,0] @@ -43,7 +35,92 @@ values[t] = [data[i,index] for index in selectedIndicators] inter.addIndicator(SeverityIndicator('selectedIndicators', values)) - interactions.append(inter) + #interactions.append(inter) file.close() - return interactions + #return interactions + return inter + +# Plotting results + +frameRate = 15. + +# To run in directory that contains the directories that contain the results (Miss-xx and Incident-xx) +#dirname = '/home/nicolas/Research/Data/kentucky-db/' + +interactingRoadUsers = {'Miss/0404052336': [(0,3)] # 0,2 and 1 vs 3 + #, + #'Incident/0306022035': [(1,3)] + #, + #'Miss/0208030956': [(4,5),(5,7)] + } + + +def getIndicatorName(filename, withUnit = False): + if withUnit: + unit = ' (s)' + else: + unit = '' + if 'collision-point' in filename: + return 'TTC'+unit + elif 'crossing' in filename: + return 'pPET'+unit + elif 'probability' in filename: + return 'P(UEA)' + +def getMethodName(fileprefix): + if fileprefix == 'constant-velocity': + return 'Con. Vel.' + elif fileprefix == 'normal-adaptation': + return 'Norm. Ad.' + elif fileprefix == 'point-set': + return 'Pos. Set' + elif fileprefix == 'evasive-action': + return 'Ev. Act.' + elif fileprefix == 'point-set-evasive-action': + return 'Pos. Set' + +indicator2TimeIdx = {'TTC':2,'pPET':2, 'P(UEA)':3} +def getDataAtInstant(data, i): + return data[data[:,2] == i] + +def getPointsAtInstant(data, i): + return getDataAtInstant(i)[3:5] + +def getIndicator(data, roadUserNumbers, indicatorName): + if data.ndim ==1: + data.shape = (1,data.shape[0]) + + # find the order for the roadUserNumbers + uniqueObj1 = np.unique(data[:,0]) + uniqueObj2 = np.unique(data[:,1]) + found = False + if roadUserNumbers[0] in uniqueObj1 and roadUserNumbers[1] in uniqueObj2: + objNum1 = roadUserNumbers[0] + objNum2 = roadUserNumbers[1] + found = True + if roadUserNumbers[1] in uniqueObj1 and roadUserNumbers[0] in uniqueObj2: + objNum1 = roadUserNumbers[1] + objNum2 = roadUserNumbers[0] + found = True + + # get subset of data for road user numbers + if found: + roadUserData = data[np.logical_and(data[:,0] == objNum1, data[:,1] == objNum2),:] + if roadUserData.size > 0: + time = np.unique(roadUserData[:,indicator2TimeIdx[indicatorName]]) + values = {} + if indicatorName == 'P(UEA)': + tmp = roadUserData[:,4] + for k,v in zip(time, tmp): + values[k]=v + return SeverityIndicator(indicatorName, values, mostSevereIsMax = False, maxValue = 1.), roadUserData + else: + for i in xrange(time[0],time[-1]+1): + try: + tmp = getDataAtInstant(roadUserData, i) + values[i] = np.sum(tmp[:,5]*tmp[:,6])/np.sum(tmp[:,5])/frameRate + except IOError: + values[i] = np.inf + return SeverityIndicator(indicatorName, values, mostSevereIsMax = False), roadUserData + return None, None \ No newline at end of file
--- a/python/prediction.py Tue Jul 15 13:25:15 2014 -0400 +++ b/python/prediction.py Wed Dec 10 14:27:03 2014 -0500 @@ -5,6 +5,7 @@ import math import random import numpy as np +from utils import LCSS class PredictedTrajectory: '''Class for predicted trajectories with lazy evaluation @@ -47,21 +48,15 @@ def getControl(self): return self.control - -def findNearestOrientation(initialPosition,prototypeTrajectory): + +def findNearestParams(initialPosition,prototypeTrajectory): + ''' nearest parameters are the index of minDistance and the orientation ''' distances=[] - for p in prototypeTrajectory.positions: - distances.append(moving.Point.distanceNorm2(initialPosition, p)) - t=np.argmin(distances) - return moving.NormAngle.fromPoint(prototypeTrajectory.velocities[t]).angle -#todo: merge with previous function -def findNearestInstant(initialPosition,prototypeTrajectory): - distances=[] - for p in prototypeTrajectory.positions: - distances.append(moving.Point.distanceNorm2(initialPosition, p)) - t=np.argmin(distances) - return t - + for position in prototypeTrajectory.positions: + distances.append(moving.Point.distanceNorm2(initialPosition, position)) + minDistanceIndex= np.argmin(distances) + return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle + class PredictedTrajectoryPrototype(PredictedTrajectory): '''Predicted trajectory that follows a prototype trajectory The prototype is in the format of a moving.Trajectory: it could be @@ -74,39 +69,36 @@ (applying a constant ratio equal to the ratio of the user instantaneous speed and the trajectory closest speed)''' - def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = True, speedList=[], probability = 1.): + def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = True, probability = 1.): self.prototypeTrajectory = prototypeTrajectory self.constantSpeed = constantSpeed - self.speedList = speedList self.probability = probability self.predictedPositions = {0: initialPosition} - self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestOrientation(initialPosition,prototypeTrajectory))}#moving.NormAngle.fromPoint(initialVelocity)} - + self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)} + def predictPosition(self, nTimeSteps): if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): if self.constantSpeed: # calculate cumulative distance speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm - anglePrototype = findNearestOrientation(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory) + anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1] self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype) self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) - #pass - elif self.speedList!=[]: - pass + else: # see c++ code, calculate ratio speedNorm= self.predictedSpeedOrientations[0].norm - instant=findNearestInstant(self.predictedPositions[0],self.prototypeTrajectory) + instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0] prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:] ratio=float(speedNorm)/prototypeSpeeds[0] resampledSpeeds=[sp*ratio for sp in prototypeSpeeds] - anglePrototype = findNearestOrientation(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory) + anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1] if nTimeSteps<len(resampledSpeeds): self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype) self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) else: self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype) self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) - # pass + return self.predictedPositions[nTimeSteps] class PredictedTrajectoryRandomControl(PredictedTrajectory): @@ -158,6 +150,192 @@ t += 1 return t, p1, p2 +def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): + from matplotlib.pyplot import figure, axis, title, close, savefig + figure() + for et in predictedTrajectories1: + et.predictPosition(timeHorizon) + et.plot('rx') + + for et in predictedTrajectories2: + et.predictPosition(timeHorizon) + et.plot('bx') + obj1.plot('r') + obj2.plot('b') + title('instant {0}'.format(currentInstant)) + axis('equal') + savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) + close() + +def calculateProbability(nMatching,similarity,objects): + sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) + prototypeProbability={} + for i in similarity.keys(): + prototypeProbability[i]= similarity[i] * float(nMatching[i])/sumFrequencies + sumProbabilities= sum([prototypeProbability[p] for p in prototypeProbability.keys()]) + probabilities={} + for i in prototypeProbability.keys(): + probabilities[objects[i]]= float(prototypeProbability[i])/sumProbabilities + return probabilities + +def findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,spatialThreshold=1.0, delta=180): + ''' behaviour prediction first step''' + if route[0] not in noiseEntryNums: + prototypesRoutes= [ x for x in sorted(prototypes.keys()) if route[0]==x[0]] + elif route[1] not in noiseExitNums: + prototypesRoutes=[ x for x in sorted(prototypes.keys()) if route[1]==x[1]] + else: + prototypesRoutes=[x for x in sorted(prototypes.keys())] + lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta) + similarity={} + for y in prototypesRoutes: + if y in prototypes.keys(): + prototypesIDs=prototypes[y] + for x in prototypesIDs: + s=lcss.computeNormalized(partialObjPositions, objects[x].positions) + if s >= minSimilarity: + similarity[x]=s + + if mostMatched==None: + probabilities= calculateProbability(nMatching,similarity,objects) + return probabilities + else: + mostMatchedValues=sorted(similarity.values(),reverse=True)[:mostMatched] + keys=[k for k in similarity.keys() if similarity[k] in mostMatchedValues] + newSimilarity={} + for i in keys: + newSimilarity[i]=similarity[i] + probabilities= calculateProbability(nMatching,newSimilarity,objects) + return probabilities + +def findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,spatialThreshold=1.0, delta=180): + if useDestination: + prototypesRoutes=[route] + else: + if route[0] not in noiseEntryNums: + prototypesRoutes= [ x for x in sorted(prototypes.keys()) if route[0]==x[0]] + elif route[1] not in noiseExitNums: + prototypesRoutes=[ x for x in sorted(prototypes.keys()) if route[1]==x[1]] + else: + prototypesRoutes=[x for x in sorted(prototypes.keys())] + lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta) + similarity={} + for y in prototypesRoutes: + if y in prototypes.keys(): + prototypesIDs=prototypes[y] + for x in prototypesIDs: + s=lcss.computeNormalized(partialObjPositions, objects[x].positions) + if s >= minSimilarity: + similarity[x]=s + + newSimilarity={} + for i in similarity.keys(): + if i in secondStepPrototypes.keys(): + for j in secondStepPrototypes[i]: + newSimilarity[j]=similarity[i] + probabilities= calculateProbability(nMatching,newSimilarity,objects) + return probabilities + +def getPrototypeTrajectory(obj,route,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): + partialInterval=moving.Interval(obj.getFirstInstant(),currentInstant) + partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions + if useSpeedPrototype: + prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) + else: + prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) + return prototypeTrajectories + +def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): + '''returns the lists of collision points and crossing zones''' + if usePrototypes: + prototypeTrajectories1=getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) + prototypeTrajectories2= getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) + predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) + predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) + else: + predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) + predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) + + collisionPoints = [] + crossingZones = [] + for et1 in predictedTrajectories1: + for et2 in predictedTrajectories2: + t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) + + if t <= timeHorizon: + collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) + elif computeCZ: # check if there is a crossing zone + # 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: + deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2() + crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV))) + t2 += 1 + t1 += 1 + + if debug: + savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) + return currentInstant,collisionPoints, crossingZones + +def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): + '''Computes all crossing and collision points at each common instant for two road users. ''' + collisionPoints={} + crossingZones={} + if timeInterval: + commonTimeInterval = timeInterval + else: + commonTimeInterval = obj1.commonTimeInterval(obj2) + if nProcesses == 1: + if usePrototypes: + firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) + commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors + commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors + for i in commonTimeIntervalList2: + i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) + if len(cp) != 0: + collisionPoints[i] = cp + if len(cz) != 0: + crossingZones[i] = cz + if collisionPoints!={} or crossingZones!={}: + for i in commonTimeIntervalList1: + if i not in commonTimeIntervalList2: + i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) + if len(cp) != 0: + collisionPoints[i] = cp + if len(cz) != 0: + crossingZones[i] = cz + else: + for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors + i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) + if len(cp) != 0: + collisionPoints[i] = cp + if len(cz) != 0: + crossingZones[i] = cz + else: + from multiprocessing import Pool + pool = Pool(processes = nProcesses) + jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]] + #results = [j.get() for j in jobs] + #results.sort() + for j in jobs: + i, cp, cz = j.get() + #if len(cp) != 0 or len(cz) != 0: + if len(cp) != 0: + collisionPoints[i] = cp + if len(cz) != 0: + crossingZones[i] = cz + pool.close() + return collisionPoints, crossingZones + class PredictionParameters: def __init__(self, name, maxSpeed): self.name = name @@ -169,70 +347,11 @@ def generatePredictedTrajectories(self, obj, instant): return [] - def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,prototypeTrajectories1=None,prototypeTrajectories2=None): - '''returns the lists of collision points and crossing zones''' - if prototypeTrajectories1==None: - predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant) - predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant) - else: - predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) - predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) - - collisionPoints = [] - crossingZones = [] - for et1 in predictedTrajectories1: - for et2 in predictedTrajectories2: - t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) + def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): + return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) - if t <= timeHorizon: - collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) - elif computeCZ: # check if there is a crossing zone - # 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: - deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2() - crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV))) - t2 += 1 - t1 += 1 - - if debug: - from matplotlib.pyplot import figure, axis, title - figure() - for et in predictedTrajectories1: - et.predictPosition(timeHorizon) - et.plot('rx') - - for et in predictedTrajectories2: - et.predictPosition(timeHorizon) - et.plot('bx') - obj1.plot('r') - obj2.plot('b') - title('instant {0}'.format(currentInstant)) - axis('equal') - - return collisionPoints, crossingZones - - def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,prototypeTrajectories1=None,prototypeTrajectories2=None): - '''Computes all crossing and collision points at each common instant for two road users. ''' - collisionPoints={} - crossingZones={} - if timeInterval: - commonTimeInterval = timeInterval - else: - commonTimeInterval = obj1.commonTimeInterval(obj2) - for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors - collisionPoints[i], crossingZones[i] = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2) - - return collisionPoints, crossingZones + def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): + return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step) def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): '''Computes only collision probabilities @@ -244,7 +363,6 @@ commonTimeInterval = obj1.commonTimeInterval(obj2) for i in list(commonTimeInterval)[:-1]: nCollisions = 0 - print(obj1.num, obj2.num, i) predictedTrajectories1 = self.generatePredictedTrajectories(obj1, i) predictedTrajectories2 = self.generatePredictedTrajectories(obj2, i) for et1 in predictedTrajectories1: @@ -257,19 +375,7 @@ collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples] if debug: - from matplotlib.pyplot import figure, axis, title - figure() - for et in predictedTrajectories1: - et.predictPosition(timeHorizon) - et.plot('rx') - - for et in predictedTrajectories2: - et.predictPosition(timeHorizon) - et.plot('bx') - obj1.plot('r') - obj2.plot('b') - title('instant {0}'.format(i)) - axis('equal') + savePredictedTrajectoriesFigure(i, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) return collisionProbabilities @@ -452,22 +558,18 @@ # Other Methods #### class prototypePredictionParameters(PredictionParameters): - def __init__(self, maxSpeed, nPredictedTrajectories,constantSpeed = True, speedList=[]): - #TODO speed profiles integration + def __init__(self, maxSpeed, nPredictedTrajectories,constantSpeed = True): name = 'prototype' PredictionParameters.__init__(self, name, maxSpeed) self.nPredictedTrajectories = nPredictedTrajectories - #self.prototypeTrajectory = prototypeTrajectory - self.constantSpeed = constantSpeed - self.speedList = speedList - #self.probability=probability + self.constantSpeed = constantSpeed def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories): predictedTrajectories = [] initialPosition = obj.getPositionAtInstant(instant) initialVelocity = obj.getVelocityAtInstant(instant) for prototypeTraj in prototypeTrajectories.keys(): - predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, speedList=self.speedList, probability = prototypeTrajectories[prototypeTraj])) + predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj])) return predictedTrajectories if __name__ == "__main__":
--- a/python/storage.py Tue Jul 15 13:25:15 2014 -0400 +++ b/python/storage.py Wed Dec 10 14:27:03 2014 -0500 @@ -34,16 +34,17 @@ except sqlite3.OperationalError as error: printDBError(error) +# TODO: add test if database connection is open # IO to sqlite -def writeTrajectoriesToSqlite(objects, outFilename, trajectoryType, objectNumbers = -1): +def writeTrajectoriesToSqlite(objects, outputFilename, trajectoryType, objectNumbers = -1): """ This function writers trajectories to a specified sqlite file @param[in] objects -> a list of trajectories @param[in] trajectoryType - - @param[out] outFile -> the .sqlite file containting the written objects + @param[out] outputFilename -> the .sqlite file containting the written objects @param[in] objectNumber : number of objects loaded """ - connection = sqlite3.connect(outFilename) + connection = sqlite3.connect(outputFilename) cursor = connection.cursor() schema = "CREATE TABLE IF NOT EXISTS \"positions\"(trajectory_id INTEGER,frame_number INTEGER, x_coordinate REAL, y_coordinate REAL, PRIMARY KEY(trajectory_id, frame_number))" @@ -64,9 +65,9 @@ connection.commit() connection.close() -def writeFeaturesToSqlite(objects, outFilename, trajectoryType, objectNumbers = -1): - '''write features trajectories maintain trajectory ID,velocities dataset ''' - connection = sqlite3.connect(outFilename) +def writeFeaturesToSqlite(objects, outputFilename, trajectoryType, objectNumbers = -1): + '''write features trajectories maintain trajectory ID,velocities dataset ''' + connection = sqlite3.connect(outputFilename) cursor = connection.cursor() cursor.execute("CREATE TABLE IF NOT EXISTS \"positions\"(trajectory_id INTEGER,frame_number INTEGER, x_coordinate REAL, y_coordinate REAL, PRIMARY KEY(trajectory_id, frame_number))") @@ -85,9 +86,9 @@ connection.commit() connection.close() -def writePrototypesToSqlite(prototypes,nMatching, outFilename): +def writePrototypesToSqlite(prototypes,nMatching, outputFilename): """ prototype dataset is a dictionary with keys== routes, values== prototypes Ids """ - connection = sqlite3.connect(outFilename) + connection = sqlite3.connect(outputFilename) cursor = connection.cursor() cursor.execute("CREATE TABLE IF NOT EXISTS \"prototypes\"(prototype_id INTEGER,routeIDstart INTEGER,routeIDend INTEGER, nMatching INTEGER, PRIMARY KEY(prototype_id))") @@ -127,10 +128,10 @@ connection.close() return prototypes,nMatching -def writeLabelsToSqlite(labels, outFilename): +def writeLabelsToSqlite(labels, outputFilename): """ labels is a dictionary with keys: routes, values: prototypes Ids """ - connection = sqlite3.connect(outFilename) + connection = sqlite3.connect(outputFilename) cursor = connection.cursor() cursor.execute("CREATE TABLE IF NOT EXISTS \"labels\"(object_id INTEGER,routeIDstart INTEGER,routeIDend INTEGER, prototype_id INTEGER, PRIMARY KEY(object_id))") @@ -167,10 +168,54 @@ connection.close() return labels +def writeSpeedPrototypeToSqlite(prototypes,nmatching, outFilename): + """ to match the format of second layer prototypes""" + connection = sqlite3.connect(outFilename) + cursor = connection.cursor() -def writeRoutesToSqlite(Routes, outFilename): + cursor.execute("CREATE TABLE IF NOT EXISTS \"speedprototypes\"(spdprototype_id INTEGER,prototype_id INTEGER,routeID_start INTEGER, routeID_end INTEGER, nMatching INTEGER, PRIMARY KEY(spdprototype_id))") + + for route in prototypes.keys(): + if prototypes[route]!={}: + for i in prototypes[route]: + if prototypes[route][i]!= []: + for j in prototypes[route][i]: + cursor.execute("insert into speedprototypes (spdprototype_id,prototype_id, routeID_start, routeID_end, nMatching) values (?,?,?,?,?)",(j,i,route[0],route[1],nmatching[j])) + + connection.commit() + connection.close() + +def loadSpeedPrototypeFromSqlite(filename): + """ + This function loads the prototypes table in the database of name <filename>. + """ + prototypes = {} + nMatching={} + connection = sqlite3.connect(filename) + cursor = connection.cursor() + + try: + cursor.execute('SELECT * from speedprototypes order by spdprototype_id,prototype_id, routeID_start, routeID_end, nMatching') + except sqlite3.OperationalError as error: + utils.printDBError(error) + return [] + + for row in cursor: + route=(row[2],row[3]) + if route not in prototypes.keys(): + prototypes[route]={} + if row[1] not in prototypes[route].keys(): + prototypes[route][row[1]]=[] + prototypes[route][row[1]].append(row[0]) + nMatching[row[0]]=row[4] + + connection.close() + return prototypes,nMatching + + +def writeRoutesToSqlite(Routes, outputFilename): """ This function writes the activity path define by start and end IDs""" - connection = sqlite3.connect(outFilename) + connection = sqlite3.connect(outputFilename) cursor = connection.cursor() cursor.execute("CREATE TABLE IF NOT EXISTS \"routes\"(object_id INTEGER,routeIDstart INTEGER,routeIDend INTEGER, PRIMARY KEY(object_id))") @@ -249,20 +294,21 @@ if trajectoryType == 'feature': statementBeginning = 'where trajectory_id ' elif trajectoryType == 'object': - statementBeginning = 'and OF.object_id ' + statementBeginning = 'and OF.object_id ' + elif trajectoryType == 'bbtop' or 'bbbottom': + statementBeginning = 'where object_id ' else: print('no trajectory type was chosen') - if type(objectNumbers) == int: - if objectNumbers == -1: - query = '' - else: - query = statementBeginning+'between 0 and {0} '.format(objectNumbers) + if objectNumbers is None: + query = '' + elif type(objectNumbers) == int: + query = statementBeginning+'between 0 and {0} '.format(objectNumbers) elif type(objectNumbers) == list: query = statementBeginning+'in ('+', '.join([str(n) for n in objectNumbers])+') ' return query -def loadTrajectoriesFromTable(connection, tableName, trajectoryType, objectNumbers = -1): +def loadTrajectoriesFromTable(connection, tableName, trajectoryType, objectNumbers = None): '''Loads trajectories (in the general sense) from the given table can be positions or velocities @@ -270,14 +316,21 @@ cursor = connection.cursor() try: + idQuery = getTrajectoryIdQuery(objectNumbers, trajectoryType) if trajectoryType == 'feature': - trajectoryIdQuery = getTrajectoryIdQuery(objectNumbers, trajectoryType) - queryStatement = 'SELECT * from '+tableName+' '+trajectoryIdQuery+'order by trajectory_id, frame_number' + queryStatement = 'SELECT * from '+tableName+' '+idQuery+'ORDER BY trajectory_id, frame_number' cursor.execute(queryStatement) logging.debug(queryStatement) elif trajectoryType == 'object': - objectIdQuery = getTrajectoryIdQuery(objectNumbers, trajectoryType) - queryStatement = 'SELECT OF.object_id, P.frame_number, avg(P.x_coordinate), avg(P.y_coordinate) from '+tableName+' P, objects_features OF where P.trajectory_id = OF.trajectory_id '+objectIdQuery+'group by OF.object_id, P.frame_number order by OF.object_id, P.frame_number' + queryStatement = 'SELECT OF.object_id, P.frame_number, avg(P.x_coordinate), avg(P.y_coordinate) from '+tableName+' P, objects_features OF where P.trajectory_id = OF.trajectory_id '+idQuery+'group by OF.object_id, P.frame_number ORDER BY OF.object_id, P.frame_number' + cursor.execute(queryStatement) + logging.debug(queryStatement) + elif trajectoryType in ['bbtop', 'bbbottom']: + if trajectoryType == 'bbtop': + corner = 'top_left' + elif trajectoryType == 'bbbottom': + corner = 'bottom_right' + queryStatement = 'SELECT object_id, frame_number, x_'+corner+', y_'+corner+' FROM '+tableName+' '+trajectoryIdQuery+'ORDER BY object_id, frame_number' cursor.execute(queryStatement) logging.debug(queryStatement) else: @@ -292,21 +345,36 @@ for row in cursor: if row[0] != objId: objId = row[0] - if obj: + if obj != None and obj.length() == obj.positions.length(): objects.append(obj) + elif obj != None: + print('Object {} is missing {} positions'.format(obj.getNum(), int(obj.length())-obj.positions.length())) obj = moving.MovingObject(row[0], timeInterval = moving.TimeInterval(row[1], row[1]), positions = moving.Trajectory([[row[2]],[row[3]]])) else: obj.timeInterval.last = row[1] obj.positions.addPositionXY(row[2],row[3]) - if obj: + if obj != None and obj.length() == obj.positions.length(): objects.append(obj) + elif obj != None: + print('Object {} is missing {} positions'.format(obj.getNum(), int(obj.length())-obj.positions.length())) return objects -def loadTrajectoriesFromSqlite(filename, trajectoryType, objectNumbers = -1): - '''Loads nObjects or the indices in objectNumbers from the database''' - connection = sqlite3.connect(filename) # add test if it open +def loadUserTypesFromTable(cursor, trajectoryType, objectNumbers): + objectIdQuery = getTrajectoryIdQuery(objectNumbers, trajectoryType) + if objectIdQuery == '': + cursor.execute('SELECT object_id, road_user_type from objects') + else: + cursor.execute('SELECT object_id, road_user_type from objects where '+objectIdQuery[7:]) + userTypes = {} + for row in cursor: + userTypes[row[0]] = row[1] + return userTypes + +def loadTrajectoriesFromSqlite(filename, trajectoryType, objectNumbers = None): + '''Loads the first objectNumbers objects or the indices in objectNumbers from the database''' + connection = sqlite3.connect(filename) objects = loadTrajectoriesFromTable(connection, 'positions', trajectoryType, objectNumbers) objectVelocities = loadTrajectoriesFromTable(connection, 'velocities', trajectoryType, objectNumbers) @@ -340,26 +408,40 @@ obj.featureNumbers = featureNumbers[obj.getNum()] # load userType - if objectIdQuery == '': - cursor.execute('SELECT object_id, road_user_type from objects') - else: - cursor.execute('SELECT object_id, road_user_type from objects where '+objectIdQuery[7:]) - userTypes = {} - for row in cursor: - userTypes[row[0]] = row[1] - + userTypes = loadUserTypesFromTable(cursor, trajectoryType, objectNumbers) for obj in objects: obj.userType = userTypes[obj.getNum()] except sqlite3.OperationalError as error: printDBError(error) - return [] + objects = [] connection.close() return objects -def removeFromSqlite(filename, dataType): - 'Removes some tables in the filename depending on type of data' +def loadGroundTruthFromSqlite(filename, gtType, gtNumbers = None): + 'Loads bounding box annotations (ground truth) from an SQLite ' + connection = sqlite3.connect(filename) + gt = [] + + if gtType == 'bb': + topCorners = loadTrajectoriesFromTable(connection, 'bounding_boxes', 'bbtop', gtNumbers) + bottomCorners = loadTrajectoriesFromTable(connection, 'bounding_boxes', 'bbbottom', gtNumbers) + userTypes = loadUserTypesFromTable(connection.cursor(), 'object', gtNumbers) # string format is same as object + + for t, b in zip(topCorners, bottomCorners): + num = t.getNum() + if t.getNum() == b.getNum(): + annotation = moving.BBAnnotation(num, t.getTimeInterval(), t, b, userTypes[num]) + gt.append(annotation) + else: + print ('Unknown type of annotation {}'.format(gtType)) + + connection.close() + return gt + +def deleteFromSqlite(filename, dataType): + 'Deletes (drops) some tables in the filename depending on type of data' import os if os.path.isfile(filename): connection = sqlite3.connect(filename) @@ -481,7 +563,7 @@ connection.commit() connection.close() -def loadBoundingBoxTable(filename): +def loadBoundingBoxTableForDisplay(filename): connection = sqlite3.connect(filename) cursor = connection.cursor() boundingBoxes = {} # list of bounding boxes for each instant @@ -490,9 +572,7 @@ result = [row for row in cursor] if len(result) > 0: cursor.execute('SELECT * FROM bounding_boxes') - #objId = -1 for row in cursor: - #if row[0] != objId: boundingBoxes.setdefault(row[1], []).append([moving.Point(row[2], row[3]), moving.Point(row[4], row[5])]) except sqlite3.OperationalError as error: printDBError(error) @@ -500,6 +580,19 @@ connection.close() return boundingBoxes +def loadBoundingBoxTable(filename): + connection = sqlite3.connect(filename) + cursor = connection.cursor() + boundingBoxes = [] + + try: + pass + except sqlite3.OperationalError as error: + printDBError(error) + return boundingBoxes + connection.close() + return boundingBoxes + ######################### # txt files @@ -517,20 +610,21 @@ exit() return None -def readline(f, commentChar = commentChar): - '''Modified readline function to skip comments.''' +def readline(f, commentCharacters = commentChar): + '''Modified readline function to skip comments + Can take a list of characters or a string (in will work in both)''' s = f.readline() - while (len(s) > 0) and s.startswith(commentChar): + while (len(s) > 0) and s[0] in commentCharacters: s = f.readline() return s.strip() -def getLines(f, commentChar = commentChar): +def getLines(f, commentCharacters = commentChar): '''Gets a complete entry (all the lines) in between delimiterChar.''' dataStrings = [] - s = readline(f, commentChar) + s = readline(f, commentCharacters) while len(s) > 0: dataStrings += [s.strip()] - s = readline(f, commentChar) + s = readline(f, commentCharacters) return dataStrings def writeList(filename, l): @@ -539,15 +633,15 @@ f.write('{}\n'.format(x)) f.close() -def loadListStrings(filename, commentChar = commentChar): +def loadListStrings(filename, commentCharacters = commentChar): f = openCheck(filename, 'r') - result = getLines(f, commentChar) + result = getLines(f, commentCharacters) f.close() return result -def getValuesFromINIFile(filename, option, delimiterChar = '=', commentChar = commentChar): +def getValuesFromINIFile(filename, option, delimiterChar = '=', commentCharacters = commentChar): values = [] - for l in loadListStrings(filename, commentChar): + for l in loadListStrings(filename, commentCharacters): if l.startswith(option): values.append(l.split(delimiterChar)[1].strip()) return values @@ -568,7 +662,7 @@ finally: self.sechead = None else: return self.fp.readline() -def loadTrajectoriesFromVissimFile(filename, simulationStepsPerTimeUnit, nObjects = -1): +def loadTrajectoriesFromVissimFile(filename, simulationStepsPerTimeUnit, nObjects = -1, warmUpLastInstant = None): '''Reads data from VISSIM .fzp trajectory file simulationStepsPerTimeUnit is the number of simulation steps per unit of time used by VISSIM for example, there seems to be 5 simulation steps per simulated second in VISSIM, @@ -577,28 +671,30 @@ Assumed to be sorted over time''' objects = {} # dictionary of objects index by their id + firstInstants = {} - infile = openCheck(filename, quitting = True) + inputfile = openCheck(filename, quitting = True) # data = pd.read_csv(filename, skiprows=15, delimiter=';') # skip header: 15 lines + 1 - for i in range(16): - readline(infile) - - for line in infile: + line = readline(inputfile, '*$') + while len(line) > 0:#for line in inputfile: data = line.strip().split(';') objNum = int(data[1]) instant = int(float(data[0])*simulationStepsPerTimeUnit) s = float(data[4]) y = float(data[5]) lane = data[2]+'_'+data[3] - if objNum not in objects: - objects[objNum] = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(instant, instant)) - objects[objNum].curvilinearPositions = moving.CurvilinearTrajectory() - objects[objNum].timeInterval.last = instant - objects[objNum].curvilinearPositions.addPosition(s, y, lane) - if nObjects > 0 and len(objects) > nObjects: - return objects.values()[:nObjects] + if objNum not in firstInstants: + firstInstants[objNum] = instant + if warmUpLastInstant == None or firstInstants[objNum] >= warmUpLastInstant: + if nObjects < 0 or len(objects) < nObjects: + objects[objNum] = moving.MovingObject(num = objNum, timeInterval = moving.TimeInterval(instant, instant)) + objects[objNum].curvilinearPositions = moving.CurvilinearTrajectory() + if (warmUpLastInstant == None or firstInstants[objNum] >= warmUpLastInstant) and objNum in objects: + objects[objNum].timeInterval.last = instant + objects[objNum].curvilinearPositions.addPositionSYL(s, y, lane) + line = readline(inputfile, '*$') return objects.values() @@ -607,7 +703,7 @@ and returns the list of Feature objects''' objects = [] - infile = openCheck(filename, quitting = True) + inputfile = openCheck(filename, quitting = True) def createObject(numbers): firstFrameNum = int(numbers[1]) @@ -631,11 +727,11 @@ obj.size = [float(numbers[8]), float(numbers[9])] # 8 lengh, 9 width # TODO: temporary, should use a geometry object return obj - numbers = infile.readline().strip().split() + numbers = readline(inputfile).strip().split() if (len(numbers) > 0): obj = createObject(numbers) - for line in infile: + for line in inputfile: numbers = line.strip().split() if obj.getNum() != int(numbers[0]): # check and adapt the length to deal with issues in NGSIM data @@ -650,7 +746,7 @@ else: obj.laneNums.append(int(numbers[13])) obj.positions.addPositionXY(float(numbers[6]), float(numbers[7])) - obj.curvilinearPositions.addPosition(float(numbers[5]), float(numbers[4]), obj.laneNums[-1]) + obj.curvilinearPositions.addPositionSYL(float(numbers[5]), float(numbers[4]), obj.laneNums[-1]) obj.speeds.append(float(numbers[11])) obj.precedingVehicles.append(int(numbers[14])) obj.followingVehicles.append(int(numbers[15])) @@ -662,19 +758,19 @@ if (obj.size[1] != float(numbers[9])): print 'changed width obj %d' % (obj.getNum()) - infile.close() + inputfile.close() return objects -def convertNgsimFile(inFile, outFile, append = False, nObjects = -1, sequenceNum = 0): +def convertNgsimFile(inputfile, outputfile, append = False, nObjects = -1, sequenceNum = 0): '''Reads data from the trajectory data provided by NGSIM project and converts to our current format.''' if append: - out = openCheck(outFile,'a') + out = openCheck(outputfile,'a') else: - out = openCheck(outFile,'w') + out = openCheck(outputfile,'w') nObjectsPerType = [0,0,0] - features = loadNgsimFile(inFile, sequenceNum) + features = loadNgsimFile(inputfile, sequenceNum) for f in features: nObjectsPerType[f.userType-1] += 1 f.write(out) @@ -791,6 +887,7 @@ return configDict + if __name__ == "__main__": import doctest import unittest
--- a/python/tests/moving.txt Tue Jul 15 13:25:15 2014 -0400 +++ b/python/tests/moving.txt Wed Dec 10 14:27:03 2014 -0500 @@ -44,6 +44,8 @@ (2.000000,-3.000000) >>> -Point(1,2) (-1.000000,-2.000000) +>>> Point(1,2).multiply(0.5) +(0.500000,1.000000) >>> Point(3,2).norm2Squared() 13 @@ -59,11 +61,21 @@ >>> predictPositionNoLimit(10, Point(0,0), Point(1,1)) # doctest:+ELLIPSIS ((1.0...,1.0...), (10.0...,10.0...)) ->>> segmentIntersection(Point(0,0),Point(1,1), Point(0,1), Point(1,2)) ->>> segmentIntersection(Point(0,1),Point(1,0), Point(0,2), Point(2,1)) ->>> segmentIntersection(Point(0,0),Point(2,0), Point(1,-1),Point(1,1)) -(1.000000,0.000000) ->>> segmentIntersection(Point(0,1),Point(2,0),Point(1,1),Point(1,2)) +>>> segmentIntersection(Point(0,0), Point(0,1), Point(1,1), Point(2,3)) +>>> segmentIntersection(Point(0,1), Point(0,3), Point(1,0), Point(3,1)) +>>> segmentIntersection(Point(0.,0.), Point(2.,2.), Point(0.,2.), Point(2.,0.)) +(1.000000,1.000000) +>>> segmentIntersection(Point(0,1), Point(1,2), Point(2,0), Point(3,2)) + +>>> left = Trajectory.fromPointList([(92.291666666666686, 102.99239033124439), (56.774193548387103, 69.688898836168306)]) +>>> middle = Trajectory.fromPointList([(87.211021505376351, 93.390778871978512), (59.032258064516128, 67.540286481647257)]) +>>> right = Trajectory.fromPointList([(118.82392473118281, 115.68263205013426), (63.172043010752688, 66.600268576544309)]) +>>> alignments = [left, middle, right] +>>> for a in alignments: a.computeCumulativeDistances() +>>> getSYfromXY(Point(73, 82), alignments) +[1, 0, (73.819977,81.106170), 18.172277808821125, 18.172277808821125, 1.2129694042343868] +>>> getSYfromXY(Point(78, 83), alignments, 0.5) +[1, 0, (77.033188,84.053889), 13.811799123113715, 13.811799123113715, -1.4301775140225983] >>> Trajectory().length() 0 @@ -77,6 +89,31 @@ >>> t1.getTrajectoryInPolygonNoShapely(np.array([[10,10],[14,10],[14,13],[10,13]])).length() 0 +>>> t1.differentiate() +(1.000000,3.000000) (1.000000,3.000000) +>>> t1.differentiate(True) +(1.000000,3.000000) (1.000000,3.000000) (1.000000,3.000000) +>>> t1 = Trajectory([[0.5,1.5,3.5],[0.5,2.5,7.5]]) +>>> t1.differentiate() +(1.000000,2.000000) (2.000000,5.000000) + +>>> t1.computeCumulativeDistances() +>>> t1.getDistance(0) +2.23606797749979 +>>> t1.getDistance(1) +5.385164807134504 +>>> t1.getDistance(2) +Index 2 beyond trajectory length 3-1 +>>> t1.getCumulativeDistance(0) +0.0 +>>> t1.getCumulativeDistance(1) +2.23606797749979 +>>> t1.getCumulativeDistance(2) +7.6212327846342935 +>>> t1.getCumulativeDistance(3) +Index 3 beyond trajectory length 3 + + >>> from utils import LCSS >>> lcss = LCSS(lambda x,y: Point.distanceNorm2(x,y) <= 0.1) >>> Trajectory.lcss(t1, t1, lcss) @@ -101,6 +138,17 @@ True >>> Point.timeToCollision(p2, p1, v2, v1, 0.) == None True +>>> Point.midPoint(p1, p2) +(0.500000,0.500000) + +>>> t = CurvilinearTrajectory(S = [1., 2., 3., 5.], Y = [0.5, 0.5, 0.6, 0.7], lanes = ['1']*4) +>>> t.differentiate() # doctest:+ELLIPSIS +[1.0, 0.0, '1'] [1.0, 0.099..., '1'] [2.0, 0.099..., '1'] +>>> t.differentiate(True) # doctest:+ELLIPSIS +[1.0, 0.0, '1'] [1.0, 0.099..., '1'] [2.0, 0.099..., '1'] [2.0, 0.099..., '1'] +>>> t = CurvilinearTrajectory(S = [1.], Y = [0.5], lanes = ['1']) +>>> t.differentiate().empty() +True >>> o1 = MovingObject(positions = Trajectory([[0]*3,[2]*3]), velocities = Trajectory([[0]*3,[1]*3])) >>> o1.classifyUserTypeSpeedMotorized(0.5, np.median)
--- a/python/tests/storage.txt Tue Jul 15 13:25:15 2014 -0400 +++ b/python/tests/storage.txt Wed Dec 10 14:27:03 2014 -0500 @@ -1,12 +1,31 @@ >>> from storage import * +>>> from StringIO import StringIO >>> f = openCheck('non_existant_file.txt') File non_existant_file.txt could not be opened. ->>> loadPrototypeMatchIndexesFromSqlite("nonexistent") +>>> nonexistentFilename = "nonexistent" +>>> loadPrototypeMatchIndexesFromSqlite(nonexistentFilename) DB Error: no such table: prototypes [] ->>> loadTrajectoriesFromSqlite("nonexistent", 'feature') +>>> loadTrajectoriesFromSqlite(nonexistentFilename, 'feature') DB Error: no such table: positions DB Error: no such table: velocities [] +>>> from os import remove +>>> remove(nonexistentFilename) + +>>> strio = StringIO('# asdlfjasdlkj0\nsadlkfjsdlakjf') +>>> readline(strio) +'sadlkfjsdlakjf' +>>> strio = StringIO('# asdlfjasdlkj0\nsadlkfjsdlakjf') +>>> readline(strio, ['#']) +'sadlkfjsdlakjf' +>>> strio = StringIO('# asdlfjasdlkj0\nsadlkfjsdlakjf') +>>> readline(strio, ['%']) +'# asdlfjasdlkj0' +>>> strio = StringIO('# asdlfjasdlkj0\nsadlkfjsdlakjf') +>>> readline(strio, '%*$') +'# asdlfjasdlkj0' +>>> readline(strio, '%#') +'sadlkfjsdlakjf'
--- a/python/utils.py Tue Jul 15 13:25:15 2014 -0400 +++ b/python/utils.py Wed Dec 10 14:27:03 2014 -0500 @@ -4,6 +4,7 @@ #from numpy import * #from pylab import * from datetime import time, datetime +from math import sqrt __metaclass__ = type @@ -60,15 +61,13 @@ def nSamples(self): return sum(self.counts) -def cumulativeDensityFunction(sample): +def cumulativeDensityFunction(sample, normalized = False): '''Returns the cumulative density function of the sample of a random variable''' - from numpy.core.multiarray import array - from numpy.lib.function_base import unique - from numpy.core.fromnumeric import sum - a = array(sample) - a.sort() - xaxis = unique(a) - counts = [sum(a <= x) for x in xaxis] + from numpy import arange, cumsum + xaxis = sorted(sample) + counts = arange(1,len(sample)+1) # dtype = float + if normalized: + counts /= float(len(sample)) return xaxis, counts class EmpiricalDiscreteDistribution(EmpiricalDistribution): @@ -209,7 +208,7 @@ return median([y for observedx, y in zip(X,Y) if abs(x-observedx)<halfwidth]) def argmaxDict(d): - return max(d.iterkeys(), key=(lambda key: d[key])) + return max(d, key=d.get) def framesToTime(nFrames, frameRate, initialTime = time()): '''returns a datetime.time for the time in hour, minutes and seconds @@ -241,11 +240,28 @@ return ceil(v*tens)/tens def inBetween(bound1, bound2, x): - return bound1 <= x <= bound2 or bound2 <= x<= bound1 + return bound1 <= x <= bound2 or bound2 <= x <= bound1 + +def pointDistanceL2(x1,y1,x2,y2): + ''' Compute point-to-point distance (L2 norm, ie Euclidean distance)''' + return sqrt((x2-x1)**2+(y2-y1)**2) def crossProduct(l1, l2): return l1[0]*l2[1]-l1[1]*l2[0] +def cat_mvgavg(cat_list, halfWidth): + ''' Return a list of categories/values smoothed according to a window. + halfWidth is the search radius on either side''' + from copy import deepcopy + catgories = deepcopy(cat_list) + smoothed = catgories + for point in range(len(catgories)): + lower_bound_check = max(0,point-halfWidth) + upper_bound_check = min(len(catgories)-1,point+halfWidth+1) + window_values = catgories[lower_bound_check:upper_bound_check] + smoothed[point] = max(set(window_values), key=window_values.count) + return smoothed + def filterMovingWindow(inputSignal, halfWidth): '''Returns an array obtained after the smoothing of the input by a moving average The first and last points are copied from the original.''' @@ -345,7 +361,7 @@ based on the threshold on distance between two elements of lists l1, l2 similarityFunc returns True or False whether the two points are considered similar - if aligned, returns the best matching if using a finite delta by shiftinig the series alignments + if aligned, returns the best matching if using a finite delta by shifting the series alignments eg distance(p1, p2) < epsilon ''' @@ -552,6 +568,30 @@ ######################### +# Profiling +######################### + +def analyzeProfile(profileFilename, stripDirs = True): + '''Analyze the file produced by cProfile + + obtained by for example: + - call in script (for main() function in script) + import cProfile, os + cProfile.run('main()', os.path.join(os.getcwd(),'main.profile')) + + - or on the command line: + python -m cProfile [-o profile.bin] [-s sort] scriptfile [arg]''' + import pstats, os + p = pstats.Stats(os.path.join(os.pardir, profileFilename)) + if stripDirs: + p.strip_dirs() + p.sort_stats('time') + p.print_stats(.2) + #p.sort_stats('time') + # p.print_callees(.1, 'int_prediction.py:') + return p + +######################### # running tests #########################
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/run-tests.sh Wed Dec 10 14:27:03 2014 -0500 @@ -0,0 +1,17 @@ +#!/bin/sh +echo "------------" +echo "Python tests" +cd python +./run-tests.sh +cd .. +echo "------------" +echo "C++ tests" +if [ -f ./bin/tests ] +then + ./bin/tests +else + echo "The test executable has not been compiled" +fi +echo "------------" +echo "Script tests" +./scripts/run-tests.sh
--- a/scripts/compute-homography.py Tue Jul 15 13:25:15 2014 -0400 +++ b/scripts/compute-homography.py Wed Dec 10 14:27:03 2014 -0500 @@ -106,7 +106,7 @@ if args.displayPoints and args.videoFrameFilename != None and args.worldFilename != None and homography.size>0: worldImg = cv2.imread(args.worldFilename) videoImg = cv2.imread(args.videoFrameFilename) - if args.undistort: + if args.undistort: [map1, map2] = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients) videoImg = cv2.remap(videoImg, map1, map2, interpolation=cv2.INTER_LINEAR) if args.saveImages:
--- a/scripts/delete-tables.py Tue Jul 15 13:25:15 2014 -0400 +++ b/scripts/delete-tables.py Wed Dec 10 14:27:03 2014 -0500 @@ -5,10 +5,10 @@ import utils import storage -parser = argparse.ArgumentParser(description='The program deletes the tables in the database before saving new results (for objects, tables object_features and objects are dropped; for interactions, the tables interactions and indicators are dropped') +parser = argparse.ArgumentParser(description='The program deletes (drops) the tables in the database before saving new results (for objects, tables object_features and objects are dropped; for interactions, the tables interactions and indicators are dropped') #parser.add_argument('configFilename', help = 'name of the configuration file') parser.add_argument('-d', dest = 'databaseFilename', help = 'name of the Sqlite database', required = True) parser.add_argument('-t', dest = 'dataType', help = 'type of the data to remove', required = True, choices = ['object','interaction', 'bb']) args = parser.parse_args() -storage.removeFromSqlite(args.databaseFilename, args.dataType) +storage.deleteFromSqlite(args.databaseFilename, args.dataType)
--- a/scripts/display-trajectories.py Tue Jul 15 13:25:15 2014 -0400 +++ b/scripts/display-trajectories.py Wed Dec 10 14:27:03 2014 -0500 @@ -63,5 +63,5 @@ objects = storage.loadTrajectoriesFromSqlite(databaseFilename, args.trajectoryType) -boundingBoxes = storage.loadBoundingBoxTable(databaseFilename) +boundingBoxes = storage.loadBoundingBoxTableForDisplay(databaseFilename) cvutils.displayTrajectories(videoFilename, objects, boundingBoxes, homography, firstFrameNum, args.lastFrameNum, rescale = args.rescale, nFramesStep = args.nFramesStep, saveAllImages = args.saveAllImages, undistort = (undistort or args.undistort), intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/scripts/run-tests.sh Wed Dec 10 14:27:03 2014 -0500 @@ -0,0 +1,2 @@ +#!/bin/sh +echo 'no tests' \ No newline at end of file
--- a/scripts/safety-analysis.py Tue Jul 15 13:25:15 2014 -0400 +++ b/scripts/safety-analysis.py Wed Dec 10 14:27:03 2014 -0500 @@ -14,6 +14,7 @@ parser.add_argument('--cfg', dest = 'configFilename', help = 'name of the configuration file', required = True) 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']) parser.add_argument('--display-cp', dest = 'displayCollisionPoints', help = 'display collision points', action = 'store_true') +parser.add_argument('-n', dest = 'nProcesses', help = 'number of processes to run in parallel', type = int, default = 1) args = parser.parse_args() params = storage.ProcessParameters(args.configFilename) @@ -24,8 +25,10 @@ else: predictionMethod = params.predictionMethod -accelerationDistribution = lambda: random.triangular(-params.maxNormalAcceleration, params.maxNormalAcceleration, 0.) -steeringDistribution = lambda: random.triangular(-params.maxNormalSteering, params.maxNormalSteering, 0.) +def accelerationDistribution(): + return random.triangular(-params.maxNormalAcceleration, params.maxNormalAcceleration, 0.) +def steeringDistribution(): + return random.triangular(-params.maxNormalSteering, params.maxNormalSteering, 0.) if predictionMethod == 'cvd': # TODO add cve: constant velocity exact (Sohail's) predictionParameters = prediction.CVDirectPredictionParameters() @@ -57,7 +60,7 @@ interactions = events.createInteractions(objects) for inter in interactions: inter.computeIndicators() - inter.computeCrossingsCollisions(predictionParameters, params.collisionDistance, params.predictionTimeHorizon, params.crossingZones) + inter.computeCrossingsCollisions(predictionParameters, params.collisionDistance, params.predictionTimeHorizon, params.crossingZones, nProcesses = args.nProcesses) storage.saveIndicators(params.databaseFilename, interactions)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/scripts/undistort-video.py Wed Dec 10 14:27:03 2014 -0500 @@ -0,0 +1,47 @@ +#! /usr/bin/env python + +import sys, argparse + +import numpy as np +import cv2 + +import cvutils +from math import ceil, log10 + +parser = argparse.ArgumentParser(description='''The program converts a video into a series of images corrected for distortion. One can then use mencoder to generate a movie, eg +$ mencoder 'mf://./*.png' -mf fps=[framerate]:type=png -ovc xvid -xvidencopts bitrate=[bitrate] -nosound -o [output.avi]''') + +parser.add_argument('-i', dest = 'videoFilename', help = 'filename of the video sequence') +parser.add_argument('--intrinsic', dest = 'intrinsicCameraMatrixFilename', help = 'name of the intrinsic camera file') +parser.add_argument('--distortion-coefficients', dest = 'distortionCoefficients', help = 'distortion coefficients', nargs = '*', type = float) +parser.add_argument('--undistorted-multiplication', dest = 'undistortedImageMultiplication', help = 'undistorted image multiplication', type = float) +parser.add_argument('-f', dest = 'firstFrameNum', help = 'number of first frame number to display', type = int) +parser.add_argument('-l', dest = 'lastFrameNum', help = 'number of last frame number to save', type = int) + +args = parser.parse_args() + +intrinsicCameraMatrix = np.loadtxt(args.intrinsicCameraMatrixFilename) +#distortionCoefficients = args.distortionCoefficients +#undistortedImageMultiplication = args.undistortedImageMultiplication +#firstFrameNum = params.firstFrameNum + +capture = cv2.VideoCapture(args.videoFilename) +width = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH)) +height = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT)) +[map1, map2] = cvutils.computeUndistortMaps(width, height, args.undistortedImageMultiplication, intrinsicCameraMatrix, args.distortionCoefficients) +if capture.isOpened(): + ret = True + frameNum = args.firstFrameNum + capture.set(cv2.cv.CV_CAP_PROP_POS_FRAMES, args.firstFrameNum) + if args.lastFrameNum == None: + from sys import maxint + lastFrameNum = maxint + else: + lastFrameNum = args.lastFrameNum + nZerosFilename = int(ceil(log10(lastFrameNum))) + while ret and frameNum < lastFrameNum: + ret, img = capture.read() + if ret: + img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR) + cv2.imwrite('undistorted-{{:0{}}}.png'.format(nZerosFilename).format(frameNum), img) + frameNum += 1