Source code for tracklib.algo.Cinematics

"""
Class to manage cinematic computations on GPS tracks
"""

from tracklib.algo.Analytics import BIAF_SPEED, speed
from tracklib.algo.Analytics import BIAF_HEADING, heading
from tracklib.algo.Analytics import BIAF_DS, ds
from tracklib.algo.Analytics import BIAF_ABS_CURV
from tracklib.algo.Analytics import anglegeom
import tracklib.core.Operator as Operator
from tracklib.util.Geometry import angleBetweenThreePoints


[docs]def estimate_speed(track): """ Compute and return speed for each point Difference finie arriere-avant """ if track.hasAnalyticalFeature(BIAF_SPEED): return track.getAnalyticalFeature(BIAF_SPEED) else: return track.addAnalyticalFeature(speed)
# -------------------------------------------------- # Difference finie centree lissee # --------------------------------------------------
[docs]def smoothed_speed_calculation(track, width): """TODO :param track: TODO :param width: TODO :return: TODO """ computeAbsCurv(track) S = track.getAbsCurv() track.estimate_speed() if track.size() < width: print("warning:not enough point in track for this width") return None for i in range(width, len(S) - width): ds = S[i + width] - S[i - width] dt = track[i + width].timestamp - track[i - width].timestamp if dt != 0: track.setObsAnalyticalFeature("speed", i, ds / dt) for i in range(width): track.setObsAnalyticalFeature("speed", i, track["speed"][width]) for i in range(len(S) - width, len(S)): track.setObsAnalyticalFeature("speed", i, track["speed"][len(S) - width])
[docs]def estimate_heading(track): """ Compute and return speed for each point Difference finie arriere-avant """ if track.hasAnalyticalFeature(BIAF_HEADING): return track.getAnalyticalFeature(BIAF_HEADING) else: return track.addAnalyticalFeature(heading)
[docs]def computeAvgSpeed(track, id_ini=0, id_fin=None): """ Computes averaged speed (m/s) between two points TODO : à adapter """ if id_fin is None: id_fin = track.size() - 1 # extract ? d = computeCurvAbsBetweenTwoPoints(track, id_ini, id_fin) t = track[id_fin].timestamp - track[id_ini].timestamp return d / t
[docs]def computeAvgAscSpeed(track, id_ini=0, id_fin=None): """ Computes average ascending speed (m/s) TODO : à adapter """ if id_fin is None: id_fin = track.size() - 1 dp = computeAscDeniv(track, id_ini, id_fin) t = track[id_fin].timestamp - track[id_ini].timestamp return dp / t
[docs]def computeAbsCurv(track): """Compute and return curvilinear abscissa for each points""" if not track.hasAnalyticalFeature(BIAF_DS): track.addAnalyticalFeature(ds, BIAF_DS) if not track.hasAnalyticalFeature(BIAF_ABS_CURV): track.operate(Operator.Operator.INTEGRATOR, BIAF_DS, BIAF_ABS_CURV) track.removeAnalyticalFeature(BIAF_DS) return track.getAnalyticalFeature(BIAF_ABS_CURV)
[docs]def computeCurvAbsBetweenTwoPoints(track, id_ini=0, id_fin=None): """Computes and return the curvilinear abscissa between two points TODO : adapter avec le filtre""" if id_fin is None: id_fin = track.size() - 1 s = 0 for i in range(id_ini, id_fin): s = s + track[i].position.distance2DTo(track[i + 1].position) return s
[docs]def computeNetDeniv(track, id_ini=0, id_fin=None): """Computes net denivellation (in meters)""" if id_fin is None: id_fin = track.size() - 1 return track[id_fin].position.getZ() - track[id_ini].position.getZ()
[docs]def computeAscDeniv(track, id_ini=0, id_fin=None): """Computes positive denivellation (in meters)""" if id_fin is None: id_fin = track.size() - 1 dp = 0 for i in range(id_ini, id_fin): Z1 = track[i].position.getZ() Z2 = track[i + 1].position.getZ() if Z2 > Z1: dp += Z2 - Z1 return dp
[docs]def computeDescDeniv(track, id_ini=0, id_fin=None): """Computes negative denivellation (in meters)""" if id_fin is None: id_fin = track.size() - 1 dn = 0 for i in range(id_ini, id_fin): Z1 = track[i].position.getZ() Z2 = track[i + 1].position.getZ() if Z2 < Z1: dn += Z2 - Z1 return dn
[docs]def computeRadialSignature(track, factor=1): track = track.copy(); track.loop() R = track.getEnclosedPolygon().signature() track.createAnalyticalFeature("s", R[0]) track.createAnalyticalFeature("r", R[1]) return track
# ============================================================================= #
[docs]def inflection(track): """ Among the characteristic points, inflection points are those the curvature changes sign. In tracklib, this characteristic is modeled as an AF algorithm to detect if the observation obs(i) is an inflection point or not. Le principe de détection est fondé sur l'étude de la variation des produits vectoriels le long de la ligne. Les points d'inflexion sont détectés aux changements de signe de ces produits. Pour éviter les micros inflexion, on considère aussi qu'on a au moins 2 produits consécutifs de même signe de part et d’autre. Normalement, le point d'inflexion est le milieu de [oi, oi+1]. TODO : Pour ne pas avoir à ajouter de points, on prend oi, à changer. Parameters ----------- :param track: a track to compute inflection point :param i: the th point :type track: Track :type i: int :returns: 1 if obs(i) is a inflection point, 0 else. :rtype: int """ track.createAnalyticalFeature('inflection', 0) for i in range(track.size()): if i == 0 or i == 1 or i == 2: continue if i == track.size()-1 or i == track.size()-2 or i == track.size()-3: continue x0 = track.getObs(i-2).position.getX() y0 = track.getObs(i-2).position.getY() x1 = track.getObs(i-1).position.getX() y1 = track.getObs(i-1).position.getY() x2 = track.getObs(i).position.getX() y2 = track.getObs(i).position.getY() x3 = track.getObs(i+1).position.getX() y3 = track.getObs(i+1).position.getY() x4 = track.getObs(i+2).position.getX() y4 = track.getObs(i+2).position.getY() x5 = track.getObs(i+3).position.getX() y5 = track.getObs(i+3).position.getY() d1 = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1) d2 = (x3 - x2) * (y4 - y2) - (y3 - y2) * (x4 - x2) # Signe différent => 1 isPICandidat = 0 if d1 > 0 and d2 == 0: isPICandidat = 1 if d1 < 0 and d2 == 0: isPICandidat = 1 if d2 > 0 and d1 == 0: isPICandidat = 1 if d2 < 0 and d1 == 0: isPICandidat = 1 if isPICandidat == 0: isPICandidat = (d1 > 0 and d2 < 0) or (d1 < 0 and d2 > 0) # On regarde un coup de plus avant, il faut le même signe que d1 if isPICandidat == 1: d11 = (x1 - x0) * (y2 - y0) - (y1 - y0) * (x2 - x0) if (d1 > 0 and d11 > 0) or (d1 < 0 and d11 < 0): d22 = (x4 - x3) * (y5 - y3) - (y4 - y3) * (x5 - x3) # print (i, d11, d22) if (d2 > 0 and d22 > 0) or (d2 < 0 and d22 < 0): #print (i) track.setObsAnalyticalFeature('inflection', i, 1)
[docs]def setVertexAF(track): ''' Vertices are characteristic points of a track corresponding to the maxima of curvature between two inflexion points. "sommet" dans la thèse de Plazannet This function is an AF algorithm to detect if the observation obs(i) is a vertex point of the track or not. Parameters ----------- :param track: a track to compute inflection point :param i: the th point :type track: Track :type i: int :returns: 1 if obs(i) is a vertex point, 0 else. :rtype: int ''' track.createAnalyticalFeature('vertex', 0) if not track.hasAnalyticalFeature('inflection'): inflection(track) for i in range(track.size()): # on cherche imin, l'indice du point d'inflexion le plus proche # en amont de la trace imin = 0 j = i-1 while j >= 0: if track.getObsAnalyticalFeature('inflection', j) == 1: imin = j break j -= 1 # on cherche imax, l'indice du point d'inflexion le plus proche # en aval de la trace imax = track.size() -1 j = i+1 while j <= track.size() - 1: if track.getObsAnalyticalFeature('inflection', j) == 1: imax = j break j += 1 #print (i, imin, imax) #if imin == 0: # continue #if imax == track.size() - 1: # continue #print (i, imin, imax) # on cherche la plus petite courbure dans ]imin, imax[ K = 400 iK = -1 for j in range(imin, imax+1): kj = anglegeom(track, j) if kj < K: K = kj iK = j #print (' ', i, iK) track.setObsAnalyticalFeature('vertex', iK, 1)
from numpy import pi
[docs]def setBendAsAF(track, angle_min = pi/2): ''' Attribution des points de la trace qui composent le virage défini par le sommet et les points d'inflexion les plus proches de chaque côté. Un bon virage est un virage dont l'angle avec le sommet et ses points d'inflexion est inférieur à angle_min. AF = 'bend' and value is 1 if obs(i) is in a bend, 0 else. Parameters ---------- T : Track La trace dont on veut extraire les points autour du sommet. angle_min : float angle min in radians. ''' track.createAnalyticalFeature('bend', 0) if not track.hasAnalyticalFeature('inflection'): inflection(track) if not track.hasAnalyticalFeature('vertex'): setVertexAF(track) for i in range(track.size()): # On ne traite que les virages à partir des sommets afsommet = track.getObsAnalyticalFeature('vertex', i) if afsommet == 1: # on cherche le pt inflexion avant deb = 0 for j in range(i, -1, -1): ptinflexion = track.getObsAnalyticalFeature('inflection', j) if ptinflexion == 1: deb = j break # On cherche le pt inflexion apres fin = track.size()-1 for j in range(i, track.size()): ptinflexion = track.getObsAnalyticalFeature('inflection', j) if ptinflexion == 1: fin = j break #print (i, deb, fin) angle_virage = angleBetweenThreePoints(track.getObs(deb), track.getObs(i), track.getObs(fin)) # print (angle_virage*180/pi, angle_min*180/pi) if angle_virage < angle_min: # print (deb, fin, angle_virage, garde) # Le virage est un bon virage, on prend tous les points for j in range(deb, fin): track.setObsAnalyticalFeature('bend', j, 1) if fin < track.size(): track.setObsAnalyticalFeature('bend', fin, 1)
[docs]def setSwitchbacksAsAF(track, nb_virage_min = 3, dist_max = 150): ''' Fusion des virages (consécutifs ou pas) si leur nombre est supérieur à nb_virage_min et si la distance maximale entre deux sommets est inférieure à dist_max. Attention: c'est une structure de fonction particulière qui créée un AF, elle ne s'appelle pas avec la méthode addAnalyticalFeature. TODO: a revoir Parameters ----------- track : Track nb_virage_min : nombre dist_max : distance ''' # track.createAnalyticalFeature('switchbacks', 0) if not track.hasAnalyticalFeature('inflection'): inflection(track) if not track.hasAnalyticalFeature('vertex'): setVertexAF(track) if not track.hasAnalyticalFeature('bend'): setBendAsAF(track) SERIE = False deb = 0 fin = 0 # Nombre de virage de la série nbvirage = 0 # Calcul de la distance entre 2 sommets dist = 0 for i in range(track.size()): afsommet = track["vertex", i] afvirage = track["bend", i] if afvirage == 1 and not SERIE: #print ("on démarre à : ", i) SERIE = True deb = i dist = 0 elif SERIE and afvirage == 1: dist += track.getObs(i).distanceTo(track.getObs(i-1)) #print (dist) elif SERIE and afvirage != 1: # Est-ce qu'on a fini la série ? fini = True # Sauf si le point suivant for j in range(i+1, track.size()): afprochainvirage = track.getObsAnalyticalFeature('bend', j) if afprochainvirage == 1: dhorsvirage = track.getObs(j).distanceTo(track.getObs(i)) if dhorsvirage < dist_max: fini = False break if fini: # print ('fin, ', deb, fin, nbvirage) # On a fini la série, on passe les AF de la série à 1 fin = i - 1 if nbvirage >= nb_virage_min: for k in range(deb, fin): track.setObsAnalyticalFeature('switchbacks', k, 1) SERIE = False deb = 0 fin = 0 nbvirage = 0 dist = 0 if afsommet == 1 and afvirage == 1: if nbvirage == 0: dist = 0 nbvirage = 1 elif dist > dist_max: # On stoppe la série SERIE = False deb = 0 fin = 0 nbvirage = 0 dist = 0 # print ('stop', i) else: nbvirage += 1 dist = 0
#