Spaces:
Running
on
Zero
Running
on
Zero
File size: 3,497 Bytes
35e2073 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 |
#!/usr/bin/env python2
import os
import numpy as np
import utils.utils_poses.ATE.trajectory_utils as tu
import utils.utils_poses.ATE.transformations as tf
def compute_relative_error(p_es, q_es, p_gt, q_gt, T_cm, dist, max_dist_diff,
accum_distances=[],
scale=1.0):
if len(accum_distances) == 0:
accum_distances = tu.get_distance_from_start(p_gt)
comparisons = tu.compute_comparison_indices_length(
accum_distances, dist, max_dist_diff)
n_samples = len(comparisons)
print('number of samples = {0} '.format(n_samples))
if n_samples < 2:
print("Too few samples! Will not compute.")
return np.array([]), np.array([]), np.array([]), np.array([]), np.array([]),\
np.array([]), np.array([])
T_mc = np.linalg.inv(T_cm)
errors = []
for idx, c in enumerate(comparisons):
if not c == -1:
T_c1 = tu.get_rigid_body_trafo(q_es[idx, :], p_es[idx, :])
T_c2 = tu.get_rigid_body_trafo(q_es[c, :], p_es[c, :])
T_c1_c2 = np.dot(np.linalg.inv(T_c1), T_c2)
T_c1_c2[:3, 3] *= scale
T_m1 = tu.get_rigid_body_trafo(q_gt[idx, :], p_gt[idx, :])
T_m2 = tu.get_rigid_body_trafo(q_gt[c, :], p_gt[c, :])
T_m1_m2 = np.dot(np.linalg.inv(T_m1), T_m2)
T_m1_m2_in_c1 = np.dot(T_cm, np.dot(T_m1_m2, T_mc))
T_error_in_c2 = np.dot(np.linalg.inv(T_m1_m2_in_c1), T_c1_c2)
T_c2_rot = np.eye(4)
T_c2_rot[0:3, 0:3] = T_c2[0:3, 0:3]
T_error_in_w = np.dot(T_c2_rot, np.dot(
T_error_in_c2, np.linalg.inv(T_c2_rot)))
errors.append(T_error_in_w)
error_trans_norm = []
error_trans_perc = []
error_yaw = []
error_gravity = []
e_rot = []
e_rot_deg_per_m = []
for e in errors:
tn = np.linalg.norm(e[0:3, 3])
error_trans_norm.append(tn)
error_trans_perc.append(tn / dist * 100)
ypr_angles = tf.euler_from_matrix(e, 'rzyx')
e_rot.append(tu.compute_angle(e))
error_yaw.append(abs(ypr_angles[0])*180.0/np.pi)
error_gravity.append(
np.sqrt(ypr_angles[1]**2+ypr_angles[2]**2)*180.0/np.pi)
e_rot_deg_per_m.append(e_rot[-1] / dist)
return errors, np.array(error_trans_norm), np.array(error_trans_perc),\
np.array(error_yaw), np.array(error_gravity), np.array(e_rot),\
np.array(e_rot_deg_per_m)
def compute_absolute_error(p_es_aligned, q_es_aligned, p_gt, q_gt):
e_trans_vec = (p_gt-p_es_aligned)
e_trans = np.sqrt(np.sum(e_trans_vec**2, 1))
# orientation error
e_rot = np.zeros((len(e_trans,)))
e_ypr = np.zeros(np.shape(p_es_aligned))
for i in range(np.shape(p_es_aligned)[0]):
R_we = tf.matrix_from_quaternion(q_es_aligned[i, :])
R_wg = tf.matrix_from_quaternion(q_gt[i, :])
e_R = np.dot(R_we, np.linalg.inv(R_wg))
e_ypr[i, :] = tf.euler_from_matrix(e_R, 'rzyx')
e_rot[i] = np.rad2deg(np.linalg.norm(tf.logmap_so3(e_R[:3, :3])))
# scale drift
motion_gt = np.diff(p_gt, 0)
motion_es = np.diff(p_es_aligned, 0)
dist_gt = np.sqrt(np.sum(np.multiply(motion_gt, motion_gt), 1))
dist_es = np.sqrt(np.sum(np.multiply(motion_es, motion_es), 1))
e_scale_perc = np.abs((np.divide(dist_es, dist_gt)-1.0) * 100)
# ate = np.sqrt(np.mean(np.asarray(e_trans) ** 2))
return e_trans, e_trans_vec, e_rot, e_ypr, e_scale_perc
|