107 lines
3.4 KiB
Python
Executable File
107 lines
3.4 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
#
|
|
# BSD 3-Clause License
|
|
#
|
|
# This file is part of the Basalt project.
|
|
# https://gitlab.com/VladyslavUsenko/basalt.git
|
|
#
|
|
# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
|
|
# All rights reserved.
|
|
#
|
|
|
|
|
|
import argparse
|
|
import json
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation
|
|
|
|
|
|
def print_abs_rel(info, v_0, v_1):
|
|
diff = np.abs(np.linalg.norm(v_0 - v_1))
|
|
out = f'{info}:\t{diff:.5f}'
|
|
|
|
if diff < 10e-7:
|
|
out += ' (0.0%)'
|
|
else:
|
|
out += f' ({diff / (np.abs(np.linalg.norm(v_0)) * 100.0):.7f}%)'
|
|
|
|
print(out)
|
|
|
|
|
|
def main(calib_path_1, calib_path_2):
|
|
with open(calib_path_1, 'r') as c_1, open(calib_path_2, 'r') as c_2:
|
|
calib0 = json.load(c_1)
|
|
calib1 = json.load(c_2)
|
|
|
|
for i, (t_imu_cam_0, t_imu_cam_1) in enumerate(
|
|
zip(calib0['value0']['T_imu_cam'], calib1['value0']['T_imu_cam'])):
|
|
print(f'\nCamera {i} transformation differences')
|
|
t_0 = np.array(list(t_imu_cam_0.values())[0:2])
|
|
t_1 = np.array(list(t_imu_cam_1.values())[0:2])
|
|
r_0 = Rotation(list(t_imu_cam_0.values())[3:7])
|
|
r_1 = Rotation(list(t_imu_cam_1.values())[3:7])
|
|
|
|
print_abs_rel(f'Transformation', t_0, t_1)
|
|
print_abs_rel(f'Rotation', r_0.as_rotvec(), r_1.as_rotvec())
|
|
|
|
for i, (intrinsics0, intrinsics1) in enumerate(
|
|
zip(calib0['value0']['intrinsics'], calib1['value0']['intrinsics'])):
|
|
print(f'\nCamera {i} intrinsics differences')
|
|
|
|
for (
|
|
k_0, v_0), (_, v_1) in zip(
|
|
intrinsics0['intrinsics'].items(), intrinsics1['intrinsics'].items()):
|
|
print_abs_rel(f'Difference for {k_0}', v_0, v_1)
|
|
|
|
print_abs_rel('\nAccel Bias Difference',
|
|
np.array(calib0['value0']['calib_accel_bias'][0:2]),
|
|
np.array(calib1['value0']['calib_accel_bias'][0:2]))
|
|
|
|
print_abs_rel('Accel Scale Difference',
|
|
np.array(calib0['value0']['calib_accel_bias'][3:9]),
|
|
np.array(calib1['value0']['calib_accel_bias'][3:9]))
|
|
|
|
print_abs_rel('Gyro Bias Difference',
|
|
np.array(calib0['value0']['calib_gyro_bias'][0:2]),
|
|
np.array(calib1['value0']['calib_gyro_bias'][0:2]))
|
|
|
|
print_abs_rel('Gyro Scale Difference',
|
|
np.array(calib0['value0']['calib_gyro_bias'][3:12]),
|
|
np.array(calib1['value0']['calib_gyro_bias'][3:12]))
|
|
|
|
print_abs_rel(
|
|
'\nAccel Noise Std Difference',
|
|
calib0['value0']['accel_noise_std'],
|
|
calib1['value0']['accel_noise_std'])
|
|
print_abs_rel(
|
|
'Gyro Noise Std Difference',
|
|
calib0['value0']['gyro_noise_std'],
|
|
calib1['value0']['gyro_noise_std'])
|
|
print_abs_rel(
|
|
'Accel Bias Std Difference',
|
|
calib0['value0']['accel_bias_std'],
|
|
calib1['value0']['accel_bias_std'])
|
|
print_abs_rel(
|
|
'Gyro Bias Std Difference',
|
|
calib0['value0']['gyro_bias_std'],
|
|
calib1['value0']['gyro_bias_std'])
|
|
|
|
print_abs_rel(
|
|
'\nCam Time Offset Difference',
|
|
calib0['value0']['cam_time_offset_ns'],
|
|
calib0['value0']['cam_time_offset_ns'])
|
|
|
|
|
|
def create_parser():
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument('calib_path_1')
|
|
parser.add_argument('calib_path_2')
|
|
|
|
return parser
|
|
|
|
|
|
if __name__ == '__main__':
|
|
args = create_parser().parse_args()
|
|
|
|
main(args.calib_path_1, args.calib_path_2)
|