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 _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'])
def get_calib(self):
"""Read velodyne [x,y,z,reflectance] scan at the specified index."""
return utils.read_calib_file(self.calib_file)
