def process(self):
self.imfs = self.findIMFs(self.imfsNo)
self.hilbertAlgorithm = Hilbert(self.ecgSignal, self.tWindowSize,
self.samplingFrequency)
self.imfsSum = np.sum(self.imfs, axis=0)
self.rPeaks = self.hilbertAlgorithm.processImfs(self.imfsSum)
return self.rPeaks
class HilbertTest(unittest.TestCase):
def setUp(self):
refDir = os.path.dirname(os.getcwd())
refNumStr = '103'
refInFileDir = os.path.join(refDir, refNumStr, 'Input.csv')
refOutFileDir = os.path.join(refDir, refNumStr, 'HilbertOutput.csv')
resOutFileDir = os.path.join(refDir, refNumStr, 'HilbertResultsPython.csv')
self.samplingFreq = 360.0
self.refInFile = open(refInFileDir, 'r')
self.refOutFile = open(refOutFileDir, 'r')
self.resultFile = open(resOutFileDir, 'w')
self.refInVector = []
self.refOutVector = []
self.resultVector = []
self.refIntSignalVector = []
self.refMarksVector = []
def tearDown(self):
self.refInFile.close()
self.refOutFile.close()
self.resultFile.close()
def loadReferenceData(self):
refInVectorStr = self.refInFile.readline().split(',')
del refInVectorStr[-1]
for item in refInVectorStr:
val = float(item)
self.refInVector.append(val)
refOutVectorStr = self.refOutFile.readline().split(',')
del refOutVectorStr[-1]
for item in refOutVectorStr:
val = int(float(item))
self.refOutVector.append(val)
def runAlgorithm(self):
self.alg = Hilbert(self.refInVector, samplingFrequency=self.samplingFreq)
self.resultVector = self.alg.process()
for val in self.resultVector or []:
self.resultFile.write('%d\n' % val)
def isResultIdentical(self):
result = True
for ref, res in zip(self.refOutVector, self.resultVector):
print '{} {}'.format(ref, res)
if ref != res:
print 'reference ({}) and result ({}) are not equal'.format(ref, res)
result = False
return result
def testAlgorithm(self):
self.loadReferenceData()
self.runAlgorithm()
self.assertTrue(self.isResultIdentical(), 'Reference vector and result vector are not identical')
def run_Hilbert(self, param):
logging.debug('building Hilbert-R...')
tree = Hilbert(self.data, param)
start = time.clock()
tree.buildIndex()
tree.adjustConsistency()
end = time.clock()
logging.info('[T] Hilbert building time: %.2f' % (end - start))
return self.query(tree)
import os
from Hilbert import Hilbert
refDir = os.path.dirname(os.getcwd())
refNumStr = '102'
refInFileDir = os.path.join(refDir, refNumStr, 'Input.csv')
resOutFileDir = os.path.join(refDir, refNumStr, 'HilbertResultsPython.csv')
refInFile = open(refInFileDir, 'r')
resultFile = open(resOutFileDir, 'w')
refInVector = []
refInVectorStr = refInFile.readline().split(',')
del refInVectorStr[-1]
for item in refInVectorStr:
val = float(item)
refInVector.append(val)
alg = Hilbert(refInVector,
tWindowSize=120.0,
samplingFrequency=360)
resultVector = alg.process()
for val in resultVector or []:
resultFile.write('%d\n' % val)
refInFile.close()
resultFile.close()
class EMD(object):
SD_LIMIT_VAL = 0.3
def __init__(self,
ecgSignal,
samplingFrequency=360.0,
tWindowSize=120.0,
imfsNo=1):
self.ecgSignal = np.array(ecgSignal)
self.ecgSignalSize = len(ecgSignal)
self.samplingFrequency = samplingFrequency
self.tWindowSize = tWindowSize
self.nWindowSize = int(tWindowSize * samplingFrequency)
self.rPeaks = []
self.imfsNo = imfsNo
def process(self):
self.imfs = self.findIMFs(self.imfsNo)
self.hilbertAlgorithm = Hilbert(self.ecgSignal, self.tWindowSize,
self.samplingFrequency)
self.imfsSum = np.sum(self.imfs, axis=0)
self.rPeaks = self.hilbertAlgorithm.processImfs(self.imfsSum)
return self.rPeaks
def findIMFs(self, n=3):
imfs = np.empty([n, self.ecgSignalSize])
residue = False
N = self.ecgSignalSize
x_part = np.copy(self.ecgSignal[:N])
c = np.copy(x_part)
for i in range(0, n):
h = np.copy(c)
sd = np.inf
prev_h = np.copy(h)
while sd > EMD.SD_LIMIT_VAL:
maxima, minima = self.findExtrema(h)
# is it already a residue?
if len(maxima) + len(minima) < 2:
residue = True
break
maxima = np.insert(maxima, 0, 0)
minima = np.insert(minima, 0, 0)
maxima = np.append(maxima, len(h) - 1)
minima = np.append(minima, len(h) - 1)
maxima_val = [h[j] for j in maxima]
minima_val = [h[j] for j in minima]
upper_bound_func = spi.InterpolatedUnivariateSpline(maxima,
maxima_val,
k=3)
lower_bound_func = spi.InterpolatedUnivariateSpline(minima,
minima_val,
k=3)
x_new = np.arange(0, N, 1)
upper_bound = upper_bound_func(x_new)
lower_bound = lower_bound_func(x_new)
m = np.add(lower_bound, upper_bound) * 0.5
h = np.subtract(prev_h, m)
sd = self.calculateSD(prev_h, h, eps=0.0000001)
prev_h = np.copy(h)
imfs[i] = h
if residue:
break
c = np.subtract(c, h)
return imfs
def calculateSD(self, previous, current, eps=0.0):
eps_array = np.empty(len(current))
eps_array.fill(eps)
numerator = np.sum(np.square(np.subtract(previous, current)))
denominator = np.sum(np.add(np.square(previous), eps_array))
return np.divide(numerator, denominator)
def findExtrema(self, signal):
maxima = sps.argrelextrema(signal, np.greater)
minima = sps.argrelextrema(signal, np.less)
return maxima, minima
def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.plt = Plot()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
self.cars.append(Car(x=100, y=100, theta=-45,
speed=0)) # representation for actual car
self.cars.append(Car(x=100, y=100, theta=-45 + 15,
speed=0)) # representation for ins estimate
self.cars.append(
Car(x=100, y=100, theta=-45, speed=0)
) # representation for ins estimate xy optimization single pass
self.cars.append(
Car(x=100, y=100, theta=-45, speed=0)
) # representation for ins estimate xy optimization multi pass
self.cars.append(
Car(x=100, y=100, theta=-45,
speed=0)) # representation for ins estimate theta optimization
for car in self.cars:
car.color = Draw.WHITE
self.cars[2].color = Draw.YELLOW
self.cars[3].color = Draw.RED
self.cars[4].color = Draw.GREEN
self.action = None
self.dragdrop = DragAndDropController(self.events)
self.controller = self.dragdrop
# self.cars[0].camview = CamView(self.cars[0],self.background.arr)
# self.cars[0].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "estimate"
self.cars[2].name = "opt"
self.cars[3].name = "opt*"
self.cars[4].name = "theta"
self.cars[0].controller = self.controller
self.cars[1].controller = self.controller
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[2].controller = OptimizeNearestEdgeXYSinglePass(
optimize=self.optimize,
labels=self.labels,
label_positions=self.label_positions,
camview=self.cars[0].camview,
estimate_car=self.cars[1])
self.cars[3].controller = OptimizeNearestEdgeXYMultiPass(
optimize=self.optimize,
labels=self.labels,
label_positions=self.label_positions,
camview=self.cars[0].camview,
estimate_car=self.cars[1])
self.cars[4].controller = OptimizeThetaParable(
optimize=self.optimize,
distances=self.distances,
camview=self.cars[0].camview,
estimate_car=self.cars[3])
# self.cars[4].controller = OptimizeNearestEdgeTheta(
# optimize = self.optimize,
# labels = self.labels,
# label_positions = self.label_positions,
# hilbert = self.hilbert,
# camview = self.cars[0].camview,
# estimate_car = self.cars[1])
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.done = False
self.cursor = (0, 0)
self.register_events()
self.spin()
def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
# for k in range(1):
# self.cars.append(Car(x=150+k*5,y=100,theta=np.random.randint(0,360),speed=np.random.randint(45,180)))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 1.5 * 90))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 90)) # [1] human
self.cars.append(Car(x=50, y=250, theta=90,
speed=1 * 90)) # [2] ghost of ins estimating [0]
self.action = None
self.human = HumanController()
self.heuristic = Heuristic(self.lane)
Node.heuristic = self.heuristic
self.onestep = OneStepLookaheadController(self.cars, self.lane,
self.heuristic)
self.nstep = NStepLookaheadController(self.cars, self.lane,
self.heuristic, 2)
self.bestfirst = BestFirstController(self.cars, self.lane,
self.heuristic)
self.controller = self.bestfirst
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[0].camview.register_events(self.events)
self.cars[0].controller = self.controller
self.cars[0].collision = False
self.cars[0].imu = IMU(self.cars[0])
self.cars[0].ins = INS(self.cars[0].imu.calibration_noise)
self.cars[0].ins.update_pose(self.cars[0].x,
self.cars[0].y,
self.cars[0].theta / Utils.d2r,
gain=1)
self.insghost = INSGhostController(self.cars[0].ins)
self.cars[1].controller = self.human
self.cars[2].controller = self.insghost
self.cars[2].collision = False
self.cars[2].size *= 1.25
self.cars[2].camview = CamView(self.cars[2],
self.background.arr_dist_rgb,
width=275,
height=275,
offset=(0, 75),
angle_offset=-25)
self.cars[2].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "human"
self.cars[2].name = "estimate"
# this causes the controller of cars[0] to use the information from cars[0].ghost but act on cars[0]
# self.cars[0].ghost = self.cars[2]
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.grid = Grid(50, 50, *self.size)
self.last_distance_grid_update = time() - 10
self.update_distance_grid()
self.done = False
for car in self.cars:
# save original speed
if not hasattr(car, "speed_on"):
car.speed_on = car.speed
# toggle speed
car.speed = car.speed_on - car.speed
# car.pause = not car.pause
self.plot_window_size = 100
self.xyt_corr_ring_buffer = RingBuffer(self.plot_window_size,
channels=3)
self.xyt_corr_plot = RingBufferPlot(self.xyt_corr_ring_buffer)
# self.normal_test_p_value_plot = RingBufferPlot(RingBuffer(self.plot_window_size,channels=self.xyt_corr_ring_buffer.channels))
self.std_plot = RingBufferPlot(
RingBuffer(self.plot_window_size,
channels=self.xyt_corr_ring_buffer.channels))
self.velocity_carthesian_history_plot = RingBufferPlot(
self.cars[0].ins.velocity_carthesian_history)
# self.hist_plot = HistogramPlot(10)
self.register_events()
self.spin()
class EMD(object):
SD_LIMIT_VAL = 0.3
def __init__(self, ecgSignal, samplingFrequency=360.0, tWindowSize=120.0, imfsNo=1):
self.ecgSignal = np.array(ecgSignal)
self.ecgSignalSize = len(ecgSignal)
self.samplingFrequency = samplingFrequency
self.tWindowSize = tWindowSize
self.nWindowSize = int(tWindowSize * samplingFrequency)
self.rPeaks = []
self.imfsNo = imfsNo
def process(self):
self.imfs = self.findIMFs(self.imfsNo)
self.hilbertAlgorithm = Hilbert(self.ecgSignal, self.tWindowSize, self.samplingFrequency)
self.imfsSum = np.sum(self.imfs, axis=0)
self.rPeaks = self.hilbertAlgorithm.processImfs(self.imfsSum)
return self.rPeaks
def findIMFs(self, n=3):
imfs = np.empty([n, self.ecgSignalSize])
residue = False
N = self.ecgSignalSize
x_part = np.copy(self.ecgSignal[:N])
c = np.copy(x_part)
for i in range(0, n):
h = np.copy(c)
sd = np.inf
prev_h = np.copy(h)
while sd > EMD.SD_LIMIT_VAL:
maxima, minima = self.findExtrema(h)
# is it already a residue?
if len(maxima) + len(minima) < 2:
residue = True
break
maxima = np.insert(maxima, 0, 0)
minima = np.insert(minima, 0, 0)
maxima = np.append(maxima, len(h) - 1)
minima = np.append(minima, len(h) - 1)
maxima_val = [h[j] for j in maxima]
minima_val = [h[j] for j in minima]
upper_bound_func = spi.InterpolatedUnivariateSpline(maxima,
maxima_val,
k=3)
lower_bound_func = spi.InterpolatedUnivariateSpline(minima,
minima_val,
k=3)
x_new = np.arange(0, N, 1)
upper_bound = upper_bound_func(x_new)
lower_bound = lower_bound_func(x_new)
m = np.add(lower_bound, upper_bound) * 0.5
h = np.subtract(prev_h, m)
sd = self.calculateSD(prev_h, h, eps=0.0000001)
prev_h = np.copy(h)
imfs[i] = h
if residue:
break
c = np.subtract(c, h)
return imfs
def calculateSD(self, previous, current, eps=0.0):
eps_array = np.empty(len(current))
eps_array.fill(eps)
numerator = np.sum(np.square(np.subtract(previous, current)))
denominator = np.sum(np.add(np.square(previous), eps_array))
return np.divide(numerator, denominator)
def findExtrema(self, signal):
maxima = sps.argrelextrema(signal, np.greater)
minima = sps.argrelextrema(signal, np.less)
return maxima, minima
def process(self):
self.imfs = self.findIMFs(self.imfsNo)
self.hilbertAlgorithm = Hilbert(self.ecgSignal, self.tWindowSize, self.samplingFrequency)
self.imfsSum = np.sum(self.imfs, axis=0)
self.rPeaks = self.hilbertAlgorithm.processImfs(self.imfsSum)
return self.rPeaks
class HilbertTest(unittest.TestCase):
def setUp(self):
refDir = os.path.dirname(os.getcwd())
refNumStr = '103'
refInFileDir = os.path.join(refDir, refNumStr, 'Input.csv')
refOutFileDir = os.path.join(refDir, refNumStr, 'HilbertOutput.csv')
resOutFileDir = os.path.join(refDir, refNumStr,
'HilbertResultsPython.csv')
self.samplingFreq = 360.0
self.refInFile = open(refInFileDir, 'r')
self.refOutFile = open(refOutFileDir, 'r')
self.resultFile = open(resOutFileDir, 'w')
self.refInVector = []
self.refOutVector = []
self.resultVector = []
self.refIntSignalVector = []
self.refMarksVector = []
def tearDown(self):
self.refInFile.close()
self.refOutFile.close()
self.resultFile.close()
def loadReferenceData(self):
refInVectorStr = self.refInFile.readline().split(',')
del refInVectorStr[-1]
for item in refInVectorStr:
val = float(item)
self.refInVector.append(val)
refOutVectorStr = self.refOutFile.readline().split(',')
del refOutVectorStr[-1]
for item in refOutVectorStr:
val = int(float(item))
self.refOutVector.append(val)
def runAlgorithm(self):
self.alg = Hilbert(self.refInVector,
samplingFrequency=self.samplingFreq)
self.resultVector = self.alg.process()
for val in self.resultVector or []:
self.resultFile.write('%d\n' % val)
def isResultIdentical(self):
result = True
for ref, res in zip(self.refOutVector, self.resultVector):
print '{} {}'.format(ref, res)
if ref != res:
print 'reference ({}) and result ({}) are not equal'.format(
ref, res)
result = False
return result
def testAlgorithm(self):
self.loadReferenceData()
self.runAlgorithm()
self.assertTrue(
self.isResultIdentical(),
'Reference vector and result vector are not identical')
def runAlgorithm(self):
self.alg = Hilbert(self.refInVector,
samplingFrequency=self.samplingFreq)
self.resultVector = self.alg.process()
for val in self.resultVector or []:
self.resultFile.write('%d\n' % val)
def runAlgorithm(self):
self.alg = Hilbert(self.refInVector, samplingFrequency=self.samplingFreq)
self.resultVector = self.alg.process()
for val in self.resultVector or []:
self.resultFile.write('%d\n' % val)