Commit 11b75783 authored by Joris Wachsmuth's avatar Joris Wachsmuth
Browse files

Added Quaternion Variation

parent 51f1a30c
......@@ -129,6 +129,10 @@ class Tracking(object):
self.last_pos_hist_index = 0
self.history_sum = np.zeros((3,))
self.qu_history = np.tile(np.array([0., 0., 0., 1.]), (history_length, 1))
self.last_qu_hist_index = 0
self.last_qu_sum = np.array([0., 0., 0., 1.])
self.history_length = history_length
self.boards = [BoardTracker(name) for name in marker_list]
......@@ -185,6 +189,7 @@ 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))
qu_variation_pub.publish(data=self.qu_variation(quart))
# Distanzen Gewichten
def jitter_pos(self, new_pos):
......@@ -210,6 +215,16 @@ class Tracking(object):
self.last_pos_hist_index = (self.last_pos_hist_index + 1) % (self.history_length - 1)
return np.average(distance)
# maybe replace with true quaternion average
def qu_variation(self, new_qu):
self.last_qu_sum -= self.qu_history[self.last_qu_hist_index]
self.last_qu_sum += new_qu
self.qu_history[self.last_qu_hist_index] = new_qu
avg = np.average(self.qu_history)
angles = vector_angle(avg, self.qu_history)
self.last_qu_hist_index = (self.last_pos_hist_index + 1) % (self.history_length - 1)
return np.average(angles)
rospy.init_node('track_boards')
tracking = Tracking(['cube'])
......@@ -217,6 +232,7 @@ 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)
qu_variation_pub = rospy.Publisher('qu_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