Get azimuth and elevation from Neon's scene camera using the realtime API.

neon_az_el_RT.py

import cv2
import numpy as np
from pupil_labs.realtime_api.simple import Device


def unproject_points(points_2d, camera_matrix, distortion_coefs, normalize=False):
    """
    Undistorts points according to the camera model.
    :param pts_2d, shape: Nx2
    :return: Array of unprojected 3d points, shape: Nx3
    """
    # Convert type to numpy arrays (OpenCV requirements)
    camera_matrix = np.array(camera_matrix)
    distortion_coefs = np.array(distortion_coefs)
    points_2d = np.asarray(points_2d, dtype=np.float32)

    # Add third dimension the way cv2 wants it
    points_2d = points_2d.reshape((-1, 1, 2))

    # Undistort 2d pixel coordinates
    points_2d_undist = cv2.undistortPoints(points_2d, camera_matrix, distortion_coefs)
    # Unproject 2d points into 3d directions; all points. have z=1
    points_3d = cv2.convertPointsToHomogeneous(points_2d_undist)
    points_3d.shape = -1, 3

    if normalize:
        # normalize vector length to 1
        points_3d /= np.linalg.norm(points_3d, axis=1)[:, np.newaxis]

    return points_3d


def cart_to_spherical(points_3d, apply_rad2deg=True):
    # convert cartesian to spherical coordinates
    # source: http://stackoverflow.com/questions/4116658/faster-numpy-cartesian-to-spherical-coordinate-conversion
    # print("Converting cartesian to spherical coordinates...")
    x = points_3d[:, 0]
    y = points_3d[:, 1]
    z = points_3d[:, 2]
    radius = np.sqrt(x**2 + y**2 + z**2)
    # elevation: vertical direction
    #   positive numbers point up
    #   negative numbers point bottom
    elevation = np.arccos(y / radius) - np.pi / 2
    # azimuth: horizontal direction
    #   positive numbers point right
    #   negative numbers point left
    azimuth = np.pi / 2 - np.arctan2(z, x)

    if apply_rad2deg:
        elevation = np.rad2deg(elevation)
        azimuth = np.rad2deg(azimuth)

    return radius, elevation, azimuth


def example(address: str = "192.168.1.59"):
    device = Device(address=address, port=8080)

    # https://pupil-labs-realtime-api.readthedocs.io/en/stable/examples/simple.html#camera-calibration
    calibration = device.get_calibration()

    print("Scene camera matrix:")
    print(calibration["scene_camera_matrix"][0])
    print("\nScene distortion coefficients:")
    print(calibration["scene_distortion_coefficients"][0])

    # https://pupil-labs-realtime-api.readthedocs.io/en/stable/examples/simple.html#gaze-data
    try:
        while True:
            gaze = device.receive_gaze_datum()
            points_3d = unproject_points(
                [gaze.x, gaze.y],
                camera_matrix=calibration["scene_camera_matrix"][0],
                distortion_coefs=calibration["scene_distortion_coefficients"][0],
                normalize=True,
            )
            radius, elevation, azimuth = cart_to_spherical(
                points_3d, apply_rad2deg=True
            )
            print(
                f"ts {gaze.timestamp_unix_ns}: XY: ({int(gaze.x)},{int(gaze.y)}) - R, el, az (in degrees):{np.array([radius, elevation, azimuth]).T}"
            )

    except KeyboardInterrupt:
        pass
    finally:
        print("Stopping...")
        device.close()


if __name__ == "__main__":
    example()

requirements.txt

opencv-python-headless
pupil-labs-realtime-api
numpy