This function calculate N undistorted points in ideal headset coordinate based on the given frame and world camera

calculate_points_2d_undist_ideal.py

import cv2
import numpy as np

import file_methods as fm


class Transformation:
    def __init__(self):
        self._all_extrinsic_eye1_world = fm.load_object(
            "/cluster/users/Ching/datasets/camera_extrinsics_measurement/all_extrinsic/"
            "all_extrinsic_eye1_world"
        )

        extrinsic_eye1_world_ideal = self._get_extrinsic("ideal", "ideal")
        self._extrinsic_world_eye1_ideal = self._inverse_extrinsic(
            extrinsic_eye1_world_ideal
        )

        self._scale = 40.0  # the length of a marker in the markers box is 40 mm

    def calculate_points_2d_undist_ideal(self, points_3d_world, frame_sn, world_sn):
        """
        This function calculate N undistorted points in ideal headset coordinate
        based on the given frame and world camera

        :param points_3d_world: detected 3d gaze target points (unit: mm),
        shape: (N x 3)
        :param frame_sn: serial number of the frame
        :param world_sn: serial number of the world camera
        :return: undistorted points in ideal coordinate, shape: (N x 2)

        Example:
        >>> points = [[0.0, 0.0, 1000.0], [-1000.0, 1000.0, 2000.0]]
        >>> calculate_points_2d_undist_ideal(points, "drvb2", "r6wqd")
        """

        points_3d_world = np.array(points_3d_world, dtype=np.float64) / self._scale

        points_3d_world_ideal = self._transform_points_3d_world_to_ideal(
            points_3d_world, frame_sn, world_sn
        )

        points_2d_undist_ideal = self._normalize_points(points_3d_world_ideal)
        return points_2d_undist_ideal

    def _transform_points_3d_world_to_ideal(self, points_3d_world, frame_sn, world_sn):
        extrinsic_eye1_world = self._get_extrinsic(frame_sn, world_sn)
        points_3d_eye1 = self._transform_points(points_3d_world, extrinsic_eye1_world)
        points_3d_world_ideal = self._transform_points(
            points_3d_eye1, self._extrinsic_world_eye1_ideal
        )
        return points_3d_world_ideal

    def _transform_points(self, points_3d_cam1, extrinsic_cam2_cam1):
        """
        Transform 3d points from cam1 coordinate to cam2 coordinate

        :param points_3d_cam1: 3d points in cam1 coordinate, shape: (N x 3)
        :param extrinsic_cam2_cam1: extrinsic of cam2 in cam1 coordinate, shape: (6,)
        :return: 3d points in cam2 coordinate, shape: (N x 3)
        """

        points_3d_cam1.shape = -1, 3
        points_3d_cam1_h = cv2.convertPointsToHomogeneous(points_3d_cam1).reshape(-1, 4)
        transform_matrix = self._convert_extrinsic_to_matrix(extrinsic_cam2_cam1)

        points_3d_cam2_h = np.matmul(transform_matrix, points_3d_cam1_h.T).T
        points_3d_cam2 = cv2.convertPointsFromHomogeneous(points_3d_cam2_h)
        points_3d_cam2.shape = -1, 3
        return points_3d_cam2

    def _inverse_extrinsic(self, extrinsic):
        rotation_ext, translation_ext = self._split_extrinsic(extrinsic)
        rotation_inv = -rotation_ext
        translation_inv = np.matmul(-cv2.Rodrigues(rotation_ext)[0].T, translation_ext)
        return self._merge_extrinsic(rotation_inv, translation_inv)

    def _convert_extrinsic_to_matrix(self, extrinsic):
        rotation, translation = self._split_extrinsic(extrinsic)
        extrinsic_matrix = np.eye(4, dtype=np.float64)
        extrinsic_matrix[0:3, 0:3] = cv2.Rodrigues(rotation)[0]
        extrinsic_matrix[0:3, 3] = translation
        return extrinsic_matrix

    @staticmethod
    def _split_extrinsic(extrinsic):
        extrinsic = np.array(extrinsic, dtype=np.float64)
        assert extrinsic.size == 6
        rotation = extrinsic.ravel()[0:3]
        translation = extrinsic.ravel()[3:6]
        return rotation, translation

    @staticmethod
    def _merge_extrinsic(rotation, translation):
        assert rotation.size == 3 and translation.size == 3
        extrinsic = np.concatenate((rotation.ravel(), translation.ravel()))
        return extrinsic

    @staticmethod
    def _normalize_points(points_3d):
        return points_3d[:, :-1] / points_3d[:, -1][:, np.newaxis]

    def _get_extrinsic(self, frame_sn, world_sn):
        frame_sn = frame_sn.lower()
        world_sn = world_sn.lower()

        if (
            frame_sn not in self._all_extrinsic_eye1_world
            or world_sn not in self._all_extrinsic_eye1_world[frame_sn]
        ):
            raise Exception(
                "The pair of frame '%s' and world camera '%s' is not supported!"
                % (frame_sn, world_sn)
            )
        return self._all_extrinsic_eye1_world[frame_sn][world_sn]