Spaces:
Running
on
Zero
Running
on
Zero
File size: 1,261 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 |
#!/usr/bin/env python2
"""
@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.
"""
# an invitation to 3-d vision, p 27
return np.arccos(
min(1, max(-1, (np.trace(transform[0:3, 0:3]) - 1)/2)))*180.0/np.pi
|