#!/usr/bin/env python3 # Run with ./wmr2bslt_calib.py your_wmrcalib.json > your_calib.json import json import argparse import numpy as np from numpy.linalg import inv from math import sqrt def get(j, name): assert name in ["HT0", "HT1", "Gyro", "Accelerometer"] is_imu = name in ["Gyro", "Accelerometer"] calib = j["CalibrationInformation"] sensors = calib["InertialSensors" if is_imu else "Cameras"] name_key = "SensorType" if is_imu else "Location" sensor = next(filter(lambda s: s[name_key].endswith(name), sensors)) return sensor def rt2mat(rt): R33 = np.array(rt["Rotation"]).reshape(3, 3) t31 = np.array(rt["Translation"]).reshape(3, 1) T34 = np.hstack((R33, t31)) T44 = np.vstack((T34, [0, 0, 0, 1])) return T44 def rmat2quat(r): w = sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2]) / 2 w4 = 4 * w x = (r[2, 1] - r[1, 2]) / w4 y = (r[0, 2] - r[2, 0]) / w4 z = (r[1, 0] - r[0, 1]) / w4 return np.array([x, y, z, w]) def extrinsics(j, cam): # NOTE: The `Rt` field seems to be a transform from the sensor to HT0 (i.e., # from HT0 space to sensor space). For basalt we need the transforms # expressed w.r.t IMU origin. # NOTE: The gyro and magnetometer translations are 0, probably because an # HMD is a rigid body. Therefore the accelerometer is considered as the IMU # origin. imu = get(j, "Accelerometer") T_i_c0 = rt2mat(imu["Rt"]) T = None if cam == "HT0": T = T_i_c0 elif cam == "HT1": cam1 = get(j, "HT1") T_c1_c0 = rt2mat(cam1["Rt"]) T_c0_c1 = inv(T_c1_c0) T_i_c1 = T_i_c0 @ T_c0_c1 T = T_i_c1 else: assert False q = rmat2quat(T[0:3, 0:3]) p = T[0:3, 3] return { "px": p[0], "py": p[1], "pz": p[2], "qx": q[0], "qy": q[1], "qz": q[2], "qw": q[3], } def resolution(j, cam): camera = get(j, cam) width = camera["SensorWidth"] height = camera["SensorHeight"] return [width, height] def intrinsics(j, cam): # https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/2feb3425259bf803749065bb6d628c6c180f8e77/include/k4a/k4atypes.h#L1024-L1046 camera = get(j, cam) model_params = camera["Intrinsics"]["ModelParameters"] assert ( camera["Intrinsics"]["ModelType"] == "CALIBRATION_LensDistortionModelRational6KT" ) width = camera["SensorWidth"] height = camera["SensorHeight"] return { "camera_type": "pinhole-radtan8", "intrinsics": { "fx": model_params[2] * width, "fy": model_params[3] * height, "cx": model_params[0] * width, "cy": model_params[1] * height, "k1": model_params[4], "k2": model_params[5], "p1": model_params[13], "p2": model_params[12], "k3": model_params[6], "k4": model_params[7], "k5": model_params[8], "k6": model_params[9], }, } def calib_accel_bias(j): # https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/2feb3425259bf803749065bb6d628c6c180f8e77/include/k4ainternal/calibration.h#L48-L77 # https://vladyslavusenko.gitlab.io/basalt-headers/classbasalt_1_1CalibAccelBias.html#details # https://gitlab.com/VladyslavUsenko/basalt-headers/-/issues/8 accel = get(j, "Accelerometer") bias = accel["BiasTemperatureModel"] align = accel["MixingMatrixTemperatureModel"] return [ -bias[0 * 4], -bias[1 * 4], -bias[2 * 4], align[0 * 4] - 1, # [0, 0] align[3 * 4], # [1, 0] align[6 * 4], # [2, 0] align[4 * 4] - 1, # [1, 1] align[7 * 4], # [2, 1] align[8 * 4] - 1, # [2, 2] ] def calib_gyro_bias(j): # https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/2feb3425259bf803749065bb6d628c6c180f8e77/include/k4ainternal/calibration.h#L48-L77 # https://vladyslavusenko.gitlab.io/basalt-headers/classbasalt_1_1CalibGyroBias.html#details gyro = get(j, "Gyro") bias = gyro["BiasTemperatureModel"] align = gyro["MixingMatrixTemperatureModel"] return [ -bias[0 * 4], -bias[1 * 4], -bias[2 * 4], align[0 * 4] - 1, # [0, 0] align[3 * 4], # [1, 0] align[6 * 4], # [2, 0] align[1 * 4], # [0, 1] align[4 * 4] - 1, # [1, 1] align[7 * 4], # [2, 1] align[2 * 4], # [0, 2] align[5 * 4], # [1, 2] align[8 * 4] - 1, # [2, 2] ] def noise_std(j, name): imu = get(j, name) return imu["Noise"][0:3] def bias_std(j, name): imu = get(j, name) return list(map(sqrt, imu["BiasUncertainty"])) def main(): parser = argparse.ArgumentParser() parser.add_argument("wmr_json_file", help="Input WMR json calibration file") args = parser.parse_args() in_fn = args.wmr_json_file with open(in_fn) as f: j = json.load(f) # We get 250 packets with 4 samples each per second, totalling 1000 samples per second. # But in monado we just average those 4 samples to reduce the noise. So we have 250hz. IMU_UPDATE_RATE = 250 # This is a very rough offset in pixels between the two cameras. I manually # measured it for some particular point in some particular pair of images # for my Odyssey+. In reality this offset changes based on distance to the # point, nonetheless it helps to get some features tracked in the right # camera. VIEW_OFFSET = 247 out_calib = { "value0": { "T_imu_cam": [extrinsics(j, "HT0"), extrinsics(j, "HT1")], "intrinsics": [intrinsics(j, "HT0"), intrinsics(j, "HT1")], "resolution": [resolution(j, "HT0"), resolution(j, "HT1")], "calib_accel_bias": calib_accel_bias(j), "calib_gyro_bias": calib_gyro_bias(j), "imu_update_rate": IMU_UPDATE_RATE, "accel_noise_std": noise_std(j, "Accelerometer"), "gyro_noise_std": noise_std(j, "Gyro"), "accel_bias_std": bias_std(j, "Accelerometer"), "gyro_bias_std": bias_std(j, "Gyro"), "cam_time_offset_ns": 0, "view_offset": VIEW_OFFSET, "vignette": [], } } print(json.dumps(out_calib, indent=4)) if __name__ == "__main__": main()