Mercurial Hosting > traffic-intelligence
changeset 780:1b22d81ef5ff dev
cleaned and checked storage with functions for curvilinear trajectories
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 08 Feb 2016 12:24:26 -0500 |
parents | 670bd6a35417 |
children | 7c38250ddfc7 |
files | python/moving.py python/storage.py python/tests/storage.txt |
diffstat | 3 files changed, 78 insertions(+), 43 deletions(-) [+] |
line wrap: on
line diff
--- a/python/moving.py Mon Feb 08 12:07:47 2016 -0500 +++ b/python/moving.py Mon Feb 08 12:24:26 2016 -0500 @@ -370,10 +370,10 @@ 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 + ss_spline_d[spline]=[]#.append([[],[],[]]) + ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Incremental distance + ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Cumulative distance + ss_spline_d[spline].append(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 @@ -385,6 +385,18 @@ return ss_spline_d +def prepareSplines(splines): + 'Approximates slope singularity by giving some slope roundoff; account for roundoff error' + for spline in splines: + p1 = spline[0] + for i in xrange(len(spline)-1): + p2 = spline[i+1] + if(round(p1.x, 10) == round(p2.x, 10)): + p2.x += 0.0000000001 + if(round(p1.y, 10) == round(p2.y, 10)): + p2.y += 0.0000000001 + p1 = p2 + 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 @@ -393,10 +405,10 @@ 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 + # 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) @@ -407,8 +419,9 @@ 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. - + ''' 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, @@ -422,36 +435,38 @@ ''' minOffsetY = float('inf') #For each spline - for spline in range(len(splines)): + for splineIdx in range(len(splines)): #For each spline point index - for spline_p in range(len(splines[spline])-1): + for spline_p in range(len(splines[splineIdx])-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]) + closestPoint = ppldb2p(p.x,p.y,splines[splineIdx][spline_p][0],splines[splineIdx][spline_p][1],splines[splineIdx][spline_p+1][0],splines[splineIdx][spline_p+1][1]) if closestPoint is None: - print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) + print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, 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): + # check if the projected point is in between the current segment of the alignment bounds + if utils.inBetween(splines[splineIdx][spline_p][0], splines[splineIdx][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[splineIdx][spline_p][1], splines[splineIdx][spline_p+1][1], closestPoint.y): offsetY = Point.distanceNorm2(closestPoint, p) if offsetY < minOffsetY: minOffsetY = offsetY - snappedSpline = spline + snappedSplineIdx = splineIdx 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]) + subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint]) #Get cumulative alignment distance (total segment distance) - splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance - orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal() + splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance + orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal() offsetVector = p-snappedPoint if Point.dot(orthogonalSplineVector, offsetVector) < 0: minOffsetY = -minOffsetY - return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] + return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] else: + print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p)) return None def getXYfromSY(s, y, splineNum, splines, mode = 0):
--- a/python/storage.py Mon Feb 08 12:07:47 2016 -0500 +++ b/python/storage.py Mon Feb 08 12:24:26 2016 -0500 @@ -255,10 +255,10 @@ connection.close() return objects -def loadCurvilinearTrajectoriesFromSqlite(filename, objects, objectNumbers = None): - '''load alignement curvilinear positions (s_coordinate, y_coordinate, lane) - from an object database to an existing MovingObject list''' - connection = sqlite3.connect(inputFilename) +def addCurvilinearTrajectoriesFromSqlite(filename, objects): + '''Adds curvilinear positions (s_coordinate, y_coordinate, lane) + from a database to an existing MovingObject dict (indexed by each objects's num)''' + connection = sqlite3.connect(filename) cursor = connection.cursor() try: @@ -271,17 +271,16 @@ for row in cursor: if objNum != row[0]: objNum = row[0] - objects[objNum].curvilinearPositions = moving.CurvilinearPositions() - else: - objects[objNum].curvilinearPositions.addPositionsSYL(row[2],row[3],row[4]) - - return objects + objects[objNum].curvilinearPositions = moving.CurvilinearTrajectory() + objects[objNum].curvilinearPositions.addPositionSYL(row[2],row[3],row[4]) def saveTrajectoriesToSqlite(outputFilename, objects, trajectoryType, withFeatures = False): '''Writes features, ie the trajectories positions (and velocities if exist) with their instants to a specified sqlite file + Either feature positions (and velocities if they exist) + or curvilinear positions will be saved at a time - TODO: Not implemented for other trajectoryType than features + TODO: Not implemented for trajectoryType MovingObject with features For objects, with features will control whether the features corresponding to the object are also saved''' @@ -289,15 +288,12 @@ try: cursor = connection.cursor() - if trajectoryType in ['feature', 'curvilinear']: + if trajectoryType == 'feature': 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))") cursor.execute("CREATE TABLE IF NOT EXISTS velocities (trajectory_id INTEGER, frame_number INTEGER, x_coordinate REAL, y_coordinate REAL, PRIMARY KEY(trajectory_id, frame_number))") - if trajectoryType == 'curvilinear': - cursor.execute("CREATE TABLE IF NOT EXISTS curvilinear_positions (trajectory_id INTEGER, frame_number INTEGER, s_coordinate REAL, y_coordinate REAL, lane TEXT, PRIMARY KEY(trajectory_id, frame_number))") positionQuery = "insert into positions (trajectory_id, frame_number, x_coordinate, y_coordinate) values (?,?,?,?)" velocityQuery = "insert into velocities (trajectory_id, frame_number, x_coordinate, y_coordinate) values (?,?,?,?)" - curvilinearQuery = "insert into curvilinear_positions (trajectory_id, frame_number, x_coordinate, y_coordinate) values (?,?,?,?)" for obj in objects: num = obj.getNum() frame_number = obj.getFirstInstant() @@ -312,16 +308,19 @@ v = velocities[i] cursor.execute(velocityQuery, (num, frame_number, v.x, v.y)) frame_number += 1 - # curvilinear trajectories - if trajectoryType == 'curvilinear' and hasattr(obj, 'curvilinearPositions') is not None: - frame_number = obj.getFirstInstant() - for position in obj.getCurvilinearPositions(): - cursor.execute(curvilinearQuery, (num, frame_number, position[0], position[1], position[2])) - frame_number += 1 - connection.commit() + elif trajectoryType == 'curvilinear': + cursor.execute("CREATE TABLE IF NOT EXISTS curvilinear_positions (trajectory_id INTEGER, frame_number INTEGER, s_coordinate REAL, y_coordinate REAL, lane TEXT, PRIMARY KEY(trajectory_id, frame_number))") + curvilinearQuery = "insert into curvilinear_positions (trajectory_id, frame_number, s_coordinate, y_coordinate, lane) values (?,?,?,?,?)" + for obj in objects: + num = obj.getNum() + frame_number = obj.getFirstInstant() + for position in obj.getCurvilinearPositions(): + cursor.execute(curvilinearQuery, (num, frame_number, position[0], position[1], position[2])) + frame_number += 1 #elif trajectoryType == 'object': else: print('Unknown trajectory type {}'.format(trajectoryType)) + connection.commit() except sqlite3.OperationalError as error: printDBError(error) connection.close()
--- a/python/tests/storage.txt Mon Feb 08 12:07:47 2016 -0500 +++ b/python/tests/storage.txt Mon Feb 08 12:24:26 2016 -0500 @@ -1,6 +1,6 @@ >>> from storage import * >>> from StringIO import StringIO ->>> from moving import MovingObject, Point, TimeInterval +>>> from moving import MovingObject, Point, TimeInterval, Trajectory, prepareSplines >>> f = openCheck('non_existant_file.txt') File non_existant_file.txt could not be opened. @@ -38,6 +38,27 @@ True >>> o2.getPositions() == objects[1].getPositions() True +>>> align1 = Trajectory.fromPointList([Point(-1, 0), Point(20, 0)]) +>>> align2 = Trajectory.fromPointList([Point(-9, -3), Point(6, 3)]) +>>> align1.computeCumulativeDistances() +>>> align2.computeCumulativeDistances() +>>> prepareSplines([align1, align2]) +>>> o1.projectCurvilinear([align1, align2]) +>>> o2.projectCurvilinear([align1, align2]) +>>> saveTrajectoriesToSqlite('test.sqlite', [o1, o2], 'curvilinear') +>>> addCurvilinearTrajectoriesFromSqlite('test.sqlite', {o.num: o for o in objects}) +>>> o1.curvilinearPositions[3][:2] == objects[0].curvilinearPositions[3][:2] +True +>>> o1.curvilinearPositions[7][:2] == objects[0].curvilinearPositions[7][:2] +True +>>> [str(l) for l in o1.curvilinearPositions.getLanes()] == objects[0].curvilinearPositions.getLanes() +True +>>> o2.curvilinearPositions[2][:2] == objects[1].curvilinearPositions[2][:2] +True +>>> o2.curvilinearPositions[6][:2] == objects[1].curvilinearPositions[6][:2] +True +>>> [str(l) for l in o2.curvilinearPositions.getLanes()] == objects[1].curvilinearPositions.getLanes() +True >>> remove('test.sqlite') >>> strio = StringIO('# asdlfjasdlkj0\nsadlkfjsdlakjf')