def main():
# initialization
examples_dir = os.path.dirname(__file__)
checkpoint = os.path.join(examples_dir, '..', 'weights',
'deeptam_tracker_weights', 'snapshot-300000')
datadir = os.path.join(examples_dir, '../../..', 'generating', '_out',
'Town01_0000.npy')
# datadir = os.path.join(examples_dir, '../../..', 'generating', '_out', 'kitti', '04.npy')
tracking_module_path = os.path.join(
examples_dir, '..', 'python/deeptam_tracker/models/networks.py')
sequence = np.load(datadir)
WIDTH = 320
HEIGHT = 240
fw = WIDTH / (2 * np.tan(90 * np.pi / 360))
fh = HEIGHT / (2 * np.tan(90 * np.pi / 360))
cu = WIDTH / 2
cv = HEIGHT / 2
intrinsics = [[fw, fh, cu, cv]]
# use first 3 frames as an example, the fisrt frame is selected as key frame
frame_key = sequence[0]
print(frame_key[2])
print(Vector3(frame_key[2]))
pose_key = Pose(R=Matrix3(frame_key[3]), t=Vector3(frame_key[2]))
image_key = np.reshape(frame_key[0], (1, HEIGHT, WIDTH, 3))
frame_1 = sequence[1]
pose_1 = Pose(R=Matrix3(frame_1[3]), t=Vector3(frame_1[2]))
image_1 = np.reshape(frame_1[0], (1, HEIGHT, WIDTH, 3))
frame_2 = sequence[2]
pose_2 = Pose(R=Matrix3(frame_2[3]), t=Vector3(frame_2[2]))
image_2 = np.reshape(frame_2[0], (1, HEIGHT, WIDTH, 3))
tracker_core = TrackerCore(tracking_module_path, checkpoint, intrinsics)
# set the keyframe of tracker
tracker_core.set_keyframe(image_key, np.full((1, 240, 320, 1), 0.01),
pose_key)
# track frame_1 w.r.t frame_key
print('Track frame {} w.r.t key frame:'.format(1))
results_1 = tracker_core.compute_current_pose(image_1, pose_key)
pose_pr_1 = results_1['pose']
print('prediction', rotation_matrix_to_angleaxis(pose_pr_1.R), pose_pr_1.t)
print('gt', rotation_matrix_to_angleaxis(pose_1.R), pose_1.t)
simple_visualization(image_key, image_1, results_1['warped_image'], 1)
# track frame_2 w.r.t frame_key incrementally using pose_pr_1 as pose_guess
print(
'Track frame {} w.r.t key frame incrementally using previous predicted pose:'
.format(2))
results_2 = tracker_core.compute_current_pose(image_2, pose_pr_1)
pose_pr_2 = results_2['pose']
print('prediction', rotation_matrix_to_angleaxis(pose_pr_2.R), pose_pr_2.t)
print('gt', rotation_matrix_to_angleaxis(pose_2.R), pose_2.t)
simple_visualization(image_key, image_2, results_2['warped_image'], 2)
del tracker_core
def naive_avg_pose_fusion(cams_poses):
'''
Averages the input poses of each camera provided in the list
:param cams_poses: list of poses for each camera
:return: pose after fusion
'''
from deeptam_tracker.utils.rotation_conversion import rotation_matrix_to_angleaxis, angleaxis_to_rotation_matrix
from deeptam_tracker.utils.datatypes import Vector3, Matrix3, Pose
from deeptam_tracker.utils.vis_utils import convert_between_c2w_w2c
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))
# naive approach by taking average
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 naive_pose_fusion(cams_poses):
'''
Averages the input poses of each camera provided in the list
:param cams_poses: list of list of poses for each camera
:return: list of poses after fusion
'''
from deeptam_tracker.utils.rotation_conversion import rotation_matrix_to_angleaxis, angleaxis_to_rotation_matrix
from deeptam_tracker.utils.datatypes import Vector3, Matrix3, Pose
assert isinstance(cams_poses, list)
assert all(
len(cam_poses) == len(cams_poses[0]) for cam_poses in cams_poses)
fused_poses = []
num_of_poses = len(cams_poses[0])
for idx in range(num_of_poses):
trans = []
orientation_aa = []
for cam_num in range(len(cams_poses)):
trans.append(np.array(cams_poses[cam_num][idx].t))
orientation_aa.append(
rotation_matrix_to_angleaxis(cams_poses[cam_num][idx].R))
t = np.mean(trans, axis=0)
R = angleaxis_to_rotation_matrix(
Vector3(np.mean(orientation_aa, axis=0)))
fused_poses.append(Pose(R=Matrix3(R), t=Vector3(t)))
return fused_poses
def sift_depth_pose_fusion(cams_poses, images_list, depths_list):
'''
Averages the input poses of each camera provided in the list based on features density
and homogeneity of the depth images
:param cams_poses: list of poses for each camera
:param images_list: list of images from each camera
:param depths_list: list of depth images from each camera
:return: pose after fusion
'''
assert isinstance(images_list, list)
assert isinstance(depths_list, list)
assert isinstance(cams_poses, list)
assert (len(cams_poses) == len(images_list))
assert (len(cams_poses) == len(depths_list))
## SIFT feature detection
sift_weights = []
sift = cv2.xfeatures2d.SIFT_create()
for image in images_list:
im = np.array(convert_array_to_colorimg(image.squeeze()))
kp = sift.detect(im, None)
sift_weights.append(len(kp))
sift_weights = np.asarray(sift_weights)
sift_weights = sift_weights / sift_weights.sum()
depth_weights = []
for depth in depths_list:
depth_weights.append(np.nanstd(depth))
depth_weights = np.asarray(depth_weights)
depth_weights = depth_weights / depth_weights.sum()
c1 = 0.5
c2 = 0.5
feat_weights = []
for weight_idx in range(sift_weights.shape[0]):
feat_weights.append(c1 * sift_weights[weight_idx] +
c2 * depth_weights[weight_idx])
feat_weights = np.asarray(feat_weights)
feat_weights = feat_weights / feat_weights.sum()
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))
# naive approach by taking average
t = np.average(trans, axis=0, weights=feat_weights)
R = angleaxis_to_rotation_matrix(
Vector3(np.average(orientation_aa, axis=0, weights=feat_weights)))
fused_pose_c2w = Pose(R=Matrix3(R), t=Vector3(t))
return convert_between_c2w_w2c(fused_pose_c2w)
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 sift_pose_fusion(cams_poses, images_list):
'''
Averages the input poses of each camera provided in the list based on features density
:param cams_poses: list of poses for each camera
:param images_list: list of images from each camera
:return: pose after fusion
'''
assert isinstance(images_list, list)
assert isinstance(cams_poses, list)
assert (len(cams_poses) == len(images_list))
## SIFT feature detection
feat_num = []
sift = cv2.xfeatures2d.SIFT_create()
for image in images_list:
im = np.array(convert_array_to_colorimg(image.squeeze()))
kp = sift.detect(im, None)
feat_num.append(len(kp))
feat_num = np.asarray(feat_num)
feat_weights = feat_num / feat_num.sum()
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))
# naive approach by taking average
t = np.average(trans, axis=0, weights=feat_weights)
R = angleaxis_to_rotation_matrix(
Vector3(np.average(orientation_aa, axis=0, weights=feat_weights)))
fused_pose_c2w = Pose(R=Matrix3(R), t=Vector3(t))
return convert_between_c2w_w2c(fused_pose_c2w)