def calc_rotation_amount(path):
rotation = 0.0
for i in range(len(path) - 1):
tf_start = to_transform(path[i])
tf_end = to_transform(path[i + 1])
tf_delta = tf.concatenate_matrices(tf.inverse_matrix(tf_start), tf_end)
angle, _, _ = tf.rotation_from_matrix(tf_delta)
rotation = rotation + abs(angle)
return rotation
def rebase(path):
init_pose = to_transform(path[0])
init_pose_inv = tf.inverse_matrix(init_pose)
for data_point in path:
pose = to_transform(data_point)
new_pose = tf.concatenate_matrices(init_pose_inv, pose)
q = tf.quaternion_from_matrix(new_pose)
t = tf.translation_from_matrix(new_pose)
data_point[1], data_point[2], data_point[3] = t[0], t[1], t[2]
data_point[4], data_point[5], data_point[6], data_point[7] = q[0], q[
1], q[2], q[3]
return path
def calc_tf_vel(point1, point2):
dt = point2[0] - point1[0]
tf_start = to_transform(point1)
tf_end = to_transform(point2)
tf_delta = tf.concatenate_matrices(tf.inverse_matrix(tf_start), tf_end)
roll, pitch, yaw = tf.euler_from_matrix(tf_start)
droll, dpitch, dyaw = tf.euler_from_matrix(tf_delta)
translation_vec = tf.translation_from_matrix(tf_delta)
# translation = np.linalg.norm(translation_vec)
# angle, direc, point = tf.rotation_from_matrix(tf_delta)
return [
point1[1], point1[2], point1[3], roll, pitch, yaw,
translation_vec[0] / dt, translation_vec[1] / dt,
translation_vec[2] / dt, droll / dt, dpitch / dt, dyaw / dt
]
def calc_stats(path):
# path_length = calc_length(path)
# rotation_amount = calc_rotation_amount(path)
dt = path[-1][0] - path[0][0]
tf_start = to_transform(path[0])
tf_end = to_transform(path[-1])
tf_delta = tf.concatenate_matrices(tf.inverse_matrix(tf_start), tf_end)
roll, pitch, yaw = tf.euler_from_matrix(tf_delta)
translation_vec = tf.translation_from_matrix(tf_delta)
translation = np.linalg.norm(translation_vec)
angle, direc, point = tf.rotation_from_matrix(tf_delta)
return [
translation_vec[0] / dt, translation_vec[1] / dt,
translation_vec[2] / dt, roll / dt, pitch / dt, yaw / dt,
translation_vec[0], translation_vec[1], translation_vec[2], roll,
pitch, yaw, translation, angle
]
def convert(path, scale):
new_poses = []
for pose in path:
# after long discussion with Ricard Campos we found out
# that his rotation is coded as Yaw, Pitch, Roll in a rotating frame
roll, pitch, yaw = pose[4], pose[5], pose[6]
xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1)
Rx = tf.rotation_matrix(np.pi, xaxis)
R = tf.concatenate_matrices(Rx,
tf.euler_matrix(yaw, pitch, roll, 'rzyx'))
# q = tf.quaternion_from_euler(yaw, pitch, roll, 'rzyx')
q = tf.quaternion_from_matrix(R)
new_poses.append([
pose[0], scale * pose[1], scale * pose[2], scale * pose[3], q[0],
q[1], q[2], q[3]
])
return np.array(new_poses)
def to_transform(data_point):
t = [data_point[1], data_point[2], data_point[3]]
q = [data_point[4], data_point[5], data_point[6], data_point[7]]
rot_mat = tf.quaternion_matrix(q)
trans_mat = tf.translation_matrix(t)
return tf.concatenate_matrices(trans_mat, rot_mat)