def load_calib(self): """Load and compute intrinsic and extrinsic calibration parameters.""" # We'll build the calibration parameters as a dictionary, then # convert it to a namedtuple to prevent it from being modified later data = {} # Load the calibration file calib_filepath = os.path.join(self.calib_path, "{}.txt".format(self.sequence)) filedata = utils.read_calib_file(calib_filepath) P2 = utils.pad3x4_to_4x4(np.reshape(filedata["P2"], (3, 4))) R_rect = np.reshape(filedata["R_rect"], (3, 3)) R_rect = utils.pad3x4_to_4x4( np.hstack((R_rect, np.array([0.0, 0.0, 0.0])[:, None]))) data["P2"] = P2 data["R_rect"] = R_rect # transform from left camera coordinates into left image coords data["T_cam_imgL"] = np.dot(P2, R_rect) data["T_imgL_cam"] = np.linalg.inv(data["T_cam_imgL"]) T_velo_cam = np.reshape(filedata["Tr_velo_cam"], (3, 4)) T_imu_velo = np.reshape(filedata["Tr_imu_velo"], (3, 4)) data["T_velo_cam"] = utils.pad3x4_to_4x4(T_velo_cam) data["T_imu_velo"] = utils.pad3x4_to_4x4(T_imu_velo) # convert GPS/IMU coords into camera via velo data["T_imu_cam"] = data["T_velo_cam"].dot(data["T_imu_velo"]) data["T_cam_imu"] = np.linalg.inv(data["T_imu_cam"]) self.calib = namedtuple('CalibData', data.keys())(*data.values())
def _load_calib_cam_to_cam(self, velo_to_cam_file, cam_to_cam_file): # We'll return the camera calibration as a dictionary data = {} # Load the rigid transformation from velodyne coordinates # to unrectified cam0 coordinates T_cam0unrect_velo = self._load_calib_rigid(velo_to_cam_file) data['T_cam0unrect_velo'] = T_cam0unrect_velo # Load and parse the cam-to-cam calibration data cam_to_cam_filepath = os.path.join(self.calib_path, cam_to_cam_file) filedata = utils.read_calib_file(cam_to_cam_filepath) # Create 3x4 projection matrices P_rect_00 = np.reshape(filedata['P_rect_00'], (3, 4)) P_rect_10 = np.reshape(filedata['P_rect_01'], (3, 4)) P_rect_20 = np.reshape(filedata['P_rect_02'], (3, 4)) P_rect_30 = np.reshape(filedata['P_rect_03'], (3, 4)) # Create 4x4 matrix from the rectifying rotation matrix R_rect_00 = np.eye(4) R_rect_00[0:3, 0:3] = np.reshape(filedata['R_rect_00'], (3, 3)) # Compute the rectified extrinsics from cam0 to camN T0 = np.eye(4) T0[0, 3] = P_rect_00[0, 3] / P_rect_00[0, 0] T1 = np.eye(4) T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0] T2 = np.eye(4) T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0] T3 = np.eye(4) T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0] # Compute the velodyne to rectified camera coordinate transforms data['T_cam0_velo'] = T0.dot(R_rect_00.dot(T_cam0unrect_velo)) data['T_cam1_velo'] = T1.dot(R_rect_00.dot(T_cam0unrect_velo)) data['T_cam2_velo'] = T2.dot(R_rect_00.dot(T_cam0unrect_velo)) data['T_cam3_velo'] = T3.dot(R_rect_00.dot(T_cam0unrect_velo)) # Compute the camera intrinsics data['K_cam0'] = P_rect_00[0:3, 0:3] data['K_cam1'] = P_rect_10[0:3, 0:3] data['K_cam2'] = P_rect_20[0:3, 0:3] data['K_cam3'] = P_rect_30[0:3, 0:3] # Compute the stereo baselines in meters by projecting the origin of # each camera frame into the velodyne frame and computing the distances # between them p_cam = np.array([0, 0, 0, 1]) p_velo0 = np.linalg.inv(data['T_cam0_velo']).dot(p_cam) p_velo1 = np.linalg.inv(data['T_cam1_velo']).dot(p_cam) p_velo2 = np.linalg.inv(data['T_cam2_velo']).dot(p_cam) p_velo3 = np.linalg.inv(data['T_cam3_velo']).dot(p_cam) data['b_gray'] = np.linalg.norm(p_velo1 - p_velo0) # gray baseline data['b_rgb'] = np.linalg.norm(p_velo3 - p_velo2) # rgb baseline return data
def _load_calib(self): """Load and compute intrinsic and extrinsic calibration parameters.""" # We'll build the calibration parameters as a dictionary, then # convert it to a namedtuple to prevent it from being modified later data = {} # Load the calibration file calib_filepath = os.path.join(self.sequence_path, 'calib.txt') filedata = utils.read_calib_file(calib_filepath) # Create 3x4 projection matrices P_rect_00 = np.reshape(filedata['P0'], (3, 4)) P_rect_10 = np.reshape(filedata['P1'], (3, 4)) P_rect_20 = np.reshape(filedata['P2'], (3, 4)) P_rect_30 = np.reshape(filedata['P3'], (3, 4)) data['P_rect_00'] = P_rect_00 data['P_rect_10'] = P_rect_10 data['P_rect_20'] = P_rect_20 data['P_rect_30'] = P_rect_30 # Compute the rectified extrinsics from cam0 to camN T1 = np.eye(4) T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0] T2 = np.eye(4) T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0] T3 = np.eye(4) T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0] if self.lidar: # Compute the velodyne to rectified camera coordinate transforms data['T_cam0_velo'] = np.reshape(filedata['Tr'], (3, 4)) data['T_cam0_velo'] = np.vstack( [data['T_cam0_velo'], [0, 0, 0, 1]]) data['T_cam1_velo'] = T1.dot(data['T_cam0_velo']) data['T_cam2_velo'] = T2.dot(data['T_cam0_velo']) data['T_cam3_velo'] = T3.dot(data['T_cam0_velo']) # Compute the camera intrinsics data['K_cam0'] = P_rect_00[0:3, 0:3] data['K_cam1'] = P_rect_10[0:3, 0:3] data['K_cam2'] = P_rect_20[0:3, 0:3] data['K_cam3'] = P_rect_30[0:3, 0:3] if self.lidar: # Compute the stereo baselines in meters by projecting the origin of # each camera frame into the velodyne frame and computing the distances # between them p_cam = np.array([0, 0, 0, 1]) p_velo0 = np.linalg.inv(data['T_cam0_velo']).dot(p_cam) p_velo1 = np.linalg.inv(data['T_cam1_velo']).dot(p_cam) p_velo2 = np.linalg.inv(data['T_cam2_velo']).dot(p_cam) p_velo3 = np.linalg.inv(data['T_cam3_velo']).dot(p_cam) data['b_gray'] = np.linalg.norm(p_velo1 - p_velo0) # gray baseline data['b_rgb'] = np.linalg.norm(p_velo3 - p_velo2) # rgb baseline self.calib = namedtuple('CalibData', data.keys())(*data.values())
def _load_calib_cam_to_cam(self, velo_to_cam_file, cam_to_cam_file): # We'll return the camera calibration as a dictionary data = {} # Load the rigid transformation from velodyne coordinates # to unrectified cam0 coordinates T_cam0unrect_velo = self._load_calib_rigid(velo_to_cam_file) # Load and parse the cam-to-cam calibration data cam_to_cam_filepath = os.path.join(self.calib_path, cam_to_cam_file) filedata = utils.read_calib_file(cam_to_cam_filepath) # Create 3x4 projection matrices P_rect_00 = np.reshape(filedata['P_rect_00'], (3, 4)) P_rect_10 = np.reshape(filedata['P_rect_01'], (3, 4)) P_rect_20 = np.reshape(filedata['P_rect_02'], (3, 4)) P_rect_30 = np.reshape(filedata['P_rect_03'], (3, 4)) # Create 4x4 matrix from the rectifying rotation matrix R_rect_00 = np.eye(4) R_rect_00[0:3, 0:3] = np.reshape(filedata['R_rect_00'], (3, 3)) # Compute the rectified extrinsics from cam0 to camN T0 = np.eye(4) T0[0, 3] = P_rect_00[0, 3] / P_rect_00[0, 0] T1 = np.eye(4) T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0] T2 = np.eye(4) T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0] T3 = np.eye(4) T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0] # Compute the velodyne to rectified camera coordinate transforms data['T_cam0_velo'] = T0.dot(R_rect_00.dot(T_cam0unrect_velo)) data['T_cam1_velo'] = T1.dot(R_rect_00.dot(T_cam0unrect_velo)) data['T_cam2_velo'] = T2.dot(R_rect_00.dot(T_cam0unrect_velo)) data['T_cam3_velo'] = T3.dot(R_rect_00.dot(T_cam0unrect_velo)) # Compute the camera intrinsics data['K_cam0'] = P_rect_00[0:3, 0:3] data['K_cam1'] = P_rect_10[0:3, 0:3] data['K_cam2'] = P_rect_20[0:3, 0:3] data['K_cam3'] = P_rect_30[0:3, 0:3] # Compute the stereo baselines in meters by projecting the origin of # each camera frame into the velodyne frame and computing the distances # between them p_cam = np.array([0, 0, 0, 1]) p_velo0 = np.linalg.inv(data['T_cam0_velo']).dot(p_cam) p_velo1 = np.linalg.inv(data['T_cam1_velo']).dot(p_cam) p_velo2 = np.linalg.inv(data['T_cam2_velo']).dot(p_cam) p_velo3 = np.linalg.inv(data['T_cam3_velo']).dot(p_cam) data['b_gray'] = np.linalg.norm(p_velo1 - p_velo0) # gray baseline data['b_rgb'] = np.linalg.norm(p_velo3 - p_velo2) # rgb baseline return data
def _load_calib(self): """Load and compute intrinsic and extrinsic calibration parameters.""" # We'll build the calibration parameters as a dictionary, then # convert it to a namedtuple to prevent it from being modified later data = {} # Load the calibration file calib_filepath = os.path.join(self.sequence_path, 'calib.txt') filedata = utils.read_calib_file(calib_filepath) # Create 3x4 projection matrices P_rect_00 = np.reshape(filedata['P0'], (3, 4)) P_rect_10 = np.reshape(filedata['P1'], (3, 4)) P_rect_20 = np.reshape(filedata['P2'], (3, 4)) P_rect_30 = np.reshape(filedata['P3'], (3, 4)) data['P_rect_00'] = P_rect_00 data['P_rect_10'] = P_rect_10 data['P_rect_20'] = P_rect_20 data['P_rect_30'] = P_rect_30 # Compute the rectified extrinsics from cam0 to camN T1 = np.eye(4) T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0] T2 = np.eye(4) T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0] T3 = np.eye(4) T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0] # Compute the velodyne to rectified camera coordinate transforms data['T_cam0_velo'] = np.reshape(filedata['Tr'], (3, 4)) data['T_cam0_velo'] = np.vstack([data['T_cam0_velo'], [0, 0, 0, 1]]) data['T_cam1_velo'] = T1.dot(data['T_cam0_velo']) data['T_cam2_velo'] = T2.dot(data['T_cam0_velo']) data['T_cam3_velo'] = T3.dot(data['T_cam0_velo']) # Compute the camera intrinsics data['K_cam0'] = P_rect_00[0:3, 0:3] data['K_cam1'] = P_rect_10[0:3, 0:3] data['K_cam2'] = P_rect_20[0:3, 0:3] data['K_cam3'] = P_rect_30[0:3, 0:3] # Compute the stereo baselines in meters by projecting the origin of # each camera frame into the velodyne frame and computing the distances # between them p_cam = np.array([0, 0, 0, 1]) p_velo0 = np.linalg.inv(data['T_cam0_velo']).dot(p_cam) p_velo1 = np.linalg.inv(data['T_cam1_velo']).dot(p_cam) p_velo2 = np.linalg.inv(data['T_cam2_velo']).dot(p_cam) p_velo3 = np.linalg.inv(data['T_cam3_velo']).dot(p_cam) data['b_gray'] = np.linalg.norm(p_velo1 - p_velo0) # gray baseline data['b_rgb'] = np.linalg.norm(p_velo3 - p_velo2) # rgb baseline self.calib = namedtuple('CalibData', data.keys())(*data.values())
def _load_calib_rigid(self, filename): """Read a rigid transform calibration file as a numpy.array.""" filepath = os.path.join(self.calib_path, filename) data = utils.read_calib_file(filepath) return utils.transform_from_rot_trans(data['R'], data['T'])
poses = [] for pose in kitti.get_oxts_packets_and_poses(pose_files): poses.append(pose[1]) # print len(poses) img_files = [] if os.path.isdir(img_path): for f in os.listdir(img_path): if os.path.isdir(f): continue else: img_files.append(f) img_files.sort() if calib_imu_to_velo_file: calib = kitti.read_calib_file(calib_imu_to_velo_file) # proj_velo = proj_to_velo(calib)[:, :3] # euler_static = transform.rotationMatrixToEulerAngles(np.array(calib['R']).reshape(-1, 3)) # translation_static = calib['T'] imu2vel = np.zeros((4, 4)) imu2vel[:3, :3] = np.array(calib['R']).reshape(-1, 3) imu2vel[:3, 3] = calib['T'] imu2vel[3, 3] = 1. vel2imu = trans.inverse_matrix(imu2vel) translation_static = trans.translation_from_matrix(vel2imu) quaternion_static = trans.quaternion_from_matrix(vel2imu) # print translation_static # print quaternion_static
pose_files.append(oxts_path + "/" + f) pose_files.sort() poses = [] for pose in kitti.get_oxts_packets_and_poses(pose_files): poses.append(pose[1]) img_files = [] if os.path.isdir(img_path): for f in os.listdir(img_path): if os.path.isdir(f): continue else: img_files.append(f) img_files.sort() calib_imu2velo = kitti.read_calib_file(calib_imu_to_velo_file) imu2vel = np.zeros((4, 4)) imu2vel[:3, :3] = np.array(calib_imu2velo['R']).reshape(-1, 3) imu2vel[:3, 3] = calib_imu2velo['T'] imu2vel[3, 3] = 1. vel2imu = trans.inverse_matrix(imu2vel) translation_static = trans.translation_from_matrix(vel2imu) quaternion_static = trans.quaternion_from_matrix(vel2imu) calib_cam2cam = kitti.read_calib_file(calib_cam_to_cam_file) # To project a 3D point x in reference camera coordinates to a point y on the i'th image plane, # the rectifying rotation matrix of the reference camera: R_rect_00 must be considered as well. R_rect_00 = np.zeros((4, 4)) R_rect_00[:3, :3] = np.array(calib_cam2cam['R_rect_00']).reshape(-1, 3) R_rect_00[3, 3] = 1.
def readTfLidarToCamera0(self, img_path, sequence): chunk = read_calib_file(img_path + "sequences/" + sequence + "/calib.txt") self.Tr_lidar = chunk['Tr'].reshape(3, 4) self.P_L2C = np.matmul(self.K, self.Tr_lidar)
def get_calib(self): """Read velodyne [x,y,z,reflectance] scan at the specified index.""" return utils.read_calib_file(self.calib_file)