Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
SfMLearner - merging consecutive poses sequences into single trajectory
import numpy as np
from kitti_eval.pose_evaluation_utils import pose_vec_to_mat
def convert_and_change_coordinate_system(poses, new_coord_index=0):
coord_pose = pose_vec_to_mat(poses[new_coord_index])
out = []
for pose_vec in poses:
pose = pose_vec_to_mat(pose_vec)
pose = np.dot(coord_pose, np.linalg.inv(pose))
out.append(pose)
return out
# ps - sequence of N poses (predicted pose vector from network output, e.g. N=5)
# ps_arr - array of poses sequences with a single overlapping pose (last element)
def merge_sequences_poses(ps_arr):
ps_arr = [convert_and_change_coordinate_system(ps) for ps in ps_arr]
poses_global = []
ps_prev_last = None
for ps in ps_arr:
if ps_prev_last is None: # first group - do nothing
ps_ = ps
else: # use overlapping pose to translate current ps to global coordinate system
ps_ = []
for p in ps:
p_ = np.dot(ps_prev_last, p)
ps_.append(p_)
ps_prev_last = ps_[-1]
# skip the last overlapping pose
for pose_global in ps_[:-1]:
poses_global.append(pose_global)
# get interesting values
poses_stacked = np.stack(poses_global)
txs = poses_stacked[:, 0, 3]
tys = poses_stacked[:, 1, 3]
tzs = poses_stacked[:, 2, 3]
return txs, tys, tzs # example - outputing just the position (x,y,z)
@andjsmile

This comment has been minimized.

Copy link

andjsmile commented Apr 3, 2018

Thank you your code,finally can you predict the poses sequences into single trajectory? If you can , can you share your code after modifying to me ?I just for research,please.

@BernardoTeixeira

This comment has been minimized.

Copy link

BernardoTeixeira commented Mar 28, 2019

hi, where exactly in the SfM code do you apply this transformation? Thank you in advance

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.