Commit cdad5cd4 authored by Joris Wachsmuth's avatar Joris Wachsmuth
Browse files

Added Jitter Evaluation

parent e31e93cc
#!/usr/bin/env python
from __future__ import print_function
import numpy
import numpy as np
import rospy
import tf2_ros
from tf import transformations as tf
from tf2_ros import transform_broadcaster
from geometry_msgs.msg import Pose, TransformStamped, Vector3, Quaternion
from std_msgs.msg import ColorRGBA
from std_msgs.msg import ColorRGBA, Float64
from visualization_msgs.msg import Marker
from aruco_tracking.msg import ArucoMarkers
def inverse(T):
return numpy.linalg.inv(T)
return np.linalg.inv(T)
def interpolate(qp1, qp2, scale):
......@@ -40,7 +40,7 @@ def pose_to_T(msg: Pose):
q = msg.orientation
p = msg.position
T = tf.quaternion_matrix([q.x, q.y, q.z, q.w])
T[0:3, 3] = numpy.array([p.x, p.y, p.z])
T[0:3, 3] = np.array([p.x, p.y, p.z])
return T
......@@ -51,12 +51,12 @@ class BoardTracker(object):
self.name = name
self.markers = dict()
self.T = None
markers = rospy.get_param('{name}/markers'.format(name=name), dict())
markers = rospy.get_param(f'{name}/markers', dict())
# convert transform from position, axis*angle representation to (inverse) 4x4 matrix
for m in markers.keys():
pos = numpy.array(markers[m][:3])
axis = numpy.array(markers[m][3:])
angle = numpy.linalg.norm(axis)
pos = np.array(markers[m][:3])
axis = np.array(markers[m][3:])
angle = np.linalg.norm(axis)
if angle < 1e-6:
axis = [0, 0, 1]
T = tf.rotation_matrix(angle, axis)
......@@ -66,7 +66,7 @@ class BoardTracker(object):
def transforms(self, msg: ArucoMarkers):
"""Get (estimated) transforms of board within the frame msg.header.frame_id
from aruco marker transforms w.r.t. that frame, i.e. frame_id -> marker -> board"""
return numpy.asarray([pose_to_T(pose).dot(self.markers[id]) for id, pose in zip(msg.ids, msg.poses) if id in self.markers.keys()])
return np.asarray([pose_to_T(pose).dot(self.markers[id]) for id, pose in zip(msg.ids, msg.poses) if id in self.markers.keys()])
@classmethod
def _average(cls, Ts):
......@@ -104,9 +104,20 @@ class CameraTrackers(BoardTracker):
return T
def vector_angle(vec0, vec1):
inner = np.inner(vec0, vec1)
norms = np.linalg.norm(vec0) * np.linalg.norm(vec1)
return np.arccos(np.clip(inner/norms, -1., 1.))
class Tracking(object):
def __init__(self, marker_list, reference_frame='world'):
def __init__(self, marker_list, reference_frame='world', history_length=10):
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.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)
......@@ -123,7 +134,7 @@ class Tracking(object):
for board in self.boards:
try:
# estimate board pose in world from world -> camera -> marker -> board
Ts = numpy.matmul(T, board.transforms(msg))
Ts = np.matmul(T, board.transforms(msg))
# for debugging: marker frames
for id, pose in zip(msg.ids, msg.poses):
......@@ -143,7 +154,7 @@ class Tracking(object):
self.board_markers[board.name] = markers
# average over board estimates from all cameras and markers
M = BoardTracker._average(numpy.concatenate(list(markers.values())))
M = BoardTracker._average(np.concatenate(list(markers.values())))
self.publish(self.world.name, board.name, M, msg.header.stamp)
def publish(self, parent, child, T, stamp):
......@@ -151,14 +162,56 @@ class Tracking(object):
t.header.stamp = stamp
t.header.frame_id = parent
t.child_frame_id = child
t.transform.translation = Vector3(*T[0:3, 3])
t.transform.rotation = Quaternion(*tf.quaternion_from_matrix(T))
vec = T[0:3, 3]
t.transform.translation = Vector3(*vec)
quart = tf.quaternion_from_matrix(T)
t.transform.rotation = Quaternion(*quart)
self.tf_broadcaster.sendTransform(t)
# self.track_history.append((vec, quart))
jitter_pos_pub.publish(data=self.jitter_pos(vec))
# if len(self.track_history) > self.history_length:
# self.track_history.pop(0)
# jitter_data = self.jitter_magnitude()
# jitter_pos_pub.publish(data=jitter_data[0])
# jitter_angle_pub.publish(data=jitter_data[1])
# verbesserungen: nur neue elemente berechnen, momentan viele redundante berechnungen
def jitter_magnitude(self):
elem = self.track_history[0]
next_elem = self.track_history[1]
last_slope = next_elem[0] - elem[0]
last_pos_angle = 0
pos_angle_diffs = []
last_rot_angle = vector_angle(elem[1], next_elem[1])
rot_angle_diffs = []
elem = next_elem
for next_elem in self.track_history[2: len(self.track_history)]:
slope = next_elem[0] - elem[0]
pos_angle = vector_angle(last_slope, slope)
pos_angle_diffs.append(abs(last_pos_angle - pos_angle))
last_pos_angle = pos_angle
rot_angle = vector_angle(elem[1], next_elem[1])
rot_angle_diffs.append(abs(last_rot_angle - rot_angle))
last_slope = slope
elem = next_elem
return np.average(pos_angle_diffs), np.average(rot_angle_diffs)
def jitter_pos(self, new_pos):
slope = new_pos - self.last_pos
self.angle_history.append(vector_angle(slope, self.last_slope))
self.angle_history.pop(0)
self.last_pos = new_pos
self.last_slope = slope
return np.average(self.angle_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)
# publish cube marker once
marker_pub = rospy.Publisher('marker', Marker, queue_size=1, latch=True)
m = Marker(type=Marker.CUBE, scale=Vector3(0.04, 0.04, 0.04), color=ColorRGBA(1, 1, 1, 0.5))
......
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