Example script that shows how to perform marker mapping in realtime for Pupil Invisible.

README.md

Realtime Marker Mapper for Pupil Invisible

Example script that shows how to perform marker mapping in realtime for Pupil Invisible.

Installation

1. Download the files in this gist via the Download ZIP file in the top right and extract the files to your disk
2. Follow the general marker mapper setup instructions
3. Make sure that you are running Pupil Invisible Companion v1.4.14 or newer
4. Install the python requirements

pip install -r requirements.txt

Usage

1. Connect your Pupil Invisible glasses including the scene camera
2. Run python realtime-marker-mapper.py
3. Position your glasses such that the markers can be detected correctly (green outline)
4. Press d to define an AoI - a new window will open displaying the AoI crop and mapped gaze

Note: The first time a new serial number is encountered, the script will attempt to download camera intrinsics. This requires an internet connection. See below for details.

Caveat 1: This example does not support resizing the defined surface Caveat 2: Due to an internal mixup of distorted/undistorted coordinates, the previewed image crop is not accurate in surface-tracker==0.0.1

How It Works

Python Requirements

To run this example we need the following dependencies:

# receive gaze and scene video in realtime
pupil-labs-realtime-api

# marker detection and AoI tracking
pupil-apriltags
surface-tracker

# point (un)distortion and visualization
numpy
opencv-python

# downloading scene camera intrinsics
requests

Scene Camera Intrinsics

In order to correct the image distortion, the script needs to know the scene camera intrinsics. These can be accessed via the Pupil Cloud API:

https://api.cloud.pupil-labs.com/hardware/<SCENE CAMERA SERIAL>/calibration.v1?json

To avoid requesting the same intrinsics repeatedly, the script will try to download the values the first time it encounters a new scene camera serial number and cache it to a intrinsics.<SCENE CAMERA SERIAL>.json file.

realtime-marker-mapper.py

import os
import textwrap
import time

import cv2
from pupil_labs import surface_tracker
from pupil_labs.realtime_api.simple import discover_one_device

from utils import (
    RadialDistorsionCamera,
)  # TODO: Replace with https://github.com/pupil-labs/camera
from utils import (
    ApriltagDetector,
    draw_gaze_point,
    draw_marker_outlines,
    draw_surface_outline,
    draw_text,
    load_camera_intrinsics,
    map_gaze_and_draw_on_crop,
)


def main():
    # Look for devices. Returns as soon as it has found the first device.
    print("Looking for the next best device...")
    device = discover_one_device(max_search_duration_seconds=10)
    if device is None:
        print("No device found.")
        raise SystemExit(-1)

    serial_number_scene_cam = device.serial_number_scene_cam
    if not serial_number_scene_cam:
        print("Scene camera not connected")
        device.close()
        raise SystemExit(-2)

    try:
        print(f"Loading camera intrinsics for {serial_number_scene_cam}")
        intrinsics_scene_cam = load_camera_intrinsics(serial_number_scene_cam)
    except Exception as err:
        print("Unable to fetch")
        # Unable to fetch intrinsics from Pupil Cloud API
        device.close()
        raise SystemExit(-3) from err

    # Initialise camera, marker detector, and surface tracker

    camera_model = RadialDistorsionCamera(
        name="Pupil Invisible World v1",
        resolution=(1088, 1080),
        K=intrinsics_scene_cam["camera_matrix"],
        D=intrinsics_scene_cam["dist_coefs"],
    )

    detector = ApriltagDetector(camera_model)
    tracker = surface_tracker.SurfaceTracker()
    surface = False
    freeze = False
    location = None
    crop = None

    try:
        while True:
            if not freeze:
                frame, gaze = device.receive_matched_scene_video_frame_and_gaze()
                markers = detector.detect_from_image(frame.bgr_pixels)
                if surface:
                    location = tracker.locate_surface(
                        surface=surface,
                        markers=markers,
                    )
                    if location is not None:
                        crop_transform = tracker.locate_surface_image_crop(
                            surface,
                            location,
                            camera_model,
                            width=500,
                        )
                        undistorted = camera_model.undistort_image(frame.bgr_pixels)
                        crop = crop_transform.apply_to_image(undistorted)
                        map_gaze_and_draw_on_crop(crop, gaze, location, camera_model)
            else:
                time.sleep(1 / 40)
            draw_marker_outlines(frame.bgr_pixels, markers, camera=camera_model)
            if location:
                draw_surface_outline(frame.bgr_pixels, location, camera=camera_model)
                cv2.imshow("Cropped surface", crop)
            draw_gaze_point(frame.bgr_pixels, gaze)
            draw_text(
                frame.bgr_pixels,
                textwrap.dedent(
                    f"""
                    ESC to quit
                    f to {'un' if freeze else ''}freeze
                    d to (re)define the surface
                    """,
                ).strip(),
            )
            cv2.imshow("Scene camera with gaze overlay", frame.bgr_pixels)
            pressed_key = cv2.waitKey(1)
            if pressed_key == ord("f"):
                freeze = not freeze
            elif pressed_key == ord("d"):
                surface = tracker.define_surface(name="surface", markers=markers)
                location = tracker.locate_surface(
                    surface=surface,
                    markers=markers,
                )
                crop_transform = tracker.locate_surface_image_crop(
                    surface, location, camera_model
                )
                crop = crop_transform.apply_to_image(frame.bgr_pixels)
            elif pressed_key == 27:  # ESC
                break
    except KeyboardInterrupt:
        pass
    finally:
        print("Stopping...")
        device.close()  # explicitly stop auto-update


if __name__ == "__main__":
    main()

requirements.txt

numpy
opencv-python
pupil-apriltags
pupil-labs-realtime-api
requests
surface-tracker @ git+https://github.com/pupil-labs/surface-tracker@pl_project_structure

utils.py

from codecs import backslashreplace_errors
import functools
import json
import os
import typing as T
import warnings

import cv2
import numpy as np
import pupil_apriltags
import requests

try:
    from pupil_labs import surface_tracker
except ImportError:
    warnings.warn(
        "surface_tracker is out of date - run "
        "`pip install -U surface-tracker` to update",
        category=ImportWarning,
    )
    import surface_tracker


def load_camera_intrinsics(
    serial_number_scene_cam,
    api_endpoint="https://api.cloud.pupil-labs.com/hardware/{}/calibration.v1?json",
):
    cache_file = f"intrinsics.{serial_number_scene_cam}.json"
    try:
        with open(cache_file, "r") as fh:
            return json.load(fh)
    except FileNotFoundError:
        url = api_endpoint.format(serial_number_scene_cam)
        resp = requests.get(url)
        resp.raise_for_status()
        intrinsics = resp.json()["result"]
        try:
            with open(cache_file, "w") as fh:
                json.dump(intrinsics, fh)
        except IOError:
            warnings.warn(f"Unable to cache intrinsics to {cache_file}")
        return intrinsics


# Source: pupil/pupil_src/shared_modules/camera_model.py
class RadialDistorsionCamera:
    """Camera model assuming a lense with radial distortion (this is the defaut model in opencv).
    Provides functionality to make use of a pinhole camera calibration that is also compensating for lense distortion
    """

    def __init__(self, name, resolution, K, D):
        self.name = name
        self.__resolution = resolution
        self.K = np.array(K)
        self.D = np.array(D)

    # CameraModel Interface

    @property
    def resolution(self) -> T.Tuple[int, int]:
        return self.__resolution

    def undistort_points_on_image_plane(self, points):
        points = self.__unprojectPoints(points, use_distortion=True)
        points = self.__projectPoints(points, use_distortion=False)
        return points

    def distort_points_on_image_plane(self, points):
        points = self.__unprojectPoints(points, use_distortion=False)
        points = self.__projectPoints(points, use_distortion=True)
        return points

    def distort_and_project(self, *args, **kwargs):
        return self.distort_points_on_image_plane(*args, **kwargs)

    def undistort_image(self, img):
        return cv2.undistort(img, self.K, self.D)

    # Private

    def __projectPoints(self, object_points, rvec=None, tvec=None, use_distortion=True):
        """
        Projects a set of points onto the camera plane as defined by the camera model.
        :param object_points: Set of 3D world points
        :param rvec: Set of vectors describing the rotation of the camera when recording the corresponding object point
        :param tvec: Set of vectors describing the translation of the camera when recording the corresponding object point
        :return: Projected 2D points
        """
        input_dim = object_points.ndim

        object_points = object_points.reshape((1, -1, 3))

        if rvec is None:
            rvec = np.zeros(3).reshape(1, 1, 3)
        else:
            rvec = np.array(rvec).reshape(1, 1, 3)

        if tvec is None:
            tvec = np.zeros(3).reshape(1, 1, 3)
        else:
            tvec = np.array(tvec).reshape(1, 1, 3)

        if use_distortion:
            _D = self.D
        else:
            _D = np.asarray([[0.0, 0.0, 0.0, 0.0, 0.0]])

        image_points, jacobian = cv2.projectPoints(
            object_points, rvec, tvec, self.K, _D
        )

        if input_dim == 2:
            image_points.shape = (-1, 2)
        elif input_dim == 3:
            image_points.shape = (-1, 1, 2)
        return image_points

    def __unprojectPoints(self, pts_2d, use_distortion=True, normalize=False):
        """
        Undistorts points according to the camera model.
        :param pts_2d, shape: Nx2
        :return: Array of unprojected 3d points, shape: Nx3
        """
        pts_2d = np.array(pts_2d, dtype=np.float32)

        # Delete any posibly wrong 3rd dimension
        if pts_2d.ndim == 1 or pts_2d.ndim == 3:
            pts_2d = pts_2d.reshape((-1, 2))

        # Add third dimension the way cv2 wants it
        if pts_2d.ndim == 2:
            pts_2d = pts_2d.reshape((-1, 1, 2))

        if use_distortion:
            _D = self.D
        else:
            _D = np.asarray([[0.0, 0.0, 0.0, 0.0, 0.0]])

        pts_2d_undist = cv2.undistortPoints(pts_2d, self.K, _D)

        pts_3d = cv2.convertPointsToHomogeneous(pts_2d_undist)
        pts_3d.shape = -1, 3

        if normalize:
            pts_3d /= np.linalg.norm(pts_3d, axis=1)[:, np.newaxis]

        return pts_3d


def create_apriltag_marker_uid(
    tag_family: str, tag_id: int
) -> surface_tracker.MarkerId:
    # Construct the UID by concatinating the tag family and the tag id
    return surface_tracker.MarkerId(f"{tag_family}:{tag_id}")


class ApriltagDetector:
    def __init__(self, camera_model: RadialDistorsionCamera):
        families = "tag36h11"
        self._camera_model = camera_model
        self._detector = pupil_apriltags.Detector(families=families)

    def detect_from_image(self, image) -> T.List[surface_tracker.Marker]:
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        return self.detect_from_gray(gray)

    def detect_from_gray(self, gray) -> T.List[surface_tracker.Marker]:
        # Detect apriltag markers from the gray image
        markers = self._detector.detect(gray)

        # Ensure detected markers are unique
        # TODO: Between deplicate markers, pick the one with higher confidence
        uid_fn = self.__apiltag_marker_uid
        markers = dict((uid_fn(m), m) for m in markers).values()

        # Convert apriltag markers into surface tracker markers
        marker_fn = self.__apriltag_marker_to_surface_marker
        markers = [marker_fn(m) for m in markers]

        return markers

    @staticmethod
    def __apiltag_marker_uid(
        apriltag_marker: pupil_apriltags.Detection,
    ) -> surface_tracker.MarkerId:
        family = apriltag_marker.tag_family.decode("utf-8")
        tag_id = int(apriltag_marker.tag_id)
        return create_apriltag_marker_uid(family, tag_id)

    def __apriltag_marker_to_surface_marker(
        self, apriltag_marker: pupil_apriltags.Detection
    ) -> surface_tracker.Marker:

        # Construct the surface tracker marker UID
        uid = ApriltagDetector.__apiltag_marker_uid(apriltag_marker)

        # Extract vertices in the correct format form apriltag marker
        vertices = [[point] for point in apriltag_marker.corners]
        vertices = self._camera_model.undistort_points_on_image_plane(vertices)

        # TODO: Verify this is correct...
        starting_with = surface_tracker.CornerId.TOP_LEFT
        clockwise = True

        return surface_tracker.Marker.from_vertices(
            uid=uid,
            undistorted_image_space_vertices=vertices,
            starting_with=starting_with,
            clockwise=clockwise,
        )


def draw_surface_outline(
    img,
    location: surface_tracker.SurfaceLocation,
    camera=None,
    num_points_per_edge=20,
):
    edges, top_indices = _edge_points(num_points_per_edge)  # .copy()
    points = location._map_from_surface_to_image(edges)
    if camera:
        points = camera.distort_points_on_image_plane(points)
    num_points_edge = points.shape[0] // 4
    cv2.polylines(
        img,
        np.asarray(points, dtype="int32").reshape((1, -1, 2)),
        isClosed=False,
        color=(255, 0, 0),
        thickness=5,
    )
    # draw top edge in red
    cv2.polylines(
        img,
        np.asarray(points[top_indices], dtype="int32").reshape((1, -1, 2)),
        isClosed=False,
        color=(0, 0, 255),
        thickness=5,
    )


@functools.lru_cache()
def _edge_points(num_points_per_edge: int):
    zero_to_one = np.linspace(0, 1, num_points_per_edge)
    one_to_zero = np.linspace(1, 0, num_points_per_edge)

    points = np.zeros((num_points_per_edge * 4, 2))
    # bottom, left-to-right
    points[num_points_per_edge * 0 : num_points_per_edge * 1, 0] = zero_to_one
    points[num_points_per_edge * 0 : num_points_per_edge * 1, 1] = 0
    # right, bot-to-top
    points[num_points_per_edge * 1 : num_points_per_edge * 2, 0] = 1
    points[num_points_per_edge * 1 : num_points_per_edge * 2, 1] = zero_to_one
    # top, right-to-left
    points[num_points_per_edge * 2 : num_points_per_edge * 3, 0] = one_to_zero
    points[num_points_per_edge * 2 : num_points_per_edge * 3, 1] = 1
    # left, top-to-bot
    points[num_points_per_edge * 3 : num_points_per_edge * 4, 0] = 0
    points[num_points_per_edge * 3 : num_points_per_edge * 4, 1] = one_to_zero
    return points, np.arange(num_points_per_edge * 2, num_points_per_edge * 3)


def draw_gaze_point(img, gaze):
    cv2.circle(
        img, (int(gaze.x), int(gaze.y)), radius=80, color=(0, 0, 255), thickness=15
    )


def draw_marker_outlines(img, markers, camera: RadialDistorsionCamera):
    for marker in markers:
        cv2.polylines(
            img,
            np.array(
                camera.distort_points_on_image_plane(marker.vertices()),
                dtype="int32",
            ).reshape((-1, 4, 2)),
            isClosed=True,
            color=(0, 255, 0),
            thickness=5,
        )


def draw_text(
    img,
    text,
    font=cv2.FONT_HERSHEY_SIMPLEX,
    scale=1,
    thickness=1,
    org=(10, 10),
    margin=(20, 20),
    text_color=(255, 255, 255),
    background_color=(0, 0, 0),
):
    background_size = list(margin)
    line_ys = []
    lines = text.split("\n")
    for line in lines:
        label_size, line_height = cv2.getTextSize(line, font, scale, thickness)
        background_size[0] = max(background_size[0], label_size[0] + 2 * margin[0])
        background_size[1] += label_size[1] + line_height
        line_ys.append(background_size[1] - line_height)
    background_size[1] += margin[1]

    cv2.rectangle(
        img,
        org,
        (org[0] + background_size[0], org[1] + background_size[1]),
        background_color,
        -1,
    )
    for line, y in zip(lines, line_ys):
        cv2.putText(
            img,
            line,
            (org[0] + margin[0], org[1] + y),
            font,
            scale,
            text_color,
            thickness,
            cv2.LINE_8,
        )


def map_gaze_and_draw_on_crop(crop, gaze, location, camera_model):
    gaze_undistorted = camera_model.undistort_points_on_image_plane([gaze.x, gaze.y])
    norm_x, norm_y = location._map_from_image_to_surface(gaze_undistorted)[0]
    pixel_x = norm_x * crop.shape[1]
    pixel_y = (1.0 - norm_y) * crop.shape[0]
    cv2.circle(
        crop, (int(pixel_x), int(pixel_y)), radius=30, color=(0, 0, 255), thickness=5
    )