|
|
|
""" |
|
@author: Christian Forster |
|
""" |
|
|
|
import os |
|
import numpy as np |
|
import utils.utils_poses.ATE.transformations as tf |
|
|
|
|
|
def get_rigid_body_trafo(quat, trans): |
|
T = tf.quaternion_matrix(quat) |
|
T[0:3, 3] = trans |
|
return T |
|
|
|
|
|
def get_distance_from_start(gt_translation): |
|
distances = np.diff(gt_translation[:, 0:3], axis=0) |
|
distances = np.sqrt(np.sum(np.multiply(distances, distances), 1)) |
|
distances = np.cumsum(distances) |
|
distances = np.concatenate(([0], distances)) |
|
return distances |
|
|
|
|
|
def compute_comparison_indices_length(distances, dist, max_dist_diff): |
|
max_idx = len(distances) |
|
comparisons = [] |
|
for idx, d in enumerate(distances): |
|
best_idx = -1 |
|
error = max_dist_diff |
|
for i in range(idx, max_idx): |
|
if np.abs(distances[i]-(d+dist)) < error: |
|
best_idx = i |
|
error = np.abs(distances[i] - (d+dist)) |
|
if best_idx != -1: |
|
comparisons.append(best_idx) |
|
return comparisons |
|
|
|
|
|
def compute_angle(transform): |
|
""" |
|
Compute the rotation angle from a 4x4 homogeneous matrix. |
|
""" |
|
|
|
return np.arccos( |
|
min(1, max(-1, (np.trace(transform[0:3, 0:3]) - 1)/2)))*180.0/np.pi |
|
|
