def orientations_quat_wxyz(self):
if not hasattr(self, "_orientations_quat_wxyz"):
assert hasattr(self, "_poses_se3")
self._orientations_quat_wxyz \
= np.array(
[tr.quaternion_from_matrix(p)
for p in self._poses_se3])
return self._orientations_quat_wxyz
def convert_abs_traj_to_rel_traj_lcd(df, lcd_df, to_scale=True):
"""Converts an absolute-pose trajectory to a relative-pose trajectory.
The incoming DataFrame df is processed element-wise. At each kf timestamp (which is the
index of the DataFrame row) starting from the second (index 1), the relative pose
from the match timestamp to the query stamp is calculated (in the match-
timestamp's coordinate frame). This relative pose is then appended to the
resulting DataFrame.
The resulting DataFrame has timestamp indices corresponding to poses that represent
the relative transformation between the match timestamp and the query one.
Args:
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.
lcd_df: A pandas.DataFrame object with timestamps as indices containing, at a minimum,
columns representing the timestamp of query frames and the timestamps of the
match frames.
to_scale: A boolean. If set to False, relative poses will have their translation
part normalized.
Returns:
A pandas.DataFrame object with xyz position and wxyz quaternion fields for the
relative pose trajectory corresponding to the absolute one given in 'df', and
relative by the given match and query timestamps.
"""
rows_list = []
index_list = []
for i in range(len(lcd_df.index)):
match_ts = lcd_df.timestamp_match[lcd_df.index[i]]
query_ts = lcd_df.timestamp_query[lcd_df.index[i]]
if match_ts == 0 and query_ts == 0:
continue
bi_T_bidelta = get_gt_rel_pose(df, match_ts, query_ts, to_scale)
if bi_T_bidelta is not None:
bi_R_bidelta = copy.deepcopy(bi_T_bidelta)
bi_R_bidelta[:, 3] = np.array([0, 0, 0, 1])
bi_q_bidelta = transformations.quaternion_from_matrix(bi_R_bidelta)
bi_t_bidelta = bi_T_bidelta[:3, 3]
new_row = {
"x": bi_t_bidelta[0],
"y": bi_t_bidelta[1],
"z": bi_t_bidelta[2],
"qw": bi_q_bidelta[0],
"qx": bi_q_bidelta[1],
"qy": bi_q_bidelta[2],
"qz": bi_q_bidelta[3],
}
rows_list.append(new_row)
index_list.append(lcd_df.index[i])
return pd.DataFrame(data=rows_list, index=index_list)
def se3_poses_to_xyz_quat_wxyz(poses):
xyz = np.array([pose[:3, 3] for pose in poses])
quat_wxyz = np.array([tr.quaternion_from_matrix(pose) for pose in poses])
return xyz, quat_wxyz
def se3_poses_to_xyz_quat_wxyz(
poses: typing.Sequence[np.ndarray]
) -> typing.Tuple[np.ndarray, np.ndarray]:
xyz = np.array([pose[:3, 3] for pose in poses])
quat_wxyz = np.array([tr.quaternion_from_matrix(pose) for pose in poses])
return xyz, quat_wxyz
def q_from_m(m):
A = tr.quaternion_from_matrix(m)
return A
def orientations_quat_wxyz(self):
if not hasattr(self, "_orientations_quat_wxyz"):
assert hasattr(self, "_poses_se3")
self._orientations_quat_wxyz \
= np.array([tr.quaternion_from_matrix(p) for p in self._poses_se3])
return self._orientations_quat_wxyz
def se3_poses_to_xyz_quat_wxyz(poses):
xyz = np.array([pose[:3, 3] for pose in poses])
quat_wxyz = np.array([tr.quaternion_from_matrix(pose) for pose in poses])
return xyz, quat_wxyz