calculate_matrix_diff_to_golden.py

import json
import os

import cv2
import numpy as np


def calculate_matrix_diff_to_golden(scene_sn, rotation_matrix_scene):
    golden_scene_sn = "p69tb"
    if scene_sn in ("6tgbx", "rylzd", "vtukq"):
        golden_frame_sn = "r6q7m"
    elif scene_sn in (
        "46n9z",
        "4827q",
        "gvrc8",
        "huwbp",
        "j43pn",
        "j9782",
        "jwrec",
        "krxdw",
        "rknfh",
        "wmlar",
    ):
        golden_frame_sn = "9dfjn"
    else:
        raise NotImplementedError(
            f"The scene camera '{scene_sn}' was not used in data collection"
        )
    assert rotation_matrix_scene.shape == (3, 3)

    root = "/cluster/users/Ching/datasets/PI_extrinsic/all_dc_scene_cameras"
    path = os.path.join(root, f"{golden_scene_sn}-{golden_frame_sn}.json")
    with open(path, "r") as file:
        extrinsic_golden = json.load(file)
    rotation_golden_inv = -np.array(extrinsic_golden)[0:3]
    rotation_matrix_golden_inv = cv2.Rodrigues(rotation_golden_inv)[0]

    matrix_diff_to_golden = rotation_matrix_golden_inv @ rotation_matrix_scene
    return matrix_diff_to_golden