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)