calculate camera position

calculate_camera_position.py

import cv2
import numpy as np
from scipy.optimize import least_squares

from head_pose_tracker.function import utils

np.set_printoptions(precision=4, suppress=True)


def get_camera_pose_gt(world, eye0, eye1):
    eye0_center = np.mean(eye0, axis=0)
    eye1_center = np.mean(eye1, axis=0)

    origin_translated = np.mean((eye0_center, eye1_center), axis=0)

    world_translated = world - origin_translated
    eye0_translated = eye0 - origin_translated
    eye1_translated = eye1 - origin_translated

    transform_matrix = np.eye(4, dtype=np.float64)
    transform_matrix[0:3, 0:3] = cv2.Rodrigues(np.array([0, np.pi, 0]))[0]

    world_transformed = transform_points(transform_matrix, world_translated)
    eye0_transformed = transform_points(transform_matrix, eye0_translated)
    eye1_transformed = transform_points(transform_matrix, eye1_translated)

    # world_transformed_center = np.mean(world_transformed, axis=0)
    # eye0_transformed_center = np.mean(eye0_transformed, axis=0)
    # eye1_transformed_center = np.mean(eye1_transformed, axis=0)
    # print(world_transformed_center, eye0_transformed_center, eye1_transformed_center)

    camera_pose_dict = {
        "world": calculate_camera_pose(world_transformed).tolist(),
        "eye0": calculate_camera_pose(eye0_transformed).tolist(),
        "eye1": calculate_camera_pose(eye1_transformed).tolist(),
    }
    camera_pose_array = np.array(list(camera_pose_dict.values()), dtype=np.float64)
    np.save(
        "/cluster/users/Ching/codebase/pi_extrinsics_measurer/camera_pose_gt",
        camera_pose_array,
    )
    return camera_pose_array


def transform_points(matrix, points):
    return cv2.convertPointsFromHomogeneous(
        np.matmul(
            matrix.copy(), cv2.convertPointsToHomogeneous(points.copy())[:, 0].T
        ).T
    )[:, 0]


def calculate_camera_pose(square):
    square_length = np.linalg.norm(
        square[[0, 1, 2, 3]] - square[[1, 2, 3, 0]], axis=1
    ).mean()

    rotation_matrix, translation, root_mean_squared_error = utils.svdt(
        A=get_camera_points_3d_origin() * square_length, B=square
    )
    rotation = cv2.Rodrigues(rotation_matrix)[0]
    return utils.merge_extrinsics(rotation, translation)


def get_camera_points_3d_origin():
    return np.array(
        [[-0.5, -0.5, 0], [0.5, -0.5, 0], [0.5, 0.5, 0], [-0.5, 0.5, 0]],
        dtype=np.float64,
    )


def fun(x):
    transformation_matrix = utils.convert_extrinsic_to_matrix(x)
    camera_position_detected_transformed = transform_points(
        transformation_matrix, camera_position_detected
    )

    return (camera_position_detected_transformed - camera_position_gt).ravel()


if __name__ == "__main__":
    world_raw = np.array(
        [
            [139.78915, 44.91046, 152.73462],  # 0
            [147.78915, 44.91046, 152.73462],  # 1
            [147.78915, 52.73564, 154.39790],  # 2
            [139.78915, 52.73564, 154.39790],  # 3
        ],
        dtype=np.float64,
    )
    eye0_raw = np.array(
        [
            [11.25893, 26.63306, 142.40508],  # 0
            [10.36142, 29.07311, 142.43022],  # 1
            [11.21869, 29.36330, 144.86761],  # 2
            [12.11620, 26.92325, 144.84245],  # 3
        ],
        dtype=np.float64,
    )
    eye1_raw = np.array(
        [
            [132.95219, 26.92325, 144.84245],  # 0
            [133.84970, 29.36330, 144.86761],  # 1
            [134.70697, 29.07311, 142.43022],  # 2
            [133.80946, 26.63306, 142.40508],  # 3
        ],
        dtype=np.float64,
    )

    camera_poses_gt = get_camera_pose_gt(world_raw, eye0_raw, eye1_raw)
    camera_position_gt = camera_poses_gt[:, 3:6]  # 3 x 3 matrix
    print(camera_position_gt)

    camera_position_detected = camera_position_gt.copy() + np.array([1000, 200, -500])
    initial_guess_array = np.array([0, 0, 0, 0, 0, 0], dtype=np.float64)

    result = least_squares(fun=fun, x0=initial_guess_array, diff_step=1e-3, verbose=2)
    transformation_vector = result.x
    # print(result.cost)
    print(transformation_vector)