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 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 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 test_selection_combinaison_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()
t1 = TimeConstraint(begin=GPSTime('2018-07-31 14:00:00'))
t3 = TimeConstraint(begin=GPSTime('2018-07-25 14:00:00'))
center = trace.getObs(int(trace.size() / 2)).position
radius = 91000
circle1 = Geometrics.Circle(center, radius)
circle1.plot('b-')
pt = trace.getObs(int(trace.size() / 2)).position
ll = ENUCoords(pt.getX() + 1000, pt.getY() + 15000)
ur = ENUCoords(pt.getX() + 10000, pt.getY() + 40000)
rect1 = Geometrics.Rectangle(ll, ur)
rect1.plot()
# =====================================================================
c1 = Constraint(shape=circle1, time=t1, mode=MODE_CROSSES)
s1 = Selector([c1])
c2 = Constraint(shape=rect1, time=t3, mode=MODE_CROSSES)
s2 = Selector([c2])
selector = GlobalSelector([s1, s2])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
selector = GlobalSelector([s1, s2], combination=COMBINATION_OR)
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
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 fitCircle(track, iter_max=100, epsilon=1e-10):
"""TODO"""
X = np.ones((3, 1))
c = track.getCentroid()
N = track.size()
X[0] = c.getX()
X[1] = c.getY()
J = np.zeros((N, 3))
B = np.zeros((N, 1))
for k in range(iter_max):
for i in range(N):
obs = track[i].position
x = obs.getX()
y = obs.getY()
R = math.sqrt((x - X[0])**2 + (y - X[1])**2)
J[i, 0] = 2 * (X[0, 0] - x)
J[i, 1] = 2 * (X[1, 0] - y)
J[i, 2] = -2 * R
B[i, 0] = X[2] - R
try:
dX = np.linalg.solve(np.transpose(J) @ J, np.transpose(J) @ B)
X = X + dX
except np.linalg.LinAlgError as err:
print(err)
return Circle(ENUCoords(0, 0), 0)
if X[0] != 0:
NX0 = abs(dX[0] / X[0])
else:
NX0 = 0
if X[1] != 0:
NX1 = abs(dX[1] / X[1])
else:
NX1 = 0
if X[2] != 0:
NX2 = abs(dX[2] / X[2])
else:
NX2 = 0
if max(max(NX0, NX1), NX2) < epsilon:
break
residuals = [0] * len(track)
for i in range(len(residuals)):
residuals[i] = ((track[i].position.getX() - X[0])**2 +
(track[i].position.getY() - X[1])**2 - X[2]**2)
sign = -1 * (residuals[i] < 0) + 1 * (residuals[i] > 0)
residuals[i] = sign * math.sqrt(abs(residuals[i]))
track.createAnalyticalFeature("#circle_residual", residuals)
return Circle(ENUCoords(X[0], X[1]), X[2])
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 __init__(self,
shape=None,
time=None,
mode=MODE_CROSSES,
type=TYPE_SELECT,
srid="ENU"):
"""TODO"""
if shape is None:
if srid.upper in ["GEO", "GeoCoords"]:
shape = Rectangle(GeoCoords(-180, -90), GeoCoords(180, 90))
else:
shape = Rectangle(ENUCoords(-1e300, -1e300),
ENUCoords(1e300, 1e300))
self.shape = shape
self.mode = mode
self.type = type
if time is None:
self.time = TimeConstraint()
else:
self.time = time
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), [])
def test_selection_one_shape_time_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()
t1 = TimeConstraint(begin=GPSTime('2018-07-31 14:00:00'))
t2 = TimeConstraint(begin=GPSTime('2019-07-31 14:00:00'))
t3 = TimeConstraint(begin=GPSTime('2018-07-25 14:00:00'))
center = trace.getObs(int(trace.size() / 2)).position
radius = 91000
circle1 = Geometrics.Circle(center, radius)
#circle1.plot()
#plt.show()
pt = trace.getObs(int(trace.size() / 2)).position
center = ENUCoords(pt.getX() + 10000, pt.getY() + 30000)
radius = 10000
circle4 = Geometrics.Circle(center, radius)
# =====================================================================
c1 = Constraint(shape=circle1, time=t1, mode=MODE_CROSSES)
s = Selector([c1])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
# =====================================================================
c1 = Constraint(shape=circle1, time=t2, mode=MODE_CROSSES)
s = Selector([c1])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
# =====================================================================
c1 = Constraint(shape=circle1, time=t3, mode=MODE_CROSSES)
s = Selector([c1])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
# =====================================================================
c1 = Constraint(shape=circle4, time=t1, mode=MODE_CROSSES)
s = Selector([c1])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
def wktLineStringToObs(wkt, srid):
"""
Une polyligne de n points est modélisée par une Track (timestamp = 1970/01/01 00 :00 :00)
Cas LINESTRING()
"""
# Creation d'une liste vide
TAB_OBS = list()
# Separation de la chaine
coords_string = wkt.split("(")
coords_string = coords_string[1]
coords_string = coords_string.split(")")[0]
coords = coords_string.split(",")
for i in range(0, len(coords)):
sl = coords[i].strip().split(" ")
x = float(sl[0])
y = float(sl[1])
if len(sl) == 3:
z = float(sl[2])
else:
z = 0.0
if not srid.upper() in [
"ENUCOORDS",
"ENU",
"GEOCOORDS",
"GEO",
"ECEFCOORDS",
"ECEF",
]:
print("Error: unknown coordinate type [" + str(srid) + "]")
exit()
if srid.upper() in ["ENUCOORDS", "ENU"]:
point = ENUCoords(x, y, z)
if srid.upper() in ["GEOCOORDS", "GEO"]:
point = GeoCoords(x, y, z)
if srid.upper() in ["ECEFCOORDS", "ECEF"]:
point = ECEFCoords(x, y, z)
TAB_OBS.append(Obs(point, GPSTime()))
return TAB_OBS
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)
def __welzl(C):
"""TODO
Finds minimal bounding circle with Welzl's algorithm"""
P = C.center
P = P.copy()
R = C.radius
R = R.copy()
if (len(P) == 0) or (len(R) == 3):
if len(R) == 0:
return Circle(ENUCoords(0, 0, 0), 0)
if len(R) == 1:
return __circle(R[0])
if len(R) == 2:
return __circle(R[0], R[1])
return __circle(R[0], R[1], R[2])
id = random.randint(0, len(P) - 1)
p = P[id]
P2 = []
for i in range(len(P)):
if i == id:
continue
P2.append(P[i])
P = P2
D = __welzl(Circle(P, R))
if D is None:
return None
elif p.distance2DTo(D.center) < D.radius:
return D
else:
R.append(p)
return __welzl(Circle(P, R))
def tabCoordsLineStringToObs(coords, srid):
"""TODO"""
# Creation d'une liste vide
TAB_OBS = list()
for i in range(0, len(coords)):
sl = coords[i]
x = float(sl[0])
y = float(sl[1])
if len(sl) == 3:
z = float(sl[2])
else:
z = 0.0
if not srid.upper() in [
"ENUCOORDS",
"ENU",
"GEOCOORDS",
"GEO",
"ECEFCOORDS",
"ECEF",
]:
print("Error: unknown coordinate type [" + str(srid) + "]")
exit()
if srid.upper() in ["ENUCOORDS", "ENU"]:
point = ENUCoords(x, y, z)
if srid.upper() in ["GEOCOORDS", "GEO"]:
point = GeoCoords(x, y, z)
if srid.upper() in ["ECEFCOORDS", "ECEF"]:
point = ECEFCoords(x, y, z)
TAB_OBS.append(Obs(point, GPSTime()))
return TAB_OBS
def mapOn(
track,
reference,
TP1=[],
TP2=[],
init=[],
apply: bool = True,
N_ITER_MAX: int = 20,
NPTS: int = 30,
mode: str = "2D",
verbose: bool = True,
):
"""Geometric affine transformation to align two tracks with different
coordinate systems.
For "2D" mode, coordinates must be ENU or Geo. For "3D" mode, any type of
coordinates is valid. In general, it is recommended to avoid usage of non-metric
Geo coordinates for mapping operation, since it is relying on an isotropic error
model.
TP1 and TP2 must have same size. Adjustment is performed with least squares. The
general transformation from point X to point X' is provided below:
.. math::
X' = kRX + T
with: :math:`k` a positive real value, :math:`R` a 2D or 3D rotation matrix and
:math:`T` a 2D or 3D translation vector. Transformation parameters are returned in
standard output in the following format: [theta, k, tx, ty] (theta in radians).
Track argument may also be replaced by a list of points. If TP1 is an empty list or
if it is not specified, adjustment is performed with iterative closest point (ICP)
algorithm, to solve both the transfo and the data association problems in a single
framework. This method requires however that the "initial guess" (i.e. scale
difference and rotation between both datasets) be not too far from reality, in
order to reach a good solution. For standard least squares, time complexity of the
method is :math:`O(n^2)` with :math:`n` the number of points used for data matching.
For ICP, data association step is :math:`O(n^2)` and least squares resolution
is :math:`O(n^2)` hence an overall complexity equal to N_ITER_MAX * O(NPTS^2).
In general NPTS = 30 performs fair enough.
Note that mapOn does not handle negative determinant (symetries not allowed)
:param reference: another track we want to align on or a list of points
:param TP1: list of tie points indices (relative to track)
:param TP2: list of tie points indices (relative to reference). If TP2 is not
specified, it is assumed equal to TP1.
:param init: "initial guess" vector : [rotation angle, scale, tx, ty]
:param N_ITER_MAX: maximal number of iterations (in least squares or ICP)
:param apply: boolean value to specify if estimated transfo must be performed
:param mode: could be "2D" (default) or "3D"
:param NPTS: integer specifying number of points to consider (for ICP only)
:return: transformation parameters in the following format: [theta, k, tx, ty]
"""
if len(TP2) == 0:
TP2 = TP1
if not (len(TP1) == len(TP2)):
print("Error: tie points lists must have same size")
exit()
# --------------------------------------------------------------------------
# 2D mode with non-linear least squares. Initial guess needs to be not
# too far from reality. If tie points are not provided, iterative closest
# point (ICP) algorithm is performed to find both data association and
# rotation/translation/scale parameters.
# --------------------------------------------------------------------------
if mode == "2D":
if len(TP1) == 0: # Recursive solution
track_copy = track.copy()
# Initial guess (if provided)
if len(init) == 4:
track_copy.rotate(init[0, 0])
track_copy.scale(init[1, 0])
track_copy.translate(init[2, 0], init[3, 0])
# Match data by rough scale factor
track_copy.compute_abscurv()
reference.compute_abscurv()
track_copy.operate(Operator.DIFFERENTIATOR, "abs_curv", "ds")
reference.operate(Operator.DIFFERENTIATOR, "abs_curv", "ds")
f = reference.operate(Operator.AVERAGER, "ds") / track_copy.operate(
Operator.AVERAGER, "ds"
)
track_copy.scale(f)
# Match data by rough translation
t = track_copy.getCentroid() - reference.getCentroid()
track_copy.translate(t.getX(), t.getY())
resolution_steps = range(N_ITER_MAX)
if verbose:
resolution_steps = progressbar.progressbar(resolution_steps)
# Iterative closest point
for step in resolution_steps:
# -------------------------------------------------
# Data association step
# -------------------------------------------------
TP1 = []
TP2 = []
for i in range(0, len(track_copy), (int)(len(track_copy) / NPTS)):
dmin = 1e300
jmin = 0
for j in range(len(reference)):
dist = track_copy[i].position.distance2DTo(
reference[j].position
)
if dist < dmin:
dmin = dist
jmin = j
TP1.append(i)
TP2.append(jmin)
# -------------------------------------------------
# Recursive data adjustment step
# -------------------------------------------------
mapOn(track_copy, reference, TP1, TP2, N_ITER_MAX=1, verbose=False)
# Data association application
track.createAnalyticalFeature("pair", -1)
for k in range(len(TP1)):
track.setObsAnalyticalFeature("pair", TP1[k], TP2[k])
# Adjustement application
return mapOn(
track, reference, TP1, TP2, apply=apply, N_ITER_MAX=20, verbose=verbose
)
if len(init) == 0:
init = [0, 1, 0, 0]
P1 = [track.getObs(i).position.copy() for i in TP1]
if isinstance(reference, Track):
P2 = [reference.getObs(i).position.copy() for i in TP2]
else:
P2 = reference
n = len(P1)
if verbose:
print("-----------------------------------------------------------------")
print("NUMBER OF TIE POINTS: " + str(len(TP1)))
print("-----------------------------------------------------------------")
N = int(math.log(track.size()) / math.log(10)) + 1
for i in range(len(TP1)):
message = "POINT " + ("{:0" + str(N) + "d}").format(TP1[i]) + " "
message += str(track.getObs(TP1[i]).timestamp) + " ERROR = "
message += str("{:10.2f}".format(P1[i].distance2DTo(P2[i]))) + " m"
print(message)
print("-----------------------------------------------------------------")
J = np.zeros((2 * n, 4))
B = np.zeros((2 * n, 1))
X = np.matrix([init[1], init[0], init[2], init[3]]).transpose()
track_copy = track.copy()
# Iterations
for iter in range(N_ITER_MAX):
# Current parameters
k = X[0, 0]
tx = X[1, 0]
ty = X[2, 0]
a = X[3, 0]
ca = math.cos(a)
sa = math.sin(a)
for i in range(0, 2 * n, 2):
x1 = P1[int(i / 2)].getX()
y1 = P1[int(i / 2)].getY()
x2 = P2[int(i / 2)].getX()
y2 = P2[int(i / 2)].getY()
x2_th = k * (ca * x1 - sa * y1) + tx
y2_th = k * (sa * x1 + ca * y1) + ty
J[i, 0] = ca * x1 - sa * y1
J[i, 1] = 1
J[i, 2] = 0
J[i, 3] = -k * (sa * x1 + ca * y1)
B[i] = x2 - x2_th
J[i + 1, 0] = sa * x1 + ca * y1
J[i + 1, 1] = 0
J[i + 1, 2] = 1
J[i + 1, 3] = +k * (ca * x1 - sa * y1)
B[i + 1] = y2 - y2_th
dX = np.linalg.solve(J.transpose() @ J, J.transpose() @ B)
X = X + dX
cv_param = max(
max(max(abs(dX[0, 0]) * 1e4, abs(dX[1, 0]) * 1e4), abs(dX[2, 0]) * 1e4),
abs(dX[3, 0]) * 1e4,
)
if cv_param < 1:
break
if verbose:
N = int(math.log(N_ITER_MAX - 1) / math.log(10)) + 1
message = "ITERATION " + ("{:0" + str(N) + "d}").format(iter) + " "
message += (
"RMSE = "
+ "{:10.5f}".format(math.sqrt(B.transpose() @ B / (2 * n)))
+ " m "
)
message += "MAX = " + "{:10.5f}".format(np.max(B)) + " m "
print(message)
if verbose:
print("-----------------------------------------------------------------")
print("CONVERGENCE REACHED AFTER " + str(iter) + " ITERATIONS")
glob_res = 0.0
for l in range(0, 2 * n, 2):
res = math.sqrt(B[l] ** 2 + B[l + 1] ** 2)
glob_res += res
message = "RESIDUAL (2D) POINT " + str(int(l / 2)) + ": "
message += "{:4.3f}".format(res) + " m"
print(message)
print(
"GLOBAL 2D RESIDUAL ON TIE POINTS: "
+ "{:5.3f}".format(glob_res / n)
+ " m"
)
print("-----------------------------------------------------------------")
message = (
"Theta = "
+ "{:3.2f}".format(X[3, 0])
+ " rad k = "
+ "{:5.3f}".format(X[0, 0])
)
message += (
" Tx = "
+ "{:8.3f}".format(X[1, 0])
+ " m Ty = "
+ "{:8.3f}".format(X[2, 0])
+ " m"
)
print(message)
print("-----------------------------------------------------------------")
if apply:
track.rotate(X[3, 0])
track.scale(X[0, 0])
track.translate(X[1, 0], X[2, 0])
return [X[3, 0], X[0, 0], X[1, 0], X[2, 0]]
# --------------------------------------------------------------------------
# 3D mode with singular value decomposition. Scale factor is first estimated
# by correlations of 3D distances computed on all couples of tie points.
# Then dataset are scaled and translated to an approximate match. Optimal
# rotation is subsequently computed throuh SVD decomposition of normalized
# coordinates H = UDV' where H is 'covariance matrix'. Rotation is set as
# R = VU'. If tie points are not provided, iterative closest point (ICP)
# algorithm is performed to find both data association and 3D rotation/
# translation/scale parameters.
# --------------------------------------------------------------------------
if mode == "3D":
# Scale estimation
N = 0
D = 0
v = 0
W = []
D1 = []
D2 = []
for i in range(len(TP1) - 1):
for j in range(i + 1, len(TP1)):
W.append(1)
D1.append(track[TP1[i]].distanceTo(track[TP1[j]]))
D2.append(reference[TP2[i]].distanceTo(reference[TP2[j]]))
for i in range(len(D1)):
N += W[i] * D1[i] * D2[i]
D += W[i] * D1[i] * D1[i]
scale = N / D
for i in range(len(D1)):
v += (D1[i] * scale - D2[i]) ** 2
v = math.sqrt(v / len(D1))
if verbose:
print(
"------------------------------------------------------------------------------"
)
print(
"SCALE = ",
"{:5.3f}".format(scale),
" (RMSE = ",
"{:5.3f}".format(v),
"GROUND UNITS)",
)
print(
"------------------------------------------------------------------------------"
)
# Centers of mass estimations
cm1 = [0, 0, 0]
cm2 = [0, 0, 0]
for i in range(len(TP1)):
cm1[0] += track[TP1[i]].position.getX()
cm2[0] += reference[TP2[i]].position.getX()
cm1[1] += track[TP1[i]].position.getY()
cm2[1] += reference[TP2[i]].position.getY()
cm1[2] += track[TP1[i]].position.getZ()
cm2[2] += reference[TP2[i]].position.getZ()
cm1[0] /= len(TP1)
cm2[0] /= len(TP2)
cm1[1] /= len(TP1)
cm2[1] /= len(TP2)
cm1[2] /= len(TP1)
cm2[2] /= len(TP2)
cm1 = ENUCoords(cm1[0], cm1[1], cm1[2])
cm2 = ENUCoords(cm2[0], cm2[1], cm2[2])
if verbose:
print("TRK CENTER OF MASS: ", cm1)
print("REF CENTER OF MASS: ", cm2)
print(
"------------------------------------------------------------------------------"
)
# Translation and scale applications
track.translate(-cm1.getX(), -cm1.getY(), -cm1.getZ())
reference.translate(-cm2.getX(), -cm2.getY(), -cm2.getZ())
track.scale(scale)
# Rotation computation
H = np.zeros((3, 3))
for i in range(len(TP1)):
ai = track[TP1[i]].position
bi = reference[TP2[i]].position
H[0, 0] += ai.getX() * bi.getX()
H[0, 1] += ai.getX() * bi.getY()
H[0, 2] += ai.getX() * bi.getZ()
H[1, 0] += ai.getY() * bi.getX()
H[1, 1] += ai.getY() * bi.getY()
H[1, 2] += ai.getY() * bi.getZ()
H[2, 0] += ai.getZ() * bi.getX()
H[2, 1] += ai.getZ() * bi.getY()
H[2, 2] += ai.getZ() * bi.getZ()
U, D, Vt = np.linalg.svd(H, full_matrices=True)
R = (U @ Vt).transpose()
# Rotation application
track.rotate3D(R)
# Retranslation
track.translate(cm2.getX(), cm2.getY(), cm2.getZ())
reference.translate(cm2.getX(), cm2.getY(), cm2.getZ())
# Data association application
track.createAnalyticalFeature("pair", -1)
for k in range(len(TP1)):
track.setObsAnalyticalFeature("pair", TP1[k], TP2[k])
return [R, cm2 - cm1, scale]
print("Unknown mode " + mode)
return None
def neighborhood(self, obj, j=None, unit=0):
"""TODO
retourne toutes les données (sous forme de liste simple) référencées
dans la cellule (i,j).
Si unit=-1, calcule la valeur minimale à donner à unit,
pour que la liste ne soit pas vide*.
"""
# --------------------------------------------------------
# neighborhood(i,j,unit)
# --------------------------------------------------------
if isinstance(obj, int):
i = obj
if unit != -1:
TAB = set()
NC = self.__neighboringcells(i, j, unit, False)
for cell in NC:
TAB.update(self.request(cell[0], cell[1]))
return list(TAB)
# -----------------------------------------
# Case: unit < 0 -> search for unit value
# -----------------------------------------
u = 0
TAB = set()
found = False
while u <= max(self.csize, self.lsize):
NC = self.__neighboringcells(i, j, u, True)
for cell in NC:
TAB.update(self.request(cell[0], cell[1]))
if found:
break
found = len(TAB) > 0
u += 1
return list(TAB)
# --------------------------------------------------------
# neighborhood(coord, unit)
# --------------------------------------------------------
if isinstance(obj, GeoCoords) or isinstance(obj, ENUCoords):
coord = obj
x = coord.getX()
y = coord.getY()
c = self.__getCell(ENUCoords(x, y))
return self.neighborhood(math.floor(c[0]), math.floor(c[1]), unit)
# --------------------------------------------------------
# neighborhood([c1, c2], unit)
# --------------------------------------------------------
if isinstance(obj, list):
"""cellules voisines traversées par le segment coord"""
[coord1, coord2] = obj
p1 = self.__getCell(coord1)
p2 = self.__getCell(coord2)
if unit > -1:
# Tableau à retourner
TAB = []
# Les cellules traversées par le segment
CELLS = self.__cellsCrossSegment(p1, p2)
for cell in CELLS:
NC = self.__neighboringcells(cell[0], cell[1], unit)
# print (' ', cell, NC)
for cellu in NC:
self.__addCellValuesInTAB(TAB, cellu)
return TAB
u = 0
while u <= max(self.csize, self.lsize):
TAB = []
CELLS = self.__cellsCrossSegment(p1, p2)
for cell in CELLS:
NC = self.__neighboringcells(cell[0], cell[1], u)
# print (cell, NC)
for cellu in NC:
self.__addCellValuesInTAB(TAB, cellu)
# print (TAB)
if len(TAB) <= 0:
u += 1
continue
# Plus une marge de sécurité
CELLS = self.__cellsCrossSegment(p1, p2)
for cell in CELLS:
NC = self.__neighboringcells(cell[0], cell[1], u + 1)
# print (cell, NC)
for cellu in NC:
self.__addCellValuesInTAB(TAB, cellu)
# print (TAB)
return TAB
# --------------------------------------------------------
# neighborhood(track, unit)
# --------------------------------------------------------
if isinstance(obj, Track):
"""cellules voisines traversées par Track"""
track = obj
TAB2 = []
pos1 = None
for i in range(track.size()):
obs = track.getObs(i)
pos2 = obs.position
if pos1 != None:
CELLS = self.neighborhood([pos1, pos2], None, unit)
# print (CELLS, unit)
for cell in CELLS:
if cell not in TAB2:
TAB2.append(cell)
pos1 = pos2
return TAB2
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 plotStops(stops, x="x", y="y", r="radius", rf=1, sym="r-"):
for i in range(len(stops)):
Circle(ENUCoords(stops[x][i], stops[y][i]), rf * stops[r][i]).plot(sym)
def test_selection_one_shape_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()
center = trace.getObs(int(trace.size() / 2)).position
radius = 91000
circle1 = Geometrics.Circle(center, radius)
#circle.plot()
#plt.show()
c1 = Constraint(shape=circle1, mode=MODE_CROSSES)
s = Selector([c1])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
c2 = Constraint(shape=circle1, mode=MODE_INSIDE)
s = Selector([c2])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
c3 = Constraint(shape=circle1, mode=MODE_GETS_IN)
s = Selector([c3])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
c4 = Constraint(shape=circle1, mode=MODE_GETS_OUT)
s = Selector([c4])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
# -------------------------------------------------------
center = trace.getObs(int(trace.size() / 2)).position
radius = 45000
circle2 = Geometrics.Circle(center, radius)
#circle2.plot()
#plt.show()
c1 = Constraint(shape=circle2, mode=MODE_CROSSES)
s = Selector([c1])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
c2 = Constraint(shape=circle2, mode=MODE_INSIDE)
s = Selector([c2])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
c3 = Constraint(shape=circle2, mode=MODE_GETS_IN)
s = Selector([c3])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
c4 = Constraint(shape=circle2, mode=MODE_GETS_OUT)
s = Selector([c4])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
# -------------------------------------------------------
center = trace.getObs(0).position
radius = 10000
circle3 = Geometrics.Circle(center, radius)
# circle3.plot()
# plt.show()
c1 = Constraint(shape=circle3, mode=MODE_CROSSES)
s = Selector([c1])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
c2 = Constraint(shape=circle3, mode=MODE_INSIDE)
s = Selector([c2])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
c3 = Constraint(shape=circle3, mode=MODE_GETS_IN)
s = Selector([c3])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
c4 = Constraint(shape=circle3, mode=MODE_GETS_OUT)
s = Selector([c4])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertTrue(isSelection)
# -------------------------------------------------------
pt = trace.getObs(int(trace.size() / 2)).position
center = ENUCoords(pt.getX() + 10000, pt.getY() + 30000)
radius = 10000
circle4 = Geometrics.Circle(center, radius)
circle4.plot()
plt.show()
c1 = Constraint(shape=circle4, mode=MODE_CROSSES)
s = Selector([c1])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
c2 = Constraint(shape=circle4, mode=MODE_INSIDE)
s = Selector([c2])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
c3 = Constraint(shape=circle4, mode=MODE_GETS_IN)
s = Selector([c3])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
c4 = Constraint(shape=circle4, mode=MODE_GETS_OUT)
s = Selector([c4])
selector = GlobalSelector([s])
isSelection = selector.contains(trace)
self.assertFalse(isSelection)
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 __projOnTrack(point, track):
"""TODO"""
proj = Geometry.proj_polyligne(
track.getX(), track.getY(), point.getX(), point.getY()
)
return ENUCoords(proj[1], proj[2], 0), proj[0], proj[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)