def read_euroc_csv_trajectory(file_path):
"""
parses ground truth trajectory from EuRoC MAV state estimate .csv
:param file_path: <sequence>/mav0/state_groundtruth_estimate0/data.csv
:return: trajectory.PoseTrajectory3D object
"""
mat = np.array(csv_read_matrix(file_path, delim=",",
comment_str="#")).astype(float)
if mat.shape[1] != 17:
raise FileInterfaceException(
"EuRoC MAV state ground truth must have 17 entries per row")
stamps = np.divide(mat[:, 0], 1e9) # n x 1 - nanoseconds to seconds
xyz = mat[:, 1:4] # n x 3
quat = mat[:, 4:8] # n x 4
logging.debug("loaded " + str(len(stamps)) + " stamps and poses from: " +
file_path)
return PoseTrajectory3D(xyz, quat, stamps)
def read_tum_trajectory_file(file_path):
"""
parses trajectory file in TUM format (timestamp tx ty tz qx qy qz qw)
:param file_path: the trajectory file path (or file handle)
:return: trajectory.PoseTrajectory3D object
"""
mat = np.array(csv_read_matrix(file_path, delim=" ",
comment_str="#")).astype(float)
if mat.shape[1] != 8:
raise FileInterfaceException(
"TUM trajectory files must have 8 entries per row")
stamps = mat[:, 0] # n x 1
xyz = mat[:, 1:4] # n x 3
quat = mat[:, 4:] # n x 4
quat = np.roll(quat, 1, axis=1) # shift 1 column -> w in front column
if not hasattr(file_path, 'read'): # if not file handle
logging.debug("loaded " + str(len(stamps)) +
" stamps and poses from: " + file_path)
return PoseTrajectory3D(xyz, quat, stamps)
def read_bag_trajectory(bag_handle, topic):
"""
:param bag_handle: opened bag handle, from rosbag.Bag(...)
:param topic: geometry_msgs/PoseStamped topic
:return: trajectory.PoseTrajectory3D
"""
if not bag_handle.get_message_count(topic) > 0:
raise FileInterfaceException("no messages for topic '" + topic +
"' in bag")
stamps, xyz, quat = [], [], []
for topic, msg, t in bag_handle.read_messages(topic):
stamps.append(t.secs + (t.nsecs * 1e-9))
xyz.append(
[msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
quat.append([
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
])
logging.debug("loaded " + str(len(stamps)) +
" geometry_msgs/PoseStamped messages" + " of topic: " +
topic)
quat = np.roll(quat, 1, axis=1) # shift 1 column -> w in front column
return PoseTrajectory3D(xyz, quat, stamps)