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 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 BernardoTeixeira commented Mar 28, 2019

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

@dexter2406

This comment has been minimized.

Copy link

@dexter2406 dexter2406 commented Aug 26, 2020

So if I also want rotation angles, I just need to extract more from poses_stacked right? e.g. rx=poses_stacked[:,3,3]; ry=poses_stacked[:,4:3] and so on?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment