def write_imufile_to_rosbag(bag, imufile, timerange=None, buffertime=5, topic="/imu0"):
"""
:param bag: output bag stream.
:param imufile: each line: time(ns), gx, gy, gz, ax, ay, az
:param timerange: only write imu data within this range to the bag, in seconds.
:param buffertime: time to pad the timerange, in seconds.
:param topic: imu topic
:return:
"""
utility_functions.check_file_exists(imufile)
with open(imufile, 'r') as stream:
rowcount = 0
imucount = 0
for line in stream:
row = re.split(' |,|[|]', line) # note a row is a list of strings.
if utility_functions.is_header_line(row[0]):
continue
imumsg, timestamp = create_imu_message_time_string(
row[0], row[1:4], row[4:7])
timestampinsec = timestamp.to_sec()
rowcount += 1
if timerange and \
(timestampinsec < timerange[0] - buffertime or
timestampinsec > timerange[1] + buffertime):
continue
imucount += 1
bag.write(topic, imumsg, timestamp)
print('Saved {} out of {} inertial messages to the rosbag'.format(
imucount, rowcount))
def write_imufile_remotetime_to_rosbag(bag, imufile, timerange=None, buffertime=5, topic="/imu0"):
"""
:param bag: output bag stream.
:param imufile: each line: time(sec), ax, ay, az, gx, gy, gz, device-time(sec), and others
:param timerange: only write imu data within this local time range to the bag, in seconds.
:param buffertime: time to pad the timerange, in seconds.
:param topic: imu topic
:return:
"""
utility_functions.check_file_exists(imufile)
with open(imufile, 'r') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
rowcount = 0
imucount = 0
for row in reader: # note a row is a list of strings.
if utility_functions.is_header_line(row[0]):
continue
local_timestamp = rospy.Time.from_sec(float(row[0]))
remote_timestamp = rospy.Time.from_sec(float(row[7]))
imumsg = create_imu_message(remote_timestamp, row[4:7], row[1:4])
timestampinsec = local_timestamp.to_sec()
rowcount += 1
if timerange and \
(timestampinsec < timerange[0] - buffertime or
timestampinsec > timerange[1] + buffertime):
continue
imucount += 1
bag.write(topic, imumsg, local_timestamp)
print('Saved {} out of {} inertial messages to the rosbag'.format(
imucount, rowcount))
def load_time_trans_quat(tffile):
"""
load a txt file
:param tffile:
:param line_format: TUM_RGBD format t[s] x y z qx qy qz qw
or KALIBR format t[ns] x y z qx qy qz qw
Because this function does not do normalization,
for better precision, qx qy qz qw should have 9 decimal digits
:return: a list of timed poses, each entry is
[rospy time, tx, ty, tz, qx, qy, qz, qw]
"""
chunk = []
delimiter = None
with open(tffile, "r") as stream:
for _, line in enumerate(stream):
if utility_functions.is_header_line(line):
continue
if delimiter is None:
delimiter = utility_functions.decide_delimiter(line)
if delimiter == " ":
time_pose_str = line.split()
else:
time_pose_str = line.split(delimiter)
secs, nsecs = utility_functions.parse_time(time_pose_str[0], 's')
timestamp = rospy.Time(secs, nsecs)
row = [timestamp] + [float(x) for x in time_pose_str[1:]]
chunk.append(row)
return chunk
def loadtimestamps(video_time_file):
"""Except for the header, each row has the timestamp in nanosec
as its first component"""
frame_rostimes = list()
with open(video_time_file, 'r') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
for row in reader:
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
frame_rostimes.append(rospy.Time(secs, nsecs))
return frame_rostimes
def loadtimestamps(time_file):
"""Except for the header, each row has the timestamp in nanosec
as its first component"""
frame_rostimes = list()
# https://stackoverflow.com/questions/41847146/multiple-delimiters-in-single-csv-file
with open(time_file, 'r') as stream:
for line in stream:
row = re.split(' |,|[|]', line)
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
frame_rostimes.append(rospy.Time(secs, nsecs))
return frame_rostimes
def load_local_and_remote_times(time_file):
"""
:param time_file: Except for the header lines, each line has the first column as the remote time in ns,
and the last column as the local time in secs.
:return: list of tuples (local time, remote time)
"""
frame_rostimes = list()
with open(time_file, 'r') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
for row in reader:
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
remote_time = rospy.Time(secs, nsecs)
local_time = rospy.Time.from_sec(float(row[-1]))
frame_rostimes.append((local_time, remote_time))
return frame_rostimes
def load_local_and_remote_times(time_file):
"""
:param time_file: Except for the header lines, each line has the first column as the remote time in ns,
and the last column as the local time in secs.
:return: list of tuples (local time, remote time)
"""
frame_rostimes = list()
with open(time_file, 'r') as stream:
for line in stream:
row = re.split(' |,|[|]', line)
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
remote_time = rospy.Time(secs, nsecs)
local_time = rospy.Time.from_sec(float(row[-1]))
frame_rostimes.append((local_time, remote_time))
return frame_rostimes
def main():
parsed = parse_args()
bag = rosbag.Bag(parsed.output_bag, 'w')
videotimerange = None # time range of video frames in IMU clock
if parsed.video is not None:
utility_functions.check_file_exists(parsed.video)
print('given video time offset {}'.format(parsed.video_time_offset))
if parsed.video_from_to:
print('video_from_to: {}'.format(parsed.video_from_to))
video_time_offset = parsed.video_time_offset
frame_timestamps = list()
if parsed.video_time_file:
frame_timestamps = loadtimestamps(parsed.video_time_file)
aligned_timestamps = [
time + rospy.Duration.from_sec(parsed.video_file_time_offset)
for time in frame_timestamps
]
frame_timestamps = aligned_timestamps
print('Loaded {} timestamps for frames'.format(
len(frame_timestamps)))
video_time_offset = frame_timestamps[0].to_sec()
videotimerange = write_video_to_rosbag(
bag,
parsed.video,
parsed.video_from_to,
video_time_offset,
frame_timestamps=frame_timestamps,
max_video_frame_height=parsed.max_video_frame_height,
shift_in_time=parsed.shift_secs)
elif parsed.folder is not None:
# write images
camfolders = get_cam_folders_from_dir(parsed.folder)
for camfolder in camfolders:
camdir = parsed.folder + "/{0}".format(camfolder)
image_files = get_image_files_from_dir(camdir)
for image_filename in image_files:
image_msg, timestamp = load_image_to_ros_msg(image_filename)
bag.write("/{0}/image_raw".format(camfolder), image_msg,
timestamp)
print("Saved #images {} of {} to bag".format(
len(image_files), camfolder))
else:
raise Exception('Invalid/Empty video file and image folder')
# write imu data
if (not parsed.imu) and parsed.folder is None:
print("Neither a folder nor any imu file is provided. "
"Rosbag will have only visual data")
elif not parsed.imu:
imufiles = get_imu_csv_files(parsed.folder)
for imufile in imufiles:
topic = os.path.splitext(os.path.basename(imufile))[0]
with open(imufile, 'r') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
for row in reader:
if utility_functions.is_header_line(row[0]):
continue
imumsg, timestamp = create_imu_message_time_string(
row[0], row[1:4], row[4:7])
bag.write("/{0}".format(topic), imumsg, timestamp)
elif len(parsed.imu) == 1:
imufile = parsed.imu[0]
topic = 'imu0'
utility_functions.check_file_exists(imufile)
with open(imufile, 'r') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
rowcount = 0
imucount = 0
for row in reader: # note row has many strings.
if utility_functions.is_header_line(row[0]):
continue
imumsg, timestamp = create_imu_message_time_string(
row[0], row[1:4], row[4:7])
timestampinsec = timestamp.to_sec()
rowcount += 1
# check below conditions when video and imu use different
# clocks and their lengths differ much
if videotimerange and \
(timestampinsec < videotimerange[0] - MARGIN_TIME or
timestampinsec > videotimerange[1] + MARGIN_TIME):
continue
imucount += 1
bag.write("/{0}".format(topic), imumsg, timestamp)
print('Saved {} out of {} inertial messages to the rosbag'.format(
imucount, rowcount))
else:
gyro_file = parsed.imu[0]
accel_file = parsed.imu[1]
topic = 'imu0'
for filename in parsed.imu:
utility_functions.check_file_exists(filename)
time_gyro_array = utility_functions.load_advio_imu_data(gyro_file)
time_accel_array = utility_functions.load_advio_imu_data(accel_file)
time_imu_array = utility_functions.interpolate_imu_data(
time_gyro_array, time_accel_array)
bag_imu_count = 0
for row in time_imu_array:
timestamp = rospy.Time.from_sec(row[0]) + rospy.Duration.from_sec(
parsed.shift_secs)
imumsg = create_imu_message(timestamp, row[1:4], row[4:7])
timestampinsec = timestamp.to_sec()
# check below conditions when video and imu use different clocks
# and their lengths differ much
if videotimerange and \
(timestampinsec < videotimerange[0] - MARGIN_TIME or
timestampinsec > videotimerange[1] + MARGIN_TIME):
continue
bag_imu_count += 1
bag.write("/{0}".format(topic), imumsg, timestamp)
print('Saved {} out of {} inertial messages to the rosbag'.format(
bag_imu_count, time_imu_array.shape[0]))
bag.close()
print('Saved to bag file {}'.format(parsed.output_bag))
def convert_pose_format(infile,
outfile,
in_time_unit=None,
in_quat_order="xyzw",
output_format="KALIBR",
output_delimiter=","):
# check infile line format
with open(infile, "r") as stream:
lines = []
max_lines = 2
read_next = True
line = None
while read_next:
line = stream.readline().rstrip('\n')
read_next = utility_functions.is_header_line(line)
if not read_next and len(lines) < max_lines:
lines.append(line)
read_next = True
in_delimiter = utility_functions.decide_delimiter(lines[-1])
time_index, time_unit, r_index = \
utility_functions.decide_time_index_and_unit(lines, in_delimiter)
if in_time_unit is not None:
time_unit = in_time_unit
print(
"The determined time index {} time unit {} and translation start index {}"
.format(time_index, time_unit, r_index))
q_index = r_index + 3
# set default outfile
if not outfile:
in_dir = os.path.dirname(infile)
in_base = os.path.basename(infile)
in_name, in_ext = os.path.splitext(in_base)
outfile = os.path.join(in_dir, in_name + "_canon" + in_ext)
# load infile and write outfile
with open(outfile, "w") as ostream:
with open(infile, "r") as istream:
for line in istream:
if utility_functions.is_header_line(line):
continue
rags = line.split(in_delimiter)
rags = [rag.strip() for rag in rags]
secs, nanos = utility_functions.parse_time(rags[time_index])
if output_format == "KALIBR":
if secs > 0:
ostream.write("{}{:09d}".format(secs, nanos))
else: # secs == 0
ostream.write("{}".format(nanos))
elif output_format == "TUM_RGBD":
if secs > 0:
ostream.write("{}.{:09d}".format(secs, nanos))
else: # secs == 0
ostream.write("{}".format(
float(nanos) / utility_functions.SECOND_TO_NANOS))
else:
raise ValueError(
"Unsupported output format {}!".format(output_format))
for j in range(3):
ostream.write("{}{}".format(output_delimiter,
rags[r_index + j]))
quat = utility_functions.normalize_quat_str(
rags[q_index:q_index + 4])
if in_quat_order == "xyzw":
for j in range(4):
ostream.write("{}{}".format(output_delimiter, quat[j]))
ostream.write("\n")
elif in_quat_order == "wxyz":
for j in range(3):
ostream.write("{}{}".format(output_delimiter,
quat[j + 1]))
ostream.write("{}{}\n".format(output_delimiter, quat[0]))
else:
raise ValueError(
"Unsupported input quaternion order {}!".format(
in_quat_order))
print("Successfully converted {}\ninto pose file: {}".format(
infile, outfile))
def main():
parsed = parse_args()
bag = rosbag.Bag(parsed.output_bag, 'w')
videotimerange = None # time range of video frames in IMU clock
if parsed.video is not None:
utility_functions.check_file_exists(parsed.video)
print('Given video time offset {}'.format(parsed.first_frame_imu_time))
if parsed.video_from_to:
print('Frame time range within the video: {}'.format(parsed.video_from_to))
first_frame_imu_time = parsed.first_frame_imu_time
frame_timestamps = list()
if parsed.video_time_file:
frame_timestamps = loadtimestamps(parsed.video_time_file)
aligned_timestamps = [time + rospy.Duration.from_sec(parsed.video_file_time_offset)
for time in frame_timestamps]
frame_timestamps = aligned_timestamps
print('Loaded {} timestamps for frames'.format(
len(frame_timestamps)))
first_frame_imu_time = frame_timestamps[0].to_sec()
videotimerange = write_video_to_rosbag(
bag,
parsed.video,
parsed.video_from_to,
first_frame_imu_time,
frame_timestamps,
frame_remote_timestamps=None,
max_video_frame_height=parsed.max_video_frame_height,
shift_in_time=parsed.shift_secs,
topic="/cam0/image_raw")
elif parsed.folder is not None:
# write images
camfolders = get_cam_folders_from_dir(parsed.folder)
for camfolder in camfolders:
camdir = parsed.folder + "/{0}".format(camfolder)
image_files = get_image_files_from_dir(camdir)
for image_filename in image_files:
image_msg, timestamp = load_image_to_ros_msg(image_filename)
bag.write("/{0}/image_raw".format(camfolder), image_msg,
timestamp)
print("Saved #images {} of {} to bag".format(
len(image_files), camfolder))
else:
raise Exception('Invalid/Empty video file and image folder')
# write imu data
if (not parsed.imu) and parsed.folder is None:
print("Neither a folder nor any imu file is provided. "
"Rosbag will have only visual data")
elif not parsed.imu:
imufiles = get_imu_csv_files(parsed.folder)
for imufile in imufiles:
topic = os.path.splitext(os.path.basename(imufile))[0]
with open(imufile, 'r') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
for row in reader:
if utility_functions.is_header_line(row[0]):
continue
imumsg, timestamp = create_imu_message_time_string(
row[0], row[1:4], row[4:7])
bag.write("/{0}".format(topic), imumsg, timestamp)
elif len(parsed.imu) == 1:
write_imufile_to_rosbag(bag, parsed.imu[0], videotimerange, 5, "/imu0")
else:
write_gyro_accel_to_rosbag(bag, parsed.imu, videotimerange, 5, "/imu0", parsed.shift_secs)
bag.close()
print('Saved to bag file {}'.format(parsed.output_bag))
def main():
parser = argparse.ArgumentParser(description='''
Analyze trajectories
''')
parser.add_argument('trajectory',
help="File containing "
"[timestamp tx ty tz qx qy qz qw]. Denoted as T_BH")
parser.add_argument(
'--right_transform',
default="",
help="File with 4x4 transformation matrix which is to"
" multiply the trajectory poses on the right, denoted by T_HE,"
"comma separated rows")
parser.add_argument(
'--left_transform',
default="",
help="File with 4x4 transformation matrix which is to"
" multiply the trajectory poses on the left, denoted by T_WB, "
"comma separated rows")
parser.add_argument("--normalize_to_first_pose",
action='store_true',
help='Multiply all poses from left by inverse of the'
' first pose before multiplying with the '
'provided transforms. In this case, the provided'
' left_transform will be ignored and use instead'
' the inverse of the right_transform.')
parser.add_argument(
'--output_txt',
default="",
help="default to be transformed_poses.txt under trajectory dir")
args = parser.parse_args()
assert os.path.exists(args.trajectory)
T_HE = np.eye(4)
if args.right_transform:
if os.path.isfile(args.right_transform):
T_HE = np.loadtxt(args.right_transform, delimiter=',')
T_WB = np.eye(4)
if args.left_transform:
if os.path.isfile(args.left_transform):
T_WB = np.loadtxt(args.left_transform, delimiter=',')
if args.normalize_to_first_pose:
T_WB = np.linalg.inv(T_HE)
# load the file
f = open(args.trajectory, "r")
lines = f.readlines()
timestamp = []
positions = []
quats = []
i = 0
header = ""
for x in lines:
if i == 0 and utility_functions.is_header_line(x):
header = x
i = i + 1
continue
contents = x.split(' ')
timestamp.append(contents[0])
positions.append(np.array(contents[1:4], dtype=float))
quats.append(np.array(contents[4:8], dtype=float))
f.close()
print("Read {0} poses.".format(len(timestamp)))
print("Transform the first pose to identity? {}".format(
args.normalize_to_first_pose))
print("Transform to apply from the right:\n{0}".format(T_HE))
print("Transform to apply from the left:\n{0}".format(T_WB))
N = len(positions)
assert (N == len(quats))
transformed_positions = np.zeros([N, 3])
transformed_quats = np.zeros([N, 4])
print('Transforming {0} trajectory poses'.format(N))
T_BH0 = np.eye(4)
T_BH0[0:3, 0:3] = quat2dcm(quats[0])
T_BH0[0:3, 3] = positions[0]
T_H0B = np.eye(4)
if args.normalize_to_first_pose:
T_H0B[0:3, 0:3] = np.transpose(T_BH0[0:3, 0:3])
T_H0B[0:3, 3] = -np.matmul(T_H0B[0:3, 0:3], positions[0])
for i in range(N):
T_old = np.eye(4)
T_old[0:3, 0:3] = quat2dcm(quats[i])
T_old[0:3, 3] = positions[i]
T_new = np.matmul(np.matmul(T_WB, T_H0B), np.matmul(T_old, T_HE))
transformed_positions[i, :] = T_new[0:3, 3]
transformed_quats[i, :] = dcm2quat(T_new[0:3, 0:3])
file_lines = []
if header:
file_lines.append(header)
for i in range(N):
file_lines.append(''.join([
str(timestamp[i]), ' ',
str(transformed_positions[i, 0]), ' ',
str(transformed_positions[i, 1]), ' ',
str(transformed_positions[i, 2]), ' ',
str(transformed_quats[i, 0]), ' ',
str(transformed_quats[i, 1]), ' ',
str(transformed_quats[i, 2]), ' ',
str(transformed_quats[i, 3]), '\n'
]))
if args.output_txt:
outfn = args.output_txt
else:
out_dir = os.path.dirname(os.path.abspath(args.trajectory))
outfn = os.path.join(out_dir, "transformed_poses.txt")
with open(outfn, 'w') as f:
f.writelines(file_lines)
print("Wrote transformed poses to file {0}.".format(outfn))