basalt/data/monado/wmr-tools/wmr2bslt_calib.py

#!/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()