#! /usr/bin/env python
#
# LICENSE:
# Copyright (C) 2019
#
# 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, Sharath Sundaram, Nathan Mester,
# Neal Patwari, neal.patwari@gmail.com
#
# Version History:
#
# Version 1.0: Initial Release. 17 Oct 2012
# Version 2.0: 30 Oct 2014.
# Version 2.1: 3 July 2016.
# Added actualKnown option to turn off location error calculation
# Version 3.0: 17 April 2019
# Updated to accept new data structure and new overall plotting algorithm
#
# Purpose:
# This script calls functions in order to calculate radio tomographic images
# from the following inputs:
# 1) the locations of radio sensors;
# 2) received signal strength (RSS) measurements between pairs of those
# sensors, on multiple channels. This RTI code assumes that the RSS
# was recorded using listenAllLinks.py or listenx.py, which simply
# lists the RSS value on every (Tx, Rx, Channel) combination. The last
# column is the time, in integer milliseconds from the start of the
# data collection.
# The code plots the RTI estimate. Currently, RTI methods implemented are:
# 1) multi-channel attenuation-based (traditional) RTI. This is the method
# published in MASS 2012:
# O. Kaltiokallio, M. Bocca, and N. Patwari, Enhancing the accuracy
# of radio tomographic imaging using channel diversity, 9th IEEE Int'l
# Conference on Mobile Ad hoc and Sensor Systems, October 8-11, 2012.
# The code outputs coordinate estimates into the file, outputFileName.
#
# Usage:
# cat filename.txt | python rti_stub.py
# where filename.txt is the output listenAllLinks.py or listenx.py
#
# Alternatively, use
# python listenx.py -i filename.txt | python rti_stub_v2.py
# if the filename.txt was recorded at the gateway node.
#
# Alternatively, use
# ssh root@xandem-gateway.local "/opt/xandev/exec/gateway/bin/gateway -l" | python listenx.py | python rti_stub_v2.py
# for a real-time operation of the script from the data collected at the Xandem gateway node. This assumes your gateway is connected to the same local network as the computer running this script.
#
# There must be an excel file in the directory to use actualKnown == True:
# excelFile
#
# There must always be the true sensor coordinate file in the directory:
# coordFileName
#
# The output coordinate estimates (one per row of the input file)
# is saved to the file named in outputFileName. A row of -99 -99 indicates
# that no person is estimated to be present in the area.
import rti_new
import sys
import numpy as np
import matplotlib.pyplot as plt
import matplotlib
import pandas as pd
from scipy import interpolate
#Library Parameters
matplotlib.rc('xtick', labelsize=16)
matplotlib.rc('ytick', labelsize=16)
# Number of lines that numpy should print
np.set_printoptions(threshold=1000, linewidth = 160)
# Parameters you may change:
startSkip = 80 # Desired number of lines to be skipped at the start. Default = 80
plotSkip = 20 # Number of lines that should be received between plot updates. Also used for error calculation and
# outputting the person's estimated location to a txt outputFileName.
# Error is reduced if plotSkip is a multiple of the number of nodes, so that the same tx node data is
# received before each plot update.
calSets = 50 # Number of full sets of all channel's rss data that should be used for calibration. Default = 50.
delta_p = 0.1 # Distance between pixels. Lower values will increase resolution. Default = 0.1
sigma_x2 = 0.50 # image value^2 for covariance matrix. Default = 0.5
delta = 1.0 # Pixel covariance factor. Default = 1.0
falloff = 0.01 # Falloff for pixel weight matrix. Smaller values mean quicker falloff. Default = 0.01
excessPathLen = 0.05 # Distance for pixel weight matrix ellipse. Default = 0.05
chLinkValue = 4.0 # How much should each channel value be weighted by? Default = 4.0
topChs = 8 # Number of channels to include in multichannel RTI weighting. Default = 8
alpha = 0.985 # Value that all of the previous combined images should be weighted by according to the formula:
# y[n] = alpha*y[n-1]+(alpha-1)*x[n], where y[n] is the totalImage, and x[n] is the new sub-image.
personInAreaThreshold = 1.0 # An image max value above this makes us believe someone is in the area. Default = 1.0
units = 'm' # Distance units for plot label.
outputFileName= "basement/new_estimate.txt" # The person's estimated location will be output to this file.
actualKnown = True # Whether the actual coordinates are known.
plottingEnabled = False # Whether a plot should be shown. Turn this off to speed up error calculation.
# List of channel ID in order by index.
channel_list = [26, 18, 24, 16, 22, 14, 20, 12]
# ActualCoords are taken from an excel document and interpolated for all time for error calculation.
if actualKnown:
excelFile = "exp_2019April16/Take 2019-04-16 04.37.00 PM_001.csv.xlsx"
dataframe = pd.read_excel(excelFile, usecols = [1, 34, 36])
excelData = dataframe.to_numpy()
excelTime = excelData[7:,0]
excel_xy_coords = excelData[7:,1:]
fx = interpolate.interp1d(excelTime, excel_xy_coords[:, 0], kind='linear', fill_value=0.)
fy = interpolate.interp1d(excelTime, excel_xy_coords[:, 1], kind='linear', fill_value=0.)
else:
fx = 0
fy = 0
# Load the sensor coordinate file, find the total number of sensors
coordFileName = 'exp_2019April16/sensor_coords_4-16.txt'
sensorCoords = np.loadtxt(coordFileName)
sensors = len(sensorCoords)
# Open a file to write the coordinate estimates to
fout = open(outputFileName, "w")
# It looks nice to have the sensor coordinates plotted on top of any image.
plt.ion()
numChannels = len(channel_list) # Number of channels pulled from channel_list.
# There can only be as many top channels as there are total channels.
if topChs > numChannels:
topChs = numChannels
# initialize the RTI projection matrix, create a rectangular grid of pixels.
inversion, xVals, yVals, pixelCoords = rti_new.initRTI(sensorCoords, delta_p, sigma_x2, delta, falloff, excessPathLen)
imageExtent = (min(xVals) - delta_p/2, max(xVals) + delta_p/2,
min(yVals) - delta_p/2, max(yVals) + delta_p/2) # the min, max for plot axes
xValsLen = len(xVals) # how many pixels along the x-axis.
yValsLen = len(yVals) # how many pixels along the y-axis.
# Open the file in argv[1]; if none, use stdin as the file
if len(sys.argv) == 1:
infile = sys.stdin
fromStdin = True
else:
infile = open(sys.argv[1])
fromStdin = False
# removes startSkip lines of junk from start of file.
line = infile.readline()
for i in range(startSkip):
line = infile.readline()
# Use the first line to determine how many columns there are and pull the cdID, txID, currentTime, and RSS values.
lineList = line.split(", ")
chID = int(float(lineList.pop(0)))
chIndex = channel_list.index(chID) # Uses channel list to index channels.
txID = int(float(lineList.pop(0))) - 1 # Subtracts by 1 that TX node 1 has ID = 0.
currentTime = float(lineList.pop(-1))
startTime = currentTime
lineListRSS = [int(float(i)) for i in lineList.pop(0).split()]
lineListRSS.pop(txID) # Remove the self link value. (Ex: Node 1 to Node 1)
rss = np.array(lineListRSS)
numNodes = len(rss)+1 # For N nodes there are only N-1 RSS values per node.
if numNodes != sensors:
sys.exit('Error: sensorCoords file sensors = ' + str(sensors) + "; However, " + str(numNodes) + " were found.")
numLinks = numNodes*(numNodes-1)
startTxRxLinkIndex = txID * (numNodes - 1)
endTxRxLinkIndex = txID * (numNodes - 1) + numNodes - 1
# prevRSS holds the last RSS value for each link-channel pair in case a value is not received.
prevRSS = np.zeros((numLinks, numChannels))
prevRSS[startTxRxLinkIndex:endTxRxLinkIndex, chIndex] = rss
calLines = calSets*numNodes*numChannels
timeDelay = 0 # Automatically adjust later on to match with OptiTrack data.
personInAreaCounter = 0 # Used to keep a sum of how many frames a person was present in the image for error calculation.
counterRTI = 0 # Internal counter for calibration.
sumRSS = np.zeros((numLinks, numChannels))
countCalLines = np.zeros((numLinks, numChannels))
meanRSS = np.zeros((numLinks, numChannels))
chLinkWeight = np.zeros((numLinks, numChannels))
subImage = np.zeros(len(pixelCoords))
prevImage = np.zeros(len(pixelCoords))
RTI_errList = []
keepReading = True
startCal = False
firstPlot = True
colorBarPlotted = False
personNotYetAppeared = True
# Run forever, adding lines as they are available.
while keepReading:
line = infile.readline()
# If at the "end of file", keep reading if reading from stdin.
if not line:
keepReading = fromStdin
continue
while line[-1] != '\n': # If line is incomplete, add to it.
line += infile.readline()
# Pull the cdID, txID, currentTime, and RSS values from line read in.
lineList = line.split(", ")
chID = int(float(lineList.pop(0)))
txID = int(float(lineList.pop(0))) - 1
startTxRxLinkIndex = txID * (numNodes - 1)
endTxRxLinkIndex = txID * (numNodes - 1) + numNodes - 1
chIndex = channel_list.index(chID)
currentTime = float(lineList.pop(-1))
lineListRSS = [int(float(i)) for i in lineList.pop(0).split()]
lineListRSS.pop(txID)
rss = np.array(lineListRSS)
# data > -10 means no data measured. Replace with most recently measured value from prevRSS.
for i in range(len(rss)):
if rss[i] > -10:
rss[i] = prevRSS[startTxRxLinkIndex+i,chIndex]
# Continue to fill out prevRSS to replace missing data.
prevRSS[startTxRxLinkIndex:endTxRxLinkIndex, chIndex] = rss
#Run until the start of a channel is found for calibration.
if txID == 0 and chIndex == 0:
startCal = True
if not startCal:
continue
# Calibration Period
if counterRTI < calLines:
for i in range(len(rss)):
# Checking to see if data was measured again in case the prevRSS cell was still empty.
if rss[i] < -10:
# Keep a running sum of all channel-link rss values and increment that index.
sumRSS[startTxRxLinkIndex+i, chIndex] += rss[i]
countCalLines[startTxRxLinkIndex+i, chIndex] += 1
# Find the average RSS for each link and channel
elif counterRTI == calLines:
for i in np.nditer(countCalLines):
if i == 0:
sys.exit('Error: No calibration value for some links. Rerun with calSets at a higher value.')
# Divides sumRSS by countCalLines to find the mean value for each link-ch pair.
meanRSS = np.divide(sumRSS, countCalLines)
# Create channel weight matrix. Takes the top N channels for each link and weighs them by chLinkValue.
meanInds = meanRSS.argsort()
for i in range(meanInds.shape[0]):
for j in range(meanInds.shape[1]):
if meanInds[i,j] < topChs:
chLinkWeight[i,j] = chLinkValue
elif counterRTI > calLines:
# Take the absolute value of the difference between the mean value of the link and the current link value.
diffVec = abs(meanRSS[startTxRxLinkIndex:endTxRxLinkIndex, chIndex] - rss)
# Weigh the difference vector by the channel weight matrix.
curVec = np.multiply(chLinkWeight[startTxRxLinkIndex:endTxRxLinkIndex, chIndex], diffVec)
# Find the pseudo-inverse of the RSS values to obtain a sub-image.
subImage = np.dot(inversion[:, startTxRxLinkIndex:endTxRxLinkIndex], curVec)
if firstPlot:
totalImage = subImage # For the first plot, set the total image as the current image, as there is no prevImage
firstPlot = False
else:
# Combine all previous images with the new image with specified weights.
totalImage = alpha * prevImage + (1-alpha) * subImage
prevImage = totalImage
totalImage.shape = (yValsLen, xValsLen) #Reshape the totalImage for plotting.
# If it's time to plot:
if counterRTI % plotSkip == 0:
#Find the coordinate of the pixel with the highest value.
RTIMaxCoord = rti_new.imageMaxCoord(totalImage, xVals, yVals)
# Plot the coordinates and image if someone is there, otherwise only plot the image.
if totalImage.max() > personInAreaThreshold:
# If we have the actualCoordinate, plot that as well.
if actualKnown:
# Syncs up actualCoords and estimated coords by adding a time delay for when a person first appears.
if personNotYetAppeared:
timeDelay = currentTime - startTime - 0.01
print "Time Delay: " + str(timeDelay) + " s"
personNotYetAppeared = False
# Find the coordinates that the person was actually at, after adjusting for time delay.
actualCoordTime = currentTime - startTime - timeDelay
actualCoords = (fx(actualCoordTime).item(), fy(actualCoordTime).item())
if plottingEnabled:
rti_new.plotImage(totalImage, 2, sensorCoords, imageExtent, 1.6, units, currentTime - startTime,
RTIMaxCoord, actualCoords)
fout.write(str(currentTime - startTime) + " " + str(RTIMaxCoord[0]) + " " + str(RTIMaxCoord[1])
+ "\n")
# Error calculation
personInAreaCounter += 1
RTI_err = np.sqrt((RTIMaxCoord[0]-actualCoords[0])**2+(RTIMaxCoord[1]-actualCoords[1])**2)
RTI_errList.append(RTI_err)
else:
if plottingEnabled:
rti_new.plotImage(totalImage, 2, sensorCoords, imageExtent, 1.6, units, currentTime - startTime,
RTIMaxCoord)
else:
if plottingEnabled:
rti_new.plotImage(totalImage, 2, sensorCoords, imageExtent, 1.6, units, currentTime - startTime)
fout.write(str(currentTime - startTime) + " -99 -99\n")
plt.pause(0.0001)
# Color bar only needs to be plotted once.
if plottingEnabled:
if not colorBarPlotted:
plt.colorbar()
colorBarPlotted = True
totalImage.shape = pixelCoords.shape[0] #Reshape the totalImage back into a vector of all pixels.
counterRTI += 1
# Calculate the score from how many frames a person was detected and the average distance between the actual coordinate
# and the coordinate we estimated. Note that higher higher personInAreaCounter does not necessarily mean a better error,
# only a higher sensitivity. Used to make sure decrease in error doesn't allow decrease frames where someone is plotted.
if actualKnown:
RTI_RMSE = np.sqrt(np.mean(np.array(RTI_errList)**2))
personInAreaAdjusted = personInAreaCounter*plotSkip/100.
print "RTI RMSE = " + str(RTI_RMSE)
print "personInAreaTime = " + str(personInAreaAdjusted)
print "Score = " + str((1./RTI_RMSE)*personInAreaAdjusted)