def init_tracker(self, image, depth):
"""Initializes the tracker pose
image: np.array
depth: np.array
"""
self.clear()
self._poses.append(self._init_pose)
if depth is not None:
self.set_new_keyframe(image, depth, self._init_pose)
self._key_poses.append(self._init_pose)
self._init_tracker = False
return {
'pose': self._init_pose,
'keyframe': True,
'warped_image': None,
}
else:
mg.print_warn(
PRINT_PREFIX,
"[WARNING] cannot set new key frame because of missing depth")
return {
'pose': self._init_pose,
'keyframe': False,
'warped_image': None
}
def load_camera_config_yaml(config_file):
"""Returns the dictionary created out of parsed YAML file for a single camera
config_file: str
Path to the configuration YAML file
"""
if os.path.exists(config_file):
file = open(config_file, 'r')
try:
config = yaml.safe_load(file)
except yaml.YAMLError as exc:
mg.print_warn(exc)
raise Exception(PRINT_PREFIX +
"[ERROR] The file is not a valid YAML file!")
# print success status
mg.print_pass(
PRINT_PREFIX,
"Successfully read the camera parameters: %s" % config['cam_name'])
else:
raise Exception(
PRINT_PREFIX +
"[ERROR] YAML file not detected: {0}".format(config_file))
## get path to directory containing the YAML file
# if absolute path not provided then use relative path with respect to the configuration file location
if config.get('cam_dir', None) is None or not os.path.isabs(
config['cam_dir']):
config['cam_dir'] = os.path.join(
os.path.dirname(os.path.realpath(config_file)), config['cam_dir'])
# throw error if directory does not exist
if not os.path.isdir(config['cam_dir']):
raise Exception(PRINT_PREFIX +
"[ERROR] Could not find the data directory: %s!" %
config['cam_dir'])
# create a dictionary of dictionaries
# Reason: YAML parser creates a list out of every element in the parent tag
for key in config:
if isinstance(config[key], list):
child_dict = {}
for item in config[key]:
sub_child_dict = {}
for sub_item in item:
if isinstance(item[sub_item], list):
for sub_sub_item in item[sub_item]:
sub_child_dict.update(sub_sub_item)
child_dict[sub_item] = sub_child_dict
else:
child_dict.update(item)
break
config[key] = child_dict
return config
def __init__(self,
tracking_module_path,
checkpoint,
intrinsics,
tracking_parameters=None,
init_pose=Pose_identity()):
"""
tracking_module_path: str
Path to the networks.py
checkpoint: str
Path to a checkpoint
intrinsics: np.array
Normalized intrinsics
tracking_parameters: dict
Dictionary containing parameters for creating new frames,
keys = [key_valid_pixel_ratio_threshold, key_angle_deg_threshold, key_distance_threshold]
init_pose: Pose
Initialized pose for the starting frame
"""
# use default parameters for tracking if no input provided
if tracking_parameters is None:
mg.print_warn(
PRINT_PREFIX,
"[WARN] Using default parameters for creating new keyframes.")
tracking_parameters = {
'key_valid_pixel_ratio_threshold': 0.5,
'key_angle_deg_threshold': 6.0,
'key_distance_threshold': 0.15
}
else:
assert isinstance(tracking_parameters, dict)
self._tracker_core = TrackerCore(tracking_module_path, checkpoint,
intrinsics)
self._image_width = self._tracker_core.image_width
self._image_height = self._tracker_core.image_height
self._key_valid_pixel_ratio_threshold = tracking_parameters[
'key_valid_pixel_ratio_threshold']
self._key_angle_deg_threshold = tracking_parameters[
'key_angle_deg_threshold']
self._key_distance_threshold = tracking_parameters[
'key_distance_threshold']
self._init_pose = init_pose
self.clear()
def rejection_avg_pose_fusion(cams_poses, amt_dev=1.4):
'''
Averages the input poses of each camera provided in the list based on pose(translation only) acceptability
The acceptability is evaluated using the sigma-based rule for outlier rejection in a data.
:param cams_poses: list of poses for each camera
:param amt_dev: number of times of the standard deviation to consider for inlier acceptance
:return: pose after fusion
'''
trans = []
orientation_aa = []
for cam_num in range(len(cams_poses)):
# transform pose to the world frame
pose_c2w = convert_between_c2w_w2c(cams_poses[cam_num])
# append to the list
trans.append(np.array(pose_c2w.t))
orientation_aa.append(rotation_matrix_to_angleaxis(pose_c2w.R))
# calculate the mean and standard deviation of positions
t_mean = np.average(trans, axis=0)
t_sigma = amt_dev * np.std(trans, axis=0)
# filtering by outlier removal using 1-sigma rule
trans = []
orientation_aa = []
for cam_num in range(len(cams_poses)):
# transform pose to the world frame
pose_c2w = convert_between_c2w_w2c(cams_poses[cam_num])
trans_c2w = np.array(pose_c2w.t)
# append to the list if satisfies 1-sigma rule
if all(np.abs(trans_c2w - t_mean) <= t_sigma):
trans.append(np.array(pose_c2w.t))
orientation_aa.append(rotation_matrix_to_angleaxis(pose_c2w.R))
else:
mg.print_warn(PRINT_PREFIX,
"Camera %d ignored during fusion!" % cam_num)
# approach by taking average of only filtered poses
t = np.mean(trans, axis=0)
R = angleaxis_to_rotation_matrix(Vector3(np.mean(orientation_aa, axis=0)))
fused_pose_c2w = Pose(R=Matrix3(R), t=Vector3(t))
return convert_between_c2w_w2c(fused_pose_c2w)
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 __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 adjust_intrinsics(view, K_new, width_new, height_new, verbose=False):
"""Creates a new View with the specified intrinsics and image dimensions.
The skew parameter K[0,1] will be ignored.
view: View namedtuple
The view tuple
K_new: numpy.ndarray
3x3 calibration matrix with the new intrinsics
width_new: int
The new image width
height_new: int
The new image height
verbose: bool
Amount of verbosity of print statements
Returns a View tuple with adjusted image, depth and intrinsics
"""
# original parameters
fx = view.K[0, 0]
fy = view.K[1, 1]
cx = view.K[0, 2]
cy = view.K[1, 2]
width = view.image.width
height = view.image.height
# target param
fx_new = K_new[0, 0]
fy_new = K_new[1, 1]
cx_new = K_new[0, 2]
cy_new = K_new[1, 2]
scale_x = fx_new / fx
scale_y = fy_new / fy
# resize to get the right focal length
width_resize = int(width * scale_x)
height_resize = int(height * scale_y)
# principal point position in the resized image
cx_resize = cx * scale_x
cy_resize = cy * scale_y
img_resize = view.image.resize(
(width_resize, height_resize),
Image.BILINEAR if scale_x > 1 else Image.LANCZOS)
if view.depth is not None:
max_depth = np.max(view.depth)
depth_resize = view.depth / max_depth
depth_resize[depth_resize < 0.] = 0.
depth_resize = resize(
depth_resize,
(height_resize, width_resize), 0, mode='constant') * max_depth
else:
depth_resize = None
# crop to get the right principle point and resolution
x0 = int(round(cx_resize - cx_new))
y0 = int(round(cy_resize - cy_new))
x1 = x0 + int(width_new)
y1 = y0 + int(height_new)
if x0 < 0 or y0 < 0 or x1 > width_resize or y1 > height_resize:
if verbose:
mg.print_warn(
PRINT_PREFIX,
'[WARNING] Adjusting intrinsics adds a border to the image')
img_new = safe_crop_image(img_resize, (x0, y0, x1, y1),
(127, 127, 127))
if not depth_resize is None:
depth_new = safe_crop_array2d(depth_resize, (x0, y0, x1, y1),
0).astype(np.float32)
else:
depth_new = None
else:
img_new = img_resize.crop((x0, y0, x1, y1))
if not depth_resize is None:
depth_new = depth_resize[y0:y1, x0:x1].astype(np.float32)
else:
depth_new = None
return View(R=view.R,
t=view.t,
K=K_new,
image=img_new,
depth=depth_new,
depth_metric=view.depth_metric)