def main(args):
visualization = not args.disable_vis
config_file = args.config_file
tracking_module_path = args.tracking_network
checkpoint = os.path.realpath(args.weights)
output_dir = os.path.abspath(args.output_dir)
enable_gt = args.enable_gt
# read the tracking network path :O
if tracking_module_path is None:
tracking_module_path = os.path.abspath(
deeptam_tracker.models.networks.__file__)
mg.print_notify(
PRINT_PREFIX, "Using default argument for tracking_network: %s" %
tracking_module_path)
elif not os.path.isfile(tracking_module_path):
raise Exception(
PRINT_PREFIX,
"Could not find the network for tracking module: %s!" %
tracking_module_path)
else:
tracking_module_path = os.path.realpath(tracking_module_path)
# read the config YAML file and create a dictionary out of it
config = load_camera_config_yaml(config_file)
os.makedirs(output_dir, exist_ok=True)
track_rgbd_sequence(checkpoint=checkpoint,
config=config,
tracking_module_path=tracking_module_path,
visualization=visualization,
output_dir=output_dir,
enable_gt=enable_gt)
def write_tum_trajectory_file(file_path,
stamps,
poses,
head='# timestamp x y z qx qy qz qw\n'):
"""
:param file_path: desired text file for trajectory (string or handle)
:param stamps: list of timestamps
:param poses: list of named tuple poses
:param head: str text to print on the file
"""
assert isinstance(stamps, list)
assert isinstance(poses, list)
assert len(poses) == len(stamps)
if len(stamps) < 1 or len(poses) < 1:
raise Exception(PRINT_PREFIX +
'Input trajectory data has invalid length.')
assert isinstance(poses[-1], Pose)
os.makedirs(os.path.dirname(file_path), exist_ok=True)
with open(file_path, 'w') as file:
file.write(head)
for i in range(len(poses)):
pose = poses[i]
timestamp = stamps[i]
T = np.eye(4)
T[:3, :3] = np.array(pose.R)
T[:3, 3] = np.array(pose.t)
T = np.linalg.inv(T) # convert to cam to world
R = T[:3, :3]
t = T[:3, 3]
q = Quaternion(R)
file.write('{0} {1} {2} {3} {4} {5} {6} {7}\n'.format(
timestamp, *t, *q))
mg.print_notify("Trajectory saved to: " + file_path)
def __init__(self, config_dirs_list, tracking_module_path, checkpoint, seq_name='multi_cam'):
"""
Create an object for accessing a multi-camera RGBD sequences
:param config_dirs_list: (list) paths to yaml files containing location of
data directories for each camera
:param tracking_module_path: (str) path to file containing the Tracker network class
:param checkpoint: (str) path to the pre-trained network weights
"""
# anything with _list at the end of the names has information
# about cam_i at idx i
assert isinstance(config_dirs_list, list)
assert len(config_dirs_list) > 0
self._startup = False
self.config_dirs_list = config_dirs_list
self.seq_name = seq_name
self.num_of_cams = len(config_dirs_list)
mg.print_notify(PRINT_PREFIX, "Setting up trackers for %d cameras." % self.num_of_cams)
self.cameras_list = []
# iterate over each directory and write file path names
for idx in range(self.num_of_cams):
config = load_camera_config_yaml(config_dirs_list[idx])
self.cameras_list.append(SingleCamTracker(config, tracking_module_path, checkpoint))
self.gt_poses = [[] for _ in range(self.num_of_cams)]
self.timestamps_list = [[] for _ in range(self.num_of_cams)]
self.key_pr_poses_list = [[] for _ in range(self.num_of_cams)]
self.key_gt_poses_list = [[] for _ in range(self.num_of_cams)]
self.key_timestamps_list = [[] for _ in range(self.num_of_cams)]
# sequence length is minimum of all
self.sequence_length = min([cam.get_sequence_length() for cam in self.cameras_list])
def check_new_keyframe(self, new_pose, depth):
"""Checks if a new keyframe has to be set based on the distance, angle, valid_pixel threshold and the availability of the depth
new_pose: Pose
"""
set_new_keyframe = False
key_pose = self._tracker_core._key_pose
position_diff = self.position_diff(key_pose, new_pose)
if not set_new_keyframe and position_diff > self._key_distance_threshold:
set_new_keyframe = True
mg.print_notify(
PRINT_PREFIX,
'setting new key frame because of distance threshold {0}'.
format(position_diff))
angle_diff = self.angle_diff(key_pose, new_pose)
if not set_new_keyframe and angle_diff > self._key_angle_deg_threshold:
set_new_keyframe = True
mg.print_notify(
PRINT_PREFIX,
'setting new key frame because of angle threshold {0}'.format(
angle_diff))
if self._tracker_core._key_valid_depth_pixels != 0:
if not set_new_keyframe and self._tracker_core._valid_warped_pixels / self._tracker_core._key_valid_depth_pixels < self._key_valid_pixel_ratio_threshold:
set_new_keyframe = True
mg.print_notify(
PRINT_PREFIX,
'setting new key frame because of valid pixel ratio threshold less than {0}'
.format(self._key_valid_pixel_ratio_threshold))
if set_new_keyframe:
if depth is None:
set_new_keyframe = False
mg.print_warn(
PRINT_PREFIX,
"[WARNING] cannot set new key frame because of missing depth"
)
return set_new_keyframe
def startup(self):
# print what all is going to be plotted
mg.print_notify(
PRINT_PREFIX,
"Display individual camera pose estimates: %d" % self.enabled_pred)
mg.print_notify(
PRINT_PREFIX,
"Display ground truth pose estimates: %d" % self.enabled_gt)
mg.print_notify(
PRINT_PREFIX,
"Display fused pose estimates: %d" % self.enabled_fused)
# create axes for plotting the odometry
ax1 = self.fig.add_subplot(2, 1, 1, projection='3d', aspect='equal')
# for estimated trajectory
if self.enabled_pred:
for idx in range(self.number_of_cameras):
ax1.plot([], [], [],
PRED_COLOR_LIST[idx],
label='Prediction (Cam %d)' % idx)
# for fused estimate
if self.enabled_fused:
ax1.plot([], [], [], FUSED_COLOR, label='Fusion')
# for groundtruth
if self.enabled_gt:
ax1.plot([], [], [], GT_COLOR, label='Ground truth')
ax1.legend(loc='center left', bbox_to_anchor=(1, 0.5))
ax1.set_title('Trajectory')
self.axs.append(ax1)
# create axes for RGB image from each camera
for idx in range(self.number_of_cameras):
ax = self.fig.add_subplot(2, self.number_of_cameras,
self.number_of_cameras + idx + 1)
ax.get_xaxis().set_visible(False)
ax.get_yaxis().set_visible(False)
ax.set_title('RGB (Cam %d)' % idx)
self.axs.append(ax)
def __init__(self,
sequence_dir,
cam_name='cam',
rgb_parameters=None,
depth_parameters=None,
time_syncing_parameters=None,
require_depth=False,
require_pose=False):
"""
Creates an object for accessing an rgbd benchmark sequence
:param sequence_dir: (str) Path to the directory of a sequence
:param cam_name: (str) name of the camera
:param rgb_parameters: (dict) rgb camera parameters, keys = [f_x, f_y, c_x, c_y, width, height]
:param depth_parameters: (dict) depth camera parameters, keys = [min, max, scaling]
:param time_syncing_parameters: (dict) parameters for time-syncing, keys = [max_difference, offset]
:param require_depth:
:param require_pose:
"""
# check inputs are correct
assert isinstance(sequence_dir, str)
assert isinstance(cam_name, str)
print(rgb_parameters)
assert isinstance(rgb_parameters, dict)
assert isinstance(depth_parameters, dict)
assert isinstance(time_syncing_parameters, dict)
if rgb_parameters is None or depth_parameters is None or time_syncing_parameters is None:
raise Exception(PRINT_PREFIX +
"[ERROR] Input parameters are incorrect!")
self.sequence_dir = os.path.realpath(sequence_dir)
self.cam_name = cam_name.lower()
self.intrinsics = None
# file names with sequence information
depth_txt = os.path.join(sequence_dir, 'depth.txt')
rgb_txt = os.path.join(sequence_dir, 'rgb.txt')
groundtruth_txt = os.path.join(sequence_dir, 'groundtruth.txt')
# configuration for time-syncing operation
time_max_difference = time_syncing_parameters['max_difference']
time_offset = time_syncing_parameters['offset']
# configuration for depth camera
# TODO: @Rohit, should we scale the depth images and clip the values?
self.depth_min = depth_parameters['min']
self.depth_max = depth_parameters['max']
self.depth_scaling = depth_parameters['scaling']
# configuration for rgb camera
self.intrinsics = [
rgb_parameters['f_x'], rgb_parameters['f_y'],
rgb_parameters['c_x'], rgb_parameters['c_y']
]
self.original_image_size = (rgb_parameters['width'],
rgb_parameters['height'])
# read paths for rgb and depth images
self.rgb_dict = read_file_list(rgb_txt)
self.depth_dict = read_file_list(depth_txt)
mg.print_notify(
PRINT_PREFIX,
"Length of the read image sequence: %d" % len(self.rgb_dict))
# associate two dictionaries of (stamp,data) for rgb and depth data
self.matches_depth = associate(self.rgb_dict,
self.depth_dict,
offset=time_offset,
max_difference=time_max_difference)
self.matches_depth_dict = dict(self.matches_depth)
# create the camera matrix
self._K = np.eye(3)
self._K[0, 0] = self.intrinsics[0]
self._K[1, 1] = self.intrinsics[1]
self._K[0, 2] = self.intrinsics[2]
self._K[1, 2] = self.intrinsics[3]
# read groundtruth if available
if os.path.isfile(groundtruth_txt):
self.groundtruth_dict = read_file_list(groundtruth_txt)
else:
self.groundtruth_dict = None
# associate two dictionaries of (stamp,data) for rgb and groundtruth data
if self.groundtruth_dict is not None:
self.matches_pose = associate(self.rgb_dict,
self.groundtruth_dict,
offset=time_offset,
max_difference=time_max_difference)
self.matches_pose_dict = dict(self.matches_pose)
else:
self.matches_pose = None
self.matches_pose_dict = None
# create list of the processed (time-synced) rgb-depth images and poses on the basis of rgb timestamps
# Note: if any dictionary is empty, None is added for the entry
self.matches_depth_pose = []
for timestamp_rgb in sorted(self.rgb_dict.keys()):
# RGB image
img_path = os.path.join(self.sequence_dir,
*self.rgb_dict[timestamp_rgb])
if not os.path.exists(img_path):
continue
# Corresponding depth image
if self.matches_depth_dict.get(timestamp_rgb, None) is not None:
timestamp_depth = self.matches_depth_dict[timestamp_rgb]
depth_path = os.path.join(self.sequence_dir,
*self.depth_dict[timestamp_depth])
if not os.path.exists(depth_path):
timestamp_depth = None
else:
timestamp_depth = None
if require_depth and timestamp_depth is None:
continue
# Corresponding pose
if self.matches_pose_dict is not None and self.matches_pose_dict.get(
timestamp_rgb, None) is not None:
timestamp_pose = self.matches_pose_dict[timestamp_rgb]
else:
timestamp_pose = None
if require_pose and timestamp_pose is None:
continue
# append the timestamps of the synced information
timestamps_sync = {
'timestamp_rgb': timestamp_rgb,
'timestamp_depth': timestamp_depth,
'timestamp_pose': timestamp_pose
}
self.matches_depth_pose.append(timestamps_sync)
# make sure the initial frame has a depth map and a pose
if not any(
[temp['timestamp_pose'] for temp in self.matches_depth_pose]):
self.seq_len = len(self.matches_depth_dict)
mg.print_warn(
PRINT_PREFIX,
'No ground truth information for pose estimation comparison')
else:
while self.matches_depth_pose[0][
'timestamp_depth'] is None or self.matches_depth_pose[0][
'timestamp_pose'] is None:
del self.matches_depth_pose[0]
# get the sequence length after processing
self.seq_len = len(self.matches_depth_pose)
mg.print_notify(
PRINT_PREFIX,
"Length of the synced image sequence: %d" % self.seq_len)
# open first matched image to get the original image size
im_size = Image.open(
os.path.join(
self.sequence_dir, *self.rgb_dict[self.matches_depth_pose[0]
['timestamp_rgb']])).size
if self.original_image_size != im_size:
raise Exception(PRINT_PREFIX + "Expected input images to be of size ({}, {}) but received ({}, {})" \
.format(self.original_image_size[0], self.original_image_size[1],
im_size[0], im_size[1]))
def track_multicam_rgbd_sequence(checkpoint, config, tracking_module_path, visualization, output_dir, method):
"""Tracks a multicam rgbd sequence using deeptam tracker
checkpoint: str
directory to the weights
config: dict
dictionary containing the list of camera config files
tracking_module_path: str
file which contains the model class
visualization: bool
output_dir: str
directory path save the output data
method: str
specify method of fusion ("naive"/"sift"/"rejection")
"""
##################
# initialization #
##################
# initialize the multi-camera sequence
multicam_tracker = MultiCamTracker(config['camera_configs'], tracking_module_path, checkpoint,
seq_name=config['seq_name'])
multicam_tracker.startup()
viz = Visualizer("MultiCam Tracker", len(config['camera_configs']))
# configure the visualizer
viz.set_enable_pred(True)
viz.set_enable_gt(True)
viz.set_enable_fused(True)
viz.startup()
# Reference camera configuration
try:
camera_ref_idx = config['camera_ref_index']
except KeyError:
camera_ref_idx = 0
# Putting in higher scope so that don't need to call function again after loop
pr_poses_list = None
fused_poses = []
####################
# perform tracking #
####################
for frame_idx in range(multicam_tracker.get_sequence_length()):
print(PRINT_PREFIX, 'Input frame number: {}'.format(frame_idx))
pr_poses_list, gt_poses_list, frame_list, result_list = multicam_tracker.update(frame_idx)
# retieve the last predicted poses from the tracker
last_poses_list = []
for pr_poses in pr_poses_list:
last_poses_list.append(pr_poses[-1])
# perform pose fusion
if method == 'naive':
fused_pose = naive_avg_pose_fusion(last_poses_list)
elif method == 'sift':
last_image_list = []
for image_idx in range(len(frame_list)):
last_image_list.append(frame_list[image_idx]['image'])
fused_pose = sift_pose_fusion(last_poses_list, last_image_list)
elif method == 'sift_depth':
last_image_list = []
last_depth_list = []
for image_idx in range(len(frame_list)):
last_image_list.append(frame_list[image_idx]['image'])
last_depth_list.append(frame_list[image_idx]['depth'])
fused_pose = sift_depth_pose_fusion(last_poses_list, last_image_list, last_depth_list)
elif method == "rejection":
fused_pose = rejection_avg_pose_fusion(last_poses_list)
else:
mg.print_fail(PRINT_PREFIX, "Unknown fusion method entered!")
fused_poses.append(fused_pose)
if visualization:
viz.update(frame_list, pr_poses_list=pr_poses_list, fused_poses=fused_poses,
gt_poses=gt_poses_list[camera_ref_idx])
######################
# perform evaluation #
######################
gt_poses_list = multicam_tracker.get_gt_poses_list()
timestamps_list = multicam_tracker.get_timestamps_list()
## evaluation for the predictions
for idx in range(multicam_tracker.num_of_cams):
errors_ate = rgbd_ate(gt_poses_list[camera_ref_idx], pr_poses_list[idx], timestamps_list[idx])
print(PRINT_PREFIX, "Camera %d:" % idx)
mg.print_notify('Frame-to-keyframe odometry evaluation [ATE], translational RMSE: {}[m/s]'.format(
errors_ate['absolute_translational_error.rmse']))
## save trajectory files
name = multicam_tracker.cameras_list[idx].name
write_tum_trajectory_file(os.path.join(output_dir, name, 'stamped_traj_estimate.txt'), timestamps_list[idx],
pr_poses_list[idx])
write_tum_trajectory_file(os.path.join(output_dir, name, 'stamped_groundtruth.txt'), timestamps_list[idx],
gt_poses_list[camera_ref_idx])
## evaluation for the fusion
errors_ate = rgbd_ate(gt_poses_list[camera_ref_idx], fused_poses, timestamps_list[camera_ref_idx])
print(PRINT_PREFIX, "Fused Poses from %d cameras" % multicam_tracker.num_of_cams)
mg.print_notify('Frame-to-keyframe odometry evaluation [ATE], translational RMSE: {}[m/s]'.format(
errors_ate['absolute_translational_error.rmse']))
## save trajectory files
name = "naive_fusion"
write_tum_trajectory_file(os.path.join(output_dir, name, 'stamped_traj_estimate.txt'), timestamps_list[idx],
pr_poses_list[idx])
write_tum_trajectory_file(os.path.join(output_dir, name, 'stamped_groundtruth.txt'), timestamps_list[idx],
gt_poses_list[idx])
# final visualization
viz.update(frame_list, pr_poses_list=pr_poses_list, fused_poses=fused_poses, gt_poses=gt_poses_list[camera_ref_idx])
viz.save_trajectory_plot(os.path.join(output_dir, 'traj_plot_%s.png' % method))
plt.show()
multicam_tracker.delete_tracker()
def __del__(self):
"""
"""
del self._session
tf.reset_default_graph()
mg.print_notify(PRINT_PREFIX, "Tensorflow graph reseted")
def track_rgbd_sequence(checkpoint, config, tracking_module_path,
visualization, output_dir, enable_gt):
"""Tracks a rgbd sequence using deeptam tracker
checkpoint: str
directory to the weights
config: dict
dictionary containing all the parameters for rgbd sequence and tracker
tracking_module_path: str
file which contains the model class
visualization: bool
output_dir: str
directory path save the output data
"""
### initialization
# initialize the camera sequence
sequence = RGBDSequence(
config['cam_dir'],
rgb_parameters=config['rgb_parameters'],
depth_parameters=config['depth_parameters'],
time_syncing_parameters=config['time_syncing_parameters'])
intrinsics = sequence.get_original_normalized_intrinsics()
# initialize corresponding tracker
tracker = Tracker(tracking_module_path,
checkpoint,
intrinsics,
tracking_parameters=config['tracking_parameters'])
gt_poses = []
timestamps = []
key_pr_poses = []
key_gt_poses = []
key_timestamps = []
axes = init_visualization(enable_gt)
frame = sequence.get_dict(0, intrinsics, tracker.image_width,
tracker.image_height)
pose0_gt = frame['pose']
tracker.clear()
# WIP: If gt_poses is aligned such that it starts from identity pose, you may comment this line
# TODO: @Rohit, should we make this base-to-cam transformation?
tracker.set_init_pose(Pose_identity())
## track a sequence
result = {}
for frame_idx in range(sequence.get_sequence_length()):
print(PRINT_PREFIX, 'Input frame number: {}'.format(frame_idx))
frame = sequence.get_dict(frame_idx, intrinsics, tracker.image_width,
tracker.image_height)
timestamps.append(sequence.get_timestamp(frame_idx))
result = tracker.feed_frame(frame['image'], frame['depth'])
gt_poses.append(frame['pose'])
pr_poses = tracker.poses
# transform pose to base frame
print("Before: ", pr_poses[-1].t)
pr_poses = transform_poses_to_base(config, pr_poses)
print("After: ", pr_poses[-1].t)
print('----------------')
if visualization:
update_visualization(axes, pr_poses, gt_poses, frame['image'],
result['warped_image'], enable_gt)
if result['keyframe']:
key_pr_poses.append(tracker.poses[-1])
key_gt_poses.append(frame['pose'])
key_timestamps.append(sequence.get_timestamp(frame_idx))
# transform pose to base frame
pr_poses = tracker.poses
pr_poses = transform_poses_to_base(config, pr_poses)
## evaluation
errors_rpe = rgbd_rpe(gt_poses, pr_poses, timestamps)
mg.print_notify(
PRINT_PREFIX,
'Frame-to-keyframe odometry evaluation [RPE], translational RMSE: {}[m/s]'
.format(errors_rpe['translational_error.rmse']))
## fuse the poses naively
fused_poses = naive_pose_fusion([gt_poses, pr_poses])
errors_rpe = rgbd_rpe(gt_poses, fused_poses, timestamps)
mg.print_notify(
PRINT_PREFIX,
'After fusion, frame-to-keyframe odometry evaluation [RPE], translational RMSE: {}[m/s]'
.format(errors_rpe['translational_error.rmse']))
## save trajectory files
write_tum_trajectory_file(
os.path.join(output_dir, sequence.cam_name,
'stamped_traj_estimate.txt'), timestamps, pr_poses)
write_tum_trajectory_file(
os.path.join(output_dir, sequence.cam_name, 'stamped_groundtruth.txt'),
timestamps, gt_poses)
## update visualization
update_visualization(axes, pr_poses, gt_poses, frame['image'],
result['warped_image'], enable_gt)
plt.show()
del tracker