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))