#! /usr/bin/env python # # LICENSE: # Copyright (C) 2019 Joel Hahn # # This program is free software: you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by # the Free Software Foundation, either version 3 of the License, or # (at your option) any later version. # # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # # You should have received a copy of the GNU General Public License # along with this program. If not, see . # # Authors: Joel Hahn, Nathan Mester, Sharath Sundaram # Neal Patwari, neal.patwari@gmail.com # # # Version History: # # Version 1.0: Initial Release. 27 Oct 2014. # Version 2.0: 17 April 2019. # Updated to accept new data structure and new overall plotting algorithm # Removed unnecessary methods and changed weighting method in initRTI. import sys import numpy as np import scipy.spatial.distance as dist import numpy.linalg as linalg import matplotlib.pyplot as plt import matplotlib matplotlib.rc('xtick', labelsize=16) matplotlib.rc('ytick', labelsize=16) # Convert link number to Tx and Rx numbers def txRxForLinkNum(linknum, nodes): tx = linknum / (nodes-1) rx = linknum % (nodes-1) if rx >= tx: rx+=1 if tx >= nodes: sys.exit("Error in txRxForLinkNum: linknum too high for nodes value") return tx, rx # Creates the pixel grid based on the bottom left and top right coordinates and the desired distances between pixels. def calcGridPixelCoords(personLL, personUR, delta_p): xVals = np.arange(personLL[0], personUR[0], delta_p) yVals = np.arange(personLL[1], personUR[1], delta_p) cols = len(xVals) pixels = cols * len(yVals) # len(yVals) is the number of rows of pixels # fill the first row, then the 2nd row, etc. pixelCoords = np.array([[xVals[i%cols], yVals[i/cols]] for i in range(pixels)]) return pixelCoords, xVals, yVals # Plot the node/sensor locations with their node numbers def plotLocs(nodeLocs): plt.plot(nodeLocs[:,0], nodeLocs[:,1], '.', markersize=14.0) #Increase the axes to show full map. xmin, xmax, ymin, ymax = plt.axis() deltay = ymax-ymin epsy = deltay*0.1 deltax = xmax-xmin epsx = deltax*0.1 plt.axis((xmin-epsx, xmax+epsx, ymin-epsy, ymax+epsy)) for number, coord in enumerate(nodeLocs): plt.text(coord[0], coord[1]+ epsy*0.2, str(number+1), # +1 to start node numbers at 1 instead of 0 horizontalalignment='center', verticalalignment='bottom', fontsize=16) plt.xlabel('X Coordinate (m)', fontsize=18) plt.ylabel('Y Coordinate (m)', fontsize=18) plt.grid() # Plot the estimated image. Label the X axis with the actual time (ms), and # mark the true coordinate with an X if it is known, and mark the sensor coordinates. def plotImage(image, figNumber, sensorCoords, imageExtent, vmaxval, units, time_ms=None, estCoord=None, actualCoord=None): # Replace the image already in Figure figNumber plt.figure(figNumber,figsize=(8,8)) plt.cla() plotLocs(sensorCoords) plt.imshow(image, interpolation='none', origin='lower', extent=imageExtent, vmin=0, vmax=vmaxval) plt.ylabel('Y Coordinate (' + units + ')') if time_ms is None: plt.xlabel('X Coordinate (' + units + ')') else: plt.xlabel('X Coordinate (' + units + ') at time ' + str(time_ms) + ' s') if estCoord is not None: if len(estCoord) > 0: plt.text(estCoord[0], estCoord[1], 'X', color ='red', horizontalalignment='center', verticalalignment='center') if actualCoord is not None: if len(estCoord) > 0: plt.text(actualCoord[0], actualCoord[1], 'X', color ='black', horizontalalignment='center', verticalalignment='center') plt.draw() # Do initial calculations to compute the RTI projection matrix # # Inputs: nodeLocs: Sensor node locations (nodes x 2) # delta_p: distance between pixel centers (meters) # sigmax2: variance of any pixel's image value (units^2) # delta: correlation distance (distance at which correlation coefficient is e^-1, meters) # excessPathLen: the size of the ellipse (meters) # # Outputs: # inversion: a pixels x links projection matrix # xVals, yVals: x and y coordinates of pixel grid. # # Authors: Neal Patwari, 12 July 2012 # Edited and updated by Nathan Mester, Joel Hahn, 4 April 2019 # def initRTI(nodeLocs, delta_p, sigmax2, delta, falloff, excessPathLen): # Set up pixel locations as a grid. personLL = nodeLocs.min(axis=0) personUR = nodeLocs.max(axis=0) pixelCoords, xVals, yVals = calcGridPixelCoords(personLL, personUR, delta_p) pixels = pixelCoords.shape[0] #plt.figure(3) #plotLocs(pixelCoords) # Find distances between pixels and transceivers DistPixels = dist.squareform(dist.pdist(pixelCoords)) DistPixelAndNode = dist.cdist(pixelCoords, nodeLocs) DistNodes = dist.squareform(dist.pdist(nodeLocs)) # Find the (inverse of) the Covariance matrix between pixels CovPixelsInv = linalg.inv(sigmax2*np.exp(-DistPixels/delta)) # Calculate weight matrix for each link. nodes = len(nodeLocs) links = nodes*(nodes-1) W = np.zeros((links, pixels)) for ln in range(links): txNum, rxNum = txRxForLinkNum(ln, nodes) # Finds the distances from the path between a tx-rx node pair and each pixel ePL = DistPixelAndNode[:,txNum] + DistPixelAndNode[:,rxNum] - DistNodes[txNum,rxNum] # Finds the indices all pixels within the desired distance excessPathLen. inEllipseInd = np.argwhere(ePL < excessPathLen) pixelsIn = len(inEllipseInd) # For each pixel near the link, weigh its value based on an exponential falloff of its distance from the link. if pixelsIn > 0: a = 0 for i in inEllipseInd: a += np.exp(-ePL[i]/falloff) for i in inEllipseInd: # The weight matrix is structured so that all weighted pixels add up to 1 for each link. W[ln, i] = (1/a)*np.exp(-ePL[i]/falloff) # Compute the projection matrix with a pseudoinverse. inversion = np.dot(linalg.inv(np.dot(W.T, W) + CovPixelsInv), W.T) return inversion, xVals, yVals, pixelCoords def imageMaxCoord(imageMat, xVals, yVals): rowMaxInd, colMaxInd = np.unravel_index(imageMat.argmax(), imageMat.shape) return xVals[colMaxInd], yVals[rowMaxInd]