def shift_and_visualize_gt(self, path_to_gt_file):
""" Read associated groundtruth file and shift origin of the pose list so that it matches with the estimated trajectory """
gt_data = np.loadtxt(path_to_gt_file,
usecols=(0, 1, 2, 3, 4, 5, 6, 7),
dtype=float)
gt_pose_list = [None] * len(gt_data)
gt_pose_list[0] = Pose(R=np.eye(3), t=np.zeros((1, 3)))
gt_timestamps = gt_data[:, 0]
t_np = gt_data[:, 1:4]
q_np = np.roll(gt_data[:, 4:], 1, axis=1)
# Storing the initial pose of the groundtruth list as 3D Rigidbody Transform, assuming that it corresponds to origin of estimated trjectory
R_origin = np.asarray(Quaternion(*(q_np[0, :])).toRotationMatrix())
T_gt_to_est = np.eye(4)
T_gt_to_est[:3, :3] = R_origin
T_gt_to_est[:3, 3] = t_np[0, :]
T_gt_to_est = np.linalg.inv(T_gt_to_est)
for ii in range(1, len(t_np)):
T_current = np.eye(4)
T_current[:3, 3] = t_np[ii, :]
mini_q = Quaternion(*(q_np[ii, :]))
T_current[:3, :3] = np.asarray(mini_q.toRotationMatrix())
# Shift origin of current pose by multiplying it with Transformation matrix
T_shifted = np.dot(T_gt_to_est, T_current)
T_shifted = np.linalg.inv(T_shifted)
shifted_pose = Pose(R=T_shifted[:3, :3], t=T_shifted[:3, 3])
gt_pose_list[ii] = shifted_pose
return gt_pose_list, gt_timestamps
def transform_poses_to_base(config, poses):
"""
Transform the poses to the base frame with settings specified in the config
:param config:
:param poses:
:return:
"""
## initialize base to cam transformation
tf_t_BC = Vector3(config['base_to_cam_pose']['translation']['x'],
config['base_to_cam_pose']['translation']['y'],
config['base_to_cam_pose']['translation']['z'])
tf_q_BC = Quaternion(config['base_to_cam_pose']['orientation']['w'],
config['base_to_cam_pose']['orientation']['x'],
config['base_to_cam_pose']['orientation']['y'],
config['base_to_cam_pose']['orientation']['z'])
tf_R_BC = tf_q_BC.toRotationMatrix()
## transformation of poses
tf_poses = []
for pose in poses:
t = pose.t - tf_t_BC
R = pose.R * tf_R_BC.inverse()
tf_pose = Pose(R=R, t=t)
tf_poses.append(tf_pose)
return tf_poses
def compute_motion_errors(predicted_motion, gt_motion, normalize_translations=True):
"""
Computes the error of the motion.
predicted_motion: 1d numpy array with 6 elements
the motions as [aa1, aa2, aa3, tx, ty, tz]
aa1,aa2,aa3 is an angle axis representation.
The angle is the norm of the axis
gt_motion: 1d numpy array with 6 elements
ground truth motion in the same format as the predicted motion
normalize_translations: bool
If True then translations will be normalized before computing the error
Returns
rotation angular distance in radian
tranlation distance of the normalized translations
tranlation angular distance in radian
"""
def _numpy_to_Vector3(arr):
tmp = arr.astype(np.float64)
return Vector3(tmp[0], tmp[1], tmp[2])
gt_axis = _numpy_to_Vector3(gt_motion[0:3])
gt_angle = gt_axis.norm()
if gt_angle < 1e-6:
gt_angle = 0
gt_axis = Vector3(1, 0, 0)
else:
gt_axis.normalize()
gt_q = Quaternion(gt_angle, gt_axis)
predicted_axis = _numpy_to_Vector3(predicted_motion[0:3])
predicted_angle = predicted_axis.norm()
if predicted_angle < 1e-6:
predicted_angle = 0
predicted_axis = Vector3(1, 0, 0)
else:
predicted_axis.normalize()
predicted_axis.normalize()
predicted_q = Quaternion(predicted_angle, predicted_axis)
rotation_angle_dist = gt_q.angularDistance(predicted_q)
gt_trans = _numpy_to_Vector3(gt_motion[3:6])
thresh = gt_trans.norm()
if normalize_translations:
gt_trans.normalize()
predicted_trans = _numpy_to_Vector3(predicted_motion[3:6])
if normalize_translations and predicted_trans.norm() > 1e-6:
predicted_trans.normalize()
translation_dist = (gt_trans - predicted_trans).norm()
if thresh>1e-4:
translation_angle_diff = math.acos(np.clip(gt_trans.dot(predicted_trans), -1, 1))
else:
translation_angle_diff = 0
return np.rad2deg(rotation_angle_dist), translation_dist, np.rad2deg(translation_angle_diff)
def __init__(self,
config,
tracking_module_path,
checkpoint,
require_depth=False,
require_pose=False):
"""
Creates an object for a single camera tracking instance. It wraps around the DeepTAM Sequence and Tracker
classes. This is done to make it easier to extend the setup for a multi-camera tracking class.
:param config: (dict) comprising of the camera settings
:param tracking_module_path: (str) path to file containing the Tracker network class
:param checkpoint: (str) path to the pre-trained network weights
:param require_depth:
:param require_pose:
"""
assert isinstance(config, dict)
self._startup = False
self.name = config['cam_name']
# base to camera transformation
self.tf_t_BC = Vector3(config['base_to_cam_pose']['translation']['x'],
config['base_to_cam_pose']['translation']['y'],
config['base_to_cam_pose']['translation']['z'])
self.tf_q_BC = Quaternion(
config['base_to_cam_pose']['orientation']['w'],
config['base_to_cam_pose']['orientation']['x'],
config['base_to_cam_pose']['orientation']['y'],
config['base_to_cam_pose']['orientation']['z'])
self.tf_R_BC = self.tf_q_BC.toRotationMatrix()
self.sequence = RGBDSequence(
config['cam_dir'],
rgb_parameters=config['rgb_parameters'],
depth_parameters=config['depth_parameters'],
time_syncing_parameters=config['time_syncing_parameters'],
cam_name=config['cam_name'],
require_depth=require_depth,
require_pose=require_pose)
self.intrinsics = self.sequence.get_original_normalized_intrinsics()
self.tracker = Tracker(
tracking_module_path,
checkpoint,
self.intrinsics,
tracking_parameters=config['tracking_parameters'])
def write_rgbd_pose_format(path, poses, timestamps):
"""writes a pose txt file compatible with the rgbd eval tools
path: str
Path to the output file
poses: list of Pose
timestamps: list of float
"""
assert len(poses) == len(timestamps)
with open(path, 'w') as f:
for i in range(len(poses)):
pose = poses[i]
timestamp = timestamps[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)
f.write('{0} {1} {2} {3} {4} {5} {6} {7}\n'.format(
timestamp, *t, *q))
def get_relative_motion(self, frame1, frame2):
"""Returns the realtive transformation from frame1 to frame2
"""
if self.groundtruth_dict is None:
return None
trgb, tdepth, tpose = self.matches_depth_pose[frame1]
timestamp_pose = [tpose] + self.groundtruth_dict[tpose]
inv_T1 = transform44(timestamp_pose)
trgb, tdepth, tpose = self.matches_depth_pose[frame2]
timestamp_pose = [tpose] + self.groundtruth_dict[tpose]
T2 = transform44(timestamp_pose)
T2 = np.linalg.inv(T2) # convert to world to cam
T = T2.dot(inv_T1)
R12 = T[:3, :3]
t12 = T[:3, 3]
rotation = Quaternion(R12).toAngleAxis()
rotation = rotation[0] * np.array(rotation[1])
return {
'rotation': rotation[np.newaxis, :],
'translation': t12[np.newaxis, :],
}
def rotation_matrix_to_quaternion(R):
"""Converts the rotation matrix to the quaternion representation.
R: minieigen.Matrix3 or np.array
returns the unit quaternion
"""
return Quaternion(R)
def rotation_matrix_to_angleaxis(R):
"""Converts the rotation matrix to an angle axis vector with the angle
encoded as the magnitude.
R: minieigen.Matrix3 or np.array
returns an angle axis vector as np.array
"""
angle, axis = Quaternion(R).toAngleAxis()
aa = angle * axis
return np.array(aa)
def angleaxis_to_quaternion(aa, epsilon=1e-6):
"""Converts the angle axis vector with angle encoded as magnitude to
the quaternion representation.
aa: minieigen.Vector3
axis angle with angle as vector magnitude
epsilon: minimum angle in rad
If the angle is smaller than epsilon
then 0,(1,0,0) will be returned
returns the unit quaternion
"""
angle, axis = angleaxis_to_angle_axis(aa, epsilon)
return Quaternion(angle, axis)
def get_dict(self,
frame,
normalized_intrinsics=None,
width=128,
height=96):
"""Returns image, depth and pose as a dict of numpy arrays
The depth is the inverse depth.
frame: int
The rgb frame number
normalized_intrinsics: np.array
normalized intrinsics of the camera
width: int
image width. default is 128
height: int
image height. default is 96
"""
view = self.get_view(frame,
normalized_intrinsics=normalized_intrinsics,
width=width,
height=height,
depth=True)
img_arr = np.array(view.image).transpose([2, 0, 1]).astype(
np.float32) / 255 - 0.5
rotation = Quaternion(view.R).toAngleAxis()
rotation = rotation[0] * np.array(rotation[1])
result = {
'image':
img_arr[np.newaxis, :, :, :],
'depth':
None,
'rotation':
rotation[np.newaxis, :],
'translation':
view.t[np.newaxis, :],
'pose':
Pose(R=Matrix3(angleaxis_to_rotation_matrix(rotation)),
t=Vector3(view.t))
}
if view.depth is not None:
result['depth'] = (1 / view.depth)[np.newaxis, np.newaxis, :, :]
return result
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 get_relative_motion(self, frame1, frame2):
"""Returns the relative transformation from frame1 to frame2
frame1: int
Frame number 1
frame2: int
Frame number 2
"""
if self.groundtruth_dict is None:
return None
# get associated synced timestamps for RGB-Depth-Pose measurements
# frame 1:
timestamp_sync = self.matches_depth_pose[frame1]
tpose = timestamp_sync['timestamp_pose']
pose_tuple = [tpose] + self.groundtruth_dict[tpose]
inv_T1 = transform44(pose_tuple)
# frame 2:
timestamp_sync = self.matches_depth_pose[frame2]
tpose = timestamp_sync['timestamp_pose']
pose_tuple = [tpose] + self.groundtruth_dict[tpose]
T2 = transform44(pose_tuple)
T2 = np.linalg.inv(T2) # convert to world to cam2
# compute relative motion
T = T2.dot(inv_T1)
R12 = T[:3, :3]
t12 = T[:3, 3]
rotation = Quaternion(R12).toAngleAxis()
rotation = rotation[0] * np.array(rotation[1])
return {
'rotation': rotation[np.newaxis, :],
'translation': t12[np.newaxis, :],
}
def euler2axangle(eangle):
vec, theta = transforms3d.euler.euler2axangle(eangle[0], eangle[1],
eangle[2])
return Quaternion(theta, vec)
def quaternion_to_rotation_matrix(q_np):
q_mini = Quaternion(*(np.roll(q_np, 1)))
R = q_mini.toRotationMatrix()
return np.asarray(R, dtype=np.float32)
class SingleCamTracker:
# TODO: require_depth, require_pose
def __init__(self,
config,
tracking_module_path,
checkpoint,
require_depth=False,
require_pose=False):
"""
Creates an object for a single camera tracking instance. It wraps around the DeepTAM Sequence and Tracker
classes. This is done to make it easier to extend the setup for a multi-camera tracking class.
:param config: (dict) comprising of the camera settings
:param tracking_module_path: (str) path to file containing the Tracker network class
:param checkpoint: (str) path to the pre-trained network weights
:param require_depth:
:param require_pose:
"""
assert isinstance(config, dict)
self._startup = False
self.name = config['cam_name']
# base to camera transformation
self.tf_t_BC = Vector3(config['base_to_cam_pose']['translation']['x'],
config['base_to_cam_pose']['translation']['y'],
config['base_to_cam_pose']['translation']['z'])
self.tf_q_BC = Quaternion(
config['base_to_cam_pose']['orientation']['w'],
config['base_to_cam_pose']['orientation']['x'],
config['base_to_cam_pose']['orientation']['y'],
config['base_to_cam_pose']['orientation']['z'])
self.tf_R_BC = self.tf_q_BC.toRotationMatrix()
self.sequence = RGBDSequence(
config['cam_dir'],
rgb_parameters=config['rgb_parameters'],
depth_parameters=config['depth_parameters'],
time_syncing_parameters=config['time_syncing_parameters'],
cam_name=config['cam_name'],
require_depth=require_depth,
require_pose=require_pose)
self.intrinsics = self.sequence.get_original_normalized_intrinsics()
self.tracker = Tracker(
tracking_module_path,
checkpoint,
self.intrinsics,
tracking_parameters=config['tracking_parameters'])
def __del__(self):
self.delete_tracker()
def startup(self):
"""
Clears the tracker and sets the initial pose of the camera
:return:
"""
frame = self.sequence.get_dict(0, self.intrinsics,
self.tracker.image_width,
self.tracker.image_height)
pose0_gt = frame['pose']
self.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?
self.tracker.set_init_pose(pose0_gt)
self._startup = True
def delete_tracker(self):
del self.tracker
def update(self, frame_idx):
"""
Performs the tracking for an input image marked by the frame_idx
:param frame_idx: Frame number in the data to perform tracking on
:return:
(dict) frame: contains the RGB image, depth image, and groundtruth pose for that frame index
(dict) result: contains the pose, warped image and keyframe information
(float) timestamp: timestamp corresponding to that frame index
(list) poses: list of all the poses computed so far
"""
if not self._startup:
raise Exception(
PRINT_PREFIX +
'Tracker \"%s\"has not been initialized. Please call startup() first.'
% self.name)
frame = self.sequence.get_dict(frame_idx, self.intrinsics,
self.tracker.image_width,
self.tracker.image_height)
result = self.tracker.feed_frame(frame['image'], frame['depth'])
return frame, result, self.sequence.get_timestamp(
frame_idx), self.tracker.poses
def get_sequence_length(self):
return self.sequence.get_sequence_length()
def get_transformed_pose(self):
"""
Transforms the poses estimated from the tracker to a base frame
:return:
"""
## transformation of poses
tf_poses = []
for pose in self.tracker.poses:
t = pose.t - self.tf_t_BC
R = pose.R * self.tf_R_BC.inverse()
tf_pose = Pose(R=R, t=t)
tf_poses.append(tf_pose)
return tf_poses
