def saveImuSetParametersYaml(self, resultFile):
imuSetConfig = kc.ImuSetParameters(resultFile, self.reference_sensor,
True)
for imu in self.ImuList:
imuConfig = imu.getImuConfig()
imuSetConfig.addImuParameters(imu_parameters=imuConfig)
imuSetConfig.writeYaml(resultFile)
def main():
parsed_args = parse_arguments()
if parsed_args.cam_ground_truth and parsed_args.cam_file_name_prefix:
cam_chain_ext_gt = kc.CameraChainParameters(
parsed_args.cam_ground_truth)
ext_gt_list = []
num_cam = cam_chain_ext_gt.numCameras()
for camNr in range(1, num_cam):
ext_gt_list.append(
cam_chain_ext_gt.getExtrinsicsReferenceToCam(camNr))
err_vec_list_list = [[] for _ in range(num_cam - 1)]
for file_name in glob.glob(parsed_args.cam_file_name_prefix):
cam_chain_ext = kc.CameraChainParameters(
file_name, parsed_args.reference_sensor)
for camNr in range(1, num_cam):
ext = cam_chain_ext.getExtrinsicsReferenceToCam(camNr)
err_vec = calcErrorGTAndEstimation(ext_gt_list[camNr - 1], ext)
err_vec_list_list[camNr - 1].append(err_vec)
for idx, err_vec_list in enumerate(err_vec_list_list):
err_mat = np.array(err_vec_list)
err_mean = np.mean(err_mat, axis=0)
err_variance = np.var(err_mat, axis=0)
print("cam {} extrinsic calibration error".format(idx + 1))
print("mean of error: ", err_mean)
print("variance of error: ", err_variance)
if parsed_args.lidar_ground_truth and parsed_args.lidar_file_name_prefix:
lidar_list_ext_gt = kc.LiDARListParameters(
parsed_args.lidar_ground_truth, parsed_args.reference_sensor)
ext_gt_list = []
num_lidar = lidar_list_ext_gt.numLiDARs()
for idx in range(0, num_lidar):
lidar_parameter = lidar_list_ext_gt.getLiDARParameters(idx)
ext_gt_list.append(lidar_parameter.getExtrinsicsReferenceToHere())
err_vec_list_list = [[] for _ in range(num_lidar)]
for file_name in glob.glob(parsed_args.lidar_file_name_prefix):
lidar_list_ext = kc.LiDARListParameters(
file_name, parsed_args.reference_sensor)
for idx in range(num_lidar):
lidar_parameter = lidar_list_ext.getLiDARParameters(idx)
ext = lidar_parameter.getExtrinsicsReferenceToHere()
err_vec = calcErrorGTAndEstimation(ext_gt_list[idx], ext)
err_vec_list_list[idx].append(err_vec)
for idx, err_vec_list in enumerate(err_vec_list_list):
err_mat = np.array(err_vec_list)
err_mean = np.mean(err_mat, axis=0)
err_variance = np.var(err_mat, axis=0)
print("LiDAR {} extrinsic calibration error".format(idx))
print("mean of error: ", err_mean)
print("variance of error: ", err_variance)
if parsed_args.imu_ground_truth and parsed_args.imu_file_name_prefix:
imu_list_ext_gt = kc.ImuSetParameters(parsed_args.imu_ground_truth,
parsed_args.reference_sensor)
ext_gt_list = []
num_imu = imu_list_ext_gt.numImus()
for idx in range(0, num_imu):
imu_parameter = imu_list_ext_gt.getImuParameters(idx)
ext_gt_list.append(imu_parameter.getExtrinsicsReferenceToHere())
err_vec_list_list = [[] for _ in range(num_imu)]
for file_name in glob.glob(parsed_args.imu_file_name_prefix):
imu_list_ext = kc.ImuSetParameters(file_name,
parsed_args.reference_sensor)
for idx in range(num_imu):
imu_parameter = imu_list_ext.getImuParameters(idx)
ext = imu_parameter.getExtrinsicsReferenceToHere()
err_vec = calcErrorGTAndEstimation(ext_gt_list[idx], ext)
err_vec_list_list[idx].append(err_vec)
for idx, err_vec_list in enumerate(err_vec_list_list):
err_mat = np.array(err_vec_list)
err_mean = np.mean(err_mat, axis=0)
err_variance = np.var(err_mat, axis=0)
print("IMU {} extrinsic calibration error".format(idx))
print("mean of error: ", err_mean)
print("variance of error: ", err_variance)