merged parser in repo

Author: dubh3401

img/calibration_flow.png

@@ -6,4 +6,5 @@ PyQt5 == 5.15.6
 pyqtgraph == 0.12.3
 PyQt5-stubs == 5.15.2.0
 matplotlib == 3.5.0
-numpy
+numpy
+scipy

requirements.txt

@@ -0,0 +1,65 @@
+import matplotlib.pyplot as plt
+import pandas as pd
+import src.parser.parsing as parse
+import src.parser.computeAngle as compute
+import src.parser.plotter as p
+import os
+import src.parser.export as export
+import src.parser.AngleCalculatorParameters as param
+
+EXPORT_PDOA = True
+
+dataFolderPath = "C:/Users/huber/USherbrooke/SwarmUs - General/Interlocalisation/Angles/HiveBoard"
+if not os.path.exists(dataFolderPath+"/"+"figures"):
+    os.makedirs(dataFolderPath+"/"+"figures")
+
+dataName = "20211112_1230120p024_2mOfficial2"
+usedPairs = [0, 1, 5]
+
+if not os.path.exists(dataFolderPath+"/"+"figures"+"/"+dataName):
+    os.makedirs(dataFolderPath+"/"+"figures"+"/"+dataName)
+figureFolderPath = dataFolderPath+"/"+"figures"+"/"+dataName
+#extract data from CSV
+dataCsv = pd.read_csv(dataFolderPath + "/"+ dataName + '.csv')
+data = parse.remAnormalRx(dataCsv)
+dataBundled, angles = parse.bundle(data)
+
+dPDOA = compute.PDOA(dataBundled,[data.columns.values[[3,7,11]]],[data.columns.values[[4,8,12]]],angles)
+mdPDOA,sdPDOA = compute.stats(dPDOA)
+
+PDOAscaleFactor = {}
+for nb in usedPairs:
+    PDOAscaleFactor["PDOA"+str(nb)] = p.extractSlopeExtremums(dPDOA,mdPDOA,namePDOA="PDOA"+str(nb))
+
+PDOAscaleFactor["PDOA2"]=0
+PDOAscaleFactor["PDOA3"]=0
+PDOAscaleFactor["PDOA4"]=0
+# recompute PDOA with centering (skew correction)
+dPDOA = compute.PDOA(dataBundled,[data.columns.values[[3,7,11]]],[data.columns.values[[4,8,12]]],angles,offset=PDOAscaleFactor)
+mdPDOA,sdPDOA = compute.stats(dPDOA)
+
+for nb in usedPairs:
+    p.plotPDOA(dPDOA,mdPDOA,sdPDOA,namePDOA='PDOA'+str(nb))
+# confusion zone
+for line in [30, 90]:
+    plt.plot([0,360],[-line,-line],"--r")
+    plt.plot([0,360],[line,line],"--r")
+plt.show()
+
+# export angle calculator parameters to pickle format
+if EXPORT_PDOA:
+    PDOAlineFit = {}
+    for nb in usedPairs:
+        tempDic = p.extractSlopes(dPDOA, mdPDOA, namePDOA="PDOA" + str(nb))
+        PDOAlineFit.update(tempDic)
+
+    exporter = export.Exporter(dataFolderPath)
+    pairs = {}
+    for n in usedPairs:
+        pairs["PDOA" + str(n)] = param.AngleCalculatorParameters()
+        pairs["PDOA" + str(n)].m_pairID = n
+
+    export.setPdoaSlopesCarac(pairs, PDOAlineFit, PDOAscaleFactor)
+    export.setAntennaPairs(pairs)
+
+    exporter.toPickle(pairs)

src/parser.py

@@ -0,0 +1,23 @@
+# Structure containing all parameters used to calculate an angle. These parameters are calculated
+# using the Python tooling during calibration and then transferred to the board.
+
+class AngleCalculatorParameters:
+    def __init__(self):
+        pass
+    m_pairID = None
+    # Normalization factors used to stretch PDOA angles between -90 and +90 degrees. (Applied by
+    # dividing by the normalization after applying the asin() on the PDOA)
+    m_pdoaNormalizationFactors = None
+
+    # Slopes (a in y=ax+b) of the non-reciprocated PDOA functions (calculatedAngle = a*realAngle+b)
+    # A single (positive) slope is used for all PDOA curves. The sign of the slope is applied at
+    # runtime.\
+    m_pdoaSlopes = None
+
+    # Intercepts (b in y=ax+b) of the non-reciprocated PDOA functions for every curve
+    # (calculatedAngle = a*realAngle+b)
+    m_pdoaIntercepts = None
+
+    # Lookup table used to know which antenna IDs are used in each pair ID
+    # TDOAx = m_antennaPairs[0]-m_antennaPairs[1]
+    m_antennaPairs = None

src/parser/AngleCalculatorParameters.py

@@ -0,0 +1,82 @@
+import numpy as np
+import src.parser.utils as utils
+
+def minMaxAverage(TDOA,phaseReverse):
+    return utils.mapDict(_minMaxAverage, TDOA, phaseReverse)
+
+def _minMaxAverage(TDOA,phaseReverse):
+    mean = []
+    for i in [0, 1]:
+        min = int(phaseReverse[i]-5) if (phaseReverse[i]-5) > 0 else 0
+        max = int(phaseReverse[i]+5) if (phaseReverse[i]+5) < len(TDOA.values()) else int(len(TDOA.values())-1)
+
+        mean.append(np.mean(list({list(TDOA)[val]: np.mean(TDOA[list(TDOA)[val]]) for val in
+                            range(min, max)}.values())))
+
+    return mean
+
+def PDOA(d,columnsSFD,columnsCIR,angles,offset=None):
+    columnsSFD = columnsSFD[0]
+    columnsCIR = columnsCIR[0]
+    numItr = 2**len(columnsSFD) - 2**(len(columnsSFD)-2)
+    name = []
+    for itr in range(numItr):
+        name.append('PDOA'+str(itr))
+
+
+    PDOA = {}
+    scaleFactor = {}
+    i = 0
+    for col1,_ in enumerate(columnsSFD):
+        for col2,_ in enumerate(columnsSFD):
+            if col1 != col2:
+                PDOA[name[i]] = {}
+                for a in angles:
+                    sfd1 =  np.array(d[a][columnsSFD[col1]])
+                    sfd2 =  np.array(d[a][columnsSFD[col2]])
+                    fp1 =  np.array(d[a][columnsCIR[col1]])
+                    fp2 =  np.array(d[a][columnsCIR[col2]])
+
+                    part1 = np.add(np.subtract(np.subtract(fp1,sfd1),fp2),sfd2)-np.pi/2
+                    part2 = np.mod(part1,2*np.pi)
+                    if offset is None:
+                        PDOA[name[i]][a] = part2
+                    else:
+                        scaleFactor = 1
+                        theta = np.mod((part2 + offset[name[i]]), 2 * np.pi) - np.pi
+                        # PDOA[name[i]][a] =theta *90/np.pi
+                        PDOA[name[i]][a] = np.arcsin(theta/np.pi) * 180/np.pi * scaleFactor
+                i = i+1
+    return PDOA
+
+def stats(d):
+    mean = {x: {a: np.mean(d[x][a]) for a in d[x].keys()} for x in d.keys()}
+    stdev = {x: {a: np.std(d[x][a]) for a in d[x].keys()} for x in d.keys()}
+    return mean,stdev
+
+def _filterSTD(sdPDOA):
+    rem = []
+    for d in sdPDOA:
+        if sdPDOA[d] > 15:
+            rem.append(d)
+    return rem
+
+def filterSTD(sdPDOA):
+    return utils.mapDict(_filterSTD, sdPDOA)
+
+def removeAnormalStdDev(mdPDOA,sdPDOA, mdTDOA, sdTDOA=None):
+     valOut = filterSTD(sdPDOA)
+
+     for i,name in enumerate(valOut):
+         for rem in valOut[name]:
+             mdPDOA[name].pop(rem)
+             sdPDOA[name].pop(rem)
+             mdTDOA[list(mdTDOA)[i]].pop(rem)
+             if sdTDOA is not None:
+                 sdTDOA[list(mdTDOA)[i]].pop(rem)
+     return mdPDOA,sdPDOA,mdTDOA,sdTDOA
+
+def mean(dic):
+    return utils.mapDict(_mean, dic)
+def _mean(val):
+    return np.mean(list(val.values()))

src/parser/__init__.py

@@ -0,0 +1,49 @@
+from src.parser.AngleCalculatorParameters import AngleCalculatorParameters
+import src.parser.utils as utils
+import pickle as pkl
+import os
+
+class Exporter:
+    def __init__(self,exportPath):
+        self.dataExportPath = exportPath
+
+    dataExportPath = None
+    dataExportPathFolder = None
+
+    def toPickle(self,pairs):
+        if not os.path.exists(self.dataExportPath + "/" + "angleParameters"):
+            os.makedirs(self.dataExportPath + "/" + "angleParameters")
+        self.dataExportPathFolder = self.dataExportPath + "/" + "angleParameters"
+
+        utils.mapDict(self._toPickle, pairs, {k:k for k in pairs.keys()})
+
+    def _toPickle(self,AngleClass: AngleCalculatorParameters,name):
+        pkl.dump(AngleClass,open(self.dataExportPathFolder+ "/"+str(name)[-1]+"_angleCalculatorParameters.pkl", 'wb'))
+
+def setPdoaNormalizationFactors(pairs, factors):
+    utils.mapDict(_setPdoaNormalizationFactors, pairs, factors)
+def _setPdoaNormalizationFactors(AngleClass: AngleCalculatorParameters, factors):
+    AngleClass.m_pdoaNormalizationFactors = factors
+
+def setPdoaSlopesCarac(pairs, lineCarac,PDOAscaleFactor):
+    utils.mapDict(_setPdoaSlopesCarac, pairs, lineCarac, PDOAscaleFactor)
+def _setPdoaSlopesCarac(AngleClass: AngleCalculatorParameters, lineCarac,PDOAscaleFactor):
+    AngleClass.m_pdoaSlopes = lineCarac["m"]
+    AngleClass.m_pdoaIntercepts = lineCarac["b"]
+    AngleClass.m_pdoaNormalizationFactors = PDOAscaleFactor
+
+def setAntennaPairs(pairs):
+    pair = {}
+    i = 0
+    for x in range(0, 3):
+        for y in range(0, 3):
+            if x != y:
+                pair['PDOA'+str(i)] = [x, y]
+                i += 1
+
+    utils.mapDict(_setAntennaPairs, pairs, pair)
+def _setAntennaPairs(AngleClass: AngleCalculatorParameters, pair):
+    AngleClass.m_antennaPairs = pair
+
+def testRead(file):
+    return pkl.load(open(file, 'rb'))

src/parser/computeAngle.py

@@ -0,0 +1,38 @@
+# from https://matplotlib.org/stable/users/event_handling.html
+from matplotlib import collections  as mc
+
+class LineBuilder:
+    def __init__(self, line,ax):
+        self.line = [line]
+        self.ax = ax
+        self.eventCounter = 0
+        self.nbLine = 0
+        self.xs = []
+        self.ys = []
+        self.cid = self.line[-1].figure.canvas.mpl_connect('button_press_event', self)
+        self.ms = []
+        self.bs = []
+
+
+    def __call__(self, event):
+        if event.inaxes!=self.line[-1].axes: return
+        self.xs.append(event.xdata)
+        self.ys.append(event.ydata)
+        self.eventCounter += 1
+        self.line[-1].set_data(self.xs, self.ys)
+        if self.eventCounter > 1:
+            return self.addLine()
+
+    def addLine(self):
+        self.eventCounter = 0
+        m = (self.ys[1 + 2 * self.nbLine] - self.ys[0 + 2 * self.nbLine]) / (
+                self.xs[1 + 2 * self.nbLine] - self.xs[0 + 2 * self.nbLine])
+        b = self.ys[1 + 2 * self.nbLine] - m * self.xs[1 + 2 * self.nbLine]
+        self.ms.append(m)
+        self.bs.append(b)
+        self.line[-1].figure.canvas.draw()
+        lineNew, = self.ax.plot([0], [0],'r')
+        self.line.append(lineNew)
+        self.cid = self.line[-1].figure.canvas.mpl_connect('button_press_event', self)
+        self.xs=[]
+        self.ys=[]

src/parser/export.py

@@ -0,0 +1,37 @@
+# from https://matplotlib.org/stable/users/event_handling.html
+import matplotlib
+from matplotlib import pyplot as plt
+import numpy as np
+
+class PointExtracter:
+    def __init__(self,ax):
+        self.scat = None
+        self.ax = ax
+        self.nbLine = 0
+        self.xs = []
+        self.ys = []
+        self.cid = self.ax.figure.canvas.mpl_connect('button_press_event', self)
+        self.ms = []
+        self.ptCnt = 0
+
+
+    def __call__(self, event):
+        if self.ptCnt < 1:
+            self.scat = self.ax.scatter([event.xdata],[event.ydata],c='r',zorder=10)
+            self.ys.append(event.ydata)
+            self.scat.axes.figure.canvas.draw_idle()
+        else:
+            self.addPoint([event.xdata,event.ydata])
+            self.ys.append(event.ydata)
+            # return [round(x) for x in self.xs]
+
+    def addPoint(self,new_point, c='r'):
+        old_off = self.scat.get_offsets()
+        new_off = np.concatenate([old_off, np.array(new_point, ndmin=2)])
+        old_c = self.scat.get_facecolors()
+        new_c = np.concatenate([old_c, np.array(matplotlib.colors.to_rgba(c), ndmin=2)])
+
+        self.scat.set_offsets(new_off)
+        self.scat.set_facecolors(new_c)
+
+        self.scat.axes.figure.canvas.draw_idle()