#! /usr/bin/env python # # Version History: # # Version 1.0: Initial Release. 17 Oct 2012. # from serial import * import sys import time import numpy from pylab import * import scipy.spatial.distance as dist import scipy.stats as stats import scipy import numpy.linalg as linalg import scipy.io import matplotlib.pyplot as plt import matplotlib matplotlib.rc('xtick', labelsize=16) matplotlib.rc('ytick', labelsize=16) # ######################################## # Code to provide a fixed-length buffer data type class FixedLenBuffer: def __init__(self, initlist): self.frontInd = 0 self.data = initlist self.len = len(initlist) def list(self): oldest = self.frontInd+1 return self.data[oldest:] + self.data[:oldest] # Append also deletes the oldest item def append(self, newItem): self.frontInd += 1 if self.frontInd >= self.len: self.frontInd = 0 self.data[self.frontInd] = newItem # Returns the "front" item def mostRecent(self): return self.data[self.frontInd] # Returns the N items most recently appended def mostRecentN(self,N): return [self.data[(self.frontInd-i)%self.len] for i in range(N-1,-1,-1)] # Returns the variance of the data def var(self): return array(self.data).var() # ######################################## # Convert Tx, Rx, and Ch numbers to link number def linkNumForTxRxChLists(tx, rx, ch, nodeList, channelList): if (nodeList.count(tx) == 0) or (nodeList.count(rx) == 0) or (channelList.count(ch) == 0): print "Error in linkNumForTxRx: tx, rx, or ch number invalid" rx_enum = nodeList.index(rx) tx_enum = nodeList.index(tx) ch_enum = channelList.index(ch) nodes = len(nodeList) links = nodes*(nodes-1) linknum = ch_enum*links + tx_enum*(nodes-1) + rx_enum if (rx_enum > tx_enum): linknum -= 1 return linknum # 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): print "Error in txRxForLinkNum: linknum too high for nodes value" return (tx, rx) # Convert link number to Tx, Rx, and channel numbers def txRxChForLinkNum(linknum, nodeList, channelList): nodes = len(nodeList) links = nodes*(nodes-1) ch_enum = linknum / links remLN = linknum % links tx_enum = remLN / (nodes-1) rx_enum = remLN % (nodes-1) if (rx_enum >= tx_enum): rx_enum+=1 if (tx_enum >= nodes) | (ch_enum >= len(channelList)): print "Error in txRxForLinkNum: linknum or ch too high for nodes, channels values" else: ch = channelList[ch_enum] tx = nodeList[tx_enum] rx = nodeList[rx_enum] return (tx, rx, ch) def calcGridPixelCoords(personLL, personUR, delta_p): xVals = arange(personLL[0], personUR[0], delta_p) yVals = 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 = 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), horizontalalignment='center', verticalalignment='bottom', fontsize=16) plt.xlabel('X Coordinate (m)', fontsize=18) plt.ylabel('Y Coordinate (m)', fontsize=18) plt.grid() # 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. # # Author: Neal Patwari, 12 July 2012 # def initRTI(nodeLocs, delta_p, sigmax2, delta, 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*exp(-DistPixels/delta)) # Calculate weight matrix for each link. nodes = len(nodeLocs) links = nodes*(nodes-1) W = zeros((links, pixels)) for ln in range(links): txNum, rxNum = txRxForLinkNum(ln, nodes) ePL = DistPixelAndNode[:,txNum] + DistPixelAndNode[:,rxNum] - DistNodes[txNum,rxNum] inEllipseInd = argwhere(ePL < excessPathLen) pixelsIn = len(inEllipseInd) if pixelsIn > 0: W[ln, inEllipseInd] = 1.0 / float(pixelsIn) # Compute the projection matrix inversion = dot(linalg.inv(dot(W.T, W) + CovPixelsInv), W.T) return (inversion, xVals, yVals) def callRTI(linkMeas, inversion, xValsLen, yValsLen): temp = dot(inversion, linkMeas) temp.resize(yValsLen, xValsLen) return temp def imageMaxCoord(imageMat, xVals, yVals): rowMaxInd, colMaxInd = unravel_index(imageMat.argmax(), imageMat.shape) return (xVals[colMaxInd], yVals[rowMaxInd]) # Sum the numbers in each column of the matrix data which have the highest values. # Assume that, in row i, the last column of maxInds contains the index of the row in data # which has the highest value in column i. The 2nd last column in maxInds contains the index of the row in data # which has the 2nd highest value in column i. Etc. # Assume that topChs <= channels. def sumTopRows(data, maxInds, topChs): channels, cols = data.shape outVec = zeros(cols) for i in range(cols): for j in range(topChs): outVec[i] += data[maxInds[i,channels-1-j],i] return outVec # Purpose: Determine the coordinate at a given time. # Inputs: # t_ms: a time in ms # pivotCoords: a list of coordinates a person may "hit" # pathInd: the indices of the row in pivotCoords that the person hits them # startPathTime: the time at which the person hits his first pivot point # speed: in pivot points "hit" per ms def calcActualPosition(t_ms, pivotCoords, pathInd, startPathTime, speed): endPathTime = startPathTime + (len(pathInd)-1)/speed if (t_ms < startPathTime) or (t_ms >= endPathTime): actCoord = [] else: point_real = (t_ms-startPathTime) * speed point_int = floor(point_real) point_frac = point_real - point_int prevCoord = pivotCoords[pathInd[point_int],:] nextCoord = pivotCoords[pathInd[point_int + 1],:] actCoord = prevCoord*(1-point_frac) + nextCoord*point_frac return actCoord # Return the coordinate of the maximum pixel in the image def imageMaxCoord(imageMat, xVals, yVals): rowMaxInd, colMaxInd = unravel_index(imageMat.argmax(), imageMat.shape) return (xVals[colMaxInd], yVals[rowMaxInd]) # Parameters you may change: # plotSkip: Refresh the plot after this many data lines are read # buffL: buffer length, ie, how much recent data to save for each link. # startSkip: A serial port seems to have a "memory" of several lines, # which were saved from the previous experiment run. # ** Must be greater than 0. plotSkip = 5 startSkip = 1 buffL = 4 calLines = 30 topChs = 2 # How many channels to include in multichannel RTI coordFileName = '3129_coords.txt' channels = 5 delta_p = 0.8 # distance units from coordFileName sigmax2 = 0.5 # image value^2 delta = 3.0 # distance units from coordFileName excessPathLen = 0.25 # distance units from coordFileName pivotFileName = 'pivotCoords.txt' pivotCoords = numpy.loadtxt(pivotFileName) pathFileName = 'path.txt' # list of pivot numbers in order. pathInd = numpy.loadtxt(pathFileName) startPathTime = 18000.0 # ms speed = 1.0 / 2000.0 # pivot points per millisecond. # Load the coordinate file, find the bottom left and top right sensorCoords = numpy.loadtxt(coordFileName) sensors = len(sensorCoords) # It looks nice to have the sensor coordinates plotted on top of any image. plt.ion() # initialize the RTI projection matrix, create a grid. inversion, xVals, yVals = initRTI(sensorCoords, delta_p, sigmax2, delta, excessPathLen) imageExtent = (min(xVals) - delta_p/2, max(xVals) + delta_p/2, min(yVals) - delta_p/2, max(yVals) + delta_p/2) xValsLen = len(xVals) yValsLen = len(yVals) # remove junk from start of file. for i in range(startSkip): line = sys.stdin.readline() # Use the most recent line to determine how many columns (streams) there are. lineList = [int(i) for i in line.split()] time_ms = lineList.pop() prevRSS = array(lineList) numLinks = len(prevRSS) numPairs = sensors*(sensors-1) if numLinks != numPairs*channels: print "Error in numLinks" # Initialize RSS Buffer, a list of buffers, one for each link. # For VRTI. buff = [] for i in range(numLinks): buff.append( FixedLenBuffer([0]*buffL)) # Run forever, adding lines as they are available. counter = 0 actualCoord = [] VRTI_err_list = [] RTI_err_list = [] sumRSS = zeros(numLinks) countCalLines = zeros(numLinks) while 1: #print "Counter = " + str(counter) line = sys.stdin.readline() if not line: continue while line[-1] != '\n': # If line is incomplete, add to it. line += sys.stdin.readline() # Get the integers from the line string lineList = [int(i) for i in line.split()] time_ms = lineList.pop(-1) # remove last element rss = array(lineList) actualCoord = calcActualPosition(time_ms, pivotCoords, pathInd, startPathTime, speed) # data > -10 means no data measured. Replace with most recently measured value. for i in range(numLinks): if (rss[i] > -10): rss[i] = prevRSS[i] # Store current RSS vector for each link in its FixedLenBuffer # For variance-based RTI. buff[i].append(rss[i]) # Use first "calLines" data vectors to find average RSS if counter < calLines: for i in range(numLinks): if rss[i] < -10: sumRSS[i] += rss[i] countCalLines[i] += 1 # At the end of the calLines period, compute the average RSS for each link. elif counter == calLines: meanRSS = sumRSS / countCalLines # If you have a divide-by-zero problem with the line above, use: #meanRSS = array([sumRSS[i] / max(1,countCalLines[i]) for i in range(len(sumRSS))]) # Sort the meanRSS to decide which channels have the highest average RSS. # Make each channel's RSS vector into a separate row meanRSS.shape = (channels, numPairs) maxInds = meanRSS.transpose().argsort() # Sum the highest topChs channels. calVec = sumTopRows(meanRSS, maxInds, topChs) # When the calibration period is over, calc & display RT images. if counter >= calLines: # Compute difference between calVec and current RSS measurement rss.shape = (channels, numPairs) curVec = sumTopRows(rss, maxInds, topChs) rss.shape = numLinks scoreVec = calVec - curVec # Compute shadowing-based radio tomographic image image = callRTI(scoreVec, inversion, len(xVals), len(yVals)) RTIMaxCoord = imageMaxCoord(image, xVals, yVals) #print "RTI Image range:" + str(image.min()) + " - " + str(image.max()) # Plot the RTI image each plotSkip data lines. if (counter-calLines) % plotSkip == 0: plt.figure(2) plt.cla() plotLocs(sensorCoords) imhandle = plt.imshow(image, interpolation='none', origin='lower', extent=imageExtent, vmin=0, vmax=8) plt.ylabel('Y Coordinate (ft)') plt.xlabel('X Coordinate (ft) at time ' + str(time_ms)) if len(actualCoord) > 0: text(actualCoord[0], actualCoord[1],'X', horizontalalignment='center', verticalalignment='center') if counter==calLines: plt.colorbar() plt.draw() # Calculate variance on each link and calculate VRTI image varVec = array([a.var() for a in buff]) varVec.shape = (channels, numPairs) scoreVec = sumTopRows(varVec, maxInds, topChs) # Compute variance-based radio tomographic image image = callRTI(scoreVec, inversion, len(xVals), len(yVals)) VRTIMaxCoord = imageMaxCoord(image, xVals, yVals) #print "VRTI Image range:" + str(image.min()) + " - " + str(image.max()) if (counter-calLines) % plotSkip == 0: plt.figure(3) plt.cla() plotLocs(sensorCoords) imhandle = plt.imshow(image, interpolation='none', origin='lower', extent=imageExtent, vmin=0, vmax=16) plt.ylabel('Y Coordinate (ft)') plt.xlabel('X Coordinate (ft) at time ' + str(time_ms)) if len(actualCoord) > 0: text(actualCoord[0], actualCoord[1],'X', horizontalalignment='center', verticalalignment='center') if counter==calLines: plt.colorbar() plt.draw() # Find the coordinate error if len(actualCoord) > 0: VRTI_err = sqrt(sum((actualCoord-VRTIMaxCoord)**2)) VRTI_err_list.append(VRTI_err) RTI_err = sqrt(sum((actualCoord-RTIMaxCoord)**2)) RTI_err_list.append(RTI_err) VRTI_RMSE = sqrt(mean(array(VRTI_err_list)**2)) RTI_RMSE = sqrt(mean(array(RTI_err_list)**2)) print RTI_RMSE, VRTI_RMSE # Save RSS in case next line has missing data. prevRSS = rss.copy() counter += 1