def load_transform_json(json_path):
"""
load a transformation stored in xyz + quaternion format in a .json file
:param json_path: path to the .json file
:return: t (SE(3) matrix), xyz (position), quat (orientation quaternion)
"""
import evo.core.lie_algebra as lie
import evo.core.transformations as tr
with open(json_path, 'r') as tf_file:
data = json.load(tf_file)
keys = ("x", "y", "z", "qx", "qy", "qz", "qw")
if not all(key in data for key in keys):
raise FileInterfaceException(
"invalid transform file - expected keys " + str(keys))
xyz = np.array([data["x"], data["y"], data["z"]])
quat = np.array([data["qw"], data["qx"], data["qy"], data["qz"]])
t = lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz)
return t, xyz, quat
def __init__(self, p_file, cfg, imf=None, mx=None):
if p_file is not None:
with open(p_file, 'rb') as fp:
d = pickle.load(fp, encoding='latin1')
self.images = d[0]
self.matches = d[1]
self.poses = d[2]
print("Total data {} {} from {}".format(
len(self.images), len(self.matches), p_file))
nt = 0
for m in self.matches:
nt += len(self.matches[m])
self.nt = nt
else:
with open(os.path.join(HOME, imf), 'rb') as fp:
A = pickle.load(fp)
self.data = A[0]
# self.data = pickle.load(fp)
self.id_list = A[1]
self.cfg = cfg
# nt = 0
# for ids in self.matches:
# for mm in self.matches[ids]:
# if ids[0]==1 and mm[3][0]<100:
# print(ids, mm)
# if ids[1]==1 and mm[3][1]<100:
# print(ids, mm)
for id in self.images:
img = self.images[id]
# pose = self.poses[id]
# Q = etr.quaternion_matrix(img[0])
self.poses[id].Q4 = etr.quaternion_matrix(img[0])
self.poses[id].Q4[:3, 3] = img[1]
self.poses[id].Q4 = np.linalg.inv(self.poses[id].Q4)
# print(self.poses[id].Q4)
self.poses[id].add_inv()
self.out = None
def xyz_quat_wxyz_to_se3_poses(xyz, quat):
poses = [
lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz)
for quat, xyz in zip(quat, xyz)
]
return poses
def q_to_m(q):
A = tr.quaternion_matrix(q)
return A
def xyz_quat_wxyz_to_se3_poses(xyz, quat):
poses = [lie.se3(lie.so3_from_se3(tr.quaternion_matrix(quat)), xyz)
for quat, xyz in zip(quat, xyz)]
return poses
def get_gt_rel_pose(gt_df, match_ts, query_ts, to_scale=True):
"""Returns the relative pose from match to query for given timestamps.
Args:
gt_df: A pandas.DataFrame object with timestamps as indices containing, at a minimum,
columns representing the xyz position and wxyz quaternion-rotation at each
timestamp, corresponding to the absolute pose at that time.
match_ts: An integer representing the match frame timestamp.
query_ts: An integer representing the query frame timestamp.
to_scale: A boolean. If set to False, relative poses will have their translation
part normalized.
Returns:
A 4x4 numpy array representing the relative pose from match to query frame.
"""
w_T_bmatch = None
w_T_bquery = None
try:
closest_ts = closest_num(gt_df.index, match_ts)
if closest_ts != match_ts:
# print("using closest match for timestamps")
pass
w_t_bmatch = np.array(
[gt_df.at[closest_ts, idx] for idx in ["x", "y", "z"]])
w_q_bmatch = np.array(
[gt_df.at[closest_ts, idx] for idx in ["qw", "qx", "qy", "qz"]])
w_T_bmatch = transformations.quaternion_matrix(w_q_bmatch)
w_T_bmatch[:3, 3] = w_t_bmatch
except:
print(
"Failed to convert an abs pose to a rel pose. Timestamp ",
match_ts,
" is not available in ground truth df.",
)
return None
try:
closest_ts = closest_num(gt_df.index, query_ts)
if closest_ts != query_ts:
# print("using closest match for timestamps")
pass
w_t_bquery = np.array(
[gt_df.at[closest_ts, idx] for idx in ["x", "y", "z"]])
w_q_bquery = np.array(
[gt_df.at[closest_ts, idx] for idx in ["qw", "qx", "qy", "qz"]])
w_T_bquery = transformations.quaternion_matrix(w_q_bquery)
w_T_bquery[:3, 3] = w_t_bquery
except:
print(
"Failed to convert an abs pose to a rel pose. Timestamp ",
query_ts,
" is not available in ground truth df.",
)
return None
bmatch_T_bquery = lie.relative_se3(w_T_bmatch, w_T_bquery)
bmatch_t_bquery = bmatch_T_bquery[:3, 3]
if not to_scale:
norm = np.linalg.norm(bmatch_t_bquery)
if norm > 1e-6:
bmatch_t_bquery = bmatch_t_bquery / np.linalg.norm(bmatch_t_bquery)
bmatch_T_bquery[:3, 3] = bmatch_t_bquery
return bmatch_T_bquery
def Matrix4(self):
data = np.array([self.w, self.x, self.y, self.z])
return quaternion_matrix(data)
