comparison python/moving.py @ 998:933670761a57

updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Sun, 27 May 2018 23:22:48 -0400
parents 4f3387a242a1
children cc7c6b821ae6
comparison
equal deleted inserted replaced
997:4f3387a242a1 998:933670761a57
119 119
120 def __iter__(self): 120 def __iter__(self):
121 self.iterInstantNum = -1 121 self.iterInstantNum = -1
122 return self 122 return self
123 123
124 def next(self): 124 def __next__(self):
125 if self.iterInstantNum >= self.length()-1: 125 if self.iterInstantNum >= self.length()-1:
126 raise StopIteration 126 raise StopIteration
127 else: 127 else:
128 self.iterInstantNum += 1 128 self.iterInstantNum += 1
129 return self[self.iterInstantNum] 129 return self[self.iterInstantNum]
376 '''Optimized tests of a series of points within (Shapely) polygon (not prepared)''' 376 '''Optimized tests of a series of points within (Shapely) polygon (not prepared)'''
377 if type(polygon) == PreparedGeometry: 377 if type(polygon) == PreparedGeometry:
378 prepared_polygon = polygon 378 prepared_polygon = polygon
379 else: 379 else:
380 prepared_polygon = prep(polygon) 380 prepared_polygon = prep(polygon)
381 return filter(prepared_polygon.contains, points) 381 return list(filter(prepared_polygon.contains, points))
382 382
383 # Functions for coordinate transformation 383 # Functions for coordinate transformation
384 # From Paul St-Aubin's PVA tools 384 # From Paul St-Aubin's PVA tools
385 def subsec_spline_dist(splines): 385 def subsec_spline_dist(splines):
386 ''' Prepare list of spline subsegments from a spline list. 386 ''' Prepare list of spline subsegments from a spline list.
414 414
415 def prepareSplines(splines): 415 def prepareSplines(splines):
416 'Approximates slope singularity by giving some slope roundoff; account for roundoff error' 416 'Approximates slope singularity by giving some slope roundoff; account for roundoff error'
417 for spline in splines: 417 for spline in splines:
418 p1 = spline[0] 418 p1 = spline[0]
419 for i in xrange(len(spline)-1): 419 for i in range(len(spline)-1):
420 p2 = spline[i+1] 420 p2 = spline[i+1]
421 if(round(p1.x, 10) == round(p2.x, 10)): 421 if(round(p1.x, 10) == round(p2.x, 10)):
422 p2.x += 0.0000000001 422 p2.x += 0.0000000001
423 if(round(p1.y, 10) == round(p2.y, 10)): 423 if(round(p1.y, 10) == round(p2.y, 10)):
424 p2.y += 0.0000000001 424 p2.y += 0.0000000001
663 @staticmethod 663 @staticmethod
664 def generate(p, v, nPoints): 664 def generate(p, v, nPoints):
665 t = Trajectory() 665 t = Trajectory()
666 p0 = Point(p.x, p.y) 666 p0 = Point(p.x, p.y)
667 t.addPosition(p0) 667 t.addPosition(p0)
668 for i in xrange(nPoints-1): 668 for i in range(nPoints-1):
669 p0 += v 669 p0 += v
670 t.addPosition(p0) 670 t.addPosition(p0)
671 return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints]) 671 return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints])
672 672
673 @staticmethod 673 @staticmethod
702 return Trajectory([self.positions[0][i],self.positions[1][i]]) 702 return Trajectory([self.positions[0][i],self.positions[1][i]])
703 else: 703 else:
704 raise TypeError("Invalid argument type.") 704 raise TypeError("Invalid argument type.")
705 705
706 def __str__(self): 706 def __str__(self):
707 return ' '.join([self.__getitem__(i).__str__() for i in xrange(self.length())]) 707 return ' '.join([self.__getitem__(i).__str__() for i in range(self.length())])
708 708
709 def __repr__(self): 709 def __repr__(self):
710 return self.__str__() 710 return self.__str__()
711 711
712 def __iter__(self): 712 def __iter__(self):
713 self.iterInstantNum = 0 713 self.iterInstantNum = 0
714 return self 714 return self
715 715
716 def next(self): 716 def __next__(self):
717 if self.iterInstantNum >= self.length(): 717 if self.iterInstantNum >= self.length():
718 raise StopIteration 718 raise StopIteration
719 else: 719 else:
720 self.iterInstantNum += 1 720 self.iterInstantNum += 1
721 return self[self.iterInstantNum-1] 721 return self[self.iterInstantNum-1]
816 return Trajectory([[alpha*x for x in self.getXCoordinates()], 816 return Trajectory([[alpha*x for x in self.getXCoordinates()],
817 [alpha*y for y in self.getYCoordinates()]]) 817 [alpha*y for y in self.getYCoordinates()]])
818 818
819 def differentiate(self, doubleLastPosition = False): 819 def differentiate(self, doubleLastPosition = False):
820 diff = Trajectory() 820 diff = Trajectory()
821 for i in xrange(1, self.length()): 821 for i in range(1, self.length()):
822 diff.addPosition(self[i]-self[i-1]) 822 diff.addPosition(self[i]-self[i-1])
823 if doubleLastPosition: 823 if doubleLastPosition:
824 diff.addPosition(diff[-1]) 824 diff.addPosition(diff[-1])
825 return diff 825 return diff
826 826
853 Can be accessed through getDistance(idx) and getCumulativeDistance(idx)''' 853 Can be accessed through getDistance(idx) and getCumulativeDistance(idx)'''
854 self.distances = [] 854 self.distances = []
855 self.cumulativeDistances = [0.] 855 self.cumulativeDistances = [0.]
856 p1 = self[0] 856 p1 = self[0]
857 cumulativeDistance = 0. 857 cumulativeDistance = 0.
858 for i in xrange(self.length()-1): 858 for i in range(self.length()-1):
859 p2 = self[i+1] 859 p2 = self[i+1]
860 self.distances.append(Point.distanceNorm2(p1,p2)) 860 self.distances.append(Point.distanceNorm2(p1,p2))
861 cumulativeDistance += self.distances[-1] 861 cumulativeDistance += self.distances[-1]
862 self.cumulativeDistances.append(cumulativeDistance) 862 self.cumulativeDistances.append(cumulativeDistance)
863 p1 = p2 863 p1 = p2
920 intersects with the segment of extremities p1 and p2 920 intersects with the segment of extremities p1 and p2
921 Returns an empty list if there is no crossing''' 921 Returns an empty list if there is no crossing'''
922 indices = [] 922 indices = []
923 intersections = [] 923 intersections = []
924 924
925 for i in xrange(self.length()-1): 925 for i in range(self.length()-1):
926 q1=self.__getitem__(i) 926 q1=self.__getitem__(i)
927 q2=self.__getitem__(i+1) 927 q2=self.__getitem__(i+1)
928 p = segmentIntersection(q1, q2, p1, p2) 928 p = segmentIntersection(q1, q2, p1, p2)
929 if p is not None: 929 if p is not None:
930 if q1.x != q2.x: 930 if q1.x != q2.x:
942 intersects with the line going through p1 and p2 942 intersects with the line going through p1 and p2
943 Returns an empty list if there is no crossing''' 943 Returns an empty list if there is no crossing'''
944 indices = [] 944 indices = []
945 intersections = [] 945 intersections = []
946 946
947 for i in xrange(self.length()-1): 947 for i in range(self.length()-1):
948 q1=self.__getitem__(i) 948 q1=self.__getitem__(i)
949 q2=self.__getitem__(i+1) 949 q2=self.__getitem__(i+1)
950 p = segmentLineIntersection(p1, p2, q1, q2) 950 p = segmentLineIntersection(p1, p2, q1, q2)
951 if p is not None: 951 if p is not None:
952 if q1.x != q2.x: 952 if q1.x != q2.x:
1075 self.lanes[i] = lane 1075 self.lanes[i] = lane
1076 1076
1077 def differentiate(self, doubleLastPosition = False): 1077 def differentiate(self, doubleLastPosition = False):
1078 diff = CurvilinearTrajectory() 1078 diff = CurvilinearTrajectory()
1079 p1 = self[0] 1079 p1 = self[0]
1080 for i in xrange(1, self.length()): 1080 for i in range(1, self.length()):
1081 p2 = self[i] 1081 p2 = self[i]
1082 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], p1[2]) 1082 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], p1[2])
1083 p1=p2 1083 p1=p2
1084 if doubleLastPosition and self.length() > 1: 1084 if doubleLastPosition and self.length() > 1:
1085 diff.addPosition(diff[-1]) 1085 diff.addPosition(diff[-1])
1089 '''Returns a list of the indices at which the trajectory 1089 '''Returns a list of the indices at which the trajectory
1090 goes past the curvilinear coordinate S1 1090 goes past the curvilinear coordinate S1
1091 (in provided lane if lane is not None) 1091 (in provided lane if lane is not None)
1092 Returns an empty list if there is no crossing''' 1092 Returns an empty list if there is no crossing'''
1093 indices = [] 1093 indices = []
1094 for i in xrange(self.length()-1): 1094 for i in range(self.length()-1):
1095 q1=self.__getitem__(i) 1095 q1=self.__getitem__(i)
1096 q2=self.__getitem__(i+1) 1096 q2=self.__getitem__(i+1)
1097 if q1[0] <= S1 < q2[0] and (lane is None or (self.lanes[i] == lane and self.lanes[i+1] == lane)): 1097 if q1[0] <= S1 < q2[0] and (lane is None or (self.lanes[i] == lane and self.lanes[i+1] == lane)):
1098 indices.append(i+(S1-q1[0])/(q2[0]-q1[0])) 1098 indices.append(i+(S1-q1[0])/(q2[0]-q1[0]))
1099 return indices 1099 return indices
1310 coords.append(NaN) 1310 coords.append(NaN)
1311 plot(instants, coords, options, **kwargs) 1311 plot(instants, coords, options, **kwargs)
1312 if withOrigin and len(instants)>0: 1312 if withOrigin and len(instants)>0:
1313 plot([instants[0]], [coords[0]], 'ro', **kwargs) 1313 plot([instants[0]], [coords[0]], 'ro', **kwargs)
1314 else: 1314 else:
1315 print('Object {} has no curvilinear positions'.format(self.getNum())) 1315 print('Object {} has no curvilinear positions'.format(self.getNum()))
1316 1316
1317 def setUserType(self, userType): 1317 def setUserType(self, userType):
1318 self.userType = userType 1318 self.userType = userType
1319 1319
1320 def setFeatures(self, features, featuresOrdered = False): 1320 def setFeatures(self, features, featuresOrdered = False):
1419 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.): 1419 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.):
1420 cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication) 1420 cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication)
1421 1421
1422 def speedDiagnostics(self, framerate = 1., display = False, nInstantsIgnoredAtEnds=0): 1422 def speedDiagnostics(self, framerate = 1., display = False, nInstantsIgnoredAtEnds=0):
1423 speeds = framerate*self.getSpeeds(nInstantsIgnoredAtEnds) 1423 speeds = framerate*self.getSpeeds(nInstantsIgnoredAtEnds)
1424 coef = utils.linearRegression(range(len(speeds)), speeds) 1424 coef = utils.linearRegression(list(range(len(speeds))), speeds)
1425 print('min/5th perc speed: {} / {}\nspeed diff: {}\nspeed stdev: {}\nregression: {}'.format(min(speeds), scoreatpercentile(speeds, 5), speeds[-2]-speeds[1], std(speeds), coef[0])) 1425 print('min/5th perc speed: {} / {}\nspeed diff: {}\nspeed stdev: {}\nregression: {}'.format(min(speeds), scoreatpercentile(speeds, 5), speeds[-2]-speeds[1], std(speeds), coef[0]))
1426 if display: 1426 if display:
1427 from matplotlib.pyplot import figure, axis 1427 from matplotlib.pyplot import figure, axis
1428 figure(1) 1428 figure(1)
1429 self.plot() 1429 self.plot()
1554 ''' 1554 '''
1555 1555
1556 self.curvilinearPositions = CurvilinearTrajectory() 1556 self.curvilinearPositions = CurvilinearTrajectory()
1557 1557
1558 #For each point 1558 #For each point
1559 for i in xrange(int(self.length())): 1559 for i in range(int(self.length())):
1560 result = getSYfromXY(self.getPositionAt(i), alignments) 1560 result = getSYfromXY(self.getPositionAt(i), alignments)
1561 1561
1562 # Error handling 1562 # Error handling
1563 if(result is None): 1563 if(result is None):
1564 print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i))) 1564 print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i)))
1570 #Run through objects looking for outlier point 1570 #Run through objects looking for outlier point
1571 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win) 1571 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win)
1572 ## Recalculate projected point to new lane 1572 ## Recalculate projected point to new lane
1573 lanes = self.curvilinearPositions.getLanes() 1573 lanes = self.curvilinearPositions.getLanes()
1574 if(lanes != smoothed_lanes): 1574 if(lanes != smoothed_lanes):
1575 for i in xrange(len(lanes)): 1575 for i in range(len(lanes)):
1576 if(lanes[i] != smoothed_lanes[i]): 1576 if(lanes[i] != smoothed_lanes[i]):
1577 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]]) 1577 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]])
1578 1578
1579 # Error handling 1579 # Error handling
1580 if(result is None): 1580 if(result is None):
1594 if nFeatures == 0: 1594 if nFeatures == 0:
1595 print('Empty object features\nCannot compute smooth trajectory') 1595 print('Empty object features\nCannot compute smooth trajectory')
1596 else: 1596 else:
1597 # compute the relative position vectors 1597 # compute the relative position vectors
1598 relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i 1598 relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i
1599 for i in xrange(nFeatures): 1599 for i in range(nFeatures):
1600 for j in xrange(i): 1600 for j in range(i):
1601 fi = self.features[i] 1601 fi = self.features[i]
1602 fj = self.features[j] 1602 fj = self.features[j]
1603 inter = fi.commonTimeInterval(fj) 1603 inter = fi.commonTimeInterval(fj)
1604 if inter.length() >= minCommonIntervalLength: 1604 if inter.length() >= minCommonIntervalLength:
1605 xi = array(fi.getXCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)]) 1605 xi = array(fi.getXCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)])
1848 gtMatches = {a.getNum():{} for a in annotations} 1848 gtMatches = {a.getNum():{} for a in annotations}
1849 toMatches = {o.getNum():{} for o in objects} 1849 toMatches = {o.getNum():{} for o in objects}
1850 else: 1850 else:
1851 gtMatches = None 1851 gtMatches = None
1852 toMatches = None 1852 toMatches = None
1853 for t in xrange(firstInstant, lastInstant+1): 1853 for t in range(firstInstant, lastInstant+1):
1854 previousMatches = matches.copy() 1854 previousMatches = matches.copy()
1855 # go through currently matched GT-TO and check if they are still matched withing matchingDistance 1855 # go through currently matched GT-TO and check if they are still matched withing matchingDistance
1856 toDelete = [] 1856 toDelete = []
1857 for a in matches: 1857 for a in matches:
1858 if a.existsAtInstant(t) and matches[a].existsAtInstant(t): 1858 if a.existsAtInstant(t) and matches[a].existsAtInstant(t):
1865 toDelete.append(a) 1865 toDelete.append(a)
1866 for a in toDelete: 1866 for a in toDelete:
1867 del matches[a] 1867 del matches[a]
1868 1868
1869 # match all unmatched GT-TO 1869 # match all unmatched GT-TO
1870 matchedGTs = matches.keys() 1870 matchedGTs = list(matches.keys())
1871 matchedTOs = matches.values() 1871 matchedTOs = list(matches.values())
1872 costs = [] 1872 costs = []
1873 unmatchedGTs = [a for a in annotations if a.existsAtInstant(t) and a not in matchedGTs] 1873 unmatchedGTs = [a for a in annotations if a.existsAtInstant(t) and a not in matchedGTs]
1874 unmatchedTOs = [o for o in objects if o.existsAtInstant(t) and o not in matchedTOs] 1874 unmatchedTOs = [o for o in objects if o.existsAtInstant(t) and o not in matchedTOs]
1875 nGTs = len(matchedGTs)+len(unmatchedGTs) 1875 nGTs = len(matchedGTs)+len(unmatchedGTs)
1876 nTOs = len(matchedTOs)+len(unmatchedTOs) 1876 nTOs = len(matchedTOs)+len(unmatchedTOs)
1882 for k,v in newMatches: 1882 for k,v in newMatches:
1883 if costs[k][v] < matchingDistance: 1883 if costs[k][v] < matchingDistance:
1884 matches[unmatchedGTs[k]]=unmatchedTOs[v] 1884 matches[unmatchedGTs[k]]=unmatchedTOs[v]
1885 dist += costs[k][v] 1885 dist += costs[k][v]
1886 if debug: 1886 if debug:
1887 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.iteritems()])) 1887 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.items()]))
1888 if returnMatches: 1888 if returnMatches:
1889 for a,o in matches.iteritems(): 1889 for a,o in matches.items():
1890 gtMatches[a.getNum()][t] = o.getNum() 1890 gtMatches[a.getNum()][t] = o.getNum()
1891 toMatches[o.getNum()][t] = a.getNum() 1891 toMatches[o.getNum()][t] = a.getNum()
1892 1892
1893 # compute metrics elements 1893 # compute metrics elements
1894 ct += len(matches) 1894 ct += len(matches)
1900 mismatches = [] 1900 mismatches = []
1901 for a in matches: 1901 for a in matches:
1902 if a in previousMatches: 1902 if a in previousMatches:
1903 if matches[a] != previousMatches[a]: 1903 if matches[a] != previousMatches[a]:
1904 mismatches.append(a) 1904 mismatches.append(a)
1905 elif matches[a] in previousMatches.values(): 1905 elif matches[a] in list(previousMatches.values()):
1906 mismatches.append(matches[a]) 1906 mismatches.append(matches[a])
1907 for a in previousMatches: 1907 for a in previousMatches:
1908 if a not in matches and previousMatches[a] in matches.values(): 1908 if a not in matches and previousMatches[a] in list(matches.values()):
1909 mismatches.append(previousMatches[a]) 1909 mismatches.append(previousMatches[a])
1910 if debug: 1910 if debug:
1911 for mm in set(mismatches): 1911 for mm in set(mismatches):
1912 print('{} {}'.format(type(mm), mm.getNum())) 1912 print('{} {}'.format(type(mm), mm.getNum()))
1913 # some object mismatches may appear twice 1913 # some object mismatches may appear twice