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