def __init__(self, bdf_file):
self.bdf_file = bdf_file
self.pulse_sequence = 0
self.signal = None
self.peaks = None
self.frequency = None
self.heart_rates = None
self.total_average_heart_rate = None
self.publisher = PulsePublisher("ecg")
def __init__(self, show_processed_image):
self.show_processed_image = show_processed_image
self.count = 0
self.levels = 2
self.low = 0.9
self.high = 1.7
self.amplification = 30
self.publisher = PulsePublisher("eulerian_motion_magnification")
self.fps = 30
self.video_array = []
self.buffer_size = 0
self.time_array = []
self.calculating_at = 0
self.calculating_boarder = 50
self.recording_time = 10
self.isFirst = True
self.arrayLength = 0
def __init__(self, is_video):
self.is_video = is_video
self.fps = 0
self.buffer_size = 250
self.data_buffer = []
self.times = []
self.ttimes = []
self.samples = []
self.freqs = []
self.fft = []
self.slices = [[0]]
self.bpms = []
self.bpm = 0
self.MAX_BPM = 150
self.MIN_BPM = 40
self.pulse_sequence = 0
self.publisher = PulsePublisher("legacy_measurement")
self.publish_count = 0
def __init__(self):
"""
Constructor.
"""
# set up publisher
self.publisher = PulsePublisher("pulse_head_movement")
# sequence of published pulse values, published with each pulse message
self.published_pulse_value_sequence = 0
# previous image is needed for lucas kanade optical flow tracker (see calculate_optical_flow method)
self.previous_image = None
# the refresh rate specifies how many frames each calculated feature points should be tracked
self.refresh_rate = 300
# the publish rate specifies how many frames are between each published pulse value
self.publish_rate = 50
self.frame_index = 0
# current positions of the tracking point for each buffer position
# three dimensional array of the form
# [buffer_position][point][x/y], so e.g.
# [[[x1_b1,y1_b1], [x2_b1,y2_b1],...],
# [[x1_b2,y1_b2], [x2_b2,y2_b2],...],
# ...]
# for each point
self.buffer_points = []
# time for each buffer meaning the times associated with the frames of this buffer
# two dimensional array of the form
# [buffer_position][y_points
# [[t1_b1,t2_b1,t3_b1,...]
# [t1_b2,t2_b2,t3_b2,...],
# ...]
self.buffered_time_arrays = []
# y positions over the time for each tracking point for each buffer position
# three dimensional array of the form
# [[[y1_t1_b1,y1_t2_b1,y1_t3_b1,...],[y2_t1_b1,y2_t2_b1,y2_t3_b1,...],...]
# [y1_t1_b2,y1_t2_b2,y1_t3_b2,...],[y2_t1_b2,y2_t2_b2,y2_t3_b2,...],...],
# ...]
self.buffered_y_tracking_signal = []
self.fps = 0
class LegacyMeasurement(object):
def __init__(self, is_video):
self.is_video = is_video
self.fps = 0
self.buffer_size = 250
self.data_buffer = []
self.times = []
self.ttimes = []
self.samples = []
self.freqs = []
self.fft = []
self.slices = [[0]]
self.bpms = []
self.bpm = 0
self.MAX_BPM = 150
self.MIN_BPM = 40
self.pulse_sequence = 0
self.publisher = PulsePublisher("legacy_measurement")
self.publish_count = 0
def on_image_frame(self, roi, timestamp):
self.publish_count += 1
self.times.append(timestamp.to_sec())
# calculate mean green from roi
green_mean = np.mean(self.extractGreenColorChannel(roi))
self.data_buffer.append(green_mean)
# get number of frames processed
L = len(self.data_buffer)
# remove sudden changes, if the avg value change is over 10, use the previous green mean instead
if abs(green_mean - np.mean(self.data_buffer)) > 10 and L > 99:
self.data_buffer[-1] = self.data_buffer[-2]
# only use a max amount of frames. Determined by buffer_size
if L > self.buffer_size:
self.data_buffer = self.data_buffer[-self.buffer_size:]
self.times = self.times[-self.buffer_size:]
L = self.buffer_size
# create array from average green values of all processed frames
processed = np.array(self.data_buffer)
# calculate heart rate every 30 frames
if L == self.buffer_size and self.publish_count % 30 == 0:
# remove linear trend on processed data to avoid interference of light change
processed = signal.detrend(processed)
# calculate fps
self.fps = float(L) / (self.times[-1] - self.times[0])
if self.is_video:
interpolated = processed
else:
# calculate equidistant frame times
even_times = np.linspace(self.times[0], self.times[-1], L)
# interpolate the values for the even times
interpolated = np.interp(x=even_times, xp=self.times, fp=processed)
# apply hamming window to make the signal become more periodic
interpolated = np.hamming(L) * interpolated
# normalize the interpolation
norm = interpolated / np.linalg.norm(interpolated)
# do a fast fourier transformation on the (real) interpolated values
raw = np.fft.rfft(norm)
# get amplitude spectrum
self.fft = np.abs(raw) ** 2
# create a list for mapping the fft frequencies to the correct bpm
self.freqs = (float(self.fps) / L) * np.arange(L / 2 + 1)
freqs = 60. * self.freqs
# find indeces where the frequencey is within the expected heart rate range
idx = np.where((freqs > self.MIN_BPM) & (freqs < self.MAX_BPM))
# reduce fft to "interesting" frequencies
self.fft = self.fft[idx]
# reduce frequency list to "interesting" frequencies
self.freqs = freqs[idx]
# find the frequency with the highest amplitude
if len(self.fft) > 0:
idx2 = np.argmax(self.fft)
self.bpm = self.freqs[idx2]
self.bpms.append(self.bpm)
self.publisher.publish(self.bpm, timestamp)
rospy.loginfo("[LegacyMeasurement] BPM: " + str(self.bpm))
self.samples = processed
# plot fourrier transform
if L == self.buffer_size and self.publish_count % 30 == 0:
index = np.arange(len(self.data_buffer))
data = self.data_buffer - np.mean(self.data_buffer)
plt.clf()
plt.subplot(2, 1, 1)
plt.plot(index, data, '.-')
plt.title('Green value over time')
plt.ylabel('Green value')
plt.xlabel('last x frames')
index = np.arange(len(self.freqs))
plt.subplot(2, 1, 2)
plt.bar(index, self.fft)
plt.xlabel('Frequencies (bpm)', fontsize=10)
plt.ylabel('Amplitude', fontsize=10)
plt.xticks(index, [round(x, 2) for x in self.freqs], fontsize=10, rotation=30)
plt.title('Fourier Transformation')
plt.draw()
plt.pause(0.001)
def extractGreenColorChannel(self, frame):
return frame[:, :, 1]
class PulseHeadMovement:
def __init__(self):
"""
Constructor.
"""
# set up publisher
self.publisher = PulsePublisher("pulse_head_movement")
# sequence of published pulse values, published with each pulse message
self.published_pulse_value_sequence = 0
# previous image is needed for lucas kanade optical flow tracker (see calculate_optical_flow method)
self.previous_image = None
# the refresh rate specifies how many frames each calculated feature points should be tracked
self.refresh_rate = 300
# the publish rate specifies how many frames are between each published pulse value
self.publish_rate = 50
self.frame_index = 0
# current positions of the tracking point for each buffer position
# three dimensional array of the form
# [buffer_position][point][x/y], so e.g.
# [[[x1_b1,y1_b1], [x2_b1,y2_b1],...],
# [[x1_b2,y1_b2], [x2_b2,y2_b2],...],
# ...]
# for each point
self.buffer_points = []
# time for each buffer meaning the times associated with the frames of this buffer
# two dimensional array of the form
# [buffer_position][y_points
# [[t1_b1,t2_b1,t3_b1,...]
# [t1_b2,t2_b2,t3_b2,...],
# ...]
self.buffered_time_arrays = []
# y positions over the time for each tracking point for each buffer position
# three dimensional array of the form
# [[[y1_t1_b1,y1_t2_b1,y1_t3_b1,...],[y2_t1_b1,y2_t2_b1,y2_t3_b1,...],...]
# [y1_t1_b2,y1_t2_b2,y1_t3_b2,...],[y2_t1_b2,y2_t2_b2,y2_t3_b2,...],...],
# ...]
self.buffered_y_tracking_signal = []
self.fps = 0
def pulse_callback(self, original_image, forehead_mask, bottom_mask, time):
"""
Callback method for incoming video frames
:param mask: the message published to the topic (contains gray-image, mask from bottom part of the face
and mask for forehead)
"""
# rospy.loginfo("Capture frame: " + str(self.frame_index))
self.get_current_tracking_points_position(original_image,
time.to_sec())
if self.frame_index % self.publish_rate == 0:
self.add_new_points_to_buffer(original_image, forehead_mask,
bottom_mask, time)
self.frame_index += 1
self.previous_image = original_image
def get_current_tracking_points_position(self, current_image, time):
"""
Get the current position for each tracking point in each buffer position
:param current_image: the current image to calculate the optical flow on
:param time: the corresponding time for the image frame
"""
for buffer_index, points in enumerate(self.buffer_points):
# calculate new tracking points position for each buffer position
new_points = self.calculate_optical_flow(current_image, points)
# replace old [x,y] positions of tracking points in buffer_points array
self.buffer_points[buffer_index] = new_points
# time position of each point meaning the sequential position of the points for each buffer position
time_position = (
(self.frame_index - 1) %
self.publish_rate) + buffer_index * self.publish_rate
# add y point in the time movement for the points
for tracking_point_index, point in enumerate(new_points):
self.buffered_y_tracking_signal[buffer_index][
tracking_point_index][time_position] = point[0][1]
self.buffered_time_arrays[buffer_index][time_position] = time
def calculate_optical_flow(self, image, previous_points):
"""
Calculate the optical flow (track the points) with the lucas kanade tracker
See https://docs.opencv.org/3.4/d4/dee/tutorial_optical_flow.html for more information on optical flow
:param image: the current image to calculate difference to previous image
:param previous_points: the previous position of the points
"""
# make a copy for visualization
vis = image.copy()
lk_params = dict(winSize=(35, 35),
maxLevel=2,
criteria=(cv2.TERM_CRITERIA_EPS
| cv2.TERM_CRITERIA_COUNT, 100, 0.03))
new_points, _, _ = cv2.calcOpticalFlowPyrLK(self.previous_image, image,
previous_points, None,
**lk_params)
# visualization of tracking points
for p in new_points:
cv2.circle(vis, (p[0][0], p[0][1]), 2, (0, 255, 0), -1)
cv2.imshow('lk_track', vis)
cv2.waitKey(3)
return new_points
def add_new_points_to_buffer(self, current_image, forehead_mask,
bottom_mask, timestamp):
"""
Add new points to the buffer in the frequency of self.publish_rate and add them to the buffer
"""
current_points_to_track = self.get_points_to_track(
current_image, forehead_mask, bottom_mask)
self.edit_buffer(current_points_to_track, timestamp)
def get_points_to_track(self, image, forehead_mask, bottom_mask):
"""
get new tracking points using OpenCV goodFeaturesToTrack.
New tracking points are added in the frequency of self.publish_rate
For more information on the goodFeaturesToTrack method see
https://docs.opencv.org/master/d4/d8c/tutorial_py_shi_tomasi.html and
https://docs.opencv.org/2.4/modules/imgproc/doc/feature_detection.html
"""
# get the tracking points in the bottom face region
bottom_feature_params = dict(maxCorners=100,
qualityLevel=0.01,
minDistance=7,
blockSize=7)
bottom_points = cv2.goodFeaturesToTrack(image,
mask=bottom_mask,
**bottom_feature_params)
feature_points = np.array(bottom_points, dtype=np.float32)
# get the tracking points in the forehead region
forehead_feature_params = dict(maxCorners=100,
qualityLevel=0.01,
minDistance=7,
blockSize=7)
forehead_points = cv2.goodFeaturesToTrack(image,
mask=forehead_mask,
**forehead_feature_params)
forehead_points = np.array(forehead_points, dtype=np.float32)
# put tracking points of both regions in one array and return feature points
if feature_points.ndim == forehead_points.ndim:
feature_points = np.append(feature_points, forehead_points, axis=0)
elif feature_points.size == 0 and forehead_points.size > 0:
feature_points = forehead_points
return feature_points
def edit_buffer(self, current_points_to_track, publish_time):
"""
If new points are added, the buffer has to be edited.
If the buffer is full, points in the last position are removed from the array and are further processed
to calculate pulse.
The new points are added in the front of the buffer.
:param current_points_to_track: the new points to add. [x,y] coordinates are pushed into self.buffer_points.
:param publish_time: the timestamp for the ros message to publish the pulse value
"""
if len(self.buffer_points) < self.refresh_rate / self.publish_rate:
# as the buffer is not full, there are no points yet to process.
rospy.loginfo("[PulseHeadMovement] array not full yet ")
else:
# process points on the last array position
self.buffer_points.pop()
points_calculate_pulse = self.buffered_y_tracking_signal.pop()
current_time_array = self.buffered_time_arrays.pop()
self.process_saved_points(points_calculate_pulse,
current_time_array, publish_time)
# push new points in buffer
self.buffer_points.insert(0, current_points_to_track)
# initialize empty array for the y tracking signal of the new points and the time
# and push it to self.buffered_y_tracking_signal and self.buffered_time_array
self.buffered_y_tracking_signal.insert(
0,
np.empty([len(current_points_to_track), self.refresh_rate],
dtype=np.float32))
self.buffered_time_arrays.insert(0, np.empty(self.refresh_rate))
return
def process_saved_points(self, y_tracking_signal, time_array,
publish_time):
"""
central method for processing the points, to calculate pulse in the end.
:param y_tracking_signal: the tracking signal to calculate pulse on
:param time_array: the time of the single positions of the points tracked
:param publish_time: the timestamp for the ros message to publish the pulse value
"""
self.calculate_fps(time_array)
stable_signal = self.remove_erratic_trajectories(y_tracking_signal)
interpolated_points = self.interpolate_points(stable_signal,
time_array)
filtered_signal = self.apply_butterworth_filter(
interpolated_points, time_array)
less_movement = self.discard_much_movement(filtered_signal)
pca_array = self.process_PCA(less_movement, time_array)
signal, frequency = self.find_most_periodic_signal(
pca_array, time_array)
pulse = self.calculate_pulse(signal, frequency, time_array)
self.publish_pulse(pulse, publish_time)
return
def calculate_fps(self, time_array):
"""
Helper method to calculate fps for performance measuring.
"""
timespan = time_array[-1] - time_array[0]
fps = self.refresh_rate / timespan
self.fps = fps
rospy.loginfo("[PulseHeadMovement] Estimated FPS: " + str(fps) +
" (Measured timespan: " + str(timespan) + "s)")
def remove_erratic_trajectories(self, y_tracking_signal):
"""
Some feature points behave unstable. This method removes outliers.
"""
stable_signal = []
rounded_signal = np.rint(y_tracking_signal)
y_point_distance = np.diff(rounded_signal)
y_point_distance = np.absolute(y_point_distance)
# get the maximum distance for each point
max_distances = map(lambda diff: np.amax(diff), y_point_distance)
mode, _ = stats.mode(max_distances)
# only keep the points with max_distance equal or lower than the mode
for point_index, point in enumerate(y_tracking_signal):
if max_distances[point_index] <= mode[0]:
stable_signal.append(point)
stable_signal = np.array(stable_signal)
# uncomment the following lines to see the maximum distances each point has moved. For debugging.
# xs = np.arange(len(max_distances))
# filename = "/home/studienarbeit/Dokumente/" + str(self.seq) + "max_distance"
# plt.figure(figsize=(6.5, 4))
# plt.plot(xs, max_distances, label="S")
# plt.savefig(filename)
return stable_signal
def interpolate_points(self, y_tracking_signal, time_array):
"""
As the movement of the points is measured with max. 30 fps (normally lower) we interpolate the signal to
250Hz which is the normal sample rate of an ECG.
For this, cubic spline interpolation is applied.
:param y_tracking_signal: the signal resulting from remove_erratic_trajectories
:param time_array:
"""
# uncomment the following lines to see the signal before interpolation
# for a random point (i.e. at position 6). For debugging.
# filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step1_before_interp_move_signal_"
# plt.figure(figsize=(6.5, 4))
# plt.plot(time_array, y_tracking_signal[6], label="S")
# plt.savefig(filename)
# plt.close()
sample_rate = 250
stepsize = 1. / sample_rate
interpolated_time = np.arange(time_array[0], time_array[-1], stepsize)
interpolated_points = np.empty(
[np.size(y_tracking_signal, 0),
np.size(interpolated_time)])
for point_index, row in enumerate(y_tracking_signal):
interpolation = CubicSpline(time_array, row)
array_interpolated = interpolation(interpolated_time)
for interpolated_point_index, point in enumerate(
array_interpolated):
interpolated_points[point_index][
interpolated_point_index] = point
# uncomment the following lines to see the interpolated signal
# for a random point (i.e. at position 6). For debugging.
# filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step2_move_signal_"
# plt.figure(figsize=(6.5, 4))
# plt.plot(interpolated_time, interpolated_points[6], label="S")
# plt.savefig(filename)
return interpolated_points
def apply_butterworth_filter(self, input_signal, time_array):
"""
Remove the movements which are not in the frequency of the pulse.
:param input_signal: the signal resulting from interpolate_points
:param time_array:
"""
sample_rate = len(input_signal[0]) / (time_array[-1] - time_array[0])
rospy.loginfo("[PulseHeadMovement] sample rate: " + str(sample_rate))
lowcut = 0.75
highcut = 5
filtered_signal = np.empty(
[np.size(input_signal, 0),
np.size(input_signal, 1)])
for point_index, point in enumerate(input_signal):
filtered_points = butter_bandpass_filter(point,
lowcut,
highcut,
sample_rate,
order=5)
for filtered_point_index, filtered_point in enumerate(
filtered_points):
filtered_signal[point_index][
filtered_point_index] = filtered_point
# uncomment the following lines to see the filtered signal
# for a random point (i.e. at position 6). For debugging.
# filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step3_filter"
# stepsize = 1. / sample_rate
# xs = np.arange(time_array[0], time_array[-1], stepsize)
# plt.figure(figsize=(6.5, 4))
# plt.plot(xs, filtered_signal[6], label="S")
# plt.savefig(filename)
# plt.close()
return filtered_signal
def discard_much_movement(self, signal):
"""
Discard the tracking point with the highest movements.
The parameter alpha determines, how much percent of the points should be removed.
:param signal: the signal to filter resulting from apply_butterworth_filter
"""
alpha = 0.25
number_of_rows_to_discard = int(alpha * np.size(signal, 0))
number_of_rows_to_keep = np.size(signal, 0) - number_of_rows_to_discard
filtered_signal = np.empty(
[number_of_rows_to_keep,
np.size(signal, 1)])
square_signal = np.square(signal)
square_sum = np.sum(square_signal, axis=1)
square_sum = np.squeeze(square_sum)
square_root = np.sqrt(square_sum)
indices_to_discard = square_root.argsort(
)[-number_of_rows_to_discard:][::-1]
filtered_signal_index = 0
for row_index, row in enumerate(signal):
if row_index not in indices_to_discard:
filtered_signal[filtered_signal_index] = row
filtered_signal_index += 1
return filtered_signal
def process_PCA(self, filtered_signal, time_array):
"""
process PCA to get the 5 main movement directions of the signal.
:param filtered_signal: the signal resulting from discard_much_movement
:param time_array:
"""
filtered_signal_transposed = filtered_signal.transpose()
pca = PCA(n_components=5)
pca_array = pca.fit_transform(filtered_signal_transposed)
pca_array = pca_array.transpose()
# uncomment the following lines to see the signal after PCA. For debugging.
# filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step4_pca_signal_"
# sample_rate = len(filtered_signal[0]) / (time_array[-1] - time_array[0])
# stepsize = 1. / sample_rate
# xs = np.arange(time_array[0], time_array[-1], stepsize)
# plt.figure(figsize=(6.5, 4))
# for row in pca_array:
# plt.plot(xs, row, label="S")
# plt.savefig(filename)
return pca_array
def find_most_periodic_signal(self, pca_array, time_array):
"""
Find the most periodic signal to use for pulse calculation.
Uses the highest value of the autocorrelation funciton
:param pca_array: the array resulting from process_PCA
:param time_array:
"""
sample_rate = len(pca_array[0]) / (time_array[-1] - time_array[0])
best_signal = None
best_frequency = 0
best_correlation = 0
for ind, signal in enumerate(pca_array):
spectrum, frequencies, _ = plt.magnitude_spectrum(signal,
Fs=sample_rate)
max_index = np.argmax(spectrum)
strongest_frequency = frequencies[max_index]
# strongest frequency is far too small
if strongest_frequency < 0.1:
continue
T_i = int(sample_rate / strongest_frequency)
s_i_new = np.roll(signal, T_i)
correlation = stats.pearsonr(s_i_new, signal)[0]
if correlation > best_correlation:
best_frequency = strongest_frequency
best_correlation = correlation
best_signal = signal
# uncomment the following lines to see the magnitude spectrum of the PCA singal. For debugging.
# filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step5_periodic_pca_signal_" + str(ind)
# plt.xlim(0, 10)
# plt.savefig(filename)
# plt.close()
# uncomment the following lines to see the most periodic signal of the PCA. For debugging.
# filename = "/home/studienarbeit/Dokumente/" + str(self.seq) + "_periodic_pca_signal_"
# sample_rate = len(pca_array[0]) / (time_array[-1] - time_array[0])
# stepsize = 1. / sample_rate
# pca_array = None
# xs = np.arange(time_array[0], time_array[-1], stepsize)
# plt.figure(figsize=(6.5, 4))
# plt.plot(xs, best_signal, label="S")
# plt.show()
# print(best_frequency)
return best_signal, best_frequency
def calculate_pulse(self, signal, frequency, time_array):
"""
calculates the pulse value from the processed signal.
Uses automatic peak detection and divides number of peaks through measured
this is multiplied by 60 to get bpm.
:param signal: the finally processed signal
"""
sample_rate = len(signal) / (time_array[-1] - time_array[0])
distance = 0.5 * (sample_rate / frequency)
peaks, _ = find_peaks(signal, distance=distance)
measured_time = time_array[-1] - time_array[0]
pulse = (len(peaks) / measured_time) * 60
# pulse = np.int16(pulse)
rospy.loginfo("[PulseHeadMovement] Pulse: " + str(pulse))
# uncomment the following lines to see the final singal with the detected peaks. For debugging.
# stepsize = 1. / sample_rate
# xs = np.arange(time_array[0], time_array[-1], stepsize)
# filename = "/home/studienarbeit/Dokumente/" + str(self.published_pulse_value_sequence) + "_step6_final_signal_"
# plt.figure(figsize=(6.5, 4))
# plt.plot(xs, signal, label="S")
# plt.plot(xs[peaks], signal[peaks], "x")
# plt.savefig(filename)
return pulse
def publish_pulse(self, pulse_value, publish_time):
"""
Publish the calculated pulse to ROS. Message is of type pulse() from pulse_chest_strap package.
:param pulse_value: the calculated pulse value
:param publish_time: the timestamp of the last incoming frame
"""
self.publisher.publish(pulse_value, publish_time)
self.published_pulse_value_sequence += 1
def show_image_with_mask(self, image, forehead_mask, bottom_mask):
"""
Helper function to check if ROI is selected correctly
"""
bottom_dst = cv2.bitwise_and(image, bottom_mask)
top_dst = cv2.bitwise_and(image, forehead_mask)
dst = cv2.bitwise_or(bottom_dst, top_dst)
cv2.imshow("Bottom", dst)
cv2.waitKey(3)
def __init__(self):
self.publisher = PulsePublisher("pulse_chest_strap")
self.start_time = rospy.Time.now()
class PulseChestStrap:
def __init__(self):
self.publisher = PulsePublisher("pulse_chest_strap")
self.start_time = rospy.Time.now()
def run(self, addr=None, gatttool="gatttool"):
"""
main routine to which orchestrates everything
"""
# number of measured pulse values. Increments for every measured value
seq = 0
if addr is None:
# A mac address has to be provided as command line argument
rospy.logerr(
"[PulseChestStrap] MAC address of polar H7 has not been provided"
)
return
hr_handle = None
hr_ctl_handle = None
retry = True
while retry:
while 1:
rospy.loginfo("[PulseChestStrap] Establishing connection to " +
addr)
gt = pexpect.spawn(gatttool + " -b " + addr + " -I")
gt.expect(r"\[LE\]>")
gt.sendline("connect")
try:
i = gt.expect(["Connection successful.", r"\[CON\]"],
timeout=30)
if i == 0:
gt.expect(r"\[LE\]>", timeout=30)
except pexpect.TIMEOUT:
rospy.loginfo(
"[PulseChestStrap] Connection timeout. Retrying.")
continue
except KeyboardInterrupt:
rospy.loginfo(
"[PulseChestStrap] Received keyboard interrupt. Quitting cleanly."
)
retry = False
break
break
if not retry:
break
rospy.loginfo("[PulseChestStrap] Connected to " + addr)
# We determine which handle we should read for getting the heart rate
# measurement characteristic.
gt.sendline("char-desc")
while 1:
try:
gt.expect(r"handle: (0x[0-9a-f]+), uuid: ([0-9a-f]{8})",
timeout=10)
except pexpect.TIMEOUT:
break
handle = gt.match.group(1)
uuid = gt.match.group(2)
if uuid == b"00002902" and hr_handle:
hr_ctl_handle = handle
break
elif uuid == b"00002a37":
hr_handle = handle
if hr_handle is None:
rospy.logerr(
"[PulseChestStrap] Couldn't find the heart rate measurement handle?!"
)
return
if hr_ctl_handle:
# We send the request to get HRM notifications
gt.sendline("char-write-req " + hr_ctl_handle.decode("utf-8") +
" 0100")
# Time period between two measures. This will be updated automatically.
period = 1.
last_measure = time.time() - period
hr_expect = "Notification handle = " + hr_handle.decode(
"utf-8") + " value: ([0-9a-f ]+)"
while 1:
try:
gt.expect(hr_expect, timeout=10)
except pexpect.TIMEOUT:
# If the timer expires, it means that we have lost the
# connection with the HR monitor
rospy.logwarn("[PulseChestStrap] Connection lost with " +
addr + ". Reconnecting.")
gt.sendline("quit")
try:
gt.wait()
except:
pass
time.sleep(1)
break
except KeyboardInterrupt:
rospy.loginfo(
"[PulseChestStrap] Received keyboard interrupt. Quitting cleanly."
)
retry = False
break
# We measure here the time between two measures. As the sensor
# sometimes sends a small burst, we have a simple low-pass filter
# to smooth the measure.
tmeasure = time.time()
period = period + 1 / 16. * (
(tmeasure - last_measure) - period)
last_measure = tmeasure
# Get data from gatttool
datahex = gt.match.group(1).strip()
data = map(lambda x: int(x, 16), datahex.split(b' '))
data = list(data)
res = self.interpret(data)
rospy.loginfo("[PulseChestStrap] Heart rate: " +
str(res["hr"]))
self.publisher.publish(res["hr"],
rospy.Time.now() - self.start_time)
seq += 1
# We quit close the BLE connection properly
gt.sendline("quit")
try:
gt.wait()
except:
pass
def interpret(self, data):
"""
data is a list of integers corresponding to readings from the BLE HR monitor
"""
byte0 = data[0]
res = {}
res["hrv_uint8"] = (byte0 & 1) == 0
sensor_contact = (byte0 >> 1) & 3
if sensor_contact == 2:
res["sensor_contact"] = "No contact detected"
elif sensor_contact == 3:
res["sensor_contact"] = "Contact detected"
else:
res["sensor_contact"] = "Sensor contact not supported"
res["ee_status"] = ((byte0 >> 3) & 1) == 1
res["rr_interval"] = ((byte0 >> 4) & 1) == 1
if res["hrv_uint8"]:
res["hr"] = data[1]
i = 2
else:
res["hr"] = (data[2] << 8) | data[1]
i = 3
if res["ee_status"]:
res["ee"] = (data[i + 1] << 8) | data[i]
i += 2
if res["rr_interval"]:
res["rr"] = []
while i < len(data):
# Note: Need to divide the value by 1024 to get in seconds
res["rr"].append((data[i + 1] << 8) | data[i])
i += 2
return res
class PulseMeasurement:
def __init__(self, show_processed_image):
self.show_processed_image = show_processed_image
self.count = 0
self.levels = 2
self.low = 0.9
self.high = 1.7
self.amplification = 30
self.publisher = PulsePublisher("eulerian_motion_magnification")
self.fps = 30
self.video_array = []
self.buffer_size = 0
self.time_array = []
self.calculating_at = 0
self.calculating_boarder = 50
self.recording_time = 10
self.isFirst = True
self.arrayLength = 0
def calculate_fps(self):
"""
calculate fps of incoming frames by calculating time-difference of the first timestamp (first image
in array) and last timestamp (last image in array)
"""
time_difference = self.time_array[-1] - self.time_array[0]
time_difference_in_seconds = time_difference.to_sec()
if time_difference_in_seconds == 0:
pass
self.fps = self.buffer_size / time_difference_in_seconds
rospy.loginfo("[EulerianMotionMagnification] Estimated FPS: " +
str(self.fps) + " (Measured timespan: " +
str(time_difference_in_seconds) + "s)")
rospy.loginfo("[EulerianMotionMagnification] Video array length: " +
str(len(self.video_array)))
# calculate pulse after certain amount of images taken, calculation based on a larger amount of time
def start_calulation(self, roi, timestamp):
# append timestamp to array
self.time_array.append(timestamp)
# normalize, resize and downsample image
normalized = cv2.normalize(roi.astype('float'), None, 0.0, 1.0,
cv2.NORM_MINMAX)
cropped = cv2.resize(normalized, (200, 200))
gaussian_frame = build_gaussian_frame(cropped, self.levels)
if self.show_processed_image:
self.video_array.append(gaussian_frame)
else:
red_values_images = extract_red_values(gaussian_frame,
self.show_processed_image)
self.video_array.append(red_values_images)
# check if recording images took longer than certain amount of time
time_difference = self.time_array[-1] - self.time_array[0]
time_difference_in_seconds = time_difference.to_sec()
if time_difference_in_seconds >= self.recording_time:
self.buffer_size = (len(self.time_array))
# determine how many pictures got buffered during time interval
# release first image and timestamp
self.video_array.pop(0)
self.time_array.pop(0)
self.calculating_at = self.calculating_at + 1
# calculate again after certain amount of images
if self.calculating_at >= self.calculating_boarder:
rospy.loginfo("[EulerianMotionMagnification] Length final " +
str(len(self.video_array)))
self.calculate_fps()
copy_video_array = np.copy(self.video_array)
copy_video_array = np.asarray(copy_video_array,
dtype=np.float32)
copy_video_array = temporal_bandpass_filter(
copy_video_array, self.low, self.high, self.fps)
if self.show_processed_image:
self.arrayLength = len(self.video_array)
processed_video = amplify_video(copy_video_array,
amplify=self.amplification)
show_images(processed_video, self.video_array,
self.arrayLength, self.isFirst, self.levels,
self.calculating_boarder, self.fps)
else:
copy_video_array = amplify_video(
copy_video_array, amplify=self.amplification)
pulse, red_values = calculate_pulse(copy_video_array,
self.recording_time,
self.show_processed_image)
self.publisher.publish(pulse, timestamp)
self.calculating_at = 0
self.isFirst = False
class BdfProcessor:
def __init__(self, bdf_file):
self.bdf_file = bdf_file
self.pulse_sequence = 0
self.signal = None
self.peaks = None
self.frequency = None
self.heart_rates = None
self.total_average_heart_rate = None
self.publisher = PulsePublisher("ecg")
def run(self):
self.signal, self.frequency = self.get_signal(name='EXG2')
self.total_average_heart_rate, self.peaks = self.estimate_average_heartrate(
self.signal, self.frequency)
rospy.loginfo("[BdfProcessor] Total average heart rate: " +
str(self.total_average_heart_rate))
self.heart_rates = self.calculate_heart_rates(self.peaks,
self.frequency)
# Uncomment to plot the ECG signal
# self.plot_signal(self.signal, self.frequency, 'EXG2')
# plt.tight_layout()
# plt.show()
def process_frame(self, frame_count, video_fps, timestamp):
"""
Synchronizes the processing of the video frames and the publishing of the ECG pulse values.
:param frame_count: The index of the current processed frame.
:param video_fps: The fps of the provided video
:param timestamp: The timestamp of the current processed frame.
"""
if self.pulse_sequence > len(self.heart_rates) - 1:
return
signal_position = frame_count / float(video_fps) * self.frequency
if signal_position >= self.heart_rates[self.pulse_sequence][1]:
self.publisher.publish(self.heart_rates[self.pulse_sequence][0],
timestamp)
self.pulse_sequence += 1
def get_signal(self, name='EXG2'):
"""
Extracts the ECG signal out of the BDF file.
:param name: The name of the channel to extract the signal from.
"""
reader = pyedflib.EdfReader(self.bdf_file)
# Get index of ECG channel
index = reader.getSignalLabels().index(name)
# Get sample frequency of ECG
frequency = reader.getSampleFrequency(index)
# Get index of status channel
status_index = reader.getSignalLabels().index('Status')
# Read status signal
status = reader.readSignal(status_index,
0).round().astype('int').nonzero()[0]
# Determine start of the video file in signal with status bits
video_start = status[0]
# Determine the end of the video file. It seems like the status bits are set to 0 before the videos finishing
# Therefore ignore the end of the signal and just cut the signal at the beginning.
# You can ignore if the signal is longer than the video, the remaining values will not be published.
# video_end = status[-1]
# video_length = video_end - video_start
# Read ECG signal and return as tuple with sample frequency
return reader.readSignal(index, video_start), frequency
def calculate_heart_rates(self, peaks, sampling_frequency):
"""
Calculates multiple heart rates out of the signal in equidistant time slots.
:param peaks: The detected peaks in the signal.
:param sampling_frequency: The sampling frequency of the signal.
:return: The calculated heart rates.
"""
rates = (sampling_frequency * 60) / np.diff(peaks)
# Remove instantaneous rates which are lower than 30, higher than 240
selector = (rates > 30) & (rates < 240)
rates = rates[selector]
heart_rates = []
hr = rates[:10]
heart_rates.append((hr.mean(), peaks[10]))
for index, rate in enumerate(rates[10:]):
hr[:-1] = hr[1:]
hr[-1] = rate
heart_rates.append((hr.mean(), peaks[index + 11]))
return heart_rates
def estimate_average_heartrate(self, signal, sampling_frequency):
"""
Calculates the total average heart rate of the signal.
:param peaks: The detected peaks in the signal.
:param sampling_frequency: The sampling frequency of the signal.
:return: The calculated average heart rate.
"""
peaks = qrs_detector(sampling_frequency, signal)
instantaneous_rates = (sampling_frequency * 60) / np.diff(peaks)
# remove instantaneous rates which are lower than 30, higher than 240
selector = (instantaneous_rates > 30) & (instantaneous_rates < 240)
return float(np.nan_to_num(
instantaneous_rates[selector].mean())), peaks
def plot_signal(self, signal, sampling_frequency, channel_name):
"""
Plots the ECG signal.
:param signal: The signal to plot.
:param sampling_frequency: The sampling frequency of the signal.
:param channel_name: The channel name of the signal in the BDF file.
"""
avg, peaks = self.estimate_average_heartrate(signal,
sampling_frequency)
ax = plt.gca()
ax.plot(np.arange(0,
len(signal) / sampling_frequency,
1 / sampling_frequency),
signal,
label='Raw signal')
xmin, xmax, ymin, ymax = plt.axis()
ax.vlines(peaks / sampling_frequency,
ymin,
ymax,
colors='r',
label='P-T QRS detector')
plt.xlim(0, len(signal) / sampling_frequency)
plt.ylabel('uV')
plt.xlabel('time (s)')
plt.title('Channel %s - Average heart-rate = %d bpm' %
(channel_name, avg))
ax.grid(True)
ax.legend(loc='best', fancybox=True, framealpha=0.5)