Source code for tracklib.algo.Geometrics

"""Class to manage general operations on a track

.. code-block:: python

    circle1 = Geometrics.fitCircle(trace2)
    circle1.plot()
    circle2 = Geometrics.minCircle(trace2)
    circle2.plot()


"""

import copy
import logging
import math
import matplotlib.pyplot as plt
import numpy as np
import random
import sys

from tracklib.core.ObsCoords import ENUCoords
from tracklib.util.Geometry import right, inclusion, collinear, isSegmentIntersects
from tracklib.util.Geometry import transform, transform_inverse

MODE_ENCLOSING_BBOX = 0
MODE_ENCLOSING_MBR = 1
MODE_ENCLOSING_CIRCLE = 2
MODE_ENCLOSING_CONVEX = 3

logger = logging.getLogger()


[docs]class Circle: """ A circle is defined by a center point with a radius .. code-block:: python moncircle = Geometrics.Circle(ENUCoords(3.55, 48.2), 3) """
[docs] def __init__(self, center, radius): """ Constructs a circle by defining the center and the radius. Parameters ---------- center : Coords The center of the circle radius : float The radius of the circle """ self.center = center self.radius = radius
[docs] def plot(self, sym="r-", append=plt): """ Draw the circle """ if isinstance(append, bool): if append: ax1 = plt.gca() else: fig, ax1 = plt.subplots(figsize=(12, 6)) else: ax1 = append X = [0] * 100 Y = [0] * 100 for t in range(len(X) - 1): X[t] = self.radius * math.cos(2 * math.pi * t / len(X)) + self.center.getX() Y[t] = self.radius * math.sin(2 * math.pi * t / len(Y)) + self.center.getY() X[len(X) - 1] = X[0] Y[len(Y) - 1] = Y[0] ax1.plot(X, Y, sym, linewidth=0.5) return ax1
[docs] def contains(self, point): """ Returns true if the point is in the cercle, false otherwise. Parameters ---------- point: ENUCoords The point to test Return ------ type: bool """ return (point.getX() - self.center.getX()) ** 2 + ( point.getY() - self.center.getY() ) ** 2 <= self.radius ** 2
[docs] def select(self, track): """ TODO Parameters ---------- track : TYPE DESCRIPTION. Returns ------- t : TYPE DESCRIPTION. """ from tracklib.core.Track import Track t = Track() for obs in track: if self.contains(obs.position): t.addObs(obs) return t
[docs] def copy(self): """TODO""" return copy.deepcopy(self)
[docs] def translate(self, dx, dy): """TODO""" self.center.translate(dx, dy)
[docs]class Rectangle: """ A rectangle is defined by two points .. code-block:: python ll = ENUCoords(Xmin, Ymin) ur = ENUCoords(Xmax, Ymax) bbox = Geometrics.Rectangle(ll, ur) """
[docs] def __init__(self, pmin, pmax): """ Construct a rectangle from two points. Parameters ---------- pmin : ENUCoords first point, for example the left lower point of the rectangle pmax : ENUCoords second point, for example the right upper point of the rectangle """ self.pmin = pmin self.pmax = pmax
[docs] def plot(self, sym="r-"): """ Draw the rectangle """ XR = [ self.pmin.getX(), self.pmax.getX(), self.pmax.getX(), self.pmin.getX(), self.pmin.getX(), ] YR = [ self.pmin.getY(), self.pmin.getY(), self.pmax.getY(), self.pmax.getY(), self.pmin.getY(), ] plt.plot(XR, YR, sym)
[docs] def contains(self, point): """ Returns true if the point is in the rectangle, false otherwise. Parameters ---------- point: ENUCoords The point to test Return ------ type: bool """ inside_x = (self.pmin.getX() < point.getX()) and ( point.getX() < self.pmax.getX() ) inside_y = (self.pmin.getY() < point.getY()) and ( point.getY() < self.pmax.getY() ) return inside_x and inside_y
[docs] def select(self, track): """ TODO Parameters ---------- track : TYPE DESCRIPTION. Returns ------- t : TYPE DESCRIPTION. """ from tracklib.core.Track import Track t = Track() for obs in track: if self.contains(obs.position): t.addObs(obs) return t
[docs] def copy(self): """TODO""" return copy.deepcopy(self)
# -------------------------------------------------- # Translation (2D) of shape (dx, dy in ground units) # --------------------------------------------------
[docs] def translate(self, dx, dy): """TODO""" self.pmin.translate(dx, dy) self.pmax.translate(dx, dy)
# -------------------------------------------------- # Rotation (2D) of shape (theta in radians) # --------------------------------------------------
[docs] def rotate(self, theta): """TODO""" self.pmin.rotate(theta) self.pmax.rotate(theta)
# -------------------------------------------------- # Homothetic transformation (2D) of shape # --------------------------------------------------
[docs] def scale(self, h): """TODO""" self.pmin.scale(h) self.pmax.scale(h)
[docs]class Polygon: """ A polygon is defined by two list of .. code-block:: python ll = ENUCoords(Xmin, Ymin) ur = ENUCoords(Xmax, Ymax) bbox = Geometrics.Rectangle(ll, ur) """
[docs] def __init__(self, X, Y): """ """ self.X = X self.Y = Y if not ((self.X[-1] == self.X[0]) and (self.Y[-1] == self.Y[0])): self.X.append(self.X[0]) self.Y.append(self.Y[0])
[docs] def plot(self, sym="r-"): """TODO""" plt.plot(self.X, self.Y, sym)
[docs] def contains(self, point): """TODO""" return inclusion(self.X, self.Y, point.getX(), point.getY())
[docs] def select(self, track): """ TODO Parameters ---------- track : TYPE DESCRIPTION. Returns ------- t : TYPE DESCRIPTION. """ from tracklib.core.Track import Track t = Track() for obs in track: if self.contains(obs.position): t.addObs(obs) return t
[docs] def copy(self): """TODO""" return copy.deepcopy(self)
# -------------------------------------------------- # Translation (2D) of shape (dx, dy in ground units) # --------------------------------------------------
[docs] def translate(self, dx, dy): """TODO""" for i in range(len(self.X)): self.X[i] = self.X[i] + dx self.Y[i] = self.Y[i] + dy
# -------------------------------------------------- # Rotation (2D) of shape (theta in radians) # --------------------------------------------------
[docs] def rotate(self, theta): """TODO""" cr = math.cos(theta) sr = math.sin(theta) for i in range(len(self.X)): xr = +cr * self.X[i] - sr * self.Y[i] yr = +sr * self.X[i] + cr * self.Y[i] self.X[i] = xr self.Y[i] = yr
# -------------------------------------------------- # Homotehtic transformation (2D) of shape # --------------------------------------------------
[docs] def scale(self, h): """TODO""" for i in range(len(self.X)): self.X[i] *= h self.Y[i] *= h
# -------------------------------------------------- # Polygon area # --------------------------------------------------
[docs] def area(self): """TODO""" aire = 0 for i in range(len(self.X) - 1): aire += self.X[i] * self.Y[i + 1] - self.X[i + 1] * self.Y[i] return abs(aire / 2)
# -------------------------------------------------- # Polygon centroid # --------------------------------------------------
[docs] def centroid(self): """ Returns ------- center : array[2] DESCRIPTION. """ aire = 0 center = [0, 0] dx = self.X[0] dy = self.Y[0] self.translate(-dx, -dy) for i in range(len(self.X) - 1): factor = self.X[i] * self.Y[i + 1] - self.X[i + 1] * self.Y[i] aire += factor center[0] += (self.X[i] + self.X[i + 1]) * factor center[1] += (self.Y[i] + self.Y[i + 1]) * factor center[0] /= 3 * aire center[1] /= 3 * aire center[0] += dx center[1] += dy self.translate(dx, dy) return center
# -------------------------------------------------- # Test if a polygon is star-shaped # --------------------------------------------------
[docs] def isStarShaped(self): """TODO""" eps = 1e-6 c = self.centroid() # self.plot('k-') # plt.plot(c[0], c[1], 'ro') for i in range(len(self.X) - 1): visee = [ self.X[i] - eps * (self.X[i] - c[0]), self.Y[i] - eps * (self.Y[i] - c[1]), ] S1 = [c[0], c[1], visee[0], visee[1]] intersection = False for j in range(len(self.X) - 1): S2 = [self.X[j], self.Y[j], self.X[j + 1], self.Y[j + 1]] if isSegmentIntersects(S1, S2): intersection = True break if intersection: # plt.plot([c[0],visee[0]], [c[1],visee[1]], 'r--') return False # else: # plt.plot([c[0],visee[0]], [c[1],visee[1]], 'k--') return True
# -------------------------------------------------- # Computes angular ratio of polygon star-shaped # --------------------------------------------------
[docs] def starShapedRatio(self, resolution=1, inf=1e3): """TODO""" eps = inf c = self.centroid() # self.plot('k-') N = 0.0 D = 0.0 for theta in range(0, 360, math.floor(resolution)): visee = [ c[0] + eps * math.cos(theta * math.pi / 180), c[1] + eps * math.sin(theta * math.pi / 180), ] S1 = [c[0], c[1], visee[0], visee[1]] count = 0 D += 1 for j in range(len(self.X) - 1): S2 = [self.X[j], self.Y[j], self.X[j + 1], self.Y[j + 1]] if isSegmentIntersects(S1, S2): count += 1 if count == 2: N += 1 # plt.plot([c[0],visee[0]], [c[1],visee[1]], 'r--') break if count == 0: N += 1 # plt.plot([c[0],visee[0]], [c[1],visee[1]], 'r--') return 1.0 - N / D
# -------------------------------------------------- # Radial signature of a polygon # --------------------------------------------------
[docs] def signature(self): """TODO""" C = self.centroid() S = [0] R = [math.sqrt((C[0] - self.X[0]) ** 2 + (C[1] - self.Y[0]) ** 2)] N = R[0] for i in range(1, len(self.X)): S.append( math.sqrt( (self.X[i - 1] - self.X[i]) ** 2 + (self.Y[i - 1] - self.Y[i]) ** 2 ) + S[i - 1] ) R.append(math.sqrt((C[0] - self.X[i]) ** 2 + (C[1] - self.Y[i]) ** 2)) N = max(N, R[-1]) for i in range(len(S)): S[i] /= S[-1] R[i] /= N return [S, R]
# ---------------------------------------- # Function to get enclosing shape # ----------------------------------------
[docs]def boundingShape(track, mode=MODE_ENCLOSING_BBOX): """TODO""" if mode == MODE_ENCLOSING_BBOX: return track.getBBox() if mode == MODE_ENCLOSING_MBR: return minimumBoundingRectangle(track) if mode == MODE_ENCLOSING_CIRCLE: return minCircle(track) if mode == MODE_ENCLOSING_CONVEX: return convexHull(track)
[docs]def __convexHull(T): """TODO Finds the convex hull of a set of coordinates, returned as a list of x an y coordinates : [x1, y1, x2, y2,...] Computation is performed with Jarvis march algorithm with O(n^2) time complexity. It may be needed to resample track if computation is too long.""" X = [p[0] for p in T] H = [X.index(min(X))] while (len(H) < 3) or (H[-1] != H[0]): H.append(0) for i in range(len(T)): if not (right(T[H[-2]], T[H[-1]], T[i])): H[-1] = i return H
[docs]def convexHull(track): """TODO Finds the convex hull of a track, returned as a list of x an y coordinates : [x1, y1, x2, y2,...] Computation is performed with Jarvis march algorithm with O(n^2) time complexity. It may be needed to resample track if computation is too long.""" T = [] for i in range(len(track)): T.append([track[i].position.getX(), track[i].position.getY()]) CH = __convexHull(T) T2 = [] for i in range(len(CH)): T2.append(T[CH[i]][0]) T2.append(T[CH[i]][1]) return T2
[docs]def diameter(track): """TODO Finds longest distance between points on track The two selected points are returned in a vector along with the minimal distance : [min_dist, idx_p1, idx_p2]. Exhaustive search in O(n^2) time complexity""" dmax = 0 idmax = [0, 0] for i in range(len(track)): for j in range(len(track)): d = track.getObs(i).distance2DTo(track.getObs(j)) if d > dmax: dmax = d idmax = [i, j] return [dmax, idmax[0], idmax[1]]
[docs]def __circle(p1, p2=None, p3=None): """TODO Finds circle through 1, 2 or 3 points Returns Circle(C, R) """ if not isinstance(p1, ENUCoords): print("Error: ENU coordinates are required for min circle computation") exit() if p2 is None: return Circle(p1, 0.0) if p3 is None: centre = p1 + p2 centre.scale(0.5) return Circle(centre, p1.distance2DTo(p2) / 2) if collinear( [p1.getX(), p1.getY()], [p2.getX(), p2.getY()], [p3.getX(), p3.getY()] ): logger.warning(str(p1) + "," + str(p2) + "," + str(p3) + " are collinear") return None if p1.distance2DTo(p2) == 0: p2.setX(p2.getX() + random.random() * 1e-10) p2.setY(p2.getY() + random.random() * 1e-10) if p1.distance2DTo(p3) == 0: p3.setX(p3.getX() + random.random() * 1e-10) p3.setY(p3.getY() + random.random() * 1e-10) if p2.distance2DTo(p3) == 0: p3.setX(p3.getX() + random.random() * 1e-10) p3.setY(p3.getY() + random.random() * 1e-10) C12 = __circle(p1, p2) C23 = __circle(p2, p3) C13 = __circle(p1, p3) CANDIDATS = [] if C12.center.distance2DTo(p3) < C12.radius: CANDIDATS.append(C12) if C23.center.distance2DTo(p1) < C23.radius: CANDIDATS.append(C23) if C13.center.distance2DTo(p2) < C13.radius: CANDIDATS.append(C13) if len(CANDIDATS) > 0: min = CANDIDATS[0].radius argmin = 0 for i in range(len(CANDIDATS)): if CANDIDATS[i].radius < min: min = CANDIDATS[i].radius argmin = i return CANDIDATS[argmin] x = np.complex(p1.getX(), p1.getY()) y = np.complex(p2.getX(), p2.getY()) z = np.complex(p3.getX(), p3.getY()) w = z - x w /= y - x c = (x - y) * (w - abs(w) ** 2) / 2j / np.imag(w) - x centre = p1.copy() centre.setX(-np.real(c)) centre.setY(-np.imag(c)) return Circle(centre, np.abs(c + x))
[docs]def __welzl(C): """ 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))
[docs]def plotPolygon(P, color=[1, 0, 0, 1]): """ Function to plot a polygon from a vector: R = [x1,y1,x2,y2,x3,y3,...x1,y1] Needs to call plt.show() after this function """ XR = P[::2] YR = P[1::2] plt.plot(XR, YR, color=color)
[docs]def minCircle(track): """ Finds minimal bounding circle with Welzl's recursive algorithm in O(n) complexity. Output is given as a list [p, R], where p is a Coords object defining circle center and R is its radius. Due to recursion limits, only tracks with fewer than 800 points can be processed """ if not track.getSRID() == "ENU": print("Error: ENU coordinates are required for min circle computation") exit() if track.size() > 0.5 * sys.getrecursionlimit(): message = ( "Error: too many points in track to compute minimal enclosing circle. " ) message += 'Downsample track, or use "sys.setrecursionlimit(' message += str(int((2 * track.size()) / 1000) * 1000 + 1000) + ') or higher"' print(message) exit() # centre = track.getFirstObs().position.copy() P = [obs.position for obs in track] if track.getFirstObs() == track.getLastObs(): # Si la ligne est fermée ? P = P[:-1] R = [] return __welzl(Circle(P, R))
[docs]def minCircleMatrix(track): """TODO Computes matrix of all min circles in a track. M[i,j] gives the radius of the min circle enclosing points in track from obs i to obs j (included)""" M = np.zeros((track.size(), track.size())) for i in range(track.size()): # print(i, "/", track.size()) for j in range(i, track.size() - 1): M[i, j] = minCircle(track.extract(i, j)).radius M = M + np.transpose(M) return M
[docs]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 fitCircle(track): #Computes optimal circle fit on track #Beta version, with Kalman filter I = np.identity(3) P = I*1e10 H = -1*np.ones((1,3)) X = np.zeros((3,1)); c = track.getCentroid() X[0,0] = c.getX(); X[1,0] = c.getY(); w = 1e-20 L = list(range(len(track))) random.shuffle(L) for i in L: obs = track[i].position; x = obs.getX(); y = obs.getY() R = math.sqrt((x-X[0])**2 + (y-X[1])**2) z = np.matrix(X[2]-R) H[0,0] = (X[0]-x)/R; H[0,1] = (X[1]-y)/R; K = P @ np.transpose(H) @ np.linalg.inv(H @ P @ np.transpose(H) + w) X = X + K@z P = (I - K@H) @ P return Circle(ENUCoords(X[0,0], X[1,0]), X[2,0]) """ # ------------------------------------------------------------ # Output : R = [[x1,y1],[x2,y2],[x3,y3],[x4,y4], area, l, L] # ------------------------------------------------------------
[docs]def __mbr(COORDS): """TODO""" HULL = __convexHull(COORDS) XH = [COORDS[p][0] for p in HULL] YH = [COORDS[p][1] for p in HULL] BEST_RECTANGLE = [] RECTANGLE_AREA = 10 ** 301 BEST_l = 0 BEST_L = 0 for i in range(len(HULL) - 1): # param = cartesienne([XH[i], YH[i], XH[i+1], YH[i+1]]) theta = math.atan((YH[i + 1] - YH[i]) / (XH[i + 1] - XH[i])) # 3 parameters transformation XHR, YHR = transform(theta, XH[i], YH[i], XH, YH) mx = min(XHR) my = min(YHR) if max(YHR) > abs(min(YHR)): my = max(YHR) Mx = max(XHR) XRR = [mx, Mx, Mx, mx, mx] YRR = [0, 0, my, my, 0] # 3 parameters inverse transformation XR, YR = transform_inverse(theta, XH[i], YH[i], XRR, YRR) new_area = (Mx - mx) * abs(my) if new_area < RECTANGLE_AREA: BEST_RECTANGLE = [ [XR[0], YR[0]], [XR[1], YR[1]], [XR[2], YR[2]], [XR[3], YR[3]], [XR[0], YR[0]], ] RECTANGLE_AREA = new_area BEST_l = Mx - mx BEST_L = abs(my) if BEST_L < BEST_l: G = BEST_l BEST_l = BEST_L BEST_L = G return [BEST_RECTANGLE, RECTANGLE_AREA, BEST_l, BEST_L]
[docs]def minimumBoundingRectangle(track): """TODO""" T = [] for i in range(len(track)): T.append([track[i].position.getX(), track[i].position.getY()]) return __mbr(T)