def run_routing_backward(self, target: Union[int, Node]) -> Union[Track, None]:
"""Computes shortest path between the source node used in forward step
:func:`run_routing_forward` and any target node.
If target node has not been reached during forward search, a None object is
returned by the function.
:param target: A target node
:return: A track between the source node specified in
:func:`run_routing_forward` and a target node. The track contains topologic
and non-topologic vertices. If the node target has not been reached during
forward step, None object is output
"""
target = self.__correctInputNode(target)
node = self.NODES[target]
track = Track()
track.addObs(Obs(node.coord))
if node.antecedent == "":
return None
while (node.poids != 0) and (node.antecedent != ""):
e = self.EDGES[node.antecedent_edge]
edge_geom = e.geom.copy()
if e.source != node:
edge_geom = edge_geom.reverse()
track = track + (edge_geom > 1)
node = node.antecedent
return track
class TollGateConstraint:
"""TODO"""
def __init__(self, pt1, pt2, time=None, type=TYPE_SELECT):
"""TODO"""
self.gate = Track([Obs(pt1), Obs(pt2)])
self.time = time
self.type = type
def __str__(self):
"""TODO"""
output = "Toll gate selecting constraint"
if not self.time is None:
output += " with " + str(self.time).lower()
return output
def plot(self, sym="ro-"):
"""TODO"""
plt.plot(self.gate.getX(), self.gate.getY(), sym)
def contains(self, track):
"""TODO"""
return Geometry.intersects(self.gate, track)
def select(self, tracks):
"""TODO"""
if self.type == TYPE_SELECT:
output = TrackCollection()
for track in tracks:
if self.contains(track):
output.addTrack(track)
return output
if self.type == TYPE_CUT_AND_SELECT:
return tracks
def setUp(self): self.track = Track() p1 = Obs(ENUCoords(0, 0), "2020-01-01 10:00:00") self.track.addObs(p1) p2 = Obs(ENUCoords(0, 1), "2020-01-01 10:00:01") self.track.addObs(p2) p3 = Obs(ENUCoords(1, 2), "2020-01-01 10:00:02") self.track.addObs(p3)
def optimalSimplification(track, cost, eps, mode=MODE_SEGMENTATION_MINIMIZE):
"""TODO"""
simplified = Track(user_id=track.uid, track_id=track.tid, base=track.base)
segmentation = optimalSegmentation(track, cost, eps)
for i in range(len(segmentation)):
simplified.addObs(track.getObs(segmentation[i]).copy())
return simplified
def __init__(
self,
track,
res=1,
buffer=10,
prop=0.5,
length=0,
time=None,
mode=MODE_PARALLEL,
type=TYPE_SELECT,
):
"""TODO"""
self.track = track
self.time = time
self.type = type
self.prop = prop
self.mode = mode
self.length = length
self.segments = []
for i in range(1, len(track), 1):
pt1 = track[i].position.copy()
pt2 = pt1.copy()
dx = track[i].position.getX() - track[i - 1].position.getX()
dy = track[i].position.getY() - track[i - 1].position.getY()
R = (dx * dx + dy * dy)**(0.5)
if R == 0:
continue
pt1.translate(+buffer * dy / R, -buffer * dx / R)
pt2.translate(-buffer * dy / R, +buffer * dx / R)
self.segments.append(Track([Obs(pt1), Obs(pt2)]))
def readLineAndAddToNetwork(row, fmt):
"""TODO"""
edge_id = str(row[fmt.pos_edge_id])
if fmt.pos_edge_id < 0:
edge_id = NetworkReader.counter
NetworkReader.counter = NetworkReader.counter + 1
geom = str(row[fmt.pos_wkt])
TAB_OBS = wktLineStringToObs(geom, fmt.srid.upper())
# Au moins 2 points
if len(TAB_OBS) < 2:
return
track = Track(TAB_OBS)
Cinematics.computeAbsCurv(track)
edge = Edge(edge_id, track)
# Orientation
orientation = int(row[fmt.pos_sens])
if orientation not in [
Edge.DOUBLE_SENS, Edge.SENS_DIRECT, Edge.SENS_INVERSE
]:
orientation = Edge.DOUBLE_SENS
edge.orientation = orientation
# Poids
if fmt.pos_poids == -1:
poids = track.length()
else:
poids = float(row[fmt.pos_poids])
edge.weight = poids
# Source node
source = str(row[fmt.pos_source])
noeudIni = Node(source, track.getFirstObs().position)
# Target node
target = str(row[fmt.pos_target])
noeudFin = Node(target, track.getLastObs().position)
# Return
return (edge, noeudIni, noeudFin)
def splitAR(track, pt1, pt2=None, radius=10, nb_min_pts=10, verbose=True):
if pt2 is None:
pt2 = pt1
tracks = TrackCollection()
subtrack = Track()
k = -1
while k < len(track) - 1:
k = k + 1
if (min(track[k].position.distance2DTo(pt1),
track[k].position.distance2DTo(pt2)) < radius):
if len(subtrack) > nb_min_pts:
tracks.addTrack(subtrack)
if verbose:
print(
"Add sub-track: ",
subtrack[0].timestamp,
subtrack[-1].timestamp,
"[" + str(len(tracks)) + "]",
)
subtrack = Track()
subtrack.addObs(track[k].copy())
if len(subtrack) > nb_min_pts:
tracks.addTrack(subtrack)
if verbose:
print(
"Add sub-track: ",
subtrack[0].timestamp,
subtrack[-1].timestamp,
"[" + str(len(tracks)) + "]",
)
return tracks
def douglas_peucker(track, eps):
"""TODO"""
L = track.getObsList()
n = len(L)
if n <= 2:
return Track(L)
dmax = 0
imax = 0
for i in range(0, n):
x0 = L[i].position.getX()
y0 = L[i].position.getY()
xa = L[0].position.getX()
ya = L[0].position.getY()
xb = L[n - 1].position.getX()
yb = L[n - 1].position.getY()
d = Geometry.distance_to_segment(x0, y0, xa, ya, xb, yb)
if d > dmax:
dmax = d
imax = i
if dmax < eps:
return Track([L[0], L[n - 1]],
user_id=track.uid,
track_id=track.tid,
base=track.base)
else:
XY1 = Track(L[0:imax],
user_id=track.uid,
track_id=track.tid,
base=track.base)
XY2 = Track(L[imax:n],
user_id=track.uid,
track_id=track.tid,
base=track.base)
return douglas_peucker(XY1, eps) + douglas_peucker(XY2, eps)
def __mapOnNetwork(
track, network, obs_noise=50, transition_cost=10, search_radius=50, debug=False
):
"""TODO"""
if debug:
f1 = open("observation.dat", "a")
f2 = open("transition.dat", "a")
track.createAnalyticalFeature("obs_noise", obs_noise)
verbose = True
global STATES
global net
STATES = []
net = network
to_run = range(len(track))
if verbose:
print("Map-matching preparation...")
to_run = progressbar.progressbar(to_run)
for i in to_run:
STATES.append([])
E = network.spatial_index.neighborhood(track[i].position, unit=1)
for elem in E:
eg = network.EDGES[network.getEdgeId(elem)].geom
p, d, v = __projOnTrack(track[i].position, eg)
if d < search_radius:
STATES[-1].append(
(p, elem, __distToNode(eg, p, v, 0), __distToNode(eg, p, v, 1))
)
if debug:
wkt = Track([Obs(track[i].position), Obs(p)]).toWKT()
f1.write(str(i) + ' "' + wkt + '" ' + str(d) + "\n")
model = Dynamics.HMM()
model.setStates(__states)
model.setTransitionModel(__tst_log)
model.setObservationModel(__obs_log)
model.estimate(
track,
obs=["x", "y"],
mode=Dynamics.MODE_OBS_AS_2D_POSITIONS,
verbose=verbose * Dynamics.MODE_VERBOSE_PROGRESS,
)
for k in progressbar.progressbar(range(len(track))):
X = [track[k].position.getX(), track["hmm_inference", k][0].getX()]
Y = [track[k].position.getY(), track["hmm_inference", k][0].getY()]
plt.plot(X, Y, "g-")
track[k].position.setX(track["hmm_inference", k][0].getX())
track[k].position.setY(track["hmm_inference", k][0].getY())
def setUp(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
self.track = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.track.addObs(p1)
p2 = Obs(ENUCoords(0, 1), GPSTime.readTimestamp('2020-01-01 10:00:01'))
self.track.addObs(p2)
p3 = Obs(ENUCoords(1, 1), GPSTime.readTimestamp('2020-01-01 10:00:02'))
self.track.addObs(p3)
p4 = Obs(ENUCoords(1, 2), GPSTime.readTimestamp('2020-01-01 10:00:03'))
self.track.addObs(p4)
p5 = Obs(ENUCoords(2, 2), GPSTime.readTimestamp('2020-01-01 10:00:04'))
self.track.addObs(p5)
p6 = Obs(ENUCoords(2, 3), GPSTime.readTimestamp('2020-01-01 10:00:06'))
self.track.addObs(p6)
p7 = Obs(ENUCoords(3, 3), GPSTime.readTimestamp('2020-01-01 10:00:08'))
self.track.addObs(p7)
p8 = Obs(ENUCoords(3, 4), GPSTime.readTimestamp('2020-01-01 10:00:10'))
self.track.addObs(p8)
p9 = Obs(ENUCoords(4, 4), GPSTime.readTimestamp('2020-01-01 10:00:12'))
self.track.addObs(p9)
class TestSimplificationMethods(TestCase):
def setUp(self):
self.track = Track()
p1 = Obs(ENUCoords(0, 0), "2020-01-01 10:00:00")
self.track.addObs(p1)
p2 = Obs(ENUCoords(0, 1), "2020-01-01 10:00:01")
self.track.addObs(p2)
p3 = Obs(ENUCoords(1, 2), "2020-01-01 10:00:02")
self.track.addObs(p3)
def test_douglas_peucker(self):
self.track.simplify(5)
self.assertTrue((1.289 - 1.28) < 0.01)
def test_visvalingam(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
chemin = './data/trace1.dat'
track = FileReader.readFromFile(chemin, 2, 3, -1, 4, separator=",")
track.simplify(5, MODE_SIMPLIFY_VISVALINGAM)
self.assertTrue((1.289 - 1.28) < 0.01)
def test_gaussien(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
chemin = './data/trace1.dat'
track = FileReader.readFromFile(chemin, 2, 3, -1, 4, separator=",")
kernel = GaussianKernel(201)
track.operate(Operator.FILTER, "x", kernel, "x2")
track.operate(Operator.FILTER, "y", kernel, "y2")
plt.plot(track.getT(), track.getAnalyticalFeature("y"), 'b-', markersize=1.5)
plt.plot(track.getT(), track.getAnalyticalFeature("y2"), 'r-')
plt.show()
def test_write_csv_minim(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
track = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
track.addObs(p1)
p2 = Obs(ENUCoords(0, 1), GPSTime.readTimestamp('2020-01-01 10:00:01'))
track.addObs(p2)
csvpath = os.path.join(self.resource_path, 'data/test/test_write_csv_minim.wkt')
FileWriter.writeToFile(track, csvpath, id_E=0,id_N=1,id_U=2,id_T=3,h=1, separator=";")
contents = open(csvpath).read()
txt = "#srid: ENU\n"
txt += "#E;N;U;time\n"
txt += "0.000;0.000;0.000;01/01/2020 10:00:00\n"
txt += "0.000;1.000;0.000;01/01/2020 10:00:01\n"
self.assertEqual(contents.strip(), txt.strip())
def test_create_index_collection1(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
track = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
track.addObs(p1)
p2 = Obs(ENUCoords(2.5, 3), GPSTime.readTimestamp('2020-01-01 10:08:00'))
track.addObs(p2)
p3 = Obs(ENUCoords(2.5, 5), GPSTime.readTimestamp('2020-01-01 10:17:00'))
track.addObs(p3)
p4 = Obs(ENUCoords(7, 5), GPSTime.readTimestamp('2020-01-01 10:21:00'))
track.addObs(p4)
p5 = Obs(ENUCoords(10, 10), GPSTime.readTimestamp('2020-01-01 10:25:00'))
track.addObs(p5)
#track.plot()
#track.plotAsMarkers()
TRACES = []
TRACES.append(track)
collection = TrackCollection(TRACES)
index = SpatialIndex(collection, (2, 2))
index.plot()
# =====================================================================
# =====================================================================
self.assertEqual(index.request(0, 0), [0])
self.assertEqual(index.request(1, 0), [])
self.assertEqual(index.request(0, 1), [0])
self.assertEqual(index.request(1, 1), [0])
self.assertEqual(index.request(2, 0), [])
self.assertEqual(index.request(2, 1), [])
self.assertEqual(index.request(1, 2), [0])
self.assertEqual(index.request(2, 2), [0])
self.assertEqual(index.request(3, 2), [0])
self.assertEqual(index.request(3, 3), [0])
self.assertEqual(index.request(4, 3), [0])
self.assertEqual(index.request(4, 4), [0])
# # =====================================================================
self.assertEqual(index.request(ENUCoords(0, 0)), [0])
self.assertEqual(index.request(ENUCoords(2.5, 3)), [0])
self.assertEqual(index.request(ENUCoords(2.5, 5)), [0])
self.assertEqual(index.request(ENUCoords(7, 5)), [0])
self.assertEqual(index.request(ENUCoords(10, 10)), [0])
self.assertEqual(index.request(ENUCoords(0.5, 2.5)), [0])
self.assertEqual(index.request(ENUCoords(4.2, 5.8)), [0])
# # =====================================================================
self.assertEqual(index.request([ENUCoords(2.1, 0.5), ENUCoords(1.1, 1.1)]), [0])
self.assertEqual(index.request([ENUCoords(2.1, 0.5), ENUCoords(7.1, 3.5)]), [])
self.assertEqual(index.request([ENUCoords(5.8, 5.8), ENUCoords(2.1, 1.1)]), [0])
# # =====================================================================
self.assertEqual(index.request(track), [0])
track2 = Track()
p6 = Obs(ENUCoords(2.2, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
track2.addObs(p6)
p7 = Obs(ENUCoords(2.2, 3.8), GPSTime.readTimestamp('2020-01-01 10:08:00'))
track2.addObs(p7)
p8 = Obs(ENUCoords(6.5, 3.8), GPSTime.readTimestamp('2020-01-01 10:08:00'))
track2.addObs(p8)
self.assertEqual(index.request(track2), [0])
track3 = Track()
p9 = Obs(ENUCoords(6.5, 3.8), GPSTime.readTimestamp('2020-01-01 10:00:00'))
track3.addObs(p9)
p10 = Obs(ENUCoords(6.5, 7), GPSTime.readTimestamp('2020-01-01 10:08:00'))
track3.addObs(p10)
p11 = Obs(ENUCoords(10, 7), GPSTime.readTimestamp('2020-01-01 10:08:00'))
track3.addObs(p11)
self.assertEqual(index.request(track3), [0])
# # =====================================================================
# # =====================================================================
self.assertCountEqual(index.neighborhood(0, 4, 0), [])
self.assertCountEqual(index.neighborhood(0, 4, 1), [])
self.assertCountEqual(index.neighborhood(0, 4, 2), [0])
self.assertCountEqual(index.neighborhood(0, 4, 3), [0])
self.assertCountEqual(index.neighborhood(3, 0, 0), [])
self.assertCountEqual(index.neighborhood(3, 0, 1), [])
self.assertCountEqual(index.neighborhood(3, 0, 2), [0])
self.assertCountEqual(index.neighborhood(3, 0, 3), [0])
self.assertCountEqual(index.neighborhood(2, 2, 0), [0])
self.assertCountEqual(index.neighborhood(2, 2, 1), [0])
self.assertCountEqual(index.neighborhood(2, 2, 2), [0])
#self.assertCountEqual(index.neighborhood(2, 2, 3), [0])
# # UNIT = -1
self.assertCountEqual(index.neighborhood(2, 1, -1), [0])
self.assertCountEqual(index.neighborhood(2, 0, -1), [0])
self.assertCountEqual(index.neighborhood(0, 1, -1), [0])
self.assertCountEqual(index.neighborhood(1, 1, -1), [0])
self.assertCountEqual(index.neighborhood(0, 4, -1), [0])
self.assertCountEqual(index.neighborhood(3, 4, -1), [0])
self.assertCountEqual(index.neighborhood(4, 4, -1), [0])
self.assertCountEqual(index.neighborhood(2, 4, -1), [0])
# # =====================================================================
self.assertCountEqual(index.neighborhood(ENUCoords(0, 0.1)), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(2.5, 3)), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(2.5, 5)), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(7, 5)), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(10, 10)), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(6.5, 3.8), None, 0), [])
self.assertCountEqual(index.neighborhood(ENUCoords(6.5, 3.8), None, 1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(6.5, 3.8), None, 2), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(6.5, 3.8), None, 3), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(2.2, 3.8), None, 0), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(2.2, 3.8), None, 1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(2.2, 3.8), None, 2), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(2.2, 3.8), None, 3), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(9.9, 7), None, 0), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(9.9, 7), None, 1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(9.9, 7), None, 2), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(9.9, 7), None, 3), [0])
# # UNIT = -1
self.assertCountEqual(index.neighborhood(ENUCoords(0, 0), None, -1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(2.5, 3), None, -1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(2.5, 5), None, -1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(7, 5), None, -1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(10, 10), None, -1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(6.5, 3.8), None, -1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(2.2, 3.8), None, -1), [0])
self.assertCountEqual(index.neighborhood(ENUCoords(9.9, 7), None, -1), [0])
# # =====================================================================
self.assertEqual(index.neighborhood([ENUCoords(2.1, 0.5), ENUCoords(0.1, 2.1)], None, 0), [0])
self.assertEqual(index.neighborhood([ENUCoords(2.1, 0.5), ENUCoords(0.1, 2.1)], None, 1), [0])
self.assertEqual(index.neighborhood([ENUCoords(2.1, 0.5), ENUCoords(0.1, 2.1)], None, 2), [0])
self.assertEqual(index.neighborhood([ENUCoords(2.1, 0.5), ENUCoords(0.1, 2.1)], None, -1), [0])
self.assertEqual(index.neighborhood([ENUCoords(2.1, 0.5), ENUCoords(7.1, 3.5)]), [])
self.assertEqual(index.neighborhood([ENUCoords(2.1, 0.5), ENUCoords(7.1, 3.5)], None, 2), [0])
self.assertEqual(index.neighborhood([ENUCoords(2.1, 0.5), ENUCoords(7.1, 3.5)], None, -1), [0])
self.assertEqual(index.neighborhood([ENUCoords(5.8, 5.8), ENUCoords(2.1, 1.1)]), [0])
self.assertEqual(index.neighborhood([ENUCoords(5.8, 5.8), ENUCoords(2.1, 1.1)], None, 1), [0])
self.assertEqual(index.neighborhood([ENUCoords(5.8, 5.8), ENUCoords(2.1, 1.1)], None, 2), [0])
self.assertEqual(index.neighborhood([ENUCoords(5.8, 5.8), ENUCoords(2.1, 1.1)], None, -1), [0])
# # =====================================================================
self.assertEqual(index.neighborhood(track), [0])
self.assertEqual(index.neighborhood(track, None, 1), [0])
self.assertEqual(index.neighborhood(track, None, 3), [0])
self.assertEqual(index.neighborhood(track, None, -1), [0])
self.assertEqual(index.neighborhood(track2), [0])
self.assertEqual(index.neighborhood(track2, None, 0), [0])
self.assertEqual(index.neighborhood(track2, None, 1), [0])
self.assertEqual(index.neighborhood(track2, None, 3), [0])
self.assertEqual(index.neighborhood(track2, None, -1), [0])
self.assertEqual(index.neighborhood(track3), [0])
self.assertEqual(index.neighborhood(track3, None, 0), [0])
self.assertEqual(index.neighborhood(track3, None, 1), [0])
self.assertEqual(index.neighborhood(track3, None, 2), [0])
self.assertEqual(index.neighborhood(track3, None, 3), [0])
self.assertEqual(index.neighborhood(track3, None, -1), [0])
def test_create_index_collection2(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
track = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
track.addObs(p1)
p2 = Obs(ENUCoords(3.1, 3), GPSTime.readTimestamp('2020-01-01 10:08:00'))
track.addObs(p2)
p3 = Obs(ENUCoords(3.1, 4.5), GPSTime.readTimestamp('2020-01-01 10:17:00'))
track.addObs(p3)
p4 = Obs(ENUCoords(4.5, 4.5), GPSTime.readTimestamp('2020-01-01 10:21:00'))
track.addObs(p4)
p5 = Obs(ENUCoords(6, 5.5), GPSTime.readTimestamp('2020-01-01 10:21:00'))
track.addObs(p5)
p6 = Obs(ENUCoords(7, 4.5), GPSTime.readTimestamp('2020-01-01 10:21:00'))
track.addObs(p6)
p7 = Obs(ENUCoords(11, 5.5), GPSTime.readTimestamp('2020-01-01 10:21:00'))
track.addObs(p7)
p8 = Obs(ENUCoords(13, 10), GPSTime.readTimestamp('2020-01-01 10:25:00'))
track.addObs(p8)
#track.plot()
#track.plotAsMarkers()
TRACES = []
TRACES.append(track)
collection = TrackCollection(TRACES)
index = SpatialIndex(collection, (2, 2))
index.plot()
# =====================================================================
# =====================================================================
self.assertEqual(index.request(0, 0), [0])
self.assertEqual(index.request(1, 0), [0])
self.assertEqual(index.request(0, 1), [])
self.assertEqual(index.request(1, 1), [0])
self.assertEqual(index.request(2, 0), [])
self.assertEqual(index.request(2, 1), [])
self.assertEqual(index.request(1, 2), [0])
self.assertEqual(index.request(2, 2), [0])
self.assertEqual(index.request(3, 2), [0])
self.assertEqual(index.request(3, 3), [])
self.assertEqual(index.request(4, 2), [0])
self.assertEqual(index.request(4, 3), [])
self.assertEqual(index.request(4, 4), [])
self.assertEqual(index.request(5, 2), [0])
self.assertEqual(index.request(5, 3), [0])
self.assertEqual(index.request(5, 4), [])
class TestQuery(TestCase):
__epsilon = 0.001
def setUp(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
self.track = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.track.addObs(p1)
p2 = Obs(ENUCoords(0, 1), GPSTime.readTimestamp('2020-01-01 10:00:01'))
self.track.addObs(p2)
p3 = Obs(ENUCoords(1, 1), GPSTime.readTimestamp('2020-01-01 10:00:02'))
self.track.addObs(p3)
p4 = Obs(ENUCoords(1, 2), GPSTime.readTimestamp('2020-01-01 10:00:03'))
self.track.addObs(p4)
p5 = Obs(ENUCoords(2, 2), GPSTime.readTimestamp('2020-01-01 10:00:04'))
self.track.addObs(p5)
p6 = Obs(ENUCoords(2, 3), GPSTime.readTimestamp('2020-01-01 10:00:06'))
self.track.addObs(p6)
p7 = Obs(ENUCoords(3, 3), GPSTime.readTimestamp('2020-01-01 10:00:08'))
self.track.addObs(p7)
p8 = Obs(ENUCoords(3, 4), GPSTime.readTimestamp('2020-01-01 10:00:10'))
self.track.addObs(p8)
p9 = Obs(ENUCoords(4, 4), GPSTime.readTimestamp('2020-01-01 10:00:12'))
self.track.addObs(p9)
# self.track.plotAsMarkers()
def test_selectstar(self):
self.track.addAnalyticalFeature(Analytics.speed)
trace = self.track.query("SELECT *")
self.assertEqual(9, trace.size())
self.assertTrue(trace.hasAnalyticalFeature('speed'))
self.assertLessEqual(
(0.7071 - trace.getObsAnalyticalFeatures('speed', 1)[0]),
self.__epsilon, 'erreur de vitesse obs1')
self.assertLessEqual(
(0.2070 - trace.getObsAnalyticalFeatures('speed', 8)[0]),
self.__epsilon, 'erreur de vitesse obs8')
def test_selectwhere1(self):
self.track.addAnalyticalFeature(Analytics.speed)
trace = self.track.query("SELECT * WHERE speed < 0.5")
self.assertEqual(4, trace.size())
self.assertTrue(trace.hasAnalyticalFeature('speed'))
self.assertLessEqual(
(0.2070 - trace.getObsAnalyticalFeatures('speed', 0)[0]),
self.__epsilon, 'erreur de vitesse 0')
self.assertLessEqual(
(0.2070 - trace.getObsAnalyticalFeatures('speed', 1)[0]),
self.__epsilon, 'erreur de vitesse 1')
self.assertLessEqual(
(0.2070 - trace.getObsAnalyticalFeatures('speed', 2)[0]),
self.__epsilon, 'erreur de vitesse 2')
self.assertLessEqual(
(0.2070 - trace.getObsAnalyticalFeatures('speed', 3)[0]),
self.__epsilon, 'erreur de vitesse 3')
def test_selectwhere2(self):
self.track.addAnalyticalFeature(Analytics.speed)
trace = self.track.query("SELECT * WHERE speed > 0.5")
self.assertEqual(4, trace.size())
self.assertTrue(trace.hasAnalyticalFeature('speed'))
self.assertLessEqual(
(0.7071 - trace.getObsAnalyticalFeatures('speed', 0)[0]),
self.__epsilon, 'erreur de vitesse 0')
self.assertLessEqual(
(0.7071 - trace.getObsAnalyticalFeatures('speed', 1)[0]),
self.__epsilon, 'erreur de vitesse 1')
self.assertLessEqual(
(0.7071 - trace.getObsAnalyticalFeatures('speed', 2)[0]),
self.__epsilon, 'erreur de vitesse 2')
self.assertLessEqual(
(0.7071 - trace.getObsAnalyticalFeatures('speed', 3)[0]),
self.__epsilon, 'erreur de vitesse 3')
def test_selection_track_constraint(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
chemin = os.path.join(self.resource_path, './data/trace1.dat')
trace = FileReader.readFromFile(chemin, 2, 3, -1, 4, separator=",")
trace.plot()
# =====================================================================
trace1 = Track()
c1 = trace.getObs(1350).position
c0 = ENUCoords(c1.getX() + 5000, c1.getY())
c2 = ENUCoords(c1.getX() - 5000, c1.getY())
p1 = Obs(c0, GPSTime.readTimestamp("2018-07-31 14:00:00"))
p2 = Obs(c1, GPSTime.readTimestamp("2018-07-31 14:01:00"))
p3 = Obs(c2, GPSTime.readTimestamp("2018-07-31 14:02:00"))
trace1.addObs(p1)
trace1.addObs(p2)
trace1.addObs(p3)
plt.plot(trace1.getX(), trace1.getY(), 'r-')
plt.show()
c3 = TrackConstraint(trace1, mode=MODE_PARALLEL)
s = Selector([c3])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
c4 = TrackConstraint(trace1, mode=MODE_CROSSES)
s = Selector([c4])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
# =====================================================================
trace1 = Track()
c0 = ENUCoords(
trace.getObs(1349).position.getX(),
trace.getObs(1349).position.getY())
c1 = ENUCoords(
trace.getObs(1350).position.getX(),
trace.getObs(1350).position.getY())
c2 = ENUCoords(
trace.getObs(1351).position.getX(),
trace.getObs(1351).position.getY())
p1 = Obs(c0, GPSTime.readTimestamp("2018-07-31 14:00:00"))
p2 = Obs(c1, GPSTime.readTimestamp("2018-07-31 14:01:00"))
p3 = Obs(c2, GPSTime.readTimestamp("2018-07-31 14:02:00"))
trace1.addObs(p1)
trace1.addObs(p2)
trace1.addObs(p3)
trace.plot()
plt.plot(trace1.getX(), trace1.getY(), 'r-')
plt.show()
c3 = TrackConstraint(trace1, mode=MODE_PARALLEL)
s = Selector([c3])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
c4 = TrackConstraint(trace1, mode=MODE_CROSSES)
s = Selector([c4])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
class TestAlgoGeometricsMethods(unittest.TestCase):
__epsilon = 0.001
def setUp(self):
#----------------------------------------------------------------------
# 4 sommets sur axes du cercle trigonométrique
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
self.trace1 = Track()
c1 = ENUCoords(1, 0, 0)
p1 = Obs(c1, GPSTime.readTimestamp("2018-01-01 10:00:00"))
self.trace1.addObs(p1)
c2 = ENUCoords(0, 1, 0)
p2 = Obs(c2, GPSTime.readTimestamp("2018-01-01 10:00:12"))
self.trace1.addObs(p2)
c3 = ENUCoords(-1, 0, 0)
p3 = Obs(c3, GPSTime.readTimestamp("2018-01-01 10:00:40"))
self.trace1.addObs(p3)
c4 = ENUCoords(0, -1, 0)
p4 = Obs(c4, GPSTime.readTimestamp("2018-01-01 10:01:50"))
self.trace1.addObs(p4)
self.trace1.addObs(p1)
# ---------------------------------------------------------------------
# Un escalier
self.trace2 = Track()
pm3 = Obs(ENUCoords(-2, -1),
GPSTime.readTimestamp('2020-01-01 09:59:44'))
self.trace2.addObs(pm3)
pm2 = Obs(ENUCoords(-1, -1),
GPSTime.readTimestamp('2020-01-01 09:59:48'))
self.trace2.addObs(pm2)
pm1 = Obs(ENUCoords(-1, 0),
GPSTime.readTimestamp('2020-01-01 09:59:55'))
self.trace2.addObs(pm1)
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace2.addObs(p1)
p2 = Obs(ENUCoords(0, 2), GPSTime.readTimestamp('2020-01-01 10:00:01'))
self.trace2.addObs(p2)
p3 = Obs(ENUCoords(1, 2), GPSTime.readTimestamp('2020-01-01 10:00:02'))
self.trace2.addObs(p3)
p4 = Obs(ENUCoords(1, 5), GPSTime.readTimestamp('2020-01-01 10:00:03'))
self.trace2.addObs(p4)
p5 = Obs(ENUCoords(2, 5), GPSTime.readTimestamp('2020-01-01 10:00:04'))
self.trace2.addObs(p5)
p6 = Obs(ENUCoords(2, 9), GPSTime.readTimestamp('2020-01-01 10:00:06'))
self.trace2.addObs(p6)
p7 = Obs(ENUCoords(3, 9), GPSTime.readTimestamp('2020-01-01 10:00:08'))
self.trace2.addObs(p7)
p8 = Obs(ENUCoords(3, 14),
GPSTime.readTimestamp('2020-01-01 10:00:10'))
self.trace2.addObs(p8)
p9 = Obs(ENUCoords(4, 14),
GPSTime.readTimestamp('2020-01-01 10:00:12'))
self.trace2.addObs(p9)
p10 = Obs(ENUCoords(4, 20),
GPSTime.readTimestamp('2020-01-01 10:00:15'))
self.trace2.addObs(p10)
# ---------------------------------------------------------------------
#
self.trace3 = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p1)
p2 = Obs(ENUCoords(1.5, 0.5),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p2)
p3 = Obs(ENUCoords(2, 2), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p3)
p4 = Obs(ENUCoords(3.75, 0.6),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p4)
p5 = Obs(ENUCoords(5, 0.5),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p5)
p6 = Obs(ENUCoords(3.55, -0.5),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p6)
p7 = Obs(ENUCoords(1.8, -1.2),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p7)
p8 = Obs(ENUCoords(1, -3),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p8)
def testCircleTrigo(self):
self.trace1.plot()
C1 = Geometrics.minCircle(self.trace1)
C1.plot()
self.assertLessEqual(abs(1 - C1.radius), self.__epsilon,
"Rayon du cercle")
self.assertIsInstance(C1.center, ENUCoords)
self.assertLessEqual(abs(0 - C1.center.getX()), self.__epsilon,
"coord x du centre cercle")
self.assertLessEqual(abs(0 - C1.center.getY()), self.__epsilon,
"coord y du centre cercle")
C2 = Geometrics.fitCircle(self.trace1)
C2.plot()
self.assertLessEqual(abs(1 - C2.radius), self.__epsilon,
"Rayon du cercle")
self.assertIsInstance(C2.center, ENUCoords)
self.assertLessEqual(abs(0 - C2.center.getX()), self.__epsilon,
"coord x du centre cercle")
self.assertLessEqual(abs(0 - C2.center.getY()), self.__epsilon,
"coord y du centre cercle")
plt.show()
def testCircles(self):
self.trace2.plot()
#plt.plot(track.getX(), track.getY(), 'b+')
circle1 = Geometrics.fitCircle(self.trace2)
self.assertIsInstance(circle1, Geometrics.Circle)
self.assertLessEqual(abs(28.363 - circle1.radius), self.__epsilon,
"Rayon du cercle")
self.assertIsInstance(circle1.center, ENUCoords)
self.assertLessEqual(abs(-25.09 - circle1.center.getX()),
self.__epsilon, "coord x du centre cercle")
self.assertLessEqual(abs(14.79 - circle1.center.getY()),
self.__epsilon, "coord y du centre cercle")
circle1.plot()
circle2 = Geometrics.minCircle(self.trace2)
self.assertIsInstance(circle2, Geometrics.Circle)
circle2.plot()
circle3 = Geometrics.minCircleMatrix(self.trace2)
self.assertEqual(circle3.size, 169)
# ??
plt.show()
def testDiameter(self):
D = Geometrics.diameter(self.trace1)
A = self.trace1.getObs(D[1])
B = self.trace1.getObs(D[2])
self.assertEqual(D[0], 2)
self.assertEqual(A.distanceTo(B), D[0])
self.assertEqual(A.position.getX(), 1)
self.assertEqual(A.position.getY(), 0)
self.assertEqual(B.position.getX(), -1)
self.assertEqual(B.position.getY(), 0)
D = Geometrics.diameter(self.trace2)
A = self.trace2.getObs(D[1])
B = self.trace2.getObs(D[2])
self.assertIsInstance(A, Obs)
self.assertIsInstance(B, Obs)
self.assertEqual(A.distanceTo(B), D[0])
def testConvexHull(self):
self.trace3.plot()
T = Geometrics.convexHull(self.trace3)
Geometrics.plotPolygon(T)
plt.show()
self.assertEqual(len(T), 2 * 5)
self.assertEqual(T[0], 0)
self.assertEqual(T[1], 0)
self.assertEqual(T[4], 5)
self.assertEqual(T[5], 0.5)
self.assertEqual(T[6], 1)
self.assertEqual(T[7], -3)
self.trace2.plot()
T = Geometrics.convexHull(self.trace2)
Geometrics.plotPolygon(T)
plt.show()
def testminimumBoundingRectangle(self):
self.trace3.plot()
R = Geometrics.minimumBoundingRectangle(self.trace3)
T = []
for coord in R[0]:
T.append(coord[0])
T.append(coord[1])
Geometrics.plotPolygon(T)
self.assertEqual(R[1], 16.5)
self.assertLessEqual(abs(3.104 - R[2]), self.__epsilon, "l")
self.assertLessEqual(abs(5.315 - R[3]), self.__epsilon, "L")
def findStopsGlobalForRTK(track,
std_max=2e-2,
duration=5,
downsampling=1,
verbose=True):
"""Find stop points in a track based on maximal size of a stop and minimal
time duration
Two parameters:
- Maximal size of a stop (as the standard deviation per axis, in ground units)
- Minimal time duration (in seconds)
Use downsampling parameter > 1 to speed up the process.
Default is set for precise RTK GNSS survey (2 cm for 5 sec)
"""
# If down-sampling is required
if downsampling > 1:
track = track.copy()
track **= track.size() / downsampling
# ---------------------------------------------------------------------------
# Computes cost matrix as :
# Cij = 0 if sqrt(0.33*(std_x^2 + std_y^2 + std_Z^2)) > std_max
# Cij = 0 if time duration between pi and p-1 is < duration
# Cij = (j-i)**2 = square of the number of points of segment otherwise
# ---------------------------------------------------------------------------
C = np.zeros((track.size(), track.size()))
RANGE = range(track.size() - 2)
if verbose:
print("Minimal enclosing circles computation:")
RANGE = progressbar.progressbar(RANGE)
for i in RANGE:
for j in range(i + 1, track.size() - 1):
if track[i].distanceTo(track[j - 1]) > 3 * std_max:
C[i, j] = 0
break
if track[j - 1].timestamp - track[i].timestamp <= duration:
C[i, j] = 0
continue
portion = track.extract(i, j - 1)
varx = portion.operate(Operator.Operator.VARIANCE, "x")
vary = portion.operate(Operator.Operator.VARIANCE, "y")
varz = portion.operate(Operator.Operator.VARIANCE, "z")
C[i, j] = math.sqrt(varx + vary + varz)
C[i, j] = (C[i, j] < std_max) * (j - i)**2
C = C + np.transpose(C)
# ---------------------------------------------------------------------------
# Computes optimal partition with dynamic programing
# ---------------------------------------------------------------------------
segmentation = optimalPartition(C, MODE_SEGMENTATION_MAXIMIZE, verbose)
stops = Track()
TMP_RADIUS = []
TMP_MEAN_X = []
TMP_MEAN_Y = []
TMP_MEAN_Z = []
TMP_IDSTART = []
TMP_IDEND = []
TMP_STD_X = []
TMP_STD_Y = []
TMP_STD_Z = []
TMP_DURATION = []
TMP_NBPOINTS = []
for i in range(len(segmentation) - 1):
portion = track.extract(segmentation[i], segmentation[i + 1] - 1)
radius = C[segmentation[i], segmentation[i + 1]]
if radius == 0:
continue
xm = portion.operate(Operator.Operator.AVERAGER, "x")
ym = portion.operate(Operator.Operator.AVERAGER, "y")
zm = portion.operate(Operator.Operator.AVERAGER, "z")
xv = portion.operate(Operator.Operator.VARIANCE, "x")
yv = portion.operate(Operator.Operator.VARIANCE, "y")
zv = portion.operate(Operator.Operator.VARIANCE, "z")
pt = portion[0].position.copy()
pt.setX(xm)
pt.setY(ym)
pt.setZ(zm)
stops.addObs(Obs(pt, portion[0].timestamp))
TMP_RADIUS.append(math.sqrt(xv + yv + zv))
TMP_MEAN_X.append(xm)
TMP_MEAN_Y.append(ym)
TMP_MEAN_Z.append(zm)
TMP_STD_X.append(xv**0.5)
TMP_STD_Y.append(yv**0.5)
TMP_STD_Z.append(zv**0.5)
TMP_IDSTART.append(segmentation[i] * downsampling)
TMP_IDEND.append((segmentation[i + 1] - 1) * downsampling)
TMP_NBPOINTS.append(segmentation[i + 1] - segmentation[i])
TMP_DURATION.append(portion.duration())
if stops.size() == 0:
return stops
stops.createAnalyticalFeature("radius", TMP_RADIUS)
stops.createAnalyticalFeature("mean_x", TMP_MEAN_X)
stops.createAnalyticalFeature("mean_y", TMP_MEAN_Y)
stops.createAnalyticalFeature("mean_z", TMP_MEAN_Z)
stops.createAnalyticalFeature("id_ini", TMP_IDSTART)
stops.createAnalyticalFeature("id_end", TMP_IDEND)
stops.createAnalyticalFeature("sigma_x", TMP_STD_X)
stops.createAnalyticalFeature("sigma_y", TMP_STD_Y)
stops.createAnalyticalFeature("sigma_z", TMP_STD_Z)
stops.createAnalyticalFeature("duration", TMP_DURATION)
stops.createAnalyticalFeature("nb_points", TMP_NBPOINTS)
stops.operate(Operator.Operator.QUAD_ADDER, "sigma_x", "sigma_y", "rmse")
stops.base = track.base
return stops
def setUp(self):
#----------------------------------------------------------------------
# 4 sommets sur axes du cercle trigonométrique
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
self.trace1 = Track()
c1 = ENUCoords(1, 0, 0)
p1 = Obs(c1, GPSTime.readTimestamp("2018-01-01 10:00:00"))
self.trace1.addObs(p1)
c2 = ENUCoords(0, 1, 0)
p2 = Obs(c2, GPSTime.readTimestamp("2018-01-01 10:00:12"))
self.trace1.addObs(p2)
c3 = ENUCoords(-1, 0, 0)
p3 = Obs(c3, GPSTime.readTimestamp("2018-01-01 10:00:40"))
self.trace1.addObs(p3)
c4 = ENUCoords(0, -1, 0)
p4 = Obs(c4, GPSTime.readTimestamp("2018-01-01 10:01:50"))
self.trace1.addObs(p4)
self.trace1.addObs(p1)
# ---------------------------------------------------------------------
# Un escalier
self.trace2 = Track()
pm3 = Obs(ENUCoords(-2, -1),
GPSTime.readTimestamp('2020-01-01 09:59:44'))
self.trace2.addObs(pm3)
pm2 = Obs(ENUCoords(-1, -1),
GPSTime.readTimestamp('2020-01-01 09:59:48'))
self.trace2.addObs(pm2)
pm1 = Obs(ENUCoords(-1, 0),
GPSTime.readTimestamp('2020-01-01 09:59:55'))
self.trace2.addObs(pm1)
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace2.addObs(p1)
p2 = Obs(ENUCoords(0, 2), GPSTime.readTimestamp('2020-01-01 10:00:01'))
self.trace2.addObs(p2)
p3 = Obs(ENUCoords(1, 2), GPSTime.readTimestamp('2020-01-01 10:00:02'))
self.trace2.addObs(p3)
p4 = Obs(ENUCoords(1, 5), GPSTime.readTimestamp('2020-01-01 10:00:03'))
self.trace2.addObs(p4)
p5 = Obs(ENUCoords(2, 5), GPSTime.readTimestamp('2020-01-01 10:00:04'))
self.trace2.addObs(p5)
p6 = Obs(ENUCoords(2, 9), GPSTime.readTimestamp('2020-01-01 10:00:06'))
self.trace2.addObs(p6)
p7 = Obs(ENUCoords(3, 9), GPSTime.readTimestamp('2020-01-01 10:00:08'))
self.trace2.addObs(p7)
p8 = Obs(ENUCoords(3, 14),
GPSTime.readTimestamp('2020-01-01 10:00:10'))
self.trace2.addObs(p8)
p9 = Obs(ENUCoords(4, 14),
GPSTime.readTimestamp('2020-01-01 10:00:12'))
self.trace2.addObs(p9)
p10 = Obs(ENUCoords(4, 20),
GPSTime.readTimestamp('2020-01-01 10:00:15'))
self.trace2.addObs(p10)
# ---------------------------------------------------------------------
#
self.trace3 = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p1)
p2 = Obs(ENUCoords(1.5, 0.5),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p2)
p3 = Obs(ENUCoords(2, 2), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p3)
p4 = Obs(ENUCoords(3.75, 0.6),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p4)
p5 = Obs(ENUCoords(5, 0.5),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p5)
p6 = Obs(ENUCoords(3.55, -0.5),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p6)
p7 = Obs(ENUCoords(1.8, -1.2),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p7)
p8 = Obs(ENUCoords(1, -3),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p8)
def example3():
start = GeoCoords(2.4320023, 48.84298, 100).toECEFCoords()
print(start)
path = "data/psr.dat"
track = Track()
for i in range(47):
track.addObs(Obs(ECEFCoords(0, 0, 0)))
track.createAnalyticalFeature("m0", [0] * 47)
track.createAnalyticalFeature("sx0", [0] * 47)
track.createAnalyticalFeature("sy0", [0] * 47)
track.createAnalyticalFeature("sz0", [0] * 47)
track.createAnalyticalFeature("m1", [0] * 47)
track.createAnalyticalFeature("sx1", [0] * 47)
track.createAnalyticalFeature("sy1", [0] * 47)
track.createAnalyticalFeature("sz1", [0] * 47)
track.createAnalyticalFeature("m2", [0] * 47)
track.createAnalyticalFeature("sx2", [0] * 47)
track.createAnalyticalFeature("sy2", [0] * 47)
track.createAnalyticalFeature("sz2", [0] * 47)
track.createAnalyticalFeature("m3", [0] * 47)
track.createAnalyticalFeature("sx3", [0] * 47)
track.createAnalyticalFeature("sy3", [0] * 47)
track.createAnalyticalFeature("sz3", [0] * 47)
track.createAnalyticalFeature("m4", [0] * 47)
track.createAnalyticalFeature("sx4", [0] * 47)
track.createAnalyticalFeature("sy4", [0] * 47)
track.createAnalyticalFeature("sz4", [0] * 47)
with open(path) as fp:
line = True
for i in range(47):
for j in range(5):
line = fp.readline()
vals = line[:-2].split(",")
track.setObsAnalyticalFeature("sx" + str(j), i, float(vals[1]))
track.setObsAnalyticalFeature("sy" + str(j), i, float(vals[2]))
track.setObsAnalyticalFeature("sz" + str(j), i, float(vals[3]))
track.setObsAnalyticalFeature("m" + str(j), i, float(vals[4]))
line = fp.readline()
def F(x):
plan = ECEFCoords(x[0, 0], x[1, 0], x[2, 0]).toENUCoords(start)
plan.E += x[5, 0] * math.sin(x[4, 0])
plan.N += x[5, 0] * math.cos(x[4, 0])
xyz = plan.toECEFCoords(start)
return np.array([[xyz.X], [xyz.Y], [xyz.Z], [x[3, 0] + x[6, 0]],
[x[4, 0]], [x[5, 0]], [x[6, 0]]])
def H(x, k, track):
return np.array([
[((x[0, 0] - track["sx0", k])**2 + (x[1, 0] - track["sy0", k])**2 +
(x[2, 0] - track["sz0", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx1", k])**2 + (x[1, 0] - track["sy1", k])**2 +
(x[2, 0] - track["sz1", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx2", k])**2 + (x[1, 0] - track["sy2", k])**2 +
(x[2, 0] - track["sz2", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx3", k])**2 + (x[1, 0] - track["sy3", k])**2 +
(x[2, 0] - track["sz3", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx4", k])**2 + (x[1, 0] - track["sy4", k])**2 +
(x[2, 0] - track["sz4", k])**2)**0.5 + x[3, 0]]
])
Q = 1e0 * np.eye(7, 7)
Q[3, 3] = 0
Q[4, 4] = 1e-10
Q[5, 5] = 1e-1
Q[6, 6] = 1e-1
R = 1e1 * np.eye(5, 5)
X0 = np.array([[start.getX()], [start.getY()], [start.getZ()], [0], [0],
[0], [0]])
P0 = 1e5 * np.eye(7, 7)
P0[3, 3] = 1e6
P0[4, 4] = 1e1
P0[5, 5] = 1e1
P0[6, 6] = 1e3
UKF = Kalman(spreading=1)
UKF.setTransition(F, Q)
UKF.setObservation(H, R)
UKF.setInitState(X0, P0)
UKF.summary()
UKF.estimate(track, ["m0", "m1", "m2", "m3", "m4"],
mode=Dynamics.MODE_STATES_AS_3D_POSITIONS)
track.toGeoCoords()
track.plot('r-')
plt.show()
KmlWriter.writeToKml(track, path="couplage.kml", type="LINE")
def test_create_index(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
track = Track()
p1 = Obs(ENUCoords(550, 320), GPSTime.readTimestamp('2020-01-01 10:00:00'))
track.addObs(p1)
p2 = Obs(ENUCoords(610, 325), GPSTime.readTimestamp('2020-01-01 10:08:00'))
track.addObs(p2)
p3 = Obs(ENUCoords(610, 330), GPSTime.readTimestamp('2020-01-01 10:17:00'))
track.addObs(p3)
p4 = Obs(ENUCoords(650, 330), GPSTime.readTimestamp('2020-01-01 10:21:00'))
track.addObs(p4)
p5 = Obs(ENUCoords(675, 340), GPSTime.readTimestamp('2020-01-01 10:25:00'))
track.addObs(p5)
#track.plot()
#track.plotAsMarkers()
TRACES = []
TRACES.append(track)
collection = TrackCollection(TRACES)
res = (25, 4)
index = SpatialIndex(collection, res, 0.05, True)
index.plot()
# =====================================================================
self.assertEqual(index.request(0, 0), [0])
self.assertEqual(index.request(1, 0), [0])
self.assertEqual(index.request(0, 1), [])
self.assertEqual(index.request(1, 1), [0])
self.assertEqual(index.request(2, 0), [])
self.assertEqual(index.request(2, 1), [0])
def example4():
start = GeoCoords(2.4320023, 48.84298, 100).toECEFCoords()
path = "data/psr_all.dat"
track = Track()
Nepochs = 534
for i in range(Nepochs):
track.addObs(Obs(ECEFCoords(0, 0, 0)))
track.createAnalyticalFeature("m0", [0] * Nepochs)
track.createAnalyticalFeature("sx0", [0] * Nepochs)
track.createAnalyticalFeature("sy0", [0] * Nepochs)
track.createAnalyticalFeature("sz0", [0] * Nepochs)
track.createAnalyticalFeature("m1", [0] * Nepochs)
track.createAnalyticalFeature("sx1", [0] * Nepochs)
track.createAnalyticalFeature("sy1", [0] * Nepochs)
track.createAnalyticalFeature("sz1", [0] * Nepochs)
track.createAnalyticalFeature("m2", [0] * Nepochs)
track.createAnalyticalFeature("sx2", [0] * Nepochs)
track.createAnalyticalFeature("sy2", [0] * Nepochs)
track.createAnalyticalFeature("sz2", [0] * Nepochs)
track.createAnalyticalFeature("m3", [0] * Nepochs)
track.createAnalyticalFeature("sx3", [0] * Nepochs)
track.createAnalyticalFeature("sy3", [0] * Nepochs)
track.createAnalyticalFeature("sz3", [0] * Nepochs)
track.createAnalyticalFeature("m4", [0] * Nepochs)
track.createAnalyticalFeature("sx4", [0] * Nepochs)
track.createAnalyticalFeature("sy4", [0] * Nepochs)
track.createAnalyticalFeature("sz4", [0] * Nepochs)
track.createAnalyticalFeature("m5", [0] * Nepochs)
track.createAnalyticalFeature("sx5", [0] * Nepochs)
track.createAnalyticalFeature("sy5", [0] * Nepochs)
track.createAnalyticalFeature("sz5", [0] * Nepochs)
with open(path) as fp:
line = True
for i in range(Nepochs):
for j in range(6):
line = fp.readline()
vals = line[:-1].split(",")
track.setObsAnalyticalFeature("sx" + str(j), i, float(vals[1]))
track.setObsAnalyticalFeature("sy" + str(j), i, float(vals[2]))
track.setObsAnalyticalFeature("sz" + str(j), i, float(vals[3]))
track.setObsAnalyticalFeature("m" + str(j), i, float(vals[4]))
line = fp.readline()
track = track % [False, True]
def F(x):
return np.array([[x[0, 0]], [x[1, 0]], [x[2, 0]], [x[3, 0] + x[4, 0]],
[x[4, 0]]])
def H(x, k, track):
return np.array([
[((x[0, 0] - track["sx0", k])**2 + (x[1, 0] - track["sy0", k])**2 +
(x[2, 0] - track["sz0", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx1", k])**2 + (x[1, 0] - track["sy1", k])**2 +
(x[2, 0] - track["sz1", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx2", k])**2 + (x[1, 0] - track["sy2", k])**2 +
(x[2, 0] - track["sz2", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx3", k])**2 + (x[1, 0] - track["sy3", k])**2 +
(x[2, 0] - track["sz3", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx4", k])**2 + (x[1, 0] - track["sy4", k])**2 +
(x[2, 0] - track["sz4", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx5", k])**2 + (x[1, 0] - track["sy5", k])**2 +
(x[2, 0] - track["sz5", k])**2)**0.5 + x[3, 0]]
])
Q = 1e0 * np.eye(5, 5)
Q[3, 3] = 0
Q[4, 4] = 1e0
def R(k):
P = 1e1 * np.eye(6, 6)
if (k >= 70) and (k < 267):
for i in range(3, 6):
P[i, i] = 1e16
return P
for k in range(70, 267):
for i in range(3, 6):
track.setObsAnalyticalFeature("m" + str(i), k, 20000000)
X0 = np.array([[start.X], [start.Y], [start.Z], [0], [0]])
P0 = 1e5 * np.eye(5, 5)
P0[3, 3] = 1e8
P0[4, 4] = 1e6
UKF = Kalman(spreading=1)
UKF.setTransition(F, Q)
UKF.setObservation(H, R)
UKF.setInitState(X0, P0)
UKF.summary()
UKF.estimate(track, ["m0", "m1", "m2", "m3", "m4", "m5"],
mode=Dynamics.MODE_STATES_AS_3D_POSITIONS)
track.toGeoCoords()
KmlWriter.writeToKml(track,
path="couplage.kml",
type="POINT",
c1=[0, 1, 0, 1])
track.plot('r+')
plt.show()
def test_write_csv_path(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
track = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
track.addObs(p1)
p2 = Obs(ENUCoords(0, 1), GPSTime.readTimestamp('2020-01-01 10:00:01'))
track.addObs(p2)
p3 = Obs(ENUCoords(1, 1), GPSTime.readTimestamp('2020-01-01 10:00:02'))
track.addObs(p3)
p4 = Obs(ENUCoords(1, 2), GPSTime.readTimestamp('2020-01-01 10:00:03'))
track.addObs(p4)
p5 = Obs(ENUCoords(2, 2), GPSTime.readTimestamp('2020-01-01 10:00:04'))
track.addObs(p5)
csvpath = os.path.join(self.resource_path, 'data/test/test_write_csv_path.wkt')
FileWriter.writeToFile(track, csvpath)
contents = open(csvpath).read()
txt = "0.000,0.000,0.000,01/01/2020 10:00:00\n"
txt += "0.000,1.000,0.000,01/01/2020 10:00:01\n"
txt += "1.000,1.000,0.000,01/01/2020 10:00:02\n"
txt += "1.000,2.000,0.000,01/01/2020 10:00:03\n"
txt += "2.000,2.000,0.000,01/01/2020 10:00:04\n"
self.assertEqual(contents.strip(), txt.strip())
def __init__(self, pt1, pt2, time=None, type=TYPE_SELECT):
"""TODO"""
self.gate = Track([Obs(pt1), Obs(pt2)])
self.time = time
self.type = type
def test_write_csv_2AF_desordre(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
track = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
track.addObs(p1)
p2 = Obs(ENUCoords(0, 1), GPSTime.readTimestamp('2020-01-01 10:00:01'))
track.addObs(p2)
p3 = Obs(ENUCoords(1, 1), GPSTime.readTimestamp('2020-01-01 10:00:02'))
track.addObs(p3)
p4 = Obs(ENUCoords(1, 2), GPSTime.readTimestamp('2020-01-01 10:00:03'))
track.addObs(p4)
p5 = Obs(ENUCoords(2, 2), GPSTime.readTimestamp('2020-01-01 10:00:04'))
track.addObs(p5)
track.addAnalyticalFeature(Analytics.speed)
track.compute_abscurv()
csvpath = os.path.join(self.resource_path, 'data/test/test_write_csv_2AF_desordre.wkt')
af_names = ['speed', 'abs_curv']
FileWriter.writeToFile(track, csvpath, id_E=3, id_N=2, id_U=0, id_T=1, h=1,
separator=";", af_names=af_names)
contents = open(csvpath).read()
txt = "#srid: ENU\n"
txt += "#U;time;N;E;speed;abs_curv\n"
txt += "0.000;01/01/2020 10:00:00;0.000;0.000;1.0;0\n"
txt += "0.000;01/01/2020 10:00:01;1.000;0.000;0.7071067811865476;1.0\n"
txt += "0.000;01/01/2020 10:00:02;1.000;1.000;0.7071067811865476;2.0\n"
txt += "0.000;01/01/2020 10:00:03;2.000;1.000;0.7071067811865476;3.0\n"
txt += "0.000;01/01/2020 10:00:04;2.000;2.000;1.0;4.0\n"
self.assertEqual(contents.strip(), txt.strip())
def findStopsLocal(track, speed=1, duration=10):
track = track.copy()
stops = Track()
# track.segmentation("speed", "#mark", speed)
track.operate(Operator.Operator.DIFFERENTIATOR, "#mark")
track.operate(Operator.Operator.RECTIFIER, "#mark")
TRACES = split(track, "#mark")
TMP_MEAN_X = []
TMP_MEAN_Y = []
TMP_MEAN_Z = []
TMP_STD_X = []
TMP_STD_Y = []
TMP_STD_Z = []
TMP_DURATION = []
TMP_NBPOINTS = []
TMP_SIGMA_X = []
TMP_SIGMA_Y = []
TMP_SIGMA_Z = []
for i in range(0, len(TRACES), 2):
if TRACES[i].duration() < duration:
continue
stops.addObs(
Obs(TRACES[i].getCentroid().copy(),
TRACES[i].getFirstObs().timestamp.copy()))
TMP_SIGMA_X.append(TRACES[i].operate(Operator.Operator.AVERAGER, "x"))
TMP_SIGMA_Y.append(TRACES[i].operate(Operator.Operator.AVERAGER, "y"))
TMP_SIGMA_Z.append(TRACES[i].operate(Operator.Operator.AVERAGER, "z"))
TMP_SIGMA_X.append(TRACES[i].operate(Operator.Operator.STDDEV, "x"))
TMP_SIGMA_Y.append(TRACES[i].operate(Operator.Operator.STDDEV, "y"))
TMP_SIGMA_Z.append(TRACES[i].operate(Operator.Operator.STDDEV, "z"))
TMP_NBPOINTS.append(TRACES[i].size())
TMP_DURATION.append(TRACES[i].duration())
if stops.size() == 0:
return stops
# stops.createAnalyticalFeature("radius", TMP_RADIUS)
stops.createAnalyticalFeature("mean_x", TMP_MEAN_X)
stops.createAnalyticalFeature("mean_y", TMP_MEAN_Y)
stops.createAnalyticalFeature("mean_z", TMP_MEAN_Z)
stops.createAnalyticalFeature("sigma_x", TMP_STD_X)
stops.createAnalyticalFeature("sigma_y", TMP_STD_Y)
stops.createAnalyticalFeature("sigma_z", TMP_STD_Z)
stops.createAnalyticalFeature("duration", TMP_DURATION)
stops.createAnalyticalFeature("nb_points", TMP_NBPOINTS)
stops.operate(Operator.Operator.QUAD_ADDER, "sigma_x", "sigma_y", "rmse")
return stops
def getNetwork(bbox,
proj=None,
margin=0.0,
tolerance=0.1,
spatialIndex=True,
nomproxy=None):
"""getNetwork
:param bbox: The bounding box of the selected area (The bounding box must
be expressed in WGS84).
:param proj: projection of results, optional.
For example: 'EPSG:2154' or 'EPSG:4326'
"""
# Adding margin
xmin, xmax, ymin, ymax = bbox
dx = (xmax - xmin) / 2
dy = (ymax - ymin) / 2
bbox = (
xmin - margin * dx,
xmax + margin * dx,
ymin - margin * dy,
ymax + margin * dy,
)
network = Network()
if spatialIndex:
network.spatial_index = SpatialIndex(bbox,
resolution=(dx / 1e3,
dy / 1e3),
margin=0.4)
network.spatial_index.collection = network
cptNode = 0
if proj == None:
proj = "EPSG:4326"
fmt = NetworkFormat()
fmt.createFromDict({
"name": "WFS",
"pos_edge_id": 0,
"pos_source": 5,
"pos_target": 6,
"pos_wkt": 1,
"pos_poids": 3,
"pos_sens": 4,
"srid": "GEO",
})
# PROXY
proxyDict = {}
if nomproxy != None:
with open(IgnReader.PROXY_FILE_FORMAT) as ffmt:
line = ffmt.readline().strip()
while line:
if line[0] == "#":
line = ffmt.readline().strip()
continue
tab = line.split(",")
if tab[0].strip() == nomproxy:
FIELDS = tab
proxyDict[FIELDS[1].lower()] = FIELDS[2]
line = ffmt.readline().strip()
ffmt.close()
nbRoute = IgnReader.__getNbRouteEmprise(bbox, proxyDict)
nbiter = int(nbRoute / IgnReader.NB_PER_PAGE) + 1
offset = 0
for j in range(nbiter):
print("PAGE " + str(j + 1) + "/" + str(nbiter))
URL_FEAT = IgnReader.URL_SERVER
URL_FEAT += "BBOX=" + str(bbox[2]) + "," + str(bbox[0])
URL_FEAT += "," + str(bbox[3]) + "," + str(bbox[1])
URL_FEAT += "&count=" + str(IgnReader.NB_PER_PAGE)
URL_FEAT += "&startIndex=" + str(offset)
URL_FEAT += "&RESULTTYPE=results"
URL_FEAT += "&srsname=" + proj
# print (URL_FEAT)
response = requests.get(URL_FEAT, proxies=proxyDict)
data = json.loads(response.text)
features = data["features"]
for feature in progressbar.progressbar(features):
row = []
idd = feature["id"]
# nature = feature['properties']['nature']
fictif = feature["properties"]["fictif"]
if fictif == "True":
continue
row.append(idd)
# TODO
# pos = feature['properties']['position_par_rapport_au_sol']
TAB_OBS = []
coords = feature["geometry"]["coordinates"]
if feature["geometry"]["type"] == "LineString":
# print (str(len(coords)))
# geom = coords
# print (coords)
typeCoord = "GEOCOORDS"
if proj == "EPSG:2154":
typeCoord = "ENUCoords"
TAB_OBS = tabCoordsLineStringToObs(coords, typeCoord)
if len(TAB_OBS) < 2:
continue
track = Track(TAB_OBS)
row.append(track.toWKT())
row.append("ENU")
row.append(track.length())
# Orientation
sens = feature["properties"]["sens_de_circulation"]
orientation = Edge.DOUBLE_SENS
if sens == None or sens == "":
orientation = Edge.DOUBLE_SENS
elif sens == "Double sens" or sens == "Sans objet":
orientation = Edge.DOUBLE_SENS
elif sens == "Direct" or sens == "Sens direct":
orientation = Edge.SENS_DIRECT
elif sens == "Indirect" or sens == "Sens inverse":
orientation = Edge.SENS_INVERSE
else:
print(sens)
row.append(orientation)
# Source node
idNoeudIni = str(cptNode)
p1 = track.getFirstObs().position
candidates = selectNodes(network, Node("0", p1), tolerance)
if len(candidates) > 0:
c = candidates[0]
idNoeudIni = c.id
else:
cptNode += 1
# Target node
idNoeudFin = str(cptNode)
p2 = track.getLastObs().position
candidates = selectNodes(network, Node("0", p2), tolerance)
if len(candidates) > 0:
c = candidates[0]
idNoeudFin = c.id
else:
cptNode += 1
row.append(idNoeudIni)
row.append(idNoeudFin)
(edge, noeudIni, noeudFin) = reader(row, fmt)
network.addEdge(edge, noeudIni, noeudFin)
#
offset = offset + IgnReader.NB_PER_PAGE
return network
def findStopsGlobal(track,
diameter=20,
duration=60,
downsampling=1,
verbose=True):
"""Find stop points in a track based on two parameters:
Maximal size of a stop (as the diameter of enclosing circle,
in ground units) and minimal time duration (in seconds)
Use downsampling parameter > 1 to speed up the process"""
# If down-sampling is required
if downsampling > 1:
track = track.copy()
track **= track.size() / downsampling
# ---------------------------------------------------------------------------
# Computes cost matrix as :
# Cij = 0 if size of enclosing circle of pi, pi+1, ... pj-1 is > diameter
# Cij = 0 if time duration between pi and p-1 is < duration
# Cij = (j-i)**2 = square of the number of points of segment otherwise
# ---------------------------------------------------------------------------
C = np.zeros((track.size(), track.size()))
RANGE = range(track.size() - 2)
if verbose:
print("Minimal enclosing circles computation:")
RANGE = progressbar.progressbar(RANGE)
for i in RANGE:
for j in range(i + 1, track.size() - 1):
if track[i].distance2DTo(track[j - 1]) > diameter:
C[i, j] = 0
break
if track[j - 1].timestamp - track[i].timestamp <= duration:
C[i, j] = 0
continue
C[i, j] = 2 * minCircle(track.extract(i, j - 1))[1]
C[i, j] = (C[i, j] < diameter) * (j - i)**2
C = C + np.transpose(C)
# ---------------------------------------------------------------------------
# Computes optimal partition with dynamic programing
# ---------------------------------------------------------------------------
segmentation = optimalPartition(C, MODE_SEGMENTATION_MAXIMIZE, verbose)
stops = Track()
TMP_RADIUS = []
TMP_MEAN_X = []
TMP_MEAN_Y = []
TMP_MEAN_Z = []
TMP_IDSTART = []
TMP_IDEND = []
TMP_STD_X = []
TMP_STD_Y = []
TMP_STD_Z = []
TMP_DURATION = []
TMP_NBPOINTS = []
for i in range(len(segmentation) - 1):
portion = track.extract(segmentation[i], segmentation[i + 1] - 1)
C = minCircle(portion)
if (C[1] > diameter / 2) or (portion.duration() < duration):
continue
stops.addObs(Obs(C[0], portion.getFirstObs().timestamp))
TMP_RADIUS.append(C[1])
TMP_MEAN_X.append(portion.operate(Operator.Operator.AVERAGER, "x"))
TMP_MEAN_Y.append(portion.operate(Operator.Operator.AVERAGER, "y"))
TMP_MEAN_Z.append(portion.operate(Operator.Operator.AVERAGER, "z"))
TMP_STD_X.append(portion.operate(Operator.Operator.STDDEV, "x"))
TMP_STD_Y.append(portion.operate(Operator.Operator.STDDEV, "y"))
TMP_STD_Z.append(portion.operate(Operator.Operator.STDDEV, "z"))
TMP_IDSTART.append(segmentation[i] * downsampling)
TMP_IDEND.append((segmentation[i + 1] - 1) * downsampling)
TMP_NBPOINTS.append(segmentation[i + 1] - segmentation[i])
TMP_DURATION.append(portion.duration())
if stops.size() == 0:
return stops
stops.createAnalyticalFeature("radius", TMP_RADIUS)
stops.createAnalyticalFeature("mean_x", TMP_MEAN_X)
stops.createAnalyticalFeature("mean_y", TMP_MEAN_Y)
stops.createAnalyticalFeature("mean_z", TMP_MEAN_Z)
stops.createAnalyticalFeature("id_ini", TMP_IDSTART)
stops.createAnalyticalFeature("id_end", TMP_IDEND)
stops.createAnalyticalFeature("sigma_x", TMP_STD_X)
stops.createAnalyticalFeature("sigma_y", TMP_STD_Y)
stops.createAnalyticalFeature("sigma_z", TMP_STD_Z)
stops.createAnalyticalFeature("duration", TMP_DURATION)
stops.createAnalyticalFeature("nb_points", TMP_NBPOINTS)
stops.operate(Operator.Operator.QUAD_ADDER, "sigma_x", "sigma_y", "rmse")
stops.base = track.base
return stops