Commit 60ca69fa authored by Joris Wachsmuth's avatar Joris Wachsmuth
Browse files

Added Position Variation metric

parent c9f5742a
......@@ -8,7 +8,7 @@
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)" />
<!-- RQT-Graph -->
<node name="rqt_plot" pkg="rqt_plot" type="rqt_plot" args="/aruco_tracking/jitter_pos /aruco_tracking/jitter_angle"/>
<node name="rqt_plot" pkg="rqt_plot" type="rqt_plot" args="/aruco_tracking/jitter_pos /aruco_tracking/jitter_angle /aruco_tracking/pos_variation"/>
<group ns="aruco_tracking">
<arg name="manager_name" value="aruco_manager" />
......
......@@ -107,18 +107,28 @@ class CameraTrackers(BoardTracker):
def vector_angle(vec0, vec1):
inner = np.inner(vec0, vec1)
norms = np.linalg.norm(vec0) * np.linalg.norm(vec1)
if norms == 0:
return 0
return np.arccos(np.clip(inner/norms, -1., 1.))
class Tracking(object):
def __init__(self, marker_list, reference_frame='world', history_length=10):
def __init__(self, marker_list, reference_frame='world', history_length=50):
self.world = CameraTrackers(reference_frame)
self.track_history = []
self.angle_history = [0] * history_length
self.last_slope = np.array([1, 1, 1])
self.last_pos = np.array([1, 1, 1])
self.angle_qu_history = [0] * history_length
self.last_angle = np.array([0, 0, 0, 1])
self.pos_history = [np.zeros((3,))] * history_length
self.history_sum = np.zeros((3,))
self.radius_history = [0] * history_length
self.history_length = history_length
self.boards = [BoardTracker(name) for name in marker_list]
self.board_markers = dict() # store marker transforms received from individual cameras
self.marker_sub = rospy.Subscriber('markers', ArucoMarkers, self.marker_cb)
......@@ -172,7 +182,9 @@ class Tracking(object):
jitter_pos_pub.publish(data=self.jitter_pos(vec))
jitter_angle_pub.publish(data=self.jitter_angle(quart))
pos_variation_pub.publish(data=self.pos_variation(vec))
# Distanzen Gewichten
def jitter_pos(self, new_pos):
slope = new_pos - self.last_pos
self.angle_history.append(vector_angle(slope, self.last_slope))
......@@ -187,12 +199,23 @@ class Tracking(object):
self.last_angle = new_angle
return np.average(self.angle_qu_history)
def pos_variation(self, new_pos):
self.history_sum -= self.pos_history.pop(0)
self.history_sum += new_pos
self.pos_history.append(new_pos)
delta_pos = new_pos - self.history_sum / self.history_length
distance = np.linalg.norm(delta_pos)
self.radius_history.append(distance)
self.radius_history.pop(0)
return np.average(self.radius_history)
rospy.init_node('track_boards')
tracking = Tracking(['cube'])
jitter_pos_pub = rospy.Publisher('jitter_pos', Float64, queue_size=1, latch=True)
jitter_angle_pub = rospy.Publisher('jitter_angle', Float64, queue_size=1, latch=True)
pos_variation_pub = rospy.Publisher('pos_variation', Float64, queue_size=1, latch=True)
# publish cube marker once
marker_pub = rospy.Publisher('marker', Marker, queue_size=1, latch=True)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment