diff --git a/people_tracking/src/people_tracking/colour_check.py b/people_tracking/src/people_tracking/colour_check.py
index 89fe222..fa8082c 100755
--- a/people_tracking/src/people_tracking/colour_check.py
+++ b/people_tracking/src/people_tracking/colour_check.py
@@ -5,14 +5,9 @@
from cv_bridge import CvBridge
# MSGS
-from sensor_msgs.msg import Image
-from people_tracking.msg import ColourCheckedTarget
from people_tracking.msg import DetectedPerson
from people_tracking.msg import ColourTarget
-from rospy.numpy_msg import numpy_msg
-
-
NODE_NAME = 'HoC'
TOPIC_PREFIX = '/hero/'
@@ -56,47 +51,6 @@ def get_vector(image, bins=32):
vector = np.concatenate(histograms, axis=0).reshape(-1) # Create colour histogram vector
return vector.tolist()
- @staticmethod
- def euclidean(a, b):
- """ Euclidean distance between two vectors. Closer to 0 means better match."""
- return np.linalg.norm(a - b)
-
- @staticmethod
- def cosine(a, b):
- """ Cosine distance between two vectors. Closer to 1 means a better match."""
- return np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b))
-
- def compare_hoc(self, detected_persons):
- """ Compare newly detected persons to previously detected target."""
- bridge = CvBridge()
- match = False
- idx_person = None
-
- person_vectors = [self.get_vector(bridge.imgmsg_to_cv2(person, desired_encoding='passthrough')) for person in
- detected_persons]
-
- if len(self.HoC_detections) < 1:
- self.HoC_detections.append(person_vectors[0])
- idx_person = 0
- match = True
- else:
- flag = False
- for Hoc_detection in self.HoC_detections:
- for idx_person, vector in enumerate(person_vectors):
- distance = self.euclidean(vector, Hoc_detection)
- if distance < 0.25:
- # rospy.loginfo(str(idx_person) + " " + str(distance))
- if len(self.HoC_detections) < 5:
- self.HoC_detections.append(vector)
- else:
- self.HoC_detections = self.HoC_detections[1:] + [vector]
- flag = True
- match = True
- break
- if flag:
- break
- return match, idx_person
-
def callback(self, data):
time = data.time
nr_batch = data.nr_batch
@@ -106,18 +60,13 @@ def callback(self, data):
y_positions = data.y_positions
z_positions = data.z_positions
- match = False
- idx_match = None
-
if nr_batch <= self.last_batch_processed:
return
if nr_persons < 1:
return
bridge = CvBridge()
- # match, idx_match = self.compare_hoc(detected_persons)
colour_vectors = [self.get_vector(bridge.imgmsg_to_cv2(person, desired_encoding='passthrough')) for person in
detected_persons]
- # if match:
msg = ColourTarget()
msg.time = time
@@ -127,15 +76,10 @@ def callback(self, data):
msg.y_positions = y_positions
msg.z_positions = z_positions
msg.colour_vectors = [item for sublist in colour_vectors for item in sublist]
- # msg.detected_person = detected_persons[idx_match]
self.publisher.publish(msg)
self.last_batch_processed = nr_batch
- # if nr_persons > 0 and match:
- # self.publisher_debug.publish(detected_persons[idx_match])
-
-
if __name__ == '__main__':
try:
node_hoc = HOC()
diff --git a/people_tracking/src/people_tracking/people_tracker.py b/people_tracking/src/people_tracking/people_tracker.py
index ebf4cd5..cf3a1bc 100755
--- a/people_tracking/src/people_tracking/people_tracker.py
+++ b/people_tracking/src/people_tracking/people_tracker.py
@@ -17,7 +17,6 @@
from sensor_msgs.msg import Image
from people_tracking.msg import ColourCheckedTarget, ColourTarget, DetectedPerson
-
# SrvS
from std_srvs.srv import Empty
@@ -29,7 +28,6 @@
save_data = False if sys.argv[3] == "False" else True
-
Persons = namedtuple("Persons",
["nr_batch", "time", "nr_persons", "x_positions", "y_positions", "z_positions", "colour_vectors",
"face_detected"])
@@ -65,7 +63,8 @@ def __init__(self) -> None:
self.detection_reset_proxy = rospy.ServiceProxy(TOPIC_PREFIX + 'person_detection/reset', Empty)
# Variables
- self.approved_targets = [Target(0, 0, 0, 320, 240, 0, None, False), Target(1, 1, 0, 320, 240, 0, None, False), Target(2, 2, 0, 320, 240, 0, None, False)]
+ self.approved_targets = [Target(0, 0, 0, 320, 240, 0, None, False), Target(1, 1, 0, 320, 240, 0, None, False),
+ Target(2, 2, 0, 320, 240, 0, None, False)]
self.approved_targets_hoc = [] # HoC's from approved target (only keep last 10).
self.time_since_identifiers = None
@@ -91,7 +90,6 @@ def __init__(self) -> None:
self.message_count_da = 0
self.rate_estimate_da = 0.0
-
def reset(self):
""" Reset all stored variables in Class to their default values."""
self.approved_targets = [Target(0, 0, 0, 320, 240, 0)]
@@ -150,11 +148,10 @@ def callback_hoc(self, data: ColourCheckedTarget) -> None:
self.new_detections = True
if save_data:
self.csv_writer.writerow([nr_batch, time, nr_persons,
- x_positions, y_positions, z_positions,
- colour_vectors, face_detected])
+ x_positions, y_positions, z_positions,
+ colour_vectors, face_detected])
# rospy.loginfo(f"hoc: {nr_batch}")
-
current_timestamp = rospy.get_time()
if self.last_timestamp_hoc is not None:
time_difference = current_timestamp - self.last_timestamp_hoc
@@ -227,7 +224,8 @@ def callback_persons(self, data: DetectedPerson) -> None:
self.update_target(nr_batch)
# rospy.loginfo([person.nr_batch for person in self.detections])
-###THRESHOLD FILE = SAME ####
+
+ ###THRESHOLD FILE = SAME ####
@staticmethod
def element_exists(lst, element):
@@ -242,6 +240,7 @@ def element_exists(lst, element):
return True, idx
except ValueError: # If element is not in the list
return False, None
+
@staticmethod
def euclidean_distance(point1, point2):
""" Calculate the Euclidean distance between two points.
@@ -284,7 +283,7 @@ def get_distance_hocs(self, hoc_check, hocs_existing):
s = [self.euclidean_distance(hoc_check[32:64], hoc[32:64]) for hoc in hocs_existing]
# v = [self.euclidean_distance(hoc_check[64:], hoc[64:]) for hoc in hocs_existing]
- hsv = zip(h, s)#, v]
+ hsv = zip(h, s) # , v]
distances = [0.8 * h + 0.2 * s for h, s in hsv]
return min(distances)
@@ -302,17 +301,20 @@ def check_hoc_data(self, detection, tracked_hocs):
if any(x is not None for x in detection.colour_vectors): # There is HoC data
flag_hoc = True
- distance_hoc = [self.get_distance_hocs(detection.colour_vectors[person_idx], tracked_hocs) for person_idx in range(detection.nr_persons)]
+ distance_hoc = [self.get_distance_hocs(detection.colour_vectors[person_idx], tracked_hocs) for person_idx in
+ range(detection.nr_persons)]
if any([value < HOC_THRESHOLD for value in distance_hoc]): # Check if any of the data meets the threshold
# Normalize data
- max_distance_hoc = max([distance for distance in distance_hoc if distance < HOC_THRESHOLD]) # get max distance without invalid entries
+ max_distance_hoc = max([distance for distance in distance_hoc if
+ distance < HOC_THRESHOLD]) # get max distance without invalid entries
if 0 == max_distance_hoc or len(distance_hoc) <= 1:
norm_hoc = [0 for _ in distance_hoc]
else:
- norm_hoc = [distance / max_distance_hoc if distance < HOC_THRESHOLD else 2 for distance in distance_hoc]
+ norm_hoc = [distance / max_distance_hoc if distance < HOC_THRESHOLD else 2 for distance in
+ distance_hoc]
- else: # All values are invalid, thus max normalized distance
+ else: # All values are invalid, thus max normalized distance
norm_hoc = [2] * detection.nr_persons
else: # There is no HoC data
@@ -402,14 +404,15 @@ def distance_positions(p1, p2, y_weight):
:param y_weight: how important is the y value [0-1]
:return: distance
"""
- return np.sqrt((p2[0] - p1[0]) ** 2 + y_weight*(p2[1] - p1[1]) ** 2 + (p2[2] - p1[2]) ** 2)
+ return np.sqrt((p2[0] - p1[0]) ** 2 + y_weight * (p2[1] - p1[1]) ** 2 + (p2[2] - p1[2]) ** 2)
+
def check_da_data(self, new_detection, previous_da_detection):
"""
:param new_detection: detection to calculate the distance per measurement
:param previous_da_detection: measurement to calculate the distance from (aka the previous known target location).
:return:
"""
- DA_THRESHOLD = 50/0.08 # per unit time
+ DA_THRESHOLD = 50 / 0.08 # per unit time
flag_da = True
@@ -419,20 +422,23 @@ def check_da_data(self, new_detection, previous_da_detection):
else:
predicted_location = (0, 0, 0)
- coords_detections = [(new_detection.x_positions[person_idx], new_detection.y_positions[person_idx], new_detection.z_positions[person_idx]) for person_idx in range(new_detection.nr_persons)]
+ coords_detections = [(new_detection.x_positions[person_idx], new_detection.y_positions[person_idx],
+ new_detection.z_positions[person_idx]) for person_idx in range(new_detection.nr_persons)]
distance_da = [self.distance_positions(predicted_location, detection, 0.5) for detection in coords_detections]
delta_t = new_detection.time - previous_da_detection[-1].time
if any([value < DA_THRESHOLD * delta_t for value in distance_da]):
# Normalize data
- max_distance = max([distance for distance in distance_da if distance < DA_THRESHOLD * delta_t]) # get max distance without invalid entries
+ max_distance = max([distance for distance in distance_da if
+ distance < DA_THRESHOLD * delta_t]) # get max distance without invalid entries
if 0 == max_distance or len(distance_da) <= 1:
norm_da = [0 for _ in distance_da]
else:
- norm_da = [distance / max_distance if distance < DA_THRESHOLD * delta_t else 2 for distance in distance_da]
+ norm_da = [distance / max_distance if distance < DA_THRESHOLD * delta_t else 2 for distance in
+ distance_da]
- else: # All data is invalid
+ else: # All data is invalid
norm_da = [2] * new_detection.nr_persons
return flag_da, norm_da
@@ -461,7 +467,8 @@ def get_target_value(self, new_detection, tracked_hocs, previous_da_detections,
for person in range(new_detection.nr_persons)]
idx_target = combined.index(min(combined))
- valid = True if min(combined) <= 1 else False # combined is larger than 1 if either to many of the targets have invalid measurements or the most important one is invalid.
+ valid = True if min(
+ combined) <= 1 else False # combined is larger than 1 if either to many of the targets have invalid measurements or the most important one is invalid.
# self.target_get_values.append([new_detection.nr_batch, flag_face, faces[idx_target], min(faces), flag_hoc, norm_hoc[idx_target], min(norm_hoc), flag_da, norm_da[idx_target], min(norm_da)])
@@ -473,7 +480,6 @@ def add_approved_target(self, measurement, idx_target, valid):
nr_batch = measurement.nr_batch
time = measurement.time
-
if idx_target is None:
x = self.approved_targets[-1].x
y = self.approved_targets[-1].y
@@ -548,21 +554,23 @@ def get_tracked_hocs(self, idx_tracked=None):
def update_target(self, from_batch):
""" Update the self.approved_targets from batch."""
try:
- exists_detection, idx_detection = self.element_exists([detection.nr_batch for detection in self.detections], from_batch)
+ exists_detection, idx_detection = self.element_exists([detection.nr_batch for detection in self.detections],
+ from_batch)
if not exists_detection: # Make sure existing batch number in detections
return
- exist_tracked, idx_tracked = self.element_exists([detection.nr_batch for detection in self.approved_targets], from_batch)
+ exist_tracked, idx_tracked = self.element_exists(
+ [detection.nr_batch for detection in self.approved_targets], from_batch)
- if exist_tracked: # Check new data with existing track.
- idx_compare = idx_tracked-1
+ if exist_tracked: # Check new data with existing track.
+ idx_compare = idx_tracked - 1
while not self.approved_targets[idx_compare].valid_measurement:
idx_compare -= 1
tracked_hocs = self.get_tracked_hocs(idx_tracked)
# print(f"tracked hocs exists: {tracked_hocs}")
idx_target, valid = self.get_target_value(self.detections[idx_detection], tracked_hocs,
- self.approved_targets[idx_compare-2: idx_compare],
+ self.approved_targets[idx_compare - 2: idx_compare],
self.approved_targets[idx_compare].valid_measurement)
if self.approved_targets[idx_tracked].idx_person == idx_target:
@@ -574,7 +582,7 @@ def update_target(self, from_batch):
self.approved_targets = self.approved_targets[:idx_compare]
- while idx_detection < len(self.detections)-1:
+ while idx_detection < len(self.detections) - 1:
tracked_hocs = self.get_tracked_hocs()
# print(f"tracked hocs new: {tracked_hocs}")
@@ -587,20 +595,21 @@ def update_target(self, from_batch):
return
-
-
- if self.approved_targets[-1].nr_batch < from_batch: # Add single data association step to the end of target list
+ if self.approved_targets[
+ -1].nr_batch < from_batch: # Add single data association step to the end of target list
# Get 5 previous hoc measurements from track
tracked_hocs = self.get_tracked_hocs()
# print(f"tracked hocs new: {tracked_hocs}")
- idx_target, valid = self.get_target_value(self.detections[idx_detection], tracked_hocs, self.approved_targets[-3:-1], self.approved_targets[-1].valid_measurement)
+ idx_target, valid = self.get_target_value(self.detections[idx_detection], tracked_hocs,
+ self.approved_targets[-3:-1],
+ self.approved_targets[-1].valid_measurement)
self.add_approved_target(self.detections[idx_detection], idx_target, valid)
return
- print("TUMMMMMM")
+ print("Case Not Determined")
# else: # totaly new data that can be placed somewhere between already associated data, find place, do associaiton 1 back to this, this to next, if match in next return else redo everything -> For now do nothing with it
except:
print("error updating")
@@ -615,15 +624,15 @@ def get_weights(flag_face, flag_hoc, flag_da, valid):
if sum([flag_face, flag_hoc, flag_da]) <= 0:
return 0.0, 0.0, 0.0
- if not valid: # Use different weights if target was lost in previous steps
+ if not valid: # Use different weights if target was lost in previous steps
weight_da = 0.0
nr_parameters = sum([flag_face, flag_hoc])
- weights = [[1.0, 1.0], # 1 parameter
- [0.9, 0.1]] # 2 parameters
+ weights = [[1.0, 1.0], # 1 parameter
+ [0.9, 0.1]] # 2 parameters
if flag_face:
- weight_face = weights[nr_parameters-1][0]
+ weight_face = weights[nr_parameters - 1][0]
else:
weight_face = 0.0
@@ -635,9 +644,9 @@ def get_weights(flag_face, flag_hoc, flag_da, valid):
else:
nr_parameters = sum([flag_face, flag_hoc, flag_da]) # How many measurement types available
current_weight = 2
- weights = [[0.0, 0.0, 1.0], # 1 parameter
- [0.0, 0.2, 0.8], # 2 parameters
- [0.1, 0.2, 0.7]] # 3 parameters
+ weights = [[0.0, 0.0, 1.0], # 1 parameter
+ [0.0, 0.2, 0.8], # 2 parameters
+ [0.1, 0.2, 0.7]] # 3 parameters
if flag_face:
weight_face = weights[nr_parameters - 1][current_weight]
@@ -657,6 +666,7 @@ def get_weights(flag_face, flag_hoc, flag_da, valid):
weight_hoc = 0.0
return weight_face, weight_hoc, weight_da
+
##########################################
def plot_tracker(self):
""" Plot the trackers on a camera frame and publish it.
@@ -668,7 +678,7 @@ def plot_tracker(self):
cv_image = bridge.imgmsg_to_cv2(latest_image, desired_encoding='passthrough')
# if not self.target_lost:
- if len(self.approved_targets) > 0 and self.tracked_plottable: # Plot latest approved measurement
+ if len(self.approved_targets) > 0 and self.tracked_plottable and not self.target_lost: # Plot latest approved measurement
x_approved = self.approved_targets[-1].x
y_approved = self.approved_targets[-1].y
cv2.circle(cv_image, (x_approved, y_approved), 5, (0, 0, 255, 50), -1) # BGR
@@ -686,7 +696,6 @@ def plot_tracker(self):
tracker_image = bridge.cv2_to_imgmsg(cv_image, encoding="passthrough")
self.publisher_debug.publish(tracker_image)
-
def loop(self):
""" Loop that repeats itself at self.rate. Can be used to execute methods at given rate. """
time_old = rospy.get_time()
@@ -706,7 +715,7 @@ def loop(self):
if len(self.approved_targets) > 0:
val_idx = 1
validity = False
- while not validity and val_idx < 15 and val_idx < len(self.approved_targets)-1:
+ while not validity and val_idx < 15 and val_idx < len(self.approved_targets) - 1:
validity = self.approved_targets[-val_idx].valid_measurement
val_idx += 1
@@ -717,10 +726,10 @@ def loop(self):
self.target_lost = False
if current_time - time_old > 0.1:
- rospy.loginfo( f"da: {self.rate_estimate_da:.2f} Hz, face: {self.rate_estimate_face:.2f} Hz, hoc: {self.rate_estimate_hoc:.2f} Hz")
+ rospy.loginfo(
+ f"da: {self.rate_estimate_da:.2f} Hz, face: {self.rate_estimate_face:.2f} Hz, hoc: {self.rate_estimate_hoc:.2f} Hz")
time_old = current_time
-
self.move_robot()
self.rate.sleep()
@@ -733,7 +742,6 @@ def remove_outside_batches(lst: List, start_batch: int = 0, end_batch: int = flo
result = [entry for entry in lst if start_batch <= entry.nr_batch <= end_batch]
return result
-
def move_robot(self):
""" How to move the robots head.
Convention:
@@ -746,7 +754,7 @@ def move_robot(self):
x = left/right
y = height (up = +)
"""
- if len(self.approved_targets) > 0 and self.tracked_plottable: # Plot latest approved measurement
+ if len(self.approved_targets) > 0 and self.tracked_plottable and not self.target_lost: # Plot latest approved measurement
x_approved = self.approved_targets[-1].x
y_approved = self.approved_targets[-1].y
z_approved = self.approved_targets[-1].z
diff --git a/people_tracking/test/webcam.py b/people_tracking/src/people_tracking/webcam.py
similarity index 100%
rename from people_tracking/test/webcam.py
rename to people_tracking/src/people_tracking/webcam.py
diff --git a/people_tracking/src/people_tracking/depth_image.py b/people_tracking/test/experiments/depth_image.py
similarity index 100%
rename from people_tracking/src/people_tracking/depth_image.py
rename to people_tracking/test/experiments/depth_image.py
diff --git a/people_tracking/src/people_tracking/listener.py b/people_tracking/test/experiments/listener.py
similarity index 100%
rename from people_tracking/src/people_tracking/listener.py
rename to people_tracking/test/experiments/listener.py
diff --git a/people_tracking/src/people_tracker.cpp b/people_tracking/test/experiments/people_tracker.cpp
similarity index 100%
rename from people_tracking/src/people_tracker.cpp
rename to people_tracking/test/experiments/people_tracker.cpp
diff --git a/people_tracking/src/people_tracking/skelly.py b/people_tracking/test/experiments/skelly.py
similarity index 100%
rename from people_tracking/src/people_tracking/skelly.py
rename to people_tracking/test/experiments/skelly.py
diff --git a/people_tracking/test/test_script+webcam.launch b/people_tracking/test/experiments/test_script+webcam.launch
similarity index 100%
rename from people_tracking/test/test_script+webcam.launch
rename to people_tracking/test/experiments/test_script+webcam.launch
diff --git a/people_tracking/src/people_tracking/yolo.py b/people_tracking/test/experiments/yolo.py
similarity index 100%
rename from people_tracking/src/people_tracking/yolo.py
rename to people_tracking/test/experiments/yolo.py
diff --git a/people_tracking/test/people_tracking.launch b/people_tracking/test/people_tracking.launch
index 7ac48f7..07f68d3 100644
--- a/people_tracking/test/people_tracking.launch
+++ b/people_tracking/test/people_tracking.launch
@@ -7,13 +7,13 @@
-
+
+
+
+
+
+
+