diff --git a/.gitignore b/.gitignore index e16d4b09be..4a6465142b 100644 --- a/.gitignore +++ b/.gitignore @@ -39,10 +39,6 @@ settings/ .idea #directory for test file will be ignored Pupil_Test_Files -detector_2d.cpp -detector_3d.cpp -CircleGoodnessTest.cpp -SphereCircleTest.cpp *.mkv *.egg-info hmd_cal_data @@ -55,4 +51,5 @@ deployment/pupil_v* *.pyd *.dll win_drv -.vs \ No newline at end of file +.vs +.venv diff --git a/README.md b/README.md index 1ef052742e..28cae45738 100644 --- a/README.md +++ b/README.md @@ -50,14 +50,6 @@ All setup and dependency installation instructions are contained in this repo. A - [macOS](./docs/dependencies-macos.md "Pupil dependency installation for macOS") - [Windows 10](./docs/dependencies-windows.md "Pupil dependency installation for Windows 10") -#### Intel RealSense 3D Support - -If you want to use an Intel RealSense 3D scene camera, please follow the additional setup instructions for the camera model you have. - -* **Intel RealSense R200**: Please follow our detailed [Setup Guide](./docs/dependencies-realsense-r200.md "RealSense R200 setup guide") -* **Intel RealSense D400**: You need to install the [Python wrapper for librealsense](https://github.com/IntelRealSense/librealsense/tree/master/wrappers/python#python-wrapper "Install instructions for librealsense Python wrapper") - - ### Clone the repo After you have installed all dependencies, clone this repo and start Pupil software. diff --git a/deployment/bundle.bat b/deployment/bundle.bat index 1f7d3b5db5..e2690e65c8 100644 --- a/deployment/bundle.bat +++ b/deployment/bundle.bat @@ -35,9 +35,6 @@ if not exist %release_dir% ( set PATH=%PATH%;C:\Python36\Lib\site-packages\scipy\.libs set PATH=%PATH%;C:\Python36\Lib\site-packages\zmq -python ..\pupil_src\shared_modules\cython_methods\build.py -python ..\pupil_src\shared_modules\calibration_routines\optimization_calibration\build.py - call :Bundle capture %current_tag% call :Bundle service %current_tag% call :Bundle player %current_tag% diff --git a/deployment/bundle.sh b/deployment/bundle.sh index c94fb97985..20fa5b87d3 100755 --- a/deployment/bundle.sh +++ b/deployment/bundle.sh @@ -13,12 +13,6 @@ fi echo "release_dir: ${release_dir}" mkdir ${release_dir} -printf "\n##########\nBuilding cython modules\n##########\n\n" -python3 ../pupil_src/shared_modules/cython_methods/build.py - -printf "\n##########\nBuilding calibration methods\n##########\n\n" -python3 ../pupil_src/shared_modules/calibration_routines/optimization_calibration/build.py - # bundle Pupil Capture printf "\n##########\nBundling Pupil Capture\n##########\n\n" cd deploy_capture diff --git a/deployment/deploy_capture/bundle.spec b/deployment/deploy_capture/bundle.spec index c9705dd5f5..e3d6868d92 100644 --- a/deployment/deploy_capture/bundle.spec +++ b/deployment/deploy_capture/bundle.spec @@ -56,10 +56,6 @@ if platform.system() == "Darwin": sys.path.append(".") from version import pupil_version - import pyrealsense - - pyrealsense_path = pathlib.Path(pyrealsense.__file__).parent / "lrs_parsed_classes" - del sys.path[-1] a = Analysis( ["../../pupil_src/main.py"], @@ -98,11 +94,9 @@ if platform.system() == "Darwin": a.datas, [("libuvc.0.dylib", "/usr/local/lib/libuvc.0.dylib", "BINARY")], [("libglfw.dylib", "/usr/local/lib/libglfw.dylib", "BINARY")], - [("librealsense.dylib", "/usr/local/lib/librealsense.dylib", "BINARY")], [("pyglui/OpenSans-Regular.ttf", ui.get_opensans_font_path(), "DATA")], [("pyglui/Roboto-Regular.ttf", ui.get_roboto_font_path(), "DATA")], [("pyglui/pupil_icons.ttf", ui.get_pupil_icons_font_path(), "DATA")], - [("pyrealsense/lrs_parsed_classes", pyrealsense_path, "DATA")], apriltag_libs, strip=None, upx=True, @@ -127,7 +121,7 @@ elif platform.system() == "Linux": + apriltag_hidden_imports, hookspath=None, runtime_hooks=None, - excludes=["matplotlib", "pyrealsense"], + excludes=["matplotlib"], ) pyz = PYZ(a.pure) diff --git a/deployment/deploy_player/bundle.spec b/deployment/deploy_player/bundle.spec index 3db861f62a..e8ee8f64af 100644 --- a/deployment/deploy_player/bundle.spec +++ b/deployment/deploy_player/bundle.spec @@ -69,7 +69,7 @@ if platform.system() == "Darwin": ), hookspath=None, runtime_hooks=None, - excludes=["matplotlib", "pyrealsense"], + excludes=["matplotlib"], ) pyz = PYZ(a.pure) @@ -126,7 +126,7 @@ elif platform.system() == "Linux": + apriltag_hidden_imports, hookspath=None, runtime_hooks=None, - excludes=["matplotlib", "pyrealsense"], + excludes=["matplotlib"], ) pyz = PYZ(a.pure) @@ -214,7 +214,7 @@ elif platform.system() == "Windows": + apriltag_hidden_imports, hookspath=None, runtime_hooks=None, - excludes=["matplotlib", "pyrealsense"], + excludes=["matplotlib"], ) pyz = PYZ(a.pure) diff --git a/deployment/deploy_service/bundle.spec b/deployment/deploy_service/bundle.spec index 5f01431bb7..ba415e6d74 100644 --- a/deployment/deploy_service/bundle.spec +++ b/deployment/deploy_service/bundle.spec @@ -53,7 +53,7 @@ if platform.system() == "Darwin": hiddenimports=[] + av_hidden_imports + pyglui_hidden_imports, hookspath=None, runtime_hooks=None, - excludes=["matplotlib", "pyrealsense"], + excludes=["matplotlib"], ) pyz = PYZ(a.pure) exe = EXE( @@ -99,7 +99,7 @@ elif platform.system() == "Linux": hiddenimports=[] + av_hidden_imports + pyglui_hidden_imports, hookspath=None, runtime_hooks=None, - excludes=["matplotlib", "pyrealsense"], + excludes=["matplotlib"], ) pyz = PYZ(a.pure) @@ -181,7 +181,7 @@ elif platform.system() == "Windows": runtime_hooks=None, win_no_prefer_redirects=False, win_private_assemblies=False, - excludes=["matplotlib", "pyrealsense"], + excludes=["matplotlib"], ) pyz = PYZ(a.pure) diff --git a/docs/dependencies-realsense-r200.md b/docs/dependencies-realsense-r200.md deleted file mode 100644 index 48d08a7afa..0000000000 --- a/docs/dependencies-realsense-r200.md +++ /dev/null @@ -1,41 +0,0 @@ -# Intel RealSense R200 Support - -**Note:** Support for the Intel RealSense R200 is currently not available for **Linux**. This is due to ["librealsense" requiring kernel patches for the "Video4Linux" backend](https://github.com/IntelRealSense/librealsense/blob/66e42069837ed6e0eb46351cc4aa2acca49a4728/doc/installation.md#video4linux-backend-preparation). - -## Dependencies - -### librealsense - -All Intel RealSense cameras require [`librealsense`](https://github.com/pupil-labs/librealsense/) to be installed. Please follow the [install instructions](https://github.com/pupil-labs/librealsense/#table-of-contents) for your operating system. - -### pyrealsense - -[`pyrealsense`](https://github.com/pupil-labs/pyrealsense) provides Python bindings for [`librealsense`](#librealsense). Run the following command in your terminal to install it. - -```sh -pip install git+https://github.com/pupil-labs/pyrealsense -``` - -## Usage - -Select `RealSense 3D` in the Capture Selection menu and activate your RealSense camera. Afterwards you should see the colored video stream of the selected camera. - -Pupil Capture accesses both streams, color and depth, at all times but only previews one at a time. Enable the `Preview Depth` option to see the normalized depth video stream. - -The `Record Depth Stream` option (enabled by default) will save the depth stream during a recording session to the file `depth.mp4` within your recording folder. - -By default, you can choose different resolutions for the color and depth streams. This is advantageous if you want to run both streams at full resolution. The Intel RealSense R200 has a maximum color resolution of `1920 x 1080` pixels and maximum depth resolution of `640 x 480` pixels. `librealsense` also provides the possibility to pixel-align color and depth streams. `Align Streams` enables this functionality. This is required if you want to infer from depth pixels to color pixels and vice versa. - -The `Sensor Settings` menu lists all available device options. These may differ depending on your OS, installed `librealsense` version, and device firmware. - - -**Note:** Not all resolutions support all frame rates. Try different resolutions if your desired frame rate is not listed. - - -### Color Frames - -Pupil Capture accesses the `YUVY` color stream of the RealSense camera. All color frames are accessible through the `events` object using the `frame` key within your plugin's `recent_events` method. - -### Depth Frames - -Depth frame objects are accessible through the `events` object using the `depth_frame` key within your plugin's `recent_events` method. The orginal 16-bit grayscale image of the camera can be accessed using the `depth` attribute of the frame object. The `bgr` attribute provides a colored image that is calculated using [histogram equalization](https://en.wikipedia.org/wiki/Histogram_equalization). These colored images are previewed in Pupil Capture, stored during recordings, and referred to as "normalized depth stream" in the above section. The [`librealsense` examples](https://github.com/IntelRealSense/librealsense/tree/master/examples) use the same coloring method to visualize depth images. diff --git a/pupil_src/launchables/eye.py b/pupil_src/launchables/eye.py index 3e639e631c..d824bdeb38 100644 --- a/pupil_src/launchables/eye.py +++ b/pupil_src/launchables/eye.py @@ -129,7 +129,6 @@ def eye( from gl_utils import make_coord_system_pixel_based from gl_utils import make_coord_system_norm_based from gl_utils import is_window_visible, glViewport - from ui_roi import UIRoi # monitoring import psutil @@ -145,6 +144,7 @@ def eye( from av_writer import JPEG_Writer, MPEG_Writer from ndsi import H264Writer from video_capture import source_classes, manager_classes + from roi import Roi from background_helper import IPC_Logging_Task_Proxy from pupil_detector_plugins import available_detector_plugins @@ -175,7 +175,6 @@ def interrupt_handler(sig, frame): icon_bar_width = 50 window_size = None - camera_render_size = None hdpi_factor = 1.0 # g_pool holds variables for this process @@ -188,6 +187,7 @@ def interrupt_handler(sig, frame): g_pool.eye_id = eye_id g_pool.process = f"eye{eye_id}" g_pool.timebase = timebase + g_pool.camera_render_size = None g_pool.ipc_pub = ipc_socket @@ -202,7 +202,7 @@ def get_timestamp(): manager_classes + source_classes + available_detectors - + [PupilDetectorManager] + + [PupilDetectorManager, Roi] ) g_pool.plugin_by_name = {p.__name__: p for p in plugins} @@ -213,28 +213,30 @@ def get_timestamp(): ] if eye_id == 0: preferred_names += ["HD-6000"] - default_capture_settings = ( - "UVC_Source", - { - "preferred_names": preferred_names, - "frame_size": (320, 240), - "frame_rate": 120, - }, - ) + + default_capture_name = "UVC_Source" + default_capture_settings = { + "preferred_names": preferred_names, + "frame_size": (320, 240), + "frame_rate": 120, + } default_plugins = [ # TODO: extend with plugins - default_capture_settings, + (default_capture_name, default_capture_settings), ("UVC_Manager", {}), + ("NDSI_Manager", {}), + ("HMD_Streaming_Manager", {}), + ("File_Manager", {}), # Detector needs to be loaded first to set `g_pool.pupil_detector` (default_detector_cls.__name__, {}), ("PupilDetectorManager", {}), + ("Roi", {}), ] # Callback functions def on_resize(window, w, h): nonlocal window_size - nonlocal camera_render_size nonlocal hdpi_factor active_window = glfw.glfwGetCurrentContext() @@ -242,7 +244,7 @@ def on_resize(window, w, h): hdpi_factor = glfw.getHDPIFactor(window) g_pool.gui.scale = g_pool.gui_user_scale * hdpi_factor window_size = w, h - camera_render_size = w - int(icon_bar_width * g_pool.gui.scale), h + g_pool.camera_render_size = w - int(icon_bar_width * g_pool.gui.scale), h g_pool.gui.update_window(w, h) g_pool.gui.collect_menus() for g in g_pool.graphs: @@ -264,16 +266,18 @@ def on_window_mouse_button(window, button, action, mods): g_pool.gui.update_button(button, action, mods) def on_pos(window, x, y): - x *= hdpi_factor - y *= hdpi_factor + x, y = x * hdpi_factor, y * hdpi_factor g_pool.gui.update_mouse(x, y) - if g_pool.u_r.active_edit_pt: - pos = normalize((x, y), camera_render_size) - if g_pool.flip: - pos = 1 - pos[0], 1 - pos[1] - pos = denormalize(pos, g_pool.capture.frame_size) - g_pool.u_r.move_vertex(g_pool.u_r.active_pt_idx, pos) + pos = x, y + pos = normalize(pos, g_pool.camera_render_size) + if g_pool.flip: + pos = 1 - pos[0], 1 - pos[1] + # Position in img pixels + pos = denormalize(pos, g_pool.capture.frame_size) + + for p in g_pool.plugins: + p.on_pos(pos) def on_scroll(window, x, y): g_pool.gui.update_scroll(x, y * scroll_factor) @@ -371,32 +375,6 @@ def set_window_size(): f_width += int(icon_bar_width * g_pool.gui.scale) glfw.glfwSetWindowSize(main_window, f_width, f_height) - def uroi_on_mouse_button(button, action, mods): - if g_pool.display_mode == "roi": - if action == glfw.GLFW_RELEASE and g_pool.u_r.active_edit_pt: - g_pool.u_r.active_edit_pt = False - # if the roi interacts we dont want - # the gui to interact as well - return - elif action == glfw.GLFW_PRESS: - x, y = glfw.glfwGetCursorPos(main_window) - # pos = normalize(pos, glfw.glfwGetWindowSize(main_window)) - x *= hdpi_factor - y *= hdpi_factor - pos = normalize((x, y), camera_render_size) - if g_pool.flip: - pos = 1 - pos[0], 1 - pos[1] - # Position in img pixels - pos = denormalize( - pos, g_pool.capture.frame_size - ) # Position in img pixels - if g_pool.u_r.mouse_over_edit_pt( - pos, g_pool.u_r.handle_size, g_pool.u_r.handle_size - ): - # if the roi interacts we dont want - # the gui to interact as well - return - general_settings.append(ui.Button("Reset window size", set_window_size)) general_settings.append(ui.Switch("flip", g_pool, label="Flip image display")) general_settings.append( @@ -437,12 +415,14 @@ def uroi_on_mouse_button(button, action, mods): g_pool.plugins = Plugin_List(g_pool, plugins_to_load) - g_pool.writer = None + if not g_pool.capture: + # Make sure we always have a capture running. Important if there was no + # capture stored in session settings. + g_pool.plugins.add( + g_pool.plugin_by_name[default_capture_name], default_capture_settings + ) - g_pool.u_r = UIRoi((g_pool.capture.frame_size[1], g_pool.capture.frame_size[0])) - roi_user_settings = session_settings.get("roi") - if roi_user_settings and tuple(roi_user_settings[-1]) == g_pool.u_r.get()[-1]: - g_pool.u_r.set(roi_user_settings) + g_pool.writer = None # Register callbacks main_window glfw.glfwSetFramebufferSizeCallback(main_window, on_resize) @@ -563,19 +543,6 @@ def window_should_update(): frame = event.get("frame") if frame: - f_width, f_height = g_pool.capture.frame_size - # TODO: Roi should be its own plugin. This way we could put it at the - # appropriate order for recent_events() to process frame resolution - # changes immediately after the backend. - if (g_pool.u_r.array_shape[0], g_pool.u_r.array_shape[1]) != ( - f_height, - f_width, - ): - g_pool.pupil_detector.on_resolution_change( - (g_pool.u_r.array_shape[1], g_pool.u_r.array_shape[0]), - g_pool.capture.frame_size, - ) - g_pool.u_r = UIRoi((f_height, f_width)) if should_publish_frames: try: if frame_publish_format == "jpeg": @@ -631,16 +598,10 @@ def window_should_update(): glfw.glfwMakeContextCurrent(main_window) clear_gl_screen() - glViewport(0, 0, *camera_render_size) + glViewport(0, 0, *g_pool.camera_render_size) for p in g_pool.plugins: p.gl_display() - glViewport(0, 0, *camera_render_size) - # render the ROI - g_pool.u_r.draw(g_pool.gui.scale) - if g_pool.display_mode == "roi": - g_pool.u_r.draw_points(g_pool.gui.scale) - glViewport(0, 0, *window_size) # render graphs fps_graph.draw() @@ -662,13 +623,12 @@ def window_should_update(): for button, action, mods in user_input.buttons: x, y = glfw.glfwGetCursorPos(main_window) pos = x * hdpi_factor, y * hdpi_factor - pos = normalize(pos, camera_render_size) + pos = normalize(pos, g_pool.camera_render_size) + if g_pool.flip: + pos = 1 - pos[0], 1 - pos[1] # Position in img pixels pos = denormalize(pos, g_pool.capture.frame_size) - # TODO: remove when ROI is plugin - uroi_on_mouse_button(button, action, mods) - for plugin in g_pool.plugins: if plugin.on_click(pos, button, action): break @@ -698,7 +658,6 @@ def window_should_update(): session_settings["loaded_plugins"] = g_pool.plugins.get_initializers() # save session persistent settings session_settings["gui_scale"] = g_pool.gui_user_scale - session_settings["roi"] = g_pool.u_r.get() session_settings["flip"] = g_pool.flip session_settings["display_mode"] = g_pool.display_mode session_settings["ui_config"] = g_pool.gui.configuration diff --git a/pupil_src/launchables/player.py b/pupil_src/launchables/player.py index a25342a2af..08e5a85954 100644 --- a/pupil_src/launchables/player.py +++ b/pupil_src/launchables/player.py @@ -97,7 +97,6 @@ def player(rec_dir, ipc_pub_url, ipc_sub_url, ipc_push_url, user_dir, app_versio from vis_watermark import Vis_Watermark from vis_fixation import Vis_Fixation - # from vis_scan_path import Vis_Scan_Path from seek_control import Seek_Control from surface_tracker import Surface_Tracker_Offline @@ -131,7 +130,7 @@ def player(rec_dir, ipc_pub_url, ipc_sub_url, ipc_push_url, user_dir, app_versio ) assert VersionFormat(pyglui_version) >= VersionFormat( - "1.24" + "1.27" ), "pyglui out of date, please upgrade to newest version" process_was_interrupted = False @@ -164,7 +163,6 @@ def interrupt_handler(sig, frame): Vis_Watermark, Eye_Overlay, Video_Overlay, - # Vis_Scan_Path, Offline_Fixation_Detector, Offline_Blink_Detection, Surface_Tracker_Offline, diff --git a/pupil_src/launchables/service.py b/pupil_src/launchables/service.py index 8df3181b20..6d9d7e386e 100644 --- a/pupil_src/launchables/service.py +++ b/pupil_src/launchables/service.py @@ -93,11 +93,6 @@ def stop_eye_process(eye_id): import audio from uvc import get_time_monotonic - # trigger pupil detector cpp build: - import pupil_detectors - - del pupil_detectors - # Plug-ins from plugin import Plugin, Plugin_List, import_runtime_plugins from calibration_routines import calibration_plugins, gaze_mapping_plugins diff --git a/pupil_src/launchables/world.py b/pupil_src/launchables/world.py index 66aa0ae167..e7fb49779b 100644 --- a/pupil_src/launchables/world.py +++ b/pupil_src/launchables/world.py @@ -119,7 +119,7 @@ def set_detection_mapping_mode(new_mode): from pyglui import ui, cygl, __version__ as pyglui_version assert VersionFormat(pyglui_version) >= VersionFormat( - "1.24" + "1.27" ), "pyglui out of date, please upgrade to newest version" from pyglui.cygl.utils import Named_Texture import gl_utils @@ -134,11 +134,6 @@ def set_detection_mapping_mode(new_mode): import audio - # trigger pupil detector cpp build: - import pupil_detectors - - del pupil_detectors - # Plug-ins from plugin import ( Plugin, @@ -288,6 +283,7 @@ def get_timestamp(): ] g_pool.plugin_by_name = {p.__name__: p for p in plugins} + default_capture_name = "UVC_Source" default_capture_settings = { "preferred_names": [ "Pupil Cam1 ID2", @@ -305,9 +301,12 @@ def get_timestamp(): } default_plugins = [ - ("UVC_Source", default_capture_settings), + (default_capture_name, default_capture_settings), ("Pupil_Data_Relay", {}), ("UVC_Manager", {}), + ("NDSI_Manager", {}), + ("HMD_Streaming_Manager", {}), + ("File_Manager", {}), ("Log_Display", {}), ("Dummy_Gaze_Mapper", {}), ("Display_Recent_Gaze", {}), @@ -573,6 +572,13 @@ def set_window_size(): g_pool, session_settings.get("loaded_plugins", default_plugins) ) + if not g_pool.capture: + # Make sure we always have a capture running. Important if there was no + # capture stored in session settings. + g_pool.plugins.add( + g_pool.plugin_by_name[default_capture_name], default_capture_settings + ) + # Register callbacks main_window glfw.glfwSetFramebufferSizeCallback(main_window, on_resize) glfw.glfwSetKeyCallback(main_window, on_window_key) diff --git a/pupil_src/shared_cpp/include/DistancePointEllipse.h b/pupil_src/shared_cpp/include/DistancePointEllipse.h deleted file mode 100644 index fe75babd94..0000000000 --- a/pupil_src/shared_cpp/include/DistancePointEllipse.h +++ /dev/null @@ -1,185 +0,0 @@ -// Geometric Tools, LLC -// Copyright (c) 1998-2014 -// Distributed under the Boost Software License, Version 1.0. -// http://www.boost.org/LICENSE_1_0.txt -// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt -// -// File Version: 5.0.3 (2013/01/03) -// -// Modified by Lech Swirski 2013 - -#ifndef DistancePointEllipse_h__ -#define DistancePointEllipse_h__ - -#include -#include "geometry/Ellipse.h" - -namespace singleeyefitter { - - //---------------------------------------------------------------------------- - // The ellipse is (x0/a)^2 + (x1/b)^2 = 1 with a >= b. The query point is - // (p0,p1) with p0 >= 0 and p1 >= 0. The function returns the distance from - // the query point to the ellipse. It also computes the ellipse point (x0,x1) - // in the first quadrant that is closest to (p0,p1). - //---------------------------------------------------------------------------- - template - Real DistancePointEllipseSpecial(Real a, Real b, const Array& p, Eigen::Matrix& x) - { - Real distance; - - if (p.y() > Real(0)) { - if (p.x() > Real(0)) { - // Bisect to compute the root of F(t) for t >= -e1*e1. - Eigen::Array esqr(a * a, b * b); - Eigen::Array ep(a * p.x(), b * p.y()); - Real t0 = -esqr.y() + ep.y(); - Real t1 = -esqr.y() + ep.matrix().norm(); - Real t = t0; - const int imax = 2 * std::numeric_limits::max_exponent; - - for (int i = 0; i < imax; ++i) { - t = Real(0.5) * (t0 + t1); - - if (t == t0 || t == t1) { - break; - } - - Real r[2] = { ep.x() / (t + esqr[0]), ep.y() / (t + esqr[1]) }; - Real f = r[0] * r[0] + r[1] * r[1] - Real(1); - - if (f > Real(0)) { - t0 = t; - - } else if (f < Real(0)) { - t1 = t; - - } else { - break; - } - } - - x = esqr * p / (t + esqr); - distance = (x - p.matrix()).norm(); - - } else { // y0 == 0 - x[0] = (Real) 0; - x[1] = b; - distance = fabs(p.y() - b); - } - - } else { // y1 == 0 - Real denom0 = a * a - b * b; - Real e0y0 = a * p.x(); - - if (e0y0 < denom0) { - // y0 is inside the subinterval. - Real x0de0 = e0y0 / denom0; - Real x0de0sqr = x0de0 * x0de0; - x[0] = a * x0de0; - x[1] = b * sqrt(fabs(Real(1) - x0de0sqr)); - Real d0 = x[0] - p.x(); - distance = sqrt(d0 * d0 + x[1] * x[1]); - - } else { - // y0 is outside the subinterval. The closest ellipse point has - // x1 == 0 and is on the domain-boundary interval (x0/e0)^2 = 1. - x[0] = a; - x[1] = Real(0); - distance = fabs(p.x() - a); - } - } - - return distance; - } - //---------------------------------------------------------------------------- - // The ellipse is (x0/e0)^2 + (x1/e1)^2 = 1. The query point is (y0,y1). - // The function returns the distance from the query point to the ellipse. - // It also computes the ellipse point (x0,x1) that is closest to (y0,y1). - //---------------------------------------------------------------------------- - template - Real DistancePointEllipse(const Real e[2], const Real y[2], Real x[2]) - { - // Determine reflections for y to the first quadrant. - bool reflect[2]; - int i, j; - - for (i = 0; i < 2; ++i) { - reflect[i] = (y[i] < (Real) 0); - } - - // Determine the axis order for decreasing extents. - int permute[2]; - - if (e[0] < e[1]) { - permute[0] = 1; permute[1] = 0; - - } else { - permute[0] = 0; permute[1] = 1; - } - - int invpermute[2]; - - for (i = 0; i < 2; ++i) { - invpermute[permute[i]] = i; - } - - Real locE[2], locY[2]; - - for (i = 0; i < 2; ++i) { - j = permute[i]; - locE[i] = e[j]; - locY[i] = y[j]; - - if (reflect[j]) { - locY[i] = -locY[i]; - } - } - - Real locX[2]; - Real distance = DistancePointEllipseSpecial(locE, locY, locX); - - // Restore the axis order and reflections. - for (i = 0; i < 2; ++i) { - j = invpermute[i]; - - if (reflect[j]) { - locX[j] = -locX[j]; - } - - x[i] = locX[j]; - } - - return distance; - } - //---------------------------------------------------------------------------- - - template - Scalar DistancePointEllipse(const singleeyefitter::Ellipse2D& ellipse, Scalar x, Scalar y) - { - Eigen::Matrix A; - A << cos(ellipse.angle), sin(ellipse.angle), - -sin(ellipse.angle), cos(ellipse.angle); - Eigen::Matrix p(x - ellipse.center.x(), y - ellipse.center.y()); - Eigen::Matrix Ap = A * p; - // Flip signs to make sure Ap is in the positive quadrant - Eigen::Matrix Ap_pos = Ap; - - for (int i = 0; i < 2; ++i) { - if (Ap[i] < 0) { Ap_pos[i] = -Ap_pos[i]; } - } - - assert(ellipse.major_radius > ellipse.minor_radius); - Eigen::Matrix el_x; - auto distance = DistancePointEllipseSpecial(ellipse.major_radius, ellipse.minor_radius, Ap_pos.array(), el_x); - - // Flip signs back - for (int i = 0; i < 2; ++i) { - if (Ap[i] < 0) { el_x[i] = -el_x[i]; } - } - - return distance; - } - -} - -#endif // DistancePointEllipse_h__ diff --git a/pupil_src/shared_cpp/include/ceres/CeresUtils.h b/pupil_src/shared_cpp/include/ceres/CeresUtils.h deleted file mode 100644 index 5928a50660..0000000000 --- a/pupil_src/shared_cpp/include/ceres/CeresUtils.h +++ /dev/null @@ -1,110 +0,0 @@ - -#ifndef CERESUTILS_H__ -#define CERESUTILS_H__ - -#include -#include - -namespace pupillabs { - - -template inline -void EigenQuaternionToScaledRotation(const T q[4], T R[3 * 3]) { - EigenQuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); -} - -template inline -void EigenQuaternionToScaledRotation(const T q[4], - const ceres::MatrixAdapter& R) { - // Make convenient names for elements of q. - T a = q[3]; - T b = q[0]; - T c = q[1]; - T d = q[2]; - // This is not to eliminate common sub-expression, but to - // make the lines shorter so that they fit in 80 columns! - T aa = a * a; - T ab = a * b; - T ac = a * c; - T ad = a * d; - T bb = b * b; - T bc = b * c; - T bd = b * d; - T cc = c * c; - T cd = c * d; - T dd = d * d; - - R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT - R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT - R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT -} - -template inline -void EigenQuaternionToRotation(const T q[4], T R[3 * 3]) { - EigenQuaternionToRotation(q, RowMajorAdapter3x3(R)); -} - -template inline -void EigenQuaternionToRotation(const T q[4], - const ceres::MatrixAdapter& R) { - EigenQuaternionToScaledRotation(q, R); - - T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; - CHECK_NE(normalizer, T(0)); - normalizer = T(1) / normalizer; - - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) { - R(i, j) *= normalizer; - } - } -} - -template inline -void EigenUnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { - const T t2 = q[3] * q[0]; - const T t3 = q[3] * q[1]; - const T t4 = q[3] * q[2]; - const T t5 = -q[0] * q[0]; - const T t6 = q[0] * q[1]; - const T t7 = q[0] * q[2]; - const T t8 = -q[1] * q[1]; - const T t9 = q[1] * q[2]; - const T t1 = -q[2] * q[2]; - result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT - result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT - result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT -} - -template inline -void EigenQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { - // 'scale' is 1 / norm(q). - const T scale = T(1) / sqrt(q[0] * q[0] + - q[1] * q[1] + - q[2] * q[2] + - q[3] * q[3]); - - // Make unit-norm version of q. - const T unit[4] = { - scale * q[0], - scale * q[1], - scale * q[2], - scale * q[3], - }; - - EigenUnitQuaternionRotatePoint(unit, pt, result); -} - -template inline -void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) { - zw[0] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; - zw[1] = - z[0] * w[2] + z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; - zw[2] = z[0] * w[1] - z[1] * w[0] + z[2] * w[3] + z[3] * w[2]; - zw[3] = - z[0] * w[0] - z[1] * w[1] - z[2] * w[2] + z[3] * w[3]; -} - - -} // pupillabs - - -#endif /* end of include guard: CERESUTILS_H__ */ diff --git a/pupil_src/shared_cpp/include/ceres/EigenQuaternionParameterization.h b/pupil_src/shared_cpp/include/ceres/EigenQuaternionParameterization.h deleted file mode 100644 index f390a30f42..0000000000 --- a/pupil_src/shared_cpp/include/ceres/EigenQuaternionParameterization.h +++ /dev/null @@ -1,63 +0,0 @@ - -/* - Created by Lloyd Hughes on 2014/04/11. - Copyright (c) 2014 Lloyd Hughes. All rights reserved. - hughes.lloyd@gmail.com -*/ - - -#ifndef EIGENQUATERNIONPARAMETERIZATION_H__ -#define EIGENQUATERNIONPARAMETERIZATION_H__ - - -#include -#include - - -namespace pupillabs { - -// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x -// with * being the quaternion multiplication operator. Here we assume -// that the first element of the quaternion vector is the real (cos -// theta) part. -class EigenQuaternionParameterization : public ceres::LocalParameterization { -public: - virtual ~EigenQuaternionParameterization() {} - - virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const { - const Eigen::Map x(x_raw); - const Eigen::Map delta(delta_raw); - - Eigen::Map x_plus_delta(x_plus_delta_raw); - - const double delta_norm = delta.norm(); - if ( delta_norm > 0.0 ){ - const double sin_delta_by_delta = sin(delta_norm) / delta_norm; - Eigen::Quaterniond tmp( cos(delta_norm), sin_delta_by_delta*delta[0], sin_delta_by_delta*delta[1], sin_delta_by_delta*delta[2] ); - - x_plus_delta = tmp*x; - } - else { - x_plus_delta = x; - } - return true; - } - - virtual bool ComputeJacobian(const double* x, double* jacobian) const { - jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT x - jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT y - jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT z - jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT w - return true; - } - - virtual int GlobalSize() const { return 4; } - virtual int LocalSize() const { return 3; } - -}; - - -} // pupillabs - - -#endif /* end of include guard: EIGENQUATERNIONPARAMETERIZATION_H__ */ diff --git a/pupil_src/shared_cpp/include/ceres/Fixed3DNormParametrization.h b/pupil_src/shared_cpp/include/ceres/Fixed3DNormParametrization.h deleted file mode 100644 index 3b3ac502ba..0000000000 --- a/pupil_src/shared_cpp/include/ceres/Fixed3DNormParametrization.h +++ /dev/null @@ -1,160 +0,0 @@ -/* - -Copyright (C) 2014, University of Oulu, all rights reserved. -Copyright (C) 2014, NVIDIA Corporation, all rights reserved. -Third party copyrights are property of their respective owners. - -Redistribution and use in source and binary forms, with or without modification, -are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of UNIVERSITY OF OULU, NVIDIA CORPORATION nor the names of its - contributors may be used to endorse or promote products derived - from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY -EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR -CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, -EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY -OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -*/ - -#ifndef FIXED3DNORMPARAMETRIZATION_H__ -#define FIXED3DNORMPARAMETRIZATION_H__ - - -#include -#include - -namespace pupillabs -{ - -/** - * @brief A parameterization class that is used for CERES solver. It parametrizes a 3D vector (like a translation) with two components, keeping the L2 norm fixed - */ -class Fixed3DNormParametrization: public ceres::LocalParameterization -{ -public: - Fixed3DNormParametrization(double norm) - : mFixedNorm(norm) - { - } - virtual ~Fixed3DNormParametrization() - { - } - - virtual int GlobalSize() const - { - return 3; - } - virtual int LocalSize() const - { - return 2; - } - - // Calculates two vectors that are orthogonal to X. - // It first picks a non-colinear point C then basis1=(X-C) x C and basis2=X x basis1 - static void GetBasis(const double *x, double *basis1, double *basis2) - { - const double kThreshold = 0.1; - - //Check that the point we use is not colinear with x - if (x[1] > kThreshold || x[1] < -kThreshold || x[2] > kThreshold || x[2] < -kThreshold) - { - //Use C=[1,0,0] - basis1[0] = 0; - basis1[1] = x[2]; - basis1[2] = -x[1]; - - basis2[0] = -(x[1] * x[1] + x[2] * x[2]); - basis2[1] = x[0] * x[1]; - basis2[2] = x[0] * x[2]; - } - else - { - //Use C=[0,1,0] - basis1[0] = -x[2]; - basis1[1] = 0; - basis1[2] = x[0]; - - basis2[0] = x[0] * x[1]; - basis2[1] = -(x[0] * x[0] + x[2] * x[2]); - basis2[2] = x[1] * x[2]; - } - double norm; - norm = sqrt(basis1[0] * basis1[0] + basis1[1] * basis1[1] + basis1[2] * basis1[2]); - basis1[0] /= norm; - basis1[1] /= norm; - basis1[2] /= norm; - - norm = sqrt(basis2[0] * basis2[0] + basis2[1] * basis2[1] + basis2[2] * basis2[2]); - basis2[0] /= norm; - basis2[1] /= norm; - basis2[2] /= norm; - - } - - virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const - { - double basis1[3]; - double basis2[3]; - - //Translation is constrained - GetBasis(x, basis1, basis2); - - x_plus_delta[0] = x[0] + delta[0] * basis1[0] + delta[1] * basis2[0]; - x_plus_delta[1] = x[1] + delta[0] * basis1[1] + delta[1] * basis2[1]; - x_plus_delta[2] = x[2] + delta[0] * basis1[2] + delta[1] * basis2[2]; - - double norm = sqrt( - x_plus_delta[0] * x_plus_delta[0] + x_plus_delta[1] * x_plus_delta[1] + x_plus_delta[2] * x_plus_delta[2]); - double factor = mFixedNorm / norm; - x_plus_delta[0] *= factor; - x_plus_delta[1] *= factor; - x_plus_delta[2] *= factor; - - return true; - } - - virtual bool ComputeJacobian(const double *x, double *jacobian) const - { - typedef Eigen::Matrix Matrix32d; - Matrix32d &jacobian_ = *(Matrix32d *)jacobian; - double basis1[3]; - double basis2[3]; - - //Translation is special - GetBasis(x, basis1, basis2); - - jacobian_(0, 0) = basis1[0]; - jacobian_(1, 0) = basis1[1]; - jacobian_(2, 0) = basis1[2]; - - jacobian_(0, 1) = basis2[0]; - jacobian_(1, 1) = basis2[1]; - jacobian_(2, 1) = basis2[2]; - return true; - } - - -protected: - const double mFixedNorm; -}; - -} //namespace pupillabs - - -#endif /* end of include guard: FIXED3DNORMPARAMETRIZATION_H__ */ diff --git a/pupil_src/shared_cpp/include/common/colors.h b/pupil_src/shared_cpp/include/common/colors.h deleted file mode 100644 index e00d760234..0000000000 --- a/pupil_src/shared_cpp/include/common/colors.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef singleeyefitter_colors_h__ -#define singleeyefitter_colors_h__ - -#include - - -namespace singleeyefitter { - - const cv::Scalar_ mRed_color = {0, 0, 255}; - const cv::Scalar_ mGreen_color = {0, 255, 0}; - const cv::Scalar_ mBlue_color = {255, 0, 0}; - const cv::Scalar_ mRoyalBlue_color = {255, 100, 100}; - const cv::Scalar_ mYellow_color = {255, 255, 0}; - const cv::Scalar_ mWhite_color = {255, 255, 255}; - -} // singleeyefitter namespace - -#endif //singleeyefitter_types_h__ diff --git a/pupil_src/shared_cpp/include/common/constants.h b/pupil_src/shared_cpp/include/common/constants.h deleted file mode 100644 index 54bd405097..0000000000 --- a/pupil_src/shared_cpp/include/common/constants.h +++ /dev/null @@ -1,19 +0,0 @@ - -#ifndef CONSTANTS_H__ -#define CONSTANTS_H__ - -#include -namespace singleeyefitter { - - namespace constants { - const double PI = std::acos(-1); - const double TWO_PI = 2.0 * std::acos(-1); - - } // constants - - -} // singleeyefitter - - -#endif /* end of include guard: CONSTANTS_H__ */ - diff --git a/pupil_src/shared_cpp/include/common/traits.h b/pupil_src/shared_cpp/include/common/traits.h deleted file mode 100644 index 672ed9fbe2..0000000000 --- a/pupil_src/shared_cpp/include/common/traits.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef singleeyefitter_traits_h__ -#define singleeyefitter_traits_h__ - - -#include - -namespace singleeyefitter { - - struct scalar_tag {}; - struct ceres_jet_tag {}; - - template - struct ad_traits; - - template - struct ad_traits::value >::type > { - typedef scalar_tag ad_tag; - typedef T scalar; - static inline scalar value(const T& x) { return x; } - }; - - template - struct ad_traits<::ceres::Jet> { - typedef ceres_jet_tag ad_tag; - typedef T scalar; - static inline scalar get(const ::ceres::Jet& x) { return x.a; } - }; - - template - struct ad_traits < T, typename std::enable_if < !std::is_same::type>::value >::type > - : public ad_traits::type> { - }; -} - - -#endif //singleeyefitter_traits_h__ diff --git a/pupil_src/shared_cpp/include/common/types.h b/pupil_src/shared_cpp/include/common/types.h deleted file mode 100644 index 2e74c1f074..0000000000 --- a/pupil_src/shared_cpp/include/common/types.h +++ /dev/null @@ -1,133 +0,0 @@ - -#ifndef singleeyefitter_types_h__ -#define singleeyefitter_types_h__ - -#include "geometry/Ellipse.h" -#include "geometry/Circle.h" -#include "geometry/Sphere.h" -#include "projection.h" - -#include -#include -#include - -#include - - -namespace singleeyefitter { - - - //######## 2D Detector ############ - typedef std::vector > Contours_2D; - typedef std::vector Contour_2D; - typedef std::vector Edges2D; - typedef std::vector ContourIndices; - typedef Ellipse2D Ellipse; - - //######## 3D Detector ############ - - typedef Eigen::Matrix Vector2; - typedef Eigen::Matrix Vector3; - typedef Eigen::ParametrizedLine Line; - typedef Eigen::ParametrizedLine Line3; - typedef Circle3D Circle; - typedef size_t Index; - - typedef std::vector Contour3D; - typedef std::vector Edges3D; - typedef std::vector> Contours3D; - - struct ConfidenceValue{ - ConfidenceValue(double v,double c) - { - value = v; - confidence = c; - }; - ConfidenceValue() - { - value = 0; - confidence = 0; - }; - double value; - double confidence; - }; - - // general time - typedef std::chrono::steady_clock Clock; - - - // every coordinates are relative to the roi - struct Detector2DResult { - double confidence = 0.0 ; - Ellipse ellipse = Ellipse::Null; - Edges2D final_edges; // edges used to fit the final ellipse in 2D - Edges2D raw_edges; - cv::Rect current_roi; // contains the roi for this results - double timestamp = 0.0; - int image_width = 0; - int image_height = 0; - - }; - - struct ModelDebugProperties{ - Sphere sphere; - Sphere initialSphere; - std::vector binPositions; - double maturity; - double solverFit; - double confidence; - double performance; - double performanceGradient; - int modelID; - double birthTimestamp; - }; - - struct Detector3DResult { - double confidence = 0.0 ; - Circle circle = Circle::Null; - Ellipse ellipse = Ellipse::Null; // the circle projected back to 2D - Sphere sphere = Sphere::Null; - Ellipse projectedSphere = Ellipse::Null; // the sphere projected back to 2D - double timestamp; - int modelID = 0; - double modelBirthTimestamp = 0.0; - double modelConfidence = 0.0; - //-------- For visualization ---------------- - // just valid if we want it for visualization - Edges3D edges; - Circle predictedCircle = Circle::Null; - std::vector models; - }; - - // use a struct for all properties and pass it to detect method every time we call it. - // Thus we don't need to keep track if GUI is updated and cython handles conversion from Dict to struct - struct Detector2DProperties { - int intensity_range; - int blur_size; - float canny_treshold; - float canny_ration; - int canny_aperture; - int pupil_size_max; - int pupil_size_min; - float strong_perimeter_ratio_range_min; - float strong_perimeter_ratio_range_max; - float strong_area_ratio_range_min; - float strong_area_ratio_range_max; - int contour_size_min; - float ellipse_roundness_ratio; - float initial_ellipse_fit_treshhold; - float final_perimeter_ratio_range_min; - float final_perimeter_ratio_range_max; - float ellipse_true_support_min_dist; - float support_pixel_ratio_exponent; - - }; - - struct Detector3DProperties { - float model_sensitivity; - bool model_is_frozen; - }; - -} // singleeyefitter namespace - -#endif //singleeyefitter_types_h__ diff --git a/pupil_src/shared_cpp/include/geometry/Circle.h b/pupil_src/shared_cpp/include/geometry/Circle.h deleted file mode 100644 index 41da5e9e5a..0000000000 --- a/pupil_src/shared_cpp/include/geometry/Circle.h +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef _CIRCLE_H_ -#define _CIRCLE_H_ - -#include - -namespace singleeyefitter { - - template - class Circle3D { - public: - typedef T Scalar; - typedef Eigen::Matrix Vector; - - Vector center, normal; - Scalar radius; - - Circle3D() : center(0, 0, 0), normal(0, 0, 0), radius(0) - { - } - Circle3D(Vector center, Vector normal, Scalar radius) - : center(std::move(center)), normal(std::move(normal)), radius(std::move(radius)) - { - } - - static const Circle3D Null; - - }; - - template - const Circle3D Circle3D::Null = Circle3D(); - - template - bool operator== (const Circle3D& s1, const Circle3D& s2) - { - return s1.center == s2.center - && s1.normal == s2.normal - && s1.radius == s2.radius; - } - template - bool operator!= (const Circle3D& s1, const Circle3D& s2) - { - return s1.center != s2.center - || s1.normal != s2.normal - || s1.radius != s2.radius; - } - - template - std::ostream& operator<< (std::ostream& os, const Circle3D& circle) - { - return os << "Circle { center: (" << circle.center[0] << "," << circle.center[1] << "," << circle.center[2] << "), " - "normal: (" << circle.normal[0] << "," << circle.normal[1] << "," << circle.normal[2] << "), " - "radius: " << circle.radius << " }"; - } - -} - -#endif//_CIRCLE_H_ diff --git a/pupil_src/shared_cpp/include/geometry/Conic.h b/pupil_src/shared_cpp/include/geometry/Conic.h deleted file mode 100644 index ee9d8f2b52..0000000000 --- a/pupil_src/shared_cpp/include/geometry/Conic.h +++ /dev/null @@ -1,93 +0,0 @@ -#ifndef _CONIC_H_ -#define _CONIC_H_ - -#include -#include "mathHelper.h" - -namespace singleeyefitter { - - template - class Ellipse2D; - - template - class Conic { - public: - typedef T Scalar; - - Scalar A, B, C, D, E, F; - - Conic(Scalar A, Scalar B, Scalar C, Scalar D, Scalar E, Scalar F) - : A(A), B(B), C(C), D(D), E(E), F(F) - { - } - - template - explicit Conic(const Ellipse2D& ellipse) - { - using std::sin; - using std::cos; - using singleeyefitter::math::sq; - auto ax = cos(ellipse.angle); - auto ay = sin(ellipse.angle); - auto a2 = sq(ellipse.major_radius); - auto b2 = sq(ellipse.minor_radius); - A = ax * ax / a2 + ay * ay / b2; - B = 2 * ax * ay / a2 - 2 * ax * ay / b2; - C = ay * ay / a2 + ax * ax / b2; - D = (-2 * ax * ay * ellipse.center[1] - 2 * ax * ax * ellipse.center[0]) / a2 - + (2 * ax * ay * ellipse.center[1] - 2 * ay * ay * ellipse.center[0]) / b2; - E = (-2 * ax * ay * ellipse.center[0] - 2 * ay * ay * ellipse.center[1]) / a2 - + (2 * ax * ay * ellipse.center[0] - 2 * ax * ax * ellipse.center[1]) / b2; - F = (2 * ax * ay * ellipse.center[0] * ellipse.center[1] + ax * ax * ellipse.center[0] * ellipse.center[0] + ay * ay * ellipse.center[1] * ellipse.center[1]) / a2 - + (-2 * ax * ay * ellipse.center[0] * ellipse.center[1] + ay * ay * ellipse.center[0] * ellipse.center[0] + ax * ax * ellipse.center[1] * ellipse.center[1]) / b2 - - 1; - } - - Scalar operator()(Scalar x, Scalar y) const - { - return A * x * x + B * x * y + C * y * y + D * x + E * y + F; - } - - template - Conic transformed(const Eigen::MatrixBase& a, const Eigen::MatrixBase& t) const - { - static_assert(ADerived::RowsAtCompileTime == 2 && ADerived::ColsAtCompileTime == 2, "Affine transform must be 2x2 matrix"); - static_assert(TDerived::IsVectorAtCompileTime && TDerived::SizeAtCompileTime == 2, "Translation must be 2 element vector"); - // We map x,y to a new space using - // [x y] -> affine*[x y] + translation - // - // Using a for affine and t for translation: - // x -> a_00*x + a01*y + t0 - // y -> a_10*x + a11*y + t1 - // - // So - // Ax^2 + Bxy + Cy^2 + Dx + Ey + F - // becomes - // A(a_00*x + a01*y + t0)(a_00*x + a01*y + t0) - // + B(a_00*x + a01*y + t0)(a_10*x + a11*y + t1) - // + C(a_10*x + a11*y + t1)(a_10*x + a11*y + t1) - // + D(a_00*x + a01*y + t0) - // + E(a_10*x + a11*y + t1) - // + F - // - // Collecting terms gives: - return Conic( - A * sq(a(0, 0)) + B * a(0, 0) * a(1, 0) + C * sq(a(1, 0)), - 2 * A * a(0, 0) * a(0, 1) + B * a(0, 0) * a(1, 1) + B * a(0, 1) * a(1, 0) + 2 * C * a(1, 0) * a(1, 1), - A * sq(a(0, 1)) + B * a(0, 1) * a(1, 1) + C * sq(a(1, 1)), - 2 * A * a(0, 0) * t(0) + B * a(0, 0) * t(1) + B * a(1, 0) * t(0) + 2 * C * a(1, 0) * t(1) + D * a(0, 0) + E * a(1, 0), - 2 * A * a(0, 1) * t(0) + B * a(0, 1) * t(1) + B * a(1, 1) * t(0) + 2 * C * a(1, 1) * t(1) + D * a(0, 1) + E * a(1, 1), - A * sq(t(0)) + B * t(0) * t(1) + C * sq(t(1)) + D * t(0) + E * t(1) + F - ); - } - }; - - template - std::ostream& operator<< (std::ostream& os, const Conic& conic) - { - return os << "Conic { " << conic.A << "x^2 + " << conic.B << "xy + " << conic.C << "y^2 + " << conic.D << "x + " << conic.E << "y + " << conic.F << " = 0 } "; - } - -} - -#endif diff --git a/pupil_src/shared_cpp/include/geometry/Conicoid.h b/pupil_src/shared_cpp/include/geometry/Conicoid.h deleted file mode 100644 index 6a906f52af..0000000000 --- a/pupil_src/shared_cpp/include/geometry/Conicoid.h +++ /dev/null @@ -1,119 +0,0 @@ -#ifndef _CONICOID_H_ -#define _CONICOID_H_ - -#include - -namespace singleeyefitter { - - template - class Conic; - - // Conicoid (quartic surface) of the form: - // Ax^2 + By^2 + Cz^2 + 2Fyz + 2Gzx + 2Hxy + 2Ux + 2Vy + 2Wz + D = 0 - template - class Conicoid { - public: - typedef T Scalar; - - Scalar A, B, C, F, G, H, U, V, W, D; - - Conicoid(Scalar A, Scalar B, Scalar C, Scalar F, Scalar G, Scalar H, Scalar D) - : A(A), B(B), C(C), F(F), G(G), H(H), U(U), V(V), W(W), D(D) - { - } - - template - explicit Conicoid(const Conic& conic, const Eigen::MatrixBase& vertex) - { - static_assert(Derived::IsVectorAtCompileTime && Derived::SizeAtCompileTime == 3, "Cone vertex requires 3 element vector as vector type"); - using math::sq; - // Finds conicoid with given conic base and vertex - // Assumes conic is on the plane z = 0 - auto alpha = vertex[0]; - auto beta = vertex[1]; - auto gamma = vertex[2]; - A = sq(gamma) * conic.A; - B = sq(gamma) * conic.C; - C = conic.A * sq(alpha) + conic.B * alpha * beta + conic.C * sq(beta) + conic.D * alpha + conic.E * beta + conic.F; - F = -gamma * (conic.C * beta + conic.B / 2 * alpha + conic.E / 2); - G = -gamma * (conic.B / 2 * beta + conic.A * alpha + conic.D / 2); - H = sq(gamma) * conic.B / 2; - U = sq(gamma) * conic.D / 2; - V = sq(gamma) * conic.E / 2; - W = -gamma * (conic.E / 2 * beta + conic.D / 2 * alpha + conic.F); - D = sq(gamma) * conic.F; - } - - Scalar operator()(Scalar x, Scalar y, Scalar z) const - { - return A * sq(x) + B * sq(y) + C * sq(z) + 2 * F * y * z + 2 * G * x * z + 2 * H * x * y + 2 * U * x + 2 * V * y + 2 * W * z + D; - } - - Conic intersectZ(Scalar z = Scalar(0)) const - { - // Finds conic at given z intersection - // Ax^2 + By^2 + Cz^2 + 2Fyz + 2Gzx + 2Hxy + 2Ux + 2Vy + 2Wz + D = 0 - // becomes - // Ax^2 + Bxy + Cy^2 + Fx + Ey + D = 0 - return Conic(A, - 2 * H, - B, - 2 * G * z + 2 * U, - 2 * F * z + 2 * V, - C * sq(z) + 2 * W * z + D); - } - - template - Conicoid transformed(const Eigen::MatrixBase& a, const Eigen::MatrixBase& t) const - { - static_assert(ADerived::RowsAtCompileTime == 3 && ADerived::ColsAtCompileTime == 3, "Affine transform must be 3x3 matrix"); - static_assert(TDerived::IsVectorAtCompileTime && TDerived::SizeAtCompileTime == 3, "Translation must be 3 element vector"); - // We map x,y,z to a new space using - // [x y z] -> affine*[x y z] + translation - // - // Using a for affine and t for translation: - // x -> a_00*x + a01*y + a02*z + t0 - // y -> a_10*x + a11*y + a12*z + t1 - // z -> a_20*x + a21*y + a22*z + t2 - // - // So - // Ax^2 + By^2 + Cz^2 + 2Fyz + 2Gzx + 2Hxy + 2Ux + 2Vy + 2Wz + D - // becomes - // A(a_00*x + a01*y + a02*z + t0)(a_00*x + a01*y + a02*z + t0) - // + B(a_10*x + a11*y + a12*z + t1)(a_10*x + a11*y + a12*z + t1) - // + C(a_20*x + a21*y + a22*z + t2)(a_20*x + a21*y + a22*z + t2) - // + 2F(a_10*x + a11*y + a12*z + t1)(a_20*x + a21*y + a22*z + t2) - // + 2G(a_20*x + a21*y + a22*z + t2)(a_00*x + a01*y + a02*z + t0) - // + 2H(a_00*x + a01*y + a02*z + t0)(a_10*x + a11*y + a12*z + t1) - // + 2U(a_00*x + a01*y + a02*z + t0) - // + 2V(a_10*x + a11*y + a12*z + t1) - // + 2W(a_20*x + a21*y + a22*z + t2) - // + D - // - // Collecting terms gives: - return Conicoid( - A * sq(a(0, 0)) + B * sq(a(1, 0)) + C * sq(a(2, 0)) + Scalar(2) * F * a(1, 0) * a(2, 0) + Scalar(2) * G * a(0, 0) * a(2, 0) + Scalar(2) * H * a(0, 0) * a(1, 0), - A * sq(a(0, 1)) + B * sq(a(1, 1)) + C * sq(a(2, 1)) + Scalar(2) * F * a(1, 1) * a(2, 1) + Scalar(2) * G * a(0, 1) * a(2, 1) + Scalar(2) * H * a(0, 1) * a(1, 1), - A * sq(a(0, 2)) + B * sq(a(1, 2)) + C * sq(a(2, 2)) + Scalar(2) * F * a(1, 2) * a(2, 2) + Scalar(2) * G * a(0, 2) * a(2, 2) + Scalar(2) * H * a(0, 2) * a(1, 2), - A * a(0, 1) * a(0, 2) + B * a(1, 1) * a(1, 2) + C * a(2, 1) * a(2, 2) + F * a(1, 1) * a(2, 2) + F * a(1, 2) * a(2, 1) + G * a(0, 1) * a(2, 2) + G * a(0, 2) * a(2, 1) + H * a(0, 1) * a(1, 2) + H * a(0, 2) * a(1, 1), - A * a(0, 2) * a(0, 0) + B * a(1, 2) * a(1, 0) + C * a(2, 2) * a(2, 0) + F * a(1, 2) * a(2, 0) + F * a(1, 0) * a(2, 2) + G * a(0, 2) * a(2, 0) + G * a(0, 0) * a(2, 2) + H * a(0, 2) * a(1, 0) + H * a(0, 0) * a(1, 2), - A * a(0, 0) * a(0, 1) + B * a(1, 0) * a(1, 1) + C * a(2, 0) * a(2, 1) + F * a(1, 0) * a(2, 1) + F * a(1, 1) * a(2, 0) + G * a(0, 0) * a(2, 1) + G * a(0, 1) * a(2, 0) + H * a(0, 0) * a(1, 1) + H * a(0, 1) * a(1, 0), - A * a(0, 0) * t(0) + B * a(1, 0) * t(1) + C * a(2, 0) * t(2) + F * a(1, 0) * t(2) + F * a(2, 0) * t(1) + G * a(0, 0) * t(2) + G * a(2, 0) * t(0) + H * a(0, 0) * t(1) + H * a(1, 0) * t(0) + U * a(0, 0) + V * a(1, 0) + W * a(2, 0), - A * a(0, 1) * t(0) + B * a(1, 1) * t(1) + C * a(2, 1) * t(2) + F * a(1, 1) * t(2) + F * a(2, 1) * t(1) + G * a(0, 1) * t(2) + G * a(2, 1) * t(0) + H * a(0, 1) * t(1) + H * a(1, 1) * t(0) + U * a(0, 1) + V * a(1, 1) + W * a(2, 1), - A * a(0, 2) * t(0) + B * a(1, 2) * t(1) + C * a(2, 2) * t(2) + F * a(1, 2) * t(2) + F * a(2, 2) * t(1) + G * a(0, 2) * t(2) + G * a(2, 2) * t(0) + H * a(0, 2) * t(1) + H * a(1, 2) * t(0) + U * a(0, 2) + V * a(1, 2) + W * a(2, 2), - A * sq(t(0)) + B * sq(t(1)) + C * sq(t(2)) + Scalar(2) * F * t(1) * t(2) + Scalar(2) * G * t(0) * t(2) + Scalar(2) * H * t(0) * t(1) + Scalar(2) * U * t(0) + Scalar(2) * V * t(1) + Scalar(2) * W * t(2) + D - ); - } - }; - - template - std::ostream& operator<< (std::ostream& os, const Conicoid& conicoid) - { - return os << "Conicoid { " << conicoid.A << "x^2 + " << conicoid.B << "y^2 + " << conicoid.C << "z^2 + " - "2*" << 2 * conicoid.F << "yz + 2*" << 2 * conicoid.G << "zx + 2*" << 2 * conicoid.H << "xy + " - "2*" << 2 * conicoid.U << "x + 2*" << 2 * conicoid.V << "y + 2*" << 2 * conicoid.W << "z + " << conicoid.D << " = 0 }"; - } - -} - -#endif diff --git a/pupil_src/shared_cpp/include/geometry/Ellipse.h b/pupil_src/shared_cpp/include/geometry/Ellipse.h deleted file mode 100644 index 244d0306cf..0000000000 --- a/pupil_src/shared_cpp/include/geometry/Ellipse.h +++ /dev/null @@ -1,156 +0,0 @@ -#ifndef _ELLIPSE_H_ -#define _ELLIPSE_H_ - -#include "common/constants.h" - -#include - -namespace singleeyefitter { - - template - class Conic; - - template - class Ellipse2D { - public: - typedef T Scalar; - typedef Eigen::Matrix Vector; - Vector center; - Scalar major_radius; - Scalar minor_radius; - Scalar angle; - - Ellipse2D() - : center(0, 0), major_radius(0), minor_radius(0), angle(0) - { - } - template - Ellipse2D(const Eigen::EigenBase& center, Scalar major_radius, Scalar minor_radius, Scalar angle) - : center(center), major_radius(major_radius), minor_radius(minor_radius), angle(angle) - { - } - Ellipse2D(Scalar x, Scalar y, Scalar major_radius, Scalar minor_radius, Scalar angle) - : center(x, y), major_radius(major_radius), minor_radius(minor_radius), angle(angle) - { - } - template - explicit Ellipse2D(const Conic& conic) - { - using std::atan2; - using std::sin; - using std::cos; - using std::sqrt; - using std::abs; - angle = 0.5 * atan2(conic.B, conic.A - conic.C); - auto cost = cos(angle); - auto sint = sin(angle); - auto sin_squared = sint * sint; - auto cos_squared = cost * cost; - auto Ao = conic.F; - auto Au = conic.D * cost + conic.E * sint; - auto Av = -conic.D * sint + conic.E * cost; - auto Auu = conic.A * cos_squared + conic.C * sin_squared + conic.B * sint * cost; - auto Avv = conic.A * sin_squared + conic.C * cos_squared - conic.B * sint * cost; - // ROTATED = [Ao Au Av Auu Avv] - auto tucenter = -Au / (2.0 * Auu); - auto tvcenter = -Av / (2.0 * Avv); - auto wcenter = Ao - Auu * tucenter * tucenter - Avv * tvcenter * tvcenter; - center[0] = tucenter * cost - tvcenter * sint; - center[1] = tucenter * sint + tvcenter * cost; - major_radius = sqrt(abs(-wcenter / Auu)); - minor_radius = sqrt(abs(-wcenter / Avv)); - - if (major_radius < minor_radius) { - std::swap(major_radius, minor_radius); - angle = angle + constants::PI / 2; - } - - if (angle > constants::PI ) - angle = angle - constants::PI ; - } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF((sizeof(Vector) % 16) == 0) - - Vector major_axis() const - { - using std::sin; - using std::cos; - return Vector(major_radius * sin(angle), major_radius * cos(angle)); - } - Vector minor_axis() const - { - using std::sin; - using std::cos; - return Vector(-minor_radius * cos(angle), minor_radius * sin(angle)); - } - - Scalar circumference() const - { - using std::abs; - using std::sqrt; - using std::pow; - return constants::PI * abs(3.0 * (major_radius + minor_radius) - - sqrt(10.0 * major_radius * minor_radius + 3.0 * - (pow(major_radius, 2) + pow(minor_radius, 2)))); - } - Scalar area() const - { - return constants::PI * major_radius * minor_radius; - } - - - static const Ellipse2D Null; - - private: - - }; - - template - const Ellipse2D Ellipse2D::Null = Ellipse2D(); - - template - bool operator==(const Ellipse2D& el1, const Ellipse2D& el2) - { - return el1.center[0] == el2.center[0] && - el1.center[1] == el2.center[1] && - el1.major_radius == el2.major_radius && - el1.minor_radius == el2.minor_radius && - el1.angle == el2.angle; - } - template - bool operator!=(const Ellipse2D& el1, const Ellipse2D& el2) - { - return !(el1 == el2); - } - - template - std::ostream& operator<< (std::ostream& os, const Ellipse2D& ellipse) - { - return os << "Ellipse { center: (" << ellipse.center[0] << "," << ellipse.center[1] << "), a: " << - ellipse.major_radius << ", b: " << ellipse.minor_radius << ", theta: " << (ellipse.angle / constants::PI) << "pi }"; - } - - template - Ellipse2D scaled(const Ellipse2D& ellipse, U scale) - { - return Ellipse2D( - ellipse.center[0].a, - ellipse.center[1].a, - ellipse.major_radius.a, - ellipse.minor_radius.a, - ellipse.angle.a); - } - - template - inline Eigen::Matrix::type, 2, 1> pointAlongEllipse(const Ellipse2D& el, Scalar2 t) - { - using std::sin; - using std::cos; - auto xt = el.center.x() + el.major_radius * cos(el.angle) * cos(t) - el.minor_radius * sin(el.angle) * sin(t); - auto yt = el.center.y() + el.major_radius * sin(el.angle) * cos(t) + el.minor_radius * cos(el.angle) * sin(t); - return Eigen::Matrix::type, 2, 1>(xt, yt); - } - -} - -#endif diff --git a/pupil_src/shared_cpp/include/geometry/Sphere.h b/pupil_src/shared_cpp/include/geometry/Sphere.h deleted file mode 100644 index 70432341f7..0000000000 --- a/pupil_src/shared_cpp/include/geometry/Sphere.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _SPHERE_H_ -#define _SPHERE_H_ - -#include - -namespace singleeyefitter { - - template - class Sphere { - public: - typedef T Scalar; - typedef Eigen::Matrix Vector; - - Vector center; - Scalar radius; - - Sphere() : center(0, 0, 0), radius(0) - { - } - Sphere(Vector center, Scalar radius) - : center(std::move(center)), radius(std::move(radius)) - { - } - - static const Sphere Null; - - private: - - }; - - template - const Sphere Sphere::Null = Sphere(); - - template - bool operator== (const Sphere& s1, const Sphere& s2) - { - return s1.center == s2.center - && s1.radius == s2.radius; - } - template - bool operator!= (const Sphere& s1, const Sphere& s2) - { - return s1.center != s2.center - || s1.radius != s2.radius; - } - - template - std::ostream& operator<< (std::ostream& os, const Sphere& circle) - { - return os << "Sphere { center: (" << circle.center[0] << "," << circle.center[1] << "," << circle.center[2] << "), " - "radius: " << circle.radius << " }"; - } - -} - -#endif//_SPHERE_H_ diff --git a/pupil_src/shared_cpp/include/math/distance.h b/pupil_src/shared_cpp/include/math/distance.h deleted file mode 100644 index 4a029d1790..0000000000 --- a/pupil_src/shared_cpp/include/math/distance.h +++ /dev/null @@ -1,185 +0,0 @@ -#ifndef distance_h__ -#define distance_h__ - -#include -#include -#include "geometry/Ellipse.h" -#include "DistancePointEllipse.h" - -using namespace singleeyefitter; - -template -auto euclidean_distance(const Eigen::MatrixBase& p1, const Eigen::MatrixBase& p2) -> decltype((p1 - p2).norm()) -{ - return (p1 - p2).norm(); -} -template -auto euclidean_distance_squared(const Eigen::MatrixBase& p1, const Eigen::MatrixBase& p2) -> decltype((p1 - p2).squaredNorm()) -{ - return (p1 - p2).squaredNorm(); -} -template -auto euclidean_distance(const Eigen::MatrixBase& point, - const Eigen::ParametrizedLine& line) -> decltype(point.norm()) -{ - return ((line.origin() - point) - ((line.origin() - point).dot(line.direction())) * line.direction()).norm(); -} - -template -auto euclidean_distance_squared(const Eigen::MatrixBase& point, - const Eigen::ParametrizedLine& line) -> decltype(point.norm()) -{ - return ((line.origin() - point) - ((line.origin() - point).dot(line.direction())) * line.direction()).squaredNorm(); -} - - -template -Scalar euclidean_distance(const Eigen::ParametrizedLine& line1, - const Eigen::ParametrizedLine& line2) -{ - return std::sqrt( euclidean_distance_squared(line1, line2) ) ; -} - -template -Scalar euclidean_distance_squared(const Eigen::ParametrizedLine& line1, - const Eigen::ParametrizedLine& line2) -{ - - auto closestPoints = closest_points_on_line(line1, line2); - auto diff = closestPoints.first - closestPoints.second; - return diff.dot(diff); -} - -template -std::pair< typename Eigen::ParametrizedLine::VectorType , typename Eigen::ParametrizedLine::VectorType > -closest_points_on_line(const Eigen::ParametrizedLine& line1, - const Eigen::ParametrizedLine& line2) -{ - typedef typename Eigen::ParametrizedLine::VectorType Vector; - Vector diff = line1.origin() - line2.origin(); - Scalar a01 = -line1.direction().dot(line2.direction()); - Scalar b0 = diff.dot(line1.direction()); - Scalar s0, s1; - - if (std::abs(a01) < Scalar(1) ) - { - // Lines are not parallel. - Scalar det = Scalar(1) - a01 * a01; - Scalar b1 = -diff.dot(line2.direction()); - s0 = (a01 * b1 - b0) / det; - s1 = (a01 * b0 - b1) / det; - } - else - { - // Lines are parallel, select any pair of closest points. - s0 = -b0; - s1 = Scalar(0); - } - - Vector closestPoint1 = line1.origin() + s0 * line1.direction(); - Vector closestPoint2 = line2.origin() + s1 * line2.direction(); - return std::pair(closestPoint1, closestPoint2); -} - -template -Scalar euclidean_distance(const Eigen::Matrix& p, const Eigen::Matrix& v, const Eigen::Matrix& w) -{ - // Return minimum distance between line segment vw and point p - auto l2 = (v - w).squaredNorm(); - - if (l2 == 0.0) - return euclidean_distance(p, v); // v == w case - - // Consider the line extending the segment, parameterized as v + t (w - v). - // We find projection of point p onto the line. - // It falls where t = [(p-v) . (w-v)] / |w-v|^2 - auto t = (p - v).dot(w - v) / l2; - - if (t < 0.0) - return euclidean_distance(p, v); // Beyond the 'v' end of the segment - else if (t > 1.0) - return euclidean_distance(p, w); // Beyond the 'w' end of the segment - - auto projection = v + t * (w - v); // Projection falls on the segment - return euclidean_distance(p, projection); -} - -template -Scalar euclidean_distance(const Eigen::Matrix& point, const std::vector>& polygon) -{ - auto from = polygon.back(); - Scalar min_distance = std::numeric_limits::infinity(); - - for (const auto& to : polygon) { - min_distance = std::min(min_distance, euclidean_distance(point, from, to)); - from = to; - } - - return min_distance; -} -template -Scalar euclidean_distance( const std::vector>& polygon) -{ - auto from = polygon.back(); - Scalar distance = 0.0; - - for (const auto& to : polygon) { - distance += euclidean_distance(from, to); - from = to; - } - - return distance; -} - - -template -Scalar euclidean_distance(const Eigen::Matrix& point, const Ellipse2D& ellipse) -{ - return DistancePointEllipse(ellipse, point[0], point[1]); -} -template -Scalar euclidean_distance(const Scalar x, const Scalar y, const Ellipse2D& ellipse) -{ - return DistancePointEllipse(ellipse, x, y); -} - -template -Scalar oneway_hausdorff_distance(const Ellipse2D& ellipse, const TOther& other) -{ - Scalar max_dist = -1; - - for (Scalar i = 0; i < 100; ++i) { - Scalar t = i * 2 * M_PI / 100; - auto pt = pointAlongEllipse(ellipse, t); - Scalar i_dist = euclidean_distance(pt, other); - max_dist = std::max(max_dist, i_dist); - } - - return max_dist; -} -template -Scalar oneway_hausdorff_distance(const std::vector>& polygon, const TOther& other) -{ - Scalar max_dist = -1; - - for (const auto& pt : polygon) { - Scalar pt_dist = euclidean_distance(pt, other); - max_dist = std::max(max_dist, pt_dist); - } - - return max_dist; -} - -template -Scalar hausdorff_distance(const Ellipse2D& ellipse, const TOther& other) -{ - return std::max(oneway_hausdorff_distance(ellipse, other), oneway_hausdorff_distance(ellipse, other)); -} -template -typename std::enable_if < !std::is_same>::value, Scalar >::type - hausdorff_distance(const TOther& other, const Ellipse2D& ellipse) -{ - return hausdorff_distance(ellipse, other); -} - -#endif // distance_h__ diff --git a/pupil_src/shared_cpp/include/math/intersect.h b/pupil_src/shared_cpp/include/math/intersect.h deleted file mode 100644 index c73c3ec076..0000000000 --- a/pupil_src/shared_cpp/include/math/intersect.h +++ /dev/null @@ -1,127 +0,0 @@ -#ifndef __INTERSECT_H__ -#define __INTERSECT_H__ - -#include -#include -#include "geometry/Sphere.h" -#include "mathHelper.h" - -namespace singleeyefitter { - -// Finds the intersection of 2 lines in 2D - template - Eigen::Matrix intersect(const Eigen::ParametrizedLine& line1, const Eigen::ParametrizedLine& line2) - { - Scalar x1 = line1.origin().x(); - Scalar y1 = line1.origin().y(); - Scalar x2 = (line1.origin() + line1.direction()).x(); - Scalar y2 = (line1.origin() + line1.direction()).y(); - Scalar x3 = line2.origin().x(); - Scalar y3 = line2.origin().y(); - Scalar x4 = (line2.origin() + line2.direction()).x(); - Scalar y4 = (line2.origin() + line2.direction()).y(); - Scalar denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); - Scalar px = ((x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * x4 - y3 * x4)) / denom; - Scalar py = ((x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * x4 - y3 * x4)) / denom; - return Eigen::Matrix(px, py); - } - - template - typename Range::value_type::VectorType nearest_intersect(const Range& lines); - - namespace detail { - template - struct intersect_helper { - static Eigen::Matrix nearest(const Eigen::ParametrizedLine& line1, const Eigen::ParametrizedLine& line2) - { - std::vector> lines; - lines.push_back(line1); - lines.push_back(line2); - return nearest_intersect(lines); - } - }; - - // Specialise for 2D - template - struct intersect_helper { - static Eigen::Matrix nearest(const Eigen::ParametrizedLine& line1, const Eigen::ParametrizedLine& line2) - { - return intersect(line1, line2); - } - }; - } - -// Finds the intersection (in a least-squares sense, or exact in 2D) of 2 lines - template - Eigen::Matrix nearest_intersect(const Eigen::ParametrizedLine& line1, const Eigen::ParametrizedLine& line2) - { - return detail::intersect_helper::nearest(line1, line2); - } - -// Finds the intersection (in a least-squares sense) of multiple lines - template - typename Range::value_type::VectorType nearest_intersect(const Range& lines) - { - // NOTE: The type 'Range' will be of type std::vector> - typedef typename Range::value_type::VectorType Vector; - typedef typename Eigen::Matrix Matrix; - static_assert(Vector::ColsAtCompileTime == 1, "Requires column vector"); - //size_t N = lines.size(); - std::vector Ivv; - Matrix A = Matrix::Zero(); - Vector b = Vector::Zero(); - size_t i = 0; - - for (auto& line : lines) { - Vector vi = line.direction(); - Vector pi = line.origin(); - Matrix Ivivi = Matrix::Identity() - vi * vi.transpose(); - Ivv.push_back(Ivivi); - A += Ivivi; - b += (Ivivi * pi); - i++; - } - - Vector x = A.partialPivLu().solve(b); - /*if nargout == 2 - % Calculate error term - for i = 1:n - pi = p(i,:)'; - e = e + ( (x-pi)' * Ivv(:,:,i) * (x-pi) ); - end - end*/ - return x; - } - -// Finds the intersection of a line and a sphere - template - bool intersect(const Eigen::ParametrizedLine& line, const Sphere& sphere, std::pair, Eigen::Matrix>& intersected_points ) - { - using std::sqrt; - using math::sq; - typedef typename Eigen::ParametrizedLine::VectorType Vector; - assert(std::abs(line.direction().norm() - 1) < 0.0001); - Vector v = line.direction(); - // Put p at origin - Vector p = line.origin(); - Vector c = sphere.center - p; - Scalar r = sphere.radius; - // From Wikipedia :) - Scalar vcvc_cc_rr = sq(v.dot(c)) - c.dot(c) + sq(r); - - if (vcvc_cc_rr < 0) { - return false; - } - - Scalar s1 = v.dot(c) - sqrt(vcvc_cc_rr); - Scalar s2 = v.dot(c) + sqrt(vcvc_cc_rr); - Vector p1 = p + s1 * v; - Vector p2 = p + s2 * v; - intersected_points.first = p1; - intersected_points.second = p2; - return true; - } - -} - -#endif//__INTERSECT_H__ diff --git a/pupil_src/shared_cpp/include/mathHelper.h b/pupil_src/shared_cpp/include/mathHelper.h deleted file mode 100644 index 8966dcc89d..0000000000 --- a/pupil_src/shared_cpp/include/mathHelper.h +++ /dev/null @@ -1,351 +0,0 @@ -#ifndef singleeyefitter_math_h__ -#define singleeyefitter_math_h__ - -#include -#include - -#include "common/traits.h" - -namespace singleeyefitter { - - namespace math { - -#define MAKE_SQ(TYPE) \ - inline auto sq(TYPE val) -> decltype(val*val) { return val * val; } - - MAKE_SQ(float) - MAKE_SQ(double) - MAKE_SQ(long double) - MAKE_SQ(char) - MAKE_SQ(short) - MAKE_SQ(int) - MAKE_SQ(long) - MAKE_SQ(long long) - MAKE_SQ(unsigned char) - MAKE_SQ(unsigned short) - MAKE_SQ(unsigned int) - MAKE_SQ(unsigned long) - MAKE_SQ(unsigned long long) - -#undef MAKE_STD_SQ - - template - inline T round(T value, T precision) { - T factor = T(1) / precision; - return floor( value * factor + 0.5 ) / factor; - } - - template - inline T clamp(T val, TMin min_val = std::numeric_limits::min(), TMax max_val = std::numeric_limits::max()) - { - if (min_val > max_val) - return clamp(val, max_val, min_val); - - if (val <= min_val) - return min_val; - - if (val >= max_val) - return max_val; - - return val; - } - - template - inline T lerp(const T& val1, const T& val2, const T& alpha) - { - return val1 * (1.0 - alpha) + val2 * alpha; - } - - template - Scalar getAngleABC(const T& a, const T& b, const T& c) - { - T ab = { b.x - a.x, b.y - a.y }; - T cb = { b.x - c.x, b.y - c.y }; - Scalar dot = ab.dot(cb); // dot product - Scalar cross = ab.cross(cb); // cross product - Scalar alpha = atan2(cross, dot); - return alpha * Scalar(180.0) / M_PI; - } - - template - inline T smootherstep(T edge0, T edge1, T x, scalar_tag) - { - if (x >= edge1) - return T(1); - else if (x <= edge0) - return T(0); - else { - x = (x - edge0) / (edge1 - edge0); - return x * x * x * (x * (x * T(6) - T(15)) + T(10)); - } - } - template - inline ::ceres::Jet smootherstep(T edge0, T edge1, const ::ceres::Jet& f, ceres_jet_tag) - { - if (f.a >= edge1) - return ::ceres::Jet(1); - else if (f.a <= edge0) - return ::ceres::Jet(0); - else { - T x = (f.a - edge0) / (edge1 - edge0); - // f is referenced by this function, so create new value for return. - ::ceres::Jet g; - g.a = x * x * x * (x * (x * T(6) - T(15)) + T(10)); - g.v = f.v * (x * x * (x * (x * T(30) - T(60)) + T(30)) / (edge1 - edge0)); - return g; - } - } - template - inline ::ceres::Jet smootherstep(T edge0, T edge1, ::ceres::Jet&& f, ceres_jet_tag) - { - if (f.a >= edge1) - return ::ceres::Jet(1); - else if (f.a <= edge0) - return ::ceres::Jet(0); - else { - T x = (f.a - edge0) / (edge1 - edge0); - // f is moved into this function, so reuse it. - f.a = x * x * x * (x * (x * T(6) - T(15)) + T(10)); - f.v *= (x * x * (x * (x * T(30) - T(60)) + T(30)) / (edge1 - edge0)); - return f; - } - } - template - inline auto smootherstep(typename ad_traits::scalar edge0, typename ad_traits::scalar edge1, T&& val) - -> decltype(smootherstep(edge0, edge1, std::forward(val), typename ad_traits::ad_tag())) - { - return smootherstep(edge0, edge1, std::forward(val), typename ad_traits::ad_tag()); - } - - template - inline T norm(T x, T y, scalar_tag) - { - using std::sqrt; - using math::sq; - return sqrt(sq(x) + sq(y)); - } - template - inline ::ceres::Jet norm(const ::ceres::Jet& x, const ::ceres::Jet& y, ceres_jet_tag) - { - T anorm = norm(x.a, y.a, scalar_tag()); - ::ceres::Jet g; - g.a = anorm; - g.v = (x.a / anorm) * x.v + (y.a / anorm) * y.v; - return g; - } - template - inline typename std::decay::type norm(T&& x, T&& y) - { - return norm(std::forward(x), std::forward(y), typename ad_traits::ad_tag()); - } - - template - inline auto Heaviside(T&& val, typename ad_traits::scalar epsilon) -> decltype(smootherstep(-epsilon, epsilon, std::forward(val))) - { - return smootherstep(-epsilon, epsilon, std::forward(val)); - } - - template - Eigen::Matrix sph2cart(T r, T theta, T psi) - { - using std::sin; - using std::cos; - return r * Eigen::Matrix(sin(theta) * cos(psi), cos(theta), sin(theta) * sin(psi)); - } - - template - Eigen::Matrix cart2sph(T x, T y, T z) - { - using std::sin; - using std::cos; - using std::sqrt; - double r = sqrt( x*x + y*y + z*z); - double theta = acos( y / r); - double psi = atan2(z, x ); - return Eigen::Matrix(theta,psi); - } - template - Eigen::Matrix cart2sph(const Eigen::Matrix& m ) - { - return cart2sph( m.x(), m.y(), m.z()); - } - - template - inline ::ceres::Jet sq(::ceres::Jet val) - { - val.v *= 2 * val.a; - val.a *= val.a; - return val; - } - template - inline int sign(const T& z) - { - return (z == 0) ? 0 : (z < 0) ? -1 : 1; - } - - template - T haversine(T theta1, T psi1, T theta2, T psi2 ) - { - using std::sin; - using std::cos; - using std::acos; - using std::asin; - using std::atan2; - using std::sqrt; - using singleeyefitter::math::sq; - - if (theta1 == theta2 && psi1 == psi2) { - return T(0); - } - // Haversine distance - auto dist = T(2) * asin(sqrt( (sin((theta2 - theta1) / T(2))*sin((theta2 - theta1) / T(2))) + cos(theta1) * cos(theta2) * (sin((psi2 - psi1) / T(2))*sin((psi2 - psi1) / T(2))) )); - return dist; - - } - - - - // Hash function for Eigen matrix and vector. - // The code is from `hash_combine` function of the Boost library. See - // http://www.boost.org/doc/libs/1_55_0/doc/html/hash/reference.html#boost.hash_combine . - template - struct matrix_hash : std::unary_function { - std::size_t operator()(T const& matrix) const - { - // Note that it is oblivious to the storage order of Eigen matrix (column- or - // row-major). It will give you the same hash value for two different matrices if they - // are the transpose of each other in different storage order. - size_t seed = 0; - for (size_t i = 0; i < matrix.size(); ++i) { - auto elem = *(matrix.data() + i); - seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - } - return seed; - } - }; - - template - class SMA{ //simple moving average - - public: - - SMA( int windowSize ) : mWindowSize(windowSize), mAverage(0.0), mAverageDirty(true) - {}; - - void addValue( T value ){ - mValues.push_back( value ); - // calculate moving average of value - if( mValues.size() <= mWindowSize || mAverageDirty ){ - mAverageDirty = false; - mAverage = 0.0; - for(auto& element : mValues){ - mAverage += element; - } - mAverage /= mValues.size(); - }else{ - // we can optimize if the wanted window size is reached - T first = mValues.front(); - mValues.pop_front(); - mAverage += value/mWindowSize - first/mWindowSize; - } - } - - double getAverage() const { return mAverage; }; - int getWindowSize() const { return mWindowSize; }; - - void changeWindowSize( int windowSize){ - - if( windowSize < mWindowSize){ - - if( mValues.size() > windowSize ) - mAverageDirty = true; - while( mValues.size() > windowSize){ - mValues.pop_front(); - } - - } - mWindowSize = windowSize; - - } - - private: - - SMA(){}; - - std::list mValues; - int mWindowSize; - T mAverage; - bool mAverageDirty; // when we change the window size we need to recalculate from ground up - }; - - template - class WMA{ //weighted moving average - - public: - - WMA( int windowSize ) : mWindowSize(windowSize) , mDenominator(1.0), mNumerator(0.0), mAverage(0.0), mAverageDirty(true) - {}; - - void addValue( T value , T weight ){ - mValues.emplace_back( value, weight ); - // calculate weighted moving average of value - - if( mValues.size() <= mWindowSize || mAverageDirty){ - mAverageDirty = false; - mDenominator = 0.0; - mNumerator = 0.0; - for(auto& element : mValues){ - mNumerator += element.first * element.second; - mDenominator += element.second; - } - mAverage = mNumerator / mDenominator; - }else{ - // we can optimize if the wanted window size is reached - auto observation = mValues.front(); - mValues.pop_front(); - mDenominator -= observation.second; - mDenominator += weight; - - mNumerator -= observation.first * observation.second; - mNumerator += value * weight; - mAverage = mNumerator / mDenominator; - } - - } - - double getAverage() const { return mAverage; }; - int getWindowSize() const { return mWindowSize; }; - - void changeWindowSize( int windowSize){ - - if( windowSize < mWindowSize){ - - if( mValues.size() > windowSize ) - mAverageDirty = true; - while( mValues.size() > windowSize){ - mValues.pop_front(); - } - - } - mWindowSize = windowSize; - - } - - private: - - WMA(){}; - - std::list> mValues; - int mWindowSize; - T mDenominator; - T mNumerator; - T mAverage; - bool mAverageDirty; - }; - - } // math namespace - -} // singleeyefitter namespace - - -#endif // singleeyefitter_math_h__ diff --git a/pupil_src/shared_cpp/include/projection.h b/pupil_src/shared_cpp/include/projection.h deleted file mode 100644 index 4b0f0ba58b..0000000000 --- a/pupil_src/shared_cpp/include/projection.h +++ /dev/null @@ -1,293 +0,0 @@ -#ifndef singleeyefitter_project_h__ -#define singleeyefitter_project_h__ - -#include -#include -#include "geometry/Ellipse.h" -#include "geometry/Circle.h" -#include "geometry/Conic.h" -#include "geometry/Sphere.h" -#include "geometry/Conicoid.h" -#include "mathHelper.h" -#include "solve.h" - -namespace singleeyefitter { - - template - Conic project(const Circle3D& circle, Scalar focal_length) - { - typedef typename Circle3D::Vector Vector; - using math::sq; - Vector c = circle.center; - Vector n = circle.normal; - Scalar r = circle.radius; - Scalar f = focal_length; - // Construct cone with circle as base and vertex v = (0,0,0). - // - // For the circle, - // |p - c|^2 = r^2 where (p-c).n = 0 (i.e. on the circle plane) - // - // A cone is basically concentric circles, with center on the line c->v. - // For any point p, the corresponding circle center c' is the intersection - // of the line c->v and the plane through p normal to n. So, - // - // d = ((p - v).n)/(c.n) - // c' = d c + v - // - // The radius of these circles decreases linearly as you approach 0, so - // - // |p - c'|^2 = (r*|c' - v|/|c - v|)^2 - // - // Since v = (0,0,0), this simplifies to - // - // |p - (p.n/c.n)c|^2 = (r*|(p.n/c.n)c|/|c|)^2 - // - // |(c.n)p - (p.n)c|^2 / p.n \^2 - // ------------------- = r^2 * | --- | - // (c.n)^2 \ c.n / - // - // |(c.n)p - (p.n)c|^2 - r^2 * (p.n)^2 = 0 - // - // Expanding out p, c and n gives - // - // |(c.n)x - (x*n_x + y*n_y + z*n_z)c_x|^2 - // |(c.n)y - (x*n_x + y*n_y + z*n_z)c_y| - r^2 * (x*n_x + y*n_y + z*n_z)^2 = 0 - // |(c.n)z - (x*n_x + y*n_y + z*n_z)c_z| - // - // ((c.n)x - (x*n_x + y*n_y + z*n_z)c_x)^2 - // + ((c.n)y - (x*n_x + y*n_y + z*n_z)c_y)^2 - // + ((c.n)z - (x*n_x + y*n_y + z*n_z)c_z)^2 - // - r^2 * (x*n_x + y*n_y + z*n_z)^2 = 0 - // - // (c.n)^2 x^2 - 2*(c.n)*(x*n_x + y*n_y + z*n_z)*x*c_x + (x*n_x + y*n_y + z*n_z)^2 c_x^2 - // + (c.n)^2 y^2 - 2*(c.n)*(x*n_x + y*n_y + z*n_z)*y*c_y + (x*n_x + y*n_y + z*n_z)^2 c_y^2 - // + (c.n)^2 z^2 - 2*(c.n)*(x*n_x + y*n_y + z*n_z)*z*c_z + (x*n_x + y*n_y + z*n_z)^2 c_z^2 - // - r^2 * (x*n_x + y*n_y + z*n_z)^2 = 0 - // - // (c.n)^2 x^2 - 2*(c.n)*c_x*(x*n_x + y*n_y + z*n_z)*x - // + (c.n)^2 y^2 - 2*(c.n)*c_y*(x*n_x + y*n_y + z*n_z)*y - // + (c.n)^2 z^2 - 2*(c.n)*c_z*(x*n_x + y*n_y + z*n_z)*z - // + (x*n_x + y*n_y + z*n_z)^2 * (c_x^2 + c_y^2 + c_z^2 - r^2) - // - // (c.n)^2 x^2 - 2*(c.n)*c_x*(x*n_x + y*n_y + z*n_z)*x - // + (c.n)^2 y^2 - 2*(c.n)*c_y*(x*n_x + y*n_y + z*n_z)*y - // + (c.n)^2 z^2 - 2*(c.n)*c_z*(x*n_x + y*n_y + z*n_z)*z - // + (|c|^2 - r^2) * (n_x^2*x^2 + n_y^2*y^2 + n_z^2*z^2 + 2*n_x*n_y*x*y + 2*n_x*n_z*x*z + 2*n_y*n_z*y*z) - // - // Collecting conicoid terms gives - // - // [xyz]^2 : (c.n)^2 - 2*(c.n)*c_[xyz]*n_[xyz] + (|c|^2 - r^2)*n_[xyz]^2 - // [yzx][zxy] : - 2*(c.n)*c_[yzx]*n_[zxy] - 2*(c.n)*c_[zxy]*n_[yzx] + (|c|^2 - r^2)*2*n_[yzx]*n_[zxy] - // : 2*((|c|^2 - r^2)*n_[yzx]*n_[zxy] - (c,n)*(c_[yzx]*n_[zxy] + c_[zxy]*n_[yzx])) - // [xyz] : 0 - // 1 : 0 - Scalar cn = c.dot(n); - Scalar c2r2 = (c.dot(c) - sq(r)); - Vector ABC = (sq(cn) - 2.0 * cn * c.array() * n.array() + c2r2 * n.array().square()); - Scalar F = 2.0 * (c2r2 * n(1) * n(2) - cn * (n(1) * c(2) + n(2) * c(1))); - Scalar G = 2.0 * (c2r2 * n(2) * n(0) - cn * (n(2) * c(0) + n(0) * c(2))); - Scalar H = 2.0 * (c2r2 * n(0) * n(1) - cn * (n(0) * c(1) + n(1) * c(0))); - // Then set z=f to get conic which is the result of intersecting the cone with the focal plane - return Conic( - ABC(0), // x^2 (Ax^2) - H, // xy (Hxy) - ABC(1), // y^2 (By^2) - G * f /*+ Const(0)*/, // x (Gxz + Ux, z = f) - F * f /*+ Const(0)*/, // y (Fyz + Vy, z = f) - ABC(2) * sq(f) /*+ Const(0)*f + Const(0)*/ // 1 (Cz^2 + Wz + D, z = f) - ); - } - - /*template - typename Conic project(Conic conic, const Eigen::DenseBase& point, const Eigen::DenseBase& normal, Scalar focal_length) - { - // Consider two coordinate systems: - // camera (camera at 0, x,y aligned with image plane, z going away from camera) - // conic (conic on xy-plane, with plane normal = (0,0,1) and plane point = (0,0,0) ) - // - // To project conic lying on plane defined by point and normal (point corresponding to (0,0) in conic's 2D space), do: - // - // Input as in camera space, - // Transform to conic space, - // Form conicoid with conic as base and camera center as vertex - // Transform back to camera space - // Intersect conicoid with image plane (z=f) - - Eigen::Matrix camera_center(0,0,0); - }*/ - - - template - Ellipse2D project(const Sphere& sphere, Scalar focal_length) - { - return Ellipse2D( - focal_length * sphere.center.template head<2>() / sphere.center[2], - focal_length * sphere.radius / sphere.center[2], - focal_length * sphere.radius / sphere.center[2], - 0); - } - template - typename Eigen::DenseBase::template FixedSegmentReturnType<2>::Type::PlainObject project(const Eigen::DenseBase& point, typename Eigen::DenseBase::Scalar focal_length) - { - static_assert(Derived::IsVectorAtCompileTime && Derived::SizeAtCompileTime == 3, "Point must be 3 element vector"); - return focal_length * point.template head<2>() / point(2); - } - - - template - std::pair, Circle3D> unproject(const Ellipse2D& ellipse, Scalar circle_radius, Scalar focal_length) - { - using std::sqrt; - using std::abs; - using math::sign; - using math::sq; - typedef Conic Conic; - typedef Conicoid Conicoid; - typedef Circle3D Circle; - typedef Eigen::Matrix Matrix3; - typedef Eigen::Matrix Vector3; - typedef Eigen::Array RowArray3; - typedef Eigen::Translation Translation3; - // Get cone with base of ellipse and vertex at [0 0 -f] - // Safaee-Rad 1992 eq (3) - Conic conic(ellipse); - Vector3 cam_center_in_ellipse(0, 0, -focal_length); - Conicoid pupil_cone(conic, cam_center_in_ellipse); - auto a = pupil_cone.A; - auto b = pupil_cone.B; - auto c = pupil_cone.C; - auto f = pupil_cone.F; - auto g = pupil_cone.G; - auto h = pupil_cone.H; - auto u = pupil_cone.U; - auto v = pupil_cone.V; - auto w = pupil_cone.W; - //auto d = pupil_cone.D; - // Get canonical conic form: - // lambda(1) X^2 + lambda(2) Y^2 + lambda(3) Z^2 = mu - // Safaee-Rad 1992 eq (6) - // Done by solving the discriminating cubic (10) - // Lambdas are sorted descending because order of roots doesn't - // matter, and it later eliminates the case of eq (30), where - // lambda(2) > lambda(1) - RowArray3 lambda; - std::tie(lambda(0), lambda(1), lambda(2)) = solve(1., -(a + b + c), (b * c + c * a + a * b - f * f - g * g - h * h), -(a * b * c + 2 * f * g * h - a * f * f - b * g * g - c * h * h)); - assert(lambda(0) >= lambda(1)); - assert(lambda(1) > 0); - assert(lambda(2) < 0); - // Now want to calculate l,m,n of the plane - // lX + mY + nZ = p - // which intersects the cone to create a circle. - // Safaee-Rad 1992 eq (31) - // [Safaee-Rad 1992 eq (33) comes out of this as a result of lambda(1) == lambda(2)] - auto n = sqrt((lambda(1) - lambda(2)) / (lambda(0) - lambda(2))); - auto m = 0.0; - auto l = sqrt((lambda(0) - lambda(1)) / (lambda(0) - lambda(2))); - // There are two solutions for l, positive and negative, we handle these later - // Want to calculate T1, the rotation transformation from image - // space in the canonical conic frame back to image space in the - // real world - Matrix3 T1; - // Safaee-Rad 1992 eq (8) - auto li = T1.row(0); - auto mi = T1.row(1); - auto ni = T1.row(2); - // Safaee-Rad 1992 eq (12) - RowArray3 t1 = (b - lambda) * g - f * h; - RowArray3 t2 = (a - lambda) * f - g * h; - RowArray3 t3 = -(a - lambda) * (t1 / t2) / g - h / g; - mi = 1 / sqrt(1 + (t1 / t2).square() + t3.square()); - li = (t1 / t2) * mi.array(); - ni = t3 * mi.array(); - - // If li,mi,ni follow the left hand rule, flip their signs - if ((li.cross(mi)).dot(ni) < 0) { - li = -li; - mi = -mi; - ni = -ni; - } - - // Calculate T2, a translation transformation from the canonical - // conic frame to the image space in the canonical conic frame - // Safaee-Rad 1992 eq (14) - Translation3 T2; - T2.translation() = -(u * li + v * mi + w * ni).array() / lambda; - Circle solutions[2]; - Scalar ls[2] = { l, -l }; - - for (int i = 0; i < 2; i++) { - auto l = ls[i]; - // Circle normal in image space (i.e. gaze vector) - Vector3 gaze = T1 * Vector3(l, m, n); - // Calculate T3, a rotation from a frame where Z is the circle normal - // to the canonical conic frame - // Safaee-Rad 1992 eq (19) - // Want T3 = / -m/sqrt(l*l+m*m) -l*n/sqrt(l*l+m*m) l \ - // | l/sqrt(l*l+m*m) -m*n/sqrt(l*l+m*m) m | - // \ 0 sqrt(l*l+m*m) n / - // But m = 0, so this simplifies to - // T3 = / 0 -n*l/sqrt(l*l) l \ - // | l/sqrt(l*l) 0 0 | - // \ 0 sqrt(l*l) n / - // = / 0 -n*sgn(l) l \ - // | sgn(l) 0 0 | - // \ 0 |l| n / - Matrix3 T3; - - if (l == 0) { - // Discontinuity of sgn(l), have to handle explicitly - assert(n == 1); - std::cout << "Warning: l == 0" << std::endl; - T3 << 0, -1, 0, - 1, 0, 0, - 0, 0, 1; - - } else { - //auto sgnl = sign(l); - T3 << 0, -n* sign(l), l, - sign(l), 0, 0, - 0, abs(l), n; - } - - // Calculate the circle center - // Safaee-Rad 1992 eq (38), using T3 as defined in (36) - auto A = lambda.matrix().dot(T3.col(0).cwiseAbs2()); - auto B = lambda.matrix().dot(T3.col(0).cwiseProduct(T3.col(2))); - auto C = lambda.matrix().dot(T3.col(1).cwiseProduct(T3.col(2))); - auto D = lambda.matrix().dot(T3.col(2).cwiseAbs2()); - // Safaee-Rad 1992 eq (41) - Vector3 center_in_Xprime; - center_in_Xprime(2) = A * circle_radius / sqrt(sq(B) + sq(C) - A * D); - center_in_Xprime(0) = -B / A * center_in_Xprime(2); - center_in_Xprime(1) = -C / A * center_in_Xprime(2); - // Safaee-Rad 1992 eq (34) - Translation3 T0; - T0.translation() << 0, 0, focal_length; - // Safaee-Rad 1992 eq (42) using (35) - Vector3 center = T0 * T1 * T2 * T3 * center_in_Xprime; - - // If z is negative (behind the camera), choose the other - // solution of eq (41) [maybe there's a way of calculating which - // solution should be chosen first] - - if (center(2) < 0) { - center_in_Xprime = -center_in_Xprime; - center = T0 * T1 * T2 * T3 * center_in_Xprime; - } - - // Make sure that the gaze vector is toward the camera and is normalised - if (gaze.dot(center) > 0) { - gaze = -gaze; - } - - gaze.normalize(); - // Save the results - solutions[i] = Circle(center, gaze, circle_radius); - } - - return std::make_pair(solutions[0], solutions[1]); - } - -} - -#endif // singleeyefitter_project_h__ diff --git a/pupil_src/shared_cpp/include/solve.h b/pupil_src/shared_cpp/include/solve.h deleted file mode 100644 index 8ac4619c6c..0000000000 --- a/pupil_src/shared_cpp/include/solve.h +++ /dev/null @@ -1,114 +0,0 @@ -#ifndef _SOLVE_H_ -#define _SOLVE_H_ - -#include -#include -#include - -namespace singleeyefitter { - - // a = 0 - template - T solve(T a) - { - if (a == 0) return 0; - else throw std::runtime_error("No solution"); - } - // ax + b = 0 - template - T solve(T a, T b) - { - if (a == 0) return solve(b); - - return -b / a; - } - // ax^2 + bx + c = 0 - template - std::tuple solve(T a, T b, T c) - { - using math::sq; - using std::sqrt; - - if (a == 0) { - auto root = solve(b, c); - return std::tuple(root, root); - } - - // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-6.pdf - // Pg 184 - auto det = sq(b) - 4 * a * c; - - if (det < 0) - throw std::runtime_error("No solution"); - - //auto sqrtdet = sqrt(det); - auto q = -0.5 * (b + (b >= 0 ? 1 : -1) * sqrt(det)); - return std::tuple(q / a, c / q); - } - // ax^2 + bx + c = 0 - template - std::tuple solve(T a, T b, T c, T d) - { - using std::sqrt; - using std::abs; - using math::sq; - using std::cbrt; - - if (a == 0) { - auto roots = solve(b, c, d); - return std::tuple(std::get<0>(roots), std::get<1>(roots), std::get<1>(roots)); - } - - // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-6.pdf - // http://web.archive.org/web/20120321013251/http://linus.it.uts.edu.au/~don/pubs/solving.html - auto p = b / a; - auto q = c / a; - auto r = d / a; - //auto Q = (p*p - 3*q) / 9; - //auto R = (2*p*p*p - 9*p*q + 27*r)/54; - auto u = q - sq(p) / 3; - auto v = r - p * q / 3 + 2 * p * p * p / 27; - auto j = 4 * u * u * u / 27 + v * v; - const auto M = std::numeric_limits::max(); - const auto sqrtM = sqrt(M); - const auto cbrtM = cbrt(M); - - if (b == 0 && c == 0) - return std::tuple(cbrt(-d), cbrt(-d), cbrt(-d)); - - if (abs(p) > 27 * cbrtM) - return std::tuple(-p, -p, -p); - - if (abs(q) > sqrtM) - return std::tuple(-cbrt(v), -cbrt(v), -cbrt(v)); - - if (abs(u) > 3 * cbrtM / 4) - return std::tuple(cbrt(4) * u / 3, cbrt(4) * u / 3, cbrt(4) * u / 3); - - if (j > 0) { - // One real root - auto w = sqrt(j); - T y; - - if (v > 0) - y = (u / 3) * cbrt(2 / (w + v)) - cbrt((w + v) / 2) - p / 3; - else - y = cbrt((w - v) / 2) - (u / 3) * cbrt(2 / (w - v)) - p / 3; - - return std::tuple(y, y, y); - - } else { - // Three real roots - auto s = sqrt(-u / 3); - auto t = -v / (2 * s * s * s); - auto k = acos(t) / 3; - auto y1 = 2 * s * cos(k) - p / 3; - auto y2 = s * (-cos(k) + sqrt(3.) * sin(k)) - p / 3; - auto y3 = s * (-cos(k) - sqrt(3.) * sin(k)) - p / 3; - return std::tuple(y1, y2, y3); - } - } - -} - -#endif//_SOLVE_H_ diff --git a/pupil_src/shared_modules/accuracy_visualizer.py b/pupil_src/shared_modules/accuracy_visualizer.py index e2b8ddcf98..f6d88045ea 100644 --- a/pupil_src/shared_modules/accuracy_visualizer.py +++ b/pupil_src/shared_modules/accuracy_visualizer.py @@ -9,22 +9,17 @@ ---------------------------------------------------------------------------~(*) """ -import numpy as np -from scipy.spatial import ConvexHull +import logging +from collections import namedtuple import OpenGL.GL as gl -from glfw import * - +import numpy as np from pyglui import ui from pyglui.cygl.utils import draw_points_norm, draw_polyline_norm, RGBA +from scipy.spatial import ConvexHull +from calibration_routines.data_processing import closest_matches_monocular from plugin import Plugin -from calibration_routines.calibrate import closest_matches_monocular - -from collections import namedtuple - -# logging -import logging logger = logging.getLogger(__name__) diff --git a/pupil_src/shared_modules/audio_capture.py b/pupil_src/shared_modules/audio_capture.py index f848665460..65db600072 100644 --- a/pupil_src/shared_modules/audio_capture.py +++ b/pupil_src/shared_modules/audio_capture.py @@ -78,6 +78,11 @@ def audio_dev_getter(): return devices, devices self.menu.append( + # TODO: potential race condition through selection_getter. Should ensure + # that current selection will always be present in the list returned by the + # selection_getter. Highly unlikely though as this needs to happen between + # having clicked the Selector and the next redraw. + # See https://github.com/pupil-labs/pyglui/pull/112/commits/587818e9556f14bfedd8ff8d093107358745c29b ui.Selector( "audio_src", self, diff --git a/pupil_src/shared_modules/calibration_routines/data_processing.py b/pupil_src/shared_modules/calibration_routines/data_processing.py new file mode 100644 index 0000000000..7e686523d7 --- /dev/null +++ b/pupil_src/shared_modules/calibration_routines/data_processing.py @@ -0,0 +1,312 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" + +import logging + +import numpy as np + +import file_methods as fm + +logger = logging.getLogger(__name__) + + +def get_data_for_calibration(g_pool, pupil_list, ref_list, mode): + """Returns extracted data for calibration and whether there is binocular data""" + + pupil_list = _filter_pupil_list_by_confidence( + pupil_list, g_pool.min_calibration_confidence + ) + + matched_data = _match_data(pupil_list, ref_list) + ( + matched_binocular_data, + matched_monocular_data, + matched_pupil0_data, + matched_pupil1_data, + ) = matched_data + + binocular = None + extracted_data = None + if mode == "3d": + if matched_binocular_data: + binocular = True + extracted_data = _extract_3d_data(g_pool, matched_binocular_data) + elif matched_monocular_data: + binocular = False + extracted_data = _extract_3d_data(g_pool, matched_monocular_data) + + elif mode == "2d": + if matched_binocular_data: + binocular = True + cal_pt_cloud_binocular = _extract_2d_data_binocular(matched_binocular_data) + cal_pt_cloud0 = _extract_2d_data_monocular(matched_pupil0_data) + cal_pt_cloud1 = _extract_2d_data_monocular(matched_pupil1_data) + extracted_data = cal_pt_cloud_binocular, cal_pt_cloud0, cal_pt_cloud1 + elif matched_monocular_data: + binocular = False + cal_pt_cloud = _extract_2d_data_monocular(matched_monocular_data) + extracted_data = (cal_pt_cloud,) + + return binocular, extracted_data + + +def _filter_pupil_list_by_confidence(pupil_list, threshold): + if not pupil_list: + return [] + + len_pre_filter = len(pupil_list) + pupil_list = [p for p in pupil_list if p["confidence"] >= threshold] + len_post_filter = len(pupil_list) + dismissed_percentage = 100 * (1.0 - len_post_filter / len_pre_filter) + logger.info( + f"Dismissing {dismissed_percentage:.2f}% pupil data due to " + f"confidence < {threshold:.2f}" + ) + return pupil_list + + +def _match_data(pupil_list, ref_list): + """Returns binocular and monocular matched pupil datums and ref points. + Uses a dispersion criterion to dismiss matches which are too far apart. + """ + + pupil0 = [p for p in pupil_list if p["id"] == 0] + pupil1 = [p for p in pupil_list if p["id"] == 1] + + matched_binocular_data = closest_matches_binocular(ref_list, pupil0, pupil1) + matched_pupil0_data = closest_matches_monocular(ref_list, pupil0) + matched_pupil1_data = closest_matches_monocular(ref_list, pupil1) + + if len(matched_pupil0_data) > len(matched_pupil1_data): + matched_monocular_data = matched_pupil0_data + else: + matched_monocular_data = matched_pupil1_data + + logger.info(f"Collected {len(matched_monocular_data)} monocular calibration data.") + logger.info(f"Collected {len(matched_binocular_data)} binocular calibration data.") + + return ( + matched_binocular_data, + matched_monocular_data, + matched_pupil0_data, + matched_pupil1_data, + ) + + +def closest_matches_binocular(ref_pts, pupil0, pupil1, max_dispersion=1 / 15.0): + """Get pupil positions closest in time to ref points. + Return list of dict with matching ref, pupil0 and pupil1 data triplets. + """ + + if not (ref_pts and pupil0 and pupil1): + return [] + + pupil0_ts = np.array([p["timestamp"] for p in pupil0]) + pupil1_ts = np.array([p["timestamp"] for p in pupil1]) + + matched = [] + for r in ref_pts: + closest_p0_idx = _find_nearest_idx(pupil0_ts, r["timestamp"]) + closest_p0 = pupil0[closest_p0_idx] + closest_p1_idx = _find_nearest_idx(pupil1_ts, r["timestamp"]) + closest_p1 = pupil1[closest_p1_idx] + + dispersion = max( + closest_p0["timestamp"], closest_p1["timestamp"], r["timestamp"] + ) - min(closest_p0["timestamp"], closest_p1["timestamp"], r["timestamp"]) + if dispersion < max_dispersion: + matched.append({"ref": r, "pupil": closest_p0, "pupil1": closest_p1}) + else: + logger.debug("Binocular match rejected due to time dispersion criterion") + return matched + + +def closest_matches_monocular(ref_pts, pupil, max_dispersion=1 / 15.0): + """Get pupil positions closest in time to ref points. + Return list of dict with matching ref and pupil datum. + """ + + if not (ref_pts and pupil): + return [] + + pupil_ts = np.array([p["timestamp"] for p in pupil]) + + matched = [] + for r in ref_pts: + closest_p_idx = _find_nearest_idx(pupil_ts, r["timestamp"]) + closest_p = pupil[closest_p_idx] + dispersion = max(closest_p["timestamp"], r["timestamp"]) - min( + closest_p["timestamp"], r["timestamp"] + ) + if dispersion < max_dispersion: + matched.append({"ref": r, "pupil": closest_p}) + return matched + + +def _find_nearest_idx(array, value): + """Find the index of the element in array which is closest to value""" + + idx = np.searchsorted(array, value, side="left") + try: + if abs(value - array[idx - 1]) < abs(value - array[idx]): + return idx - 1 + else: + return idx + except IndexError: + return idx - 1 + + +def _extract_3d_data(g_pool, matched_data): + """Takes matched data, splits into ref, pupil0, pupil1. + Return unprojections of ref, normals of pupil0 and pupil1 and last pupils + """ + + if not matched_data: + return None + + ref = np.array([dp["ref"]["screen_pos"] for dp in matched_data]) + ref_points_unprojected = g_pool.capture.intrinsics.unprojectPoints( + ref, normalize=True + ) + + pupil0_normals = [ + dp["pupil"]["circle_3d"]["normal"] + for dp in matched_data + if "circle_3d" in dp["pupil"] + ] + if not pupil0_normals: + return None + + # matched_monocular_data + if "pupil1" not in matched_data[0]: + last_pupil = matched_data[-1]["pupil"] + return ref_points_unprojected, np.array(pupil0_normals), last_pupil + + # matched_binocular_data + pupil1_normals = [ + dp["pupil1"]["circle_3d"]["normal"] + for dp in matched_data + if "circle_3d" in dp["pupil1"] + ] + if not pupil1_normals: + return None + + last_pupil0 = matched_data[-1]["pupil"] + last_pupil1 = matched_data[-1]["pupil1"] + return ( + ref_points_unprojected, + np.array(pupil0_normals), + np.array(pupil1_normals), + last_pupil0, + last_pupil1, + ) + + +def _extract_2d_data_binocular(matched_data): + """Takes matched pupil data and returns list of tuples, keeping only the positions + in normalized coordinates for pupil0, pupil1 and ref positions. + """ + + cal_data = [ + ( + *triplet["pupil"]["norm_pos"], + *triplet["pupil1"]["norm_pos"], + *triplet["ref"]["norm_pos"], + ) + for triplet in matched_data + ] + return cal_data + + +def _extract_2d_data_monocular(matched_data): + """Takes matched pupil data and returns list of tuples, keeping only the positions + in normalized screen coordinates for pupil and ref. + """ + + cal_data = [ + (*pair["pupil"]["norm_pos"], *pair["ref"]["norm_pos"]) for pair in matched_data + ] + return cal_data + + +def get_data_for_calibration_hmd(pupil_list, ref_list, mode): + """Returns extracted data for hmd calibration""" + + matched_data = _match_data_hmd(pupil_list, ref_list) + matched_binocular_data, matched_pupil0_data, matched_pupil1_data = matched_data + + extracted_data = None + if mode == "3d": + fm.save_object(matched_binocular_data, "hmd_cal_data") + extracted_data = _extract_3d_data_hmd(matched_binocular_data) + + elif mode == "2d": + if not (matched_pupil0_data or matched_pupil1_data): + extracted_data = None + else: + cal_pt_cloud0 = _extract_2d_data_monocular(matched_pupil0_data) + cal_pt_cloud1 = _extract_2d_data_monocular(matched_pupil1_data) + extracted_data = cal_pt_cloud0, cal_pt_cloud1 + + if not cal_pt_cloud0: + logger.warning("No matched ref<->pupil data collected for id0") + if not cal_pt_cloud1: + logger.warning("No matched ref<->pupil data collected for id1") + + return extracted_data + + +def _match_data_hmd(pupil_list, ref_list): + """Returns binocular and monocular matched pupil datums and ref points. + Uses a dispersion criterion to dismiss matches which are too far apart. + """ + + ref0 = [r for r in ref_list if r["id"] == 0] + ref1 = [r for r in ref_list if r["id"] == 1] + pupil0 = [p for p in pupil_list if p["id"] == 0] + pupil1 = [p for p in pupil_list if p["id"] == 1] + + matched_binocular_data = closest_matches_binocular(ref_list, pupil0, pupil1) + matched_pupil0_data = closest_matches_monocular(ref0, pupil0) + matched_pupil1_data = closest_matches_monocular(ref1, pupil1) + + return matched_binocular_data, matched_pupil0_data, matched_pupil1_data + + +def _extract_3d_data_hmd(matched_data): + """Takes matched data, splits into ref, pupil0, pupil1. + Return mm_pos of ref, normals of pupil0 and pupil1 and last pupils + """ + + ref_points_3d = [d["ref"]["mm_pos"] for d in matched_data] + pupil0_normals = [ + d["pupil"]["circle_3d"]["normal"] + for d in matched_data + if "3d" in d["pupil"]["method"] + ] + pupil1_normals = [ + d["pupil1"]["circle_3d"]["normal"] + for d in matched_data + if "3d" in d["pupil"]["method"] + ] + + if not ref_points_3d or not pupil0_normals or not pupil1_normals: + return None + + last_pupil0 = matched_data[-1]["pupil"] + last_pupil1 = matched_data[-1]["pupil1"] + return ( + np.array(ref_points_3d), + np.array(pupil0_normals), + np.array(pupil1_normals), + last_pupil0, + last_pupil1, + ) diff --git a/pupil_src/shared_modules/calibration_routines/finish_calibration.py b/pupil_src/shared_modules/calibration_routines/finish_calibration.py index 5cf15eb603..909abc808b 100644 --- a/pupil_src/shared_modules/calibration_routines/finish_calibration.py +++ b/pupil_src/shared_modules/calibration_routines/finish_calibration.py @@ -9,541 +9,113 @@ ---------------------------------------------------------------------------~(*) """ +import logging import os -import numpy as np - -from . import calibrate -from math_helper import * -from file_methods import load_object, save_object -from .optimization_calibration import bundle_adjust_calibration -from .calibrate import find_rigid_transform - -# logging -import logging +import file_methods as fm +from calibration_routines import data_processing +from calibration_routines.optimization_calibration import calibration_methods logger = logging.getLogger(__name__) -from .gaze_mappers import * - -not_enough_data_error_msg = ( - "Not enough ref point or pupil data available for calibration." -) -solver_failed_to_converge_error_msg = "Paramters could not be estimated from data." - - -def calibrate_3d_binocular(g_pool, matched_binocular_data, pupil0, pupil1): - method = "binocular 3d model" - hardcoded_translation0 = np.array([20, 15, -20]) - hardcoded_translation1 = np.array([-40, 15, -20]) - - # TODO model the world as cv2 pinhole camera with distorion and focal in ceres. - # right now we solve using a few permutations of K - - ref_dir, gaze0_dir, gaze1_dir = calibrate.preprocess_3d_data( - matched_binocular_data, g_pool - ) - - if len(ref_dir) < 1 or len(gaze0_dir) < 1 or len(gaze1_dir) < 1: - logger.error(not_enough_data_error_msg) - return ( - method, - { - "subject": "calibration.failed", - "reason": not_enough_data_error_msg, - "timestamp": g_pool.get_timestamp(), - "record": True, - }, - ) - - sphere_pos0 = pupil0[-1]["sphere"]["center"] - sphere_pos1 = pupil1[-1]["sphere"]["center"] - - initial_R0, initial_t0 = find_rigid_transform( - np.array(gaze0_dir) * 500, np.array(ref_dir) * 500 - ) - initial_rotation0 = math_helper.quaternion_from_rotation_matrix(initial_R0) - # initial_translation0 = np.array(initial_t0).reshape(3) # currently not used - - initial_R1, initial_t1 = find_rigid_transform( - np.array(gaze1_dir) * 500, np.array(ref_dir) * 500 - ) - initial_rotation1 = math_helper.quaternion_from_rotation_matrix(initial_R1) - # initial_translation1 = np.array(initial_t1).reshape(3) # currently not used - - eye0 = { - "observations": gaze0_dir, - "translation": hardcoded_translation0, - "rotation": initial_rotation0, - "fix": ["translation"], - } - - eye1 = { - "observations": gaze1_dir, - "translation": hardcoded_translation1, - "rotation": initial_rotation1, - "fix": ["translation"], - } - world = { - "observations": ref_dir, - "translation": (0, 0, 0), - "rotation": (1, 0, 0, 0), - "fix": ["translation", "rotation"], - } - initial_observers = [eye0, eye1, world] - initial_points = np.array(ref_dir) * 500 - - success, residual, observers, points = bundle_adjust_calibration( - initial_observers, initial_points, fix_points=False - ) - - if not success: - logger.error("Calibration solver faild to converge.") - return ( - method, - { - "subject": "calibration.failed", - "reason": solver_failed_to_converge_error_msg, - "timestamp": g_pool.get_timestamp(), - "record": True, - }, - ) - - eye0, eye1, world = observers - - t_world0 = np.array(eye0["translation"]) - R_world0 = math_helper.quaternion_rotation_matrix(np.array(eye0["rotation"])) - t_world1 = np.array(eye1["translation"]) - R_world1 = math_helper.quaternion_rotation_matrix(np.array(eye1["rotation"])) - - def toWorld0(p): - return np.dot(R_world0, p) + t_world0 - - def toWorld1(p): - return np.dot(R_world1, p) + t_world1 - - points_a = [] # world coords - points_b = [] # eye0 coords - points_c = [] # eye1 coords - for a, b, c, point in zip( - world["observations"], eye0["observations"], eye1["observations"], points - ): - line_a = np.array([0, 0, 0]), np.array(a) # observation as line - line_b = ( - toWorld0(np.array([0, 0, 0])), - toWorld0(b), - ) # eye0 observation line in world coords - line_c = ( - toWorld1(np.array([0, 0, 0])), - toWorld1(c), - ) # eye1 observation line in world coords - close_point_a, _ = math_helper.nearest_linepoint_to_point(point, line_a) - close_point_b, _ = math_helper.nearest_linepoint_to_point(point, line_b) - close_point_c, _ = math_helper.nearest_linepoint_to_point(point, line_c) - points_a.append(close_point_a.tolist()) - points_b.append(close_point_b.tolist()) - points_c.append(close_point_c.tolist()) - - # we need to take the sphere position into account - # orientation and translation are referring to the sphere center. - # but we want to have it referring to the camera center - # since the actual translation is in world coordinates, the sphere - # translation needs to be calculated in world coordinates - sphere_translation = np.array(sphere_pos0) - sphere_translation_world = np.dot(R_world0, sphere_translation) - camera_translation = t_world0 - sphere_translation_world - eye_camera_to_world_matrix0 = np.eye(4) - eye_camera_to_world_matrix0[:3, :3] = R_world0 - eye_camera_to_world_matrix0[:3, 3:4] = np.reshape(camera_translation, (3, 1)) - - sphere_translation = np.array(sphere_pos1) - sphere_translation_world = np.dot(R_world1, sphere_translation) - camera_translation = t_world1 - sphere_translation_world - eye_camera_to_world_matrix1 = np.eye(4) - eye_camera_to_world_matrix1[:3, :3] = R_world1 - eye_camera_to_world_matrix1[:3, 3:4] = np.reshape(camera_translation, (3, 1)) - - return ( - method, - { - "subject": "start_plugin", - "name": "Binocular_Vector_Gaze_Mapper", - "args": { - "eye_camera_to_world_matrix0": eye_camera_to_world_matrix0.tolist(), - "eye_camera_to_world_matrix1": eye_camera_to_world_matrix1.tolist(), - "cal_points_3d": points, - "cal_ref_points_3d": points_a, - "cal_gaze_points0_3d": points_b, - "cal_gaze_points1_3d": points_c, - }, - }, - ) - - -def calibrate_3d_monocular(g_pool, matched_monocular_data): - method = "monocular 3d model" - hardcoded_translation0 = np.array([20, 15, -20]) - hardcoded_translation1 = np.array([-40, 15, -20]) - # TODO model the world as cv2 pinhole camera with distorion and focal in ceres. - # right now we solve using a few permutations of K - smallest_residual = 1000 - - # TODO do this across different scales? - ref_dir, gaze_dir, _ = calibrate.preprocess_3d_data(matched_monocular_data, g_pool) - # save_object((ref_dir,gaze_dir),os.path.join(g_pool.user_dir, "testdata")) - if len(ref_dir) < 1 or len(gaze_dir) < 1: - logger.error(not_enough_data_error_msg + " Using:" + method) - return ( - method, - { - "subject": "calibration.failed", - "reason": not_enough_data_error_msg, - "timestamp": g_pool.get_timestamp(), - "record": True, - }, - ) - - # monocular calibration strategy: mimize the reprojection error by moving the world camera. - # we fix the eye points and work in the eye coord system. - initial_R, initial_t = find_rigid_transform( - np.array(ref_dir) * 500, np.array(gaze_dir) * 500 - ) - initial_rotation = math_helper.quaternion_from_rotation_matrix(initial_R) - # initial_translation = np.array(initial_t).reshape(3) # currently not used - # this problem is scale invariant so we scale to some sensical value. - - if matched_monocular_data[0]["pupil"]["id"] == 0: - hardcoded_translation = hardcoded_translation0 - else: - hardcoded_translation = hardcoded_translation1 - - eye = { - "observations": gaze_dir, - "translation": (0, 0, 0), - "rotation": (1, 0, 0, 0), - "fix": ["translation", "rotation"], - } - - world = { - "observations": ref_dir, - "translation": np.dot(initial_R, -hardcoded_translation), - "rotation": initial_rotation, - "fix": [], - } - - initial_observers = [eye, world] - initial_points = np.array(gaze_dir) * 500 - - success, residual, observers, points_in_eye = bundle_adjust_calibration( - initial_observers, initial_points, fix_points=True - ) - - eye, world = observers - - if not success: - logger.error("Calibration solver faild to converge.") - return ( - method, - { - "subject": "calibration.failed", - "reason": solver_failed_to_converge_error_msg, - "timestamp": g_pool.get_timestamp(), - "record": True, - }, - ) - - # pose of the world in eye coords. - rotation = np.array(world["rotation"]) - t_world = np.array(world["translation"]) - R_world = math_helper.quaternion_rotation_matrix(rotation) - - # inverse is pose of eye in world coords - R_eye = R_world.T - t_eye = np.dot(R_eye, -t_world) - def toWorld(p): - return np.dot(R_eye, p) + np.array(t_eye) - points_in_world = [toWorld(p).tolist() for p in points_in_eye] - - points_a = [] # world coords - points_b = [] # cam2 coords - for a, b, point in zip(world["observations"], eye["observations"], points_in_world): - - line_a = np.array([0, 0, 0]), np.array(a) # observation as line - line_b = ( - toWorld(np.array([0, 0, 0])), - toWorld(b), - ) # cam2 observation line in cam1 coords - close_point_a, _ = math_helper.nearest_linepoint_to_point(point, line_a) - close_point_b, _ = math_helper.nearest_linepoint_to_point(point, line_b) - # print np.linalg.norm(point-close_point_a),np.linalg.norm(point-close_point_b) +def finish_calibration(g_pool, pupil_list, ref_list): + method, result = select_method_and_perform_calibration(g_pool, pupil_list, ref_list) - points_a.append(close_point_a.tolist()) - points_b.append(close_point_b.tolist()) + # Start mapper / announce error + g_pool.active_calibration_plugin.notify_all(result) + if result["subject"] == "calibration.failed": + return - # we need to take the sphere position into account - # orientation and translation are referring to the sphere center. - # but we want to have it referring to the camera center - # since the actual translation is in world coordinates, the sphere - # translation needs to be calculated in world coordinates - sphere_translation = np.array( - matched_monocular_data[-1]["pupil"]["sphere"]["center"] - ) - sphere_translation_world = np.dot(R_eye, sphere_translation) - camera_translation = t_eye - sphere_translation_world - eye_camera_to_world_matrix = np.eye(4) - eye_camera_to_world_matrix[:3, :3] = R_eye - eye_camera_to_world_matrix[:3, 3:4] = np.reshape(camera_translation, (3, 1)) + ts = g_pool.get_timestamp() - return ( - method, + # Announce success + g_pool.active_calibration_plugin.notify_all( { - "subject": "start_plugin", - "name": "Vector_Gaze_Mapper", - "args": { - "eye_camera_to_world_matrix": eye_camera_to_world_matrix.tolist(), - "cal_points_3d": points_in_world, - "cal_ref_points_3d": points_a, - "cal_gaze_points_3d": points_b, - "gaze_distance": 500, - }, - }, - ) - - -def calibrate_2d_binocular( - g_pool, matched_binocular_data, matched_pupil0_data, matched_pupil1_data -): - method = "binocular polynomial regression" - cal_pt_cloud_binocular = calibrate.preprocess_2d_data_binocular( - matched_binocular_data - ) - cal_pt_cloud0 = calibrate.preprocess_2d_data_monocular(matched_pupil0_data) - cal_pt_cloud1 = calibrate.preprocess_2d_data_monocular(matched_pupil1_data) - - map_fn, inliers, params = calibrate.calibrate_2d_polynomial( - cal_pt_cloud_binocular, g_pool.capture.frame_size, binocular=True - ) - - def create_converge_error_msg(): - return { - "subject": "calibration.failed", - "reason": solver_failed_to_converge_error_msg, - "timestamp": g_pool.get_timestamp(), + "subject": "calibration.successful", + "method": method, + "timestamp": ts, "record": True, } - - if not inliers.any(): - return method, create_converge_error_msg() - - map_fn, inliers, params_eye0 = calibrate.calibrate_2d_polynomial( - cal_pt_cloud0, g_pool.capture.frame_size, binocular=False ) - if not inliers.any(): - return method, create_converge_error_msg() - map_fn, inliers, params_eye1 = calibrate.calibrate_2d_polynomial( - cal_pt_cloud1, g_pool.capture.frame_size, binocular=False - ) - if not inliers.any(): - return method, create_converge_error_msg() - - return ( - method, - { - "subject": "start_plugin", - "name": "Binocular_Gaze_Mapper", - "args": { - "params": params, - "params_eye0": params_eye0, - "params_eye1": params_eye1, - }, - }, - ) - - -def calibrate_2d_monocular(g_pool, matched_monocular_data): - method = "monocular polynomial regression" - cal_pt_cloud = calibrate.preprocess_2d_data_monocular(matched_monocular_data) - map_fn, inliers, params = calibrate.calibrate_2d_polynomial( - cal_pt_cloud, g_pool.capture.frame_size, binocular=False + # Announce calibration data + user_calibration_data = { + "timestamp": ts, + "pupil_list": pupil_list, + "ref_list": ref_list, + "calibration_method": method, + "mapper_name": result["name"], + "mapper_args": result["args"], + } + fm.save_object( + user_calibration_data, os.path.join(g_pool.user_dir, "user_calibration_data") ) - if not inliers.any(): - return ( - method, - { - "subject": "calibration.failed", - "reason": solver_failed_to_converge_error_msg, - "timestamp": g_pool.get_timestamp(), - "record": True, - }, - ) - - return ( - method, + g_pool.active_calibration_plugin.notify_all( { - "subject": "start_plugin", - "name": "Monocular_Gaze_Mapper", - "args": {"params": params}, - }, - ) - - -def match_data(g_pool, pupil_list, ref_list): - if pupil_list and ref_list: - pass - else: - logger.error(not_enough_data_error_msg) - return { - "subject": "calibration.failed", - "reason": not_enough_data_error_msg, - "timestamp": g_pool.get_timestamp(), + "subject": "calibration.calibration_data", "record": True, + **user_calibration_data, } - - # match eye data and check if biocular and or monocular - pupil0 = [p for p in pupil_list if p["id"] == 0] - pupil1 = [p for p in pupil_list if p["id"] == 1] - - # TODO unify this and don't do both - matched_binocular_data = calibrate.closest_matches_binocular(ref_list, pupil_list) - matched_pupil0_data = calibrate.closest_matches_monocular(ref_list, pupil0) - matched_pupil1_data = calibrate.closest_matches_monocular(ref_list, pupil1) - - if len(matched_pupil0_data) > len(matched_pupil1_data): - matched_monocular_data = matched_pupil0_data - else: - matched_monocular_data = matched_pupil1_data - - logger.info( - "Collected {} monocular calibration data.".format(len(matched_monocular_data)) ) - logger.info( - "Collected {} binocular calibration data.".format(len(matched_binocular_data)) - ) - return ( - matched_binocular_data, - matched_monocular_data, - matched_pupil0_data, - matched_pupil1_data, - pupil0, - pupil1, - ) - -def select_calibration_method(g_pool, pupil_list, ref_list): - - len_pre_filter = len(pupil_list) - pupil_list = [ - p for p in pupil_list if p["confidence"] >= g_pool.min_calibration_confidence - ] - len_post_filter = len(pupil_list) - try: - dismissed_percentage = 100 * (1.0 - len_post_filter / len_pre_filter) - except ZeroDivisionError: - pass # empty pupil_list, is being handled in match_data - else: - logger.info( - "Dismissing {:.2f}% pupil data due to confidence < {:.2f}".format( - dismissed_percentage, g_pool.min_calibration_confidence - ) - ) - - matched_data = match_data(g_pool, pupil_list, ref_list) # calculate matching data - if not isinstance(matched_data, tuple): - return None, matched_data # matched_data is an error notification - - # unpack matching data - ( - matched_binocular_data, - matched_monocular_data, - matched_pupil0_data, - matched_pupil1_data, - pupil0, - pupil1, - ) = matched_data +def select_method_and_perform_calibration(g_pool, pupil_list, ref_list): mode = g_pool.detection_mapping_mode - if mode == "3d" and not ( hasattr(g_pool.capture, "intrinsics") or g_pool.capture.intrinsics ): mode = "2d" logger.warning( - "Please calibrate your world camera using 'camera intrinsics estimation' for 3d gaze mapping." + "Please calibrate your world camera using 'camera intrinsics estimation' " + "for 3d gaze mapping." ) - if mode == "3d": - if matched_binocular_data: - return calibrate_3d_binocular( - g_pool, matched_binocular_data, pupil0, pupil1 - ) - elif matched_monocular_data: - return calibrate_3d_monocular(g_pool, matched_monocular_data) - else: - logger.error(not_enough_data_error_msg) - return ( - None, - { - "subject": "calibration.failed", - "reason": not_enough_data_error_msg, - "timestamp": g_pool.get_timestamp(), - "record": True, - }, - ) - - elif mode == "2d": - if matched_binocular_data: - return calibrate_2d_binocular( - g_pool, matched_binocular_data, matched_pupil0_data, matched_pupil1_data - ) - elif matched_monocular_data: - return calibrate_2d_monocular(g_pool, matched_monocular_data) - else: - logger.error(not_enough_data_error_msg) - return ( - None, - { - "subject": "calibration.failed", - "reason": not_enough_data_error_msg, - "timestamp": g_pool.get_timestamp(), - "record": True, - }, - ) + binocular, extracted_data = data_processing.get_data_for_calibration( + g_pool, pupil_list, ref_list, mode + ) + if not extracted_data: + return None, create_not_enough_data_error_msg(g_pool) -def finish_calibration(g_pool, pupil_list, ref_list): - method, result = select_calibration_method(g_pool, pupil_list, ref_list) - g_pool.active_calibration_plugin.notify_all(result) - if result["subject"] != "calibration.failed": - ts = g_pool.get_timestamp() - g_pool.active_calibration_plugin.notify_all( - { - "subject": "calibration.successful", - "method": method, - "timestamp": ts, - "record": True, - } + if mode == "3d" and binocular: + method, result = calibration_methods.calibrate_3d_binocular(*extracted_data) + elif mode == "3d" and not binocular: + method, result = calibration_methods.calibrate_3d_monocular(*extracted_data) + elif mode == "2d" and binocular: + method, result = calibration_methods.calibrate_2d_binocular( + g_pool, *extracted_data + ) + elif mode == "2d" and not binocular: + method, result = calibration_methods.calibrate_2d_monocular( + g_pool, *extracted_data ) + else: + raise RuntimeError("This case should not happen.") - user_calibration_data = { - "timestamp": ts, - "pupil_list": pupil_list, - "ref_list": ref_list, - "calibration_method": method, - "mapper_name": result["name"], - "mapper_args": result["args"], - } + if result is None: + return method, create_converge_error_msg(g_pool) - save_object( - user_calibration_data, - os.path.join(g_pool.user_dir, "user_calibration_data"), - ) + return method, result - g_pool.active_calibration_plugin.notify_all( - { - "subject": "calibration.calibration_data", - "record": True, - **user_calibration_data, - } - ) + +def create_not_enough_data_error_msg(g_pool): + msg = "Not enough ref points or pupil data available for calibration." + logger.error(msg) + return { + "subject": "calibration.failed", + "reason": msg, + "timestamp": g_pool.get_timestamp(), + "record": True, + } + + +def create_converge_error_msg(g_pool): + msg = "Parameters could not be estimated from data." + logger.error(msg) + return { + "subject": "calibration.failed", + "reason": msg, + "timestamp": g_pool.get_timestamp(), + "record": True, + } diff --git a/pupil_src/shared_modules/calibration_routines/gaze_mappers.py b/pupil_src/shared_modules/calibration_routines/gaze_mappers.py index 346a2a040e..4b526af674 100644 --- a/pupil_src/shared_modules/calibration_routines/gaze_mappers.py +++ b/pupil_src/shared_modules/calibration_routines/gaze_mappers.py @@ -16,10 +16,9 @@ from pyglui import ui import math_helper -from plugin import Plugin +from calibration_routines.optimization_calibration import calibrate_2d from methods import normalize - -from . import calibrate +from plugin import Plugin from .visualizer_calibration import Calibration_Visualizer @@ -195,7 +194,7 @@ class Monocular_Gaze_Mapper(Monocular_Gaze_Mapper_Base, Gaze_Mapping_Plugin): def __init__(self, g_pool, params): super().__init__(g_pool) self.params = params - self.map_fn = calibrate.make_map_function(*self.params) + self.map_fn = calibrate_2d.make_map_function(*self.params) def _map_monocular(self, p): gaze_point = self.map_fn(p["norm_pos"]) @@ -220,8 +219,8 @@ def __init__(self, g_pool, params0, params1): self.params0 = params0 self.params1 = params1 self.map_fns = ( - calibrate.make_map_function(*self.params0), - calibrate.make_map_function(*self.params1), + calibrate_2d.make_map_function(*self.params0), + calibrate_2d.make_map_function(*self.params1), ) def _map_monocular(self, p): @@ -246,10 +245,10 @@ def __init__(self, g_pool, params, params_eye0, params_eye1): self.params_eye0 = params_eye0 self.params_eye1 = params_eye1 self.multivariate = True - self.map_fn = calibrate.make_map_function(*self.params) + self.map_fn = calibrate_2d.make_map_function(*self.params) self.map_fn_fallback = [] - self.map_fn_fallback.append(calibrate.make_map_function(*self.params_eye0)) - self.map_fn_fallback.append(calibrate.make_map_function(*self.params_eye1)) + self.map_fn_fallback.append(calibrate_2d.make_map_function(*self.params_eye0)) + self.map_fn_fallback.append(calibrate_2d.make_map_function(*self.params_eye1)) def init_ui(self): self.add_menu() diff --git a/pupil_src/shared_modules/calibration_routines/hmd_calibration.py b/pupil_src/shared_modules/calibration_routines/hmd_calibration.py index d7adfb93c9..8e25dd7fc4 100644 --- a/pupil_src/shared_modules/calibration_routines/hmd_calibration.py +++ b/pupil_src/shared_modules/calibration_routines/hmd_calibration.py @@ -9,29 +9,20 @@ ---------------------------------------------------------------------------~(*) """ +import logging import os -import audio -import numpy as np -from file_methods import save_object - from pyglui import ui -from .calibration_plugin_base import Calibration_Plugin -from .finish_calibration import ( - not_enough_data_error_msg, - solver_failed_to_converge_error_msg, -) -from . import calibrate -from .gaze_mappers import ( - Monocular_Gaze_Mapper, - Dual_Monocular_Gaze_Mapper, - Binocular_Vector_Gaze_Mapper, -) -from .optimization_calibration import bundle_adjust_calibration -import math_helper -# logging -import logging +import audio +import file_methods as fm +from calibration_routines import data_processing +from calibration_routines.calibration_plugin_base import Calibration_Plugin +from calibration_routines.finish_calibration import ( + create_converge_error_msg, + create_not_enough_data_error_msg, +) +from calibration_routines.optimization_calibration import calibration_methods logger = logging.getLogger(__name__) @@ -91,12 +82,10 @@ def on_notify(self, notification): self.ref_list += notification["ref_data"] else: logger.error( - "Ref data can only be added when calibratio is runnings." + "Ref data can only be added when calibration is running." ) except KeyError as e: - logger.error( - "Notification: {} not conform. Raised error {}".format(notification, e) - ) + logger.error(f"Notification: {notification} not conform. Raised error {e}") def start(self, hmd_video_frame_size, outlier_threshold): audio.say("Starting Calibration") @@ -126,97 +115,48 @@ def stop(self): def finish_calibration(self): pupil_list = self.pupil_list ref_list = self.ref_list - hmd_video_frame_size = self.hmd_video_frame_size - g_pool = self.g_pool - pupil0 = [p for p in pupil_list if p["id"] == 0] - pupil1 = [p for p in pupil_list if p["id"] == 1] + extracted_data = data_processing.get_data_for_calibration_hmd( + pupil_list, ref_list, mode="2d" + ) + if not extracted_data: + self.notify_all(create_not_enough_data_error_msg(g_pool)) + return - ref0 = [r for r in ref_list if r["id"] == 0] - ref1 = [r for r in ref_list if r["id"] == 1] + method, result = calibration_methods.calibrate_2d_hmd( + self.hmd_video_frame_size, *extracted_data + ) + if result is None: + self.notify_all(create_converge_error_msg(g_pool)) + return - matched_pupil0_data = calibrate.closest_matches_monocular(ref0, pupil0) - matched_pupil1_data = calibrate.closest_matches_monocular(ref1, pupil1) + ts = g_pool.get_timestamp() - if matched_pupil0_data: - cal_pt_cloud = calibrate.preprocess_2d_data_monocular(matched_pupil0_data) - map_fn0, inliers0, params0 = calibrate.calibrate_2d_polynomial( - cal_pt_cloud, hmd_video_frame_size, binocular=False - ) - if not inliers0.any(): - self.notify_all( - { - "subject": "calibration.failed", - "reason": solver_failed_to_converge_error_msg, - } - ) - return - else: - logger.warning("No matched ref<->pupil data collected for id0") - params0 = None - - if matched_pupil1_data: - cal_pt_cloud = calibrate.preprocess_2d_data_monocular(matched_pupil1_data) - map_fn1, inliers1, params1 = calibrate.calibrate_2d_polynomial( - cal_pt_cloud, hmd_video_frame_size, binocular=False - ) - if not inliers1.any(): - self.notify_all( - { - "subject": "calibration.failed", - "reason": solver_failed_to_converge_error_msg, - } - ) - return - else: - logger.warning("No matched ref<->pupil data collected for id1") - params1 = None - - if params0 or params1: - ts = g_pool.get_timestamp() - if params0 and params1: - method = "dual monocular polynomial regression" - mapper = "Dual_Monocular_Gaze_Mapper" - args = {"params0": params0, "params1": params1} - elif params0: - method = "monocular polynomial regression" - mapper = "Monocular_Gaze_Mapper" - args = {"params": params0} - elif params1: - method = "monocular polynomial regression" - mapper = "Monocular_Gaze_Mapper" - args = {"params": params1} - - # Announce success - self.notify_all( - { - "subject": "calibration.successful", - "method": method, - "timestamp": ts, - "record": True, - } - ) + # Announce success + self.notify_all( + { + "subject": "calibration.successful", + "method": method, + "timestamp": ts, + "record": True, + } + ) - # Announce calibration data - self.notify_all( - { - "subject": "calibration.calibration_data", - "timestamp": ts, - "pupil_list": pupil_list, - "ref_list": ref_list, - "calibration_method": method, - "record": True, - } - ) + # Announce calibration data + self.notify_all( + { + "subject": "calibration.calibration_data", + "timestamp": ts, + "pupil_list": pupil_list, + "ref_list": ref_list, + "calibration_method": method, + "record": True, + } + ) - # Start mapper - self.notify_all({"subject": "start_plugin", "name": mapper, "args": args}) - else: - logger.error("Calibration failed for both eyes. No data found") - self.notify_all( - {"subject": "calibration.failed", "reason": not_enough_data_error_msg} - ) + # Start mapper + self.notify_all(result) def recent_events(self, events): if self.active: @@ -271,7 +211,7 @@ def on_notify(self, notification): self.ref_list += notification["ref_data"] else: logger.error( - "Ref data can only be added when calibratio is running." + "Ref data can only be added when calibration is running." ) except KeyError as e: logger.error( @@ -281,156 +221,25 @@ def on_notify(self, notification): def finish_calibration(self): pupil_list = self.pupil_list ref_list = self.ref_list - g_pool = self.g_pool - not_enough_data_error_msg = "Did not collect enough data during calibration." - solver_failed_to_converge_error_msg = ( - "Paramters could not be estimated from data." + extracted_data = data_processing.get_data_for_calibration_hmd( + pupil_list, ref_list, mode="3d" ) - - matched_data = calibrate.closest_matches_binocular(ref_list, pupil_list) - - save_object(matched_data, "hmd_cal_data") - - ref_points_3d_unscaled = np.array([d["ref"]["mm_pos"] for d in matched_data]) - gaze0_dir = [ - d["pupil"]["circle_3d"]["normal"] - for d in matched_data - if "3d" in d["pupil"]["method"] - ] - gaze1_dir = [ - d["pupil1"]["circle_3d"]["normal"] - for d in matched_data - if "3d" in d["pupil"]["method"] - ] - - if len(ref_points_3d_unscaled) < 1 or len(gaze0_dir) < 1 or len(gaze1_dir) < 1: - logger.error(not_enough_data_error_msg) - self.notify_all( - { - "subject": "calibration.failed", - "reason": not_enough_data_error_msg, - "timestamp": self.g_pool.get_timestamp(), - "record": True, - } - ) + if not extracted_data: + self.notify_all(create_not_enough_data_error_msg(g_pool)) return - smallest_residual = 1000 - scales = list(np.linspace(0.7, 10, 50)) - for s in scales: - - ref_points_3d = ref_points_3d_unscaled * (1, -1, s) - - initial_translation0 = np.array(self.eye_translations[0]) - initial_translation1 = np.array(self.eye_translations[1]) - method = "binocular 3d model hmd" - - sphere_pos0 = matched_data[-1]["pupil"]["sphere"]["center"] - sphere_pos1 = matched_data[-1]["pupil1"]["sphere"]["center"] - - initial_R0, initial_t0 = calibrate.find_rigid_transform( - np.array(gaze0_dir) * 500, np.array(ref_points_3d) * 1 - ) - initial_rotation0 = math_helper.quaternion_from_rotation_matrix(initial_R0) - - initial_R1, initial_t1 = calibrate.find_rigid_transform( - np.array(gaze1_dir) * 500, np.array(ref_points_3d) * 1 - ) - initial_rotation1 = math_helper.quaternion_from_rotation_matrix(initial_R1) - - eye0 = { - "observations": gaze0_dir, - "translation": initial_translation0, - "rotation": initial_rotation0, - "fix": ["translation"], - } - eye1 = { - "observations": gaze1_dir, - "translation": initial_translation1, - "rotation": initial_rotation1, - "fix": ["translation"], - } - initial_observers = [eye0, eye1] - initial_points = np.array(ref_points_3d) - - success, residual, observers, points = bundle_adjust_calibration( - initial_observers, initial_points, fix_points=True - ) - - if residual <= smallest_residual: - smallest_residual = residual - scales[-1] = s - - if not success: - self.notify_all( - { - "subject": "calibration.failed", - "reason": solver_failed_to_converge_error_msg, - "timestamp": self.g_pool.get_timestamp(), - "record": True, - } - ) - logger.error("Calibration solver faild to converge.") + method, result = calibration_methods.calibrate_3d_hmd( + *extracted_data, self.eye_translations + ) + if result is None: + self.notify_all(create_converge_error_msg(g_pool)) return - eye0, eye1 = observers - - t_world0 = np.array(eye0["translation"]) - R_world0 = math_helper.quaternion_rotation_matrix(np.array(eye0["rotation"])) - t_world1 = np.array(eye1["translation"]) - R_world1 = math_helper.quaternion_rotation_matrix(np.array(eye1["rotation"])) - - def toWorld0(p): - return np.dot(R_world0, p) + t_world0 - - def toWorld1(p): - return np.dot(R_world1, p) + t_world1 - - points_a = points # world coords - points_b = [] # eye0 coords - points_c = [] # eye1 coords - - for a, b, c, point in zip( - points, eye0["observations"], eye1["observations"], points - ): - line_a = np.array([0, 0, 0]), np.array(a) # observation as line - line_b = ( - toWorld0(np.array([0, 0, 0])), - toWorld0(b), - ) # eye0 observation line in world coords - line_c = ( - toWorld1(np.array([0, 0, 0])), - toWorld1(c), - ) # eye1 observation line in world coords - close_point_a, _ = math_helper.nearest_linepoint_to_point(point, line_a) - close_point_b, _ = math_helper.nearest_linepoint_to_point(point, line_b) - close_point_c, _ = math_helper.nearest_linepoint_to_point(point, line_c) - points_a.append(close_point_a.tolist()) - points_b.append(close_point_b.tolist()) - points_c.append(close_point_c.tolist()) - - # we need to take the sphere position into account - # orientation and translation are referring to the sphere center. - # but we want to have it referring to the camera center - # since the actual translation is in world coordinates, the sphere translation needs to be calculated in world coordinates - sphere_translation = np.array(sphere_pos0) - sphere_translation_world = np.dot(R_world0, sphere_translation) - camera_translation = t_world0 - sphere_translation_world - eye_camera_to_world_matrix0 = np.eye(4) - eye_camera_to_world_matrix0[:3, :3] = R_world0 - eye_camera_to_world_matrix0[:3, 3:4] = np.reshape(camera_translation, (3, 1)) - - sphere_translation = np.array(sphere_pos1) - sphere_translation_world = np.dot(R_world1, sphere_translation) - camera_translation = t_world1 - sphere_translation_world - eye_camera_to_world_matrix1 = np.eye(4) - eye_camera_to_world_matrix1[:3, :3] = R_world1 - eye_camera_to_world_matrix1[:3, 3:4] = np.reshape(camera_translation, (3, 1)) - - method = "hmd binocular 3d model" ts = g_pool.get_timestamp() + + # Announce success g_pool.active_calibration_plugin.notify_all( { "subject": "calibration.successful", @@ -439,40 +248,27 @@ def toWorld1(p): "record": True, } ) - g_pool.active_calibration_plugin.notify_all( - { - "subject": "calibration.calibration_data", - "timestamp": ts, - "pupil_list": pupil_list, - "ref_list": ref_list, - "calibration_method": method, - "record": True, - } - ) - # this is only used by show calibration. TODO: rewrite show calibraiton. + # Announce calibration data + # this is only used by show calibration. TODO: rewrite show calibration. user_calibration_data = { "timestamp": ts, "pupil_list": pupil_list, "ref_list": ref_list, "calibration_method": method, } - save_object( + fm.save_object( user_calibration_data, os.path.join(g_pool.user_dir, "user_calibration_data"), ) + g_pool.active_calibration_plugin.notify_all( + { + "subject": "calibration.calibration_data", + "record": True, + **user_calibration_data, + } + ) - mapper_args = { - "subject": "start_plugin", - "name": "Binocular_Vector_Gaze_Mapper", - "args": { - "eye_camera_to_world_matrix0": eye_camera_to_world_matrix0.tolist(), - "eye_camera_to_world_matrix1": eye_camera_to_world_matrix1.tolist(), - "cal_points_3d": points, - "cal_ref_points_3d": points_a, - "cal_gaze_points0_3d": points_b, - "cal_gaze_points1_3d": points_c, - "backproject": hasattr(self.g_pool, "capture"), - }, - } - self.g_pool.active_calibration_plugin.notify_all(mapper_args) + # Start mapper + result["args"]["backproject"] = hasattr(g_pool, "capture") + self.g_pool.active_calibration_plugin.notify_all(result) diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/.gitignore b/pupil_src/shared_modules/calibration_routines/optimization_calibration/.gitignore deleted file mode 100644 index 0536e865ea..0000000000 --- a/pupil_src/shared_modules/calibration_routines/optimization_calibration/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -*.cpp -*.o -*.so \ No newline at end of file diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/__init__.py b/pupil_src/shared_modules/calibration_routines/optimization_calibration/__init__.py index 896a6a95f2..60fb4f5eee 100644 --- a/pupil_src/shared_modules/calibration_routines/optimization_calibration/__init__.py +++ b/pupil_src/shared_modules/calibration_routines/optimization_calibration/__init__.py @@ -2,17 +2,8 @@ (*)~--------------------------------------------------------------------------- Pupil - eye tracking platform Copyright (C) 2012-2020 Pupil Labs - Distributed under the terms of the GNU Lesser General Public License (LGPL v3.0). See COPYING and COPYING.LESSER for license details. ---------------------------------------------------------------------------~(*) """ - -try: - from .calibration_methods import bundle_adjust_calibration -except ModuleNotFoundError: - # when running from source compile cpp extension if necessary. - from .build import build_cpp_extension - build_cpp_extension() - from .calibration_methods import bundle_adjust_calibration diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/build.py b/pupil_src/shared_modules/calibration_routines/optimization_calibration/build.py deleted file mode 100644 index 39e734a1ae..0000000000 --- a/pupil_src/shared_modules/calibration_routines/optimization_calibration/build.py +++ /dev/null @@ -1,39 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -import logging - -logger = logging.getLogger(__name__) - - -def build_cpp_extension(): - import subprocess as sp - import os, sys - - src_loc = os.path.dirname(os.path.realpath(__file__)) - install_loc = os.path.split(os.path.split(src_loc)[0])[0] - cwd = os.getcwd() - os.chdir(src_loc) - logger.info("Building extension modules...") - build_cmd = [ - sys.executable, - "setup.py", - "install", - "--install-lib={}".format(install_loc), - ] - ret = sp.check_output(build_cmd).decode(sys.stdout.encoding) - logger.debug("Build log:\n{}".format(ret)) - os.chdir(cwd) - - -if __name__ == "__main__": - logging.basicConfig(level=logging.DEBUG) - build_cpp_extension() diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/bundleCalibration.h b/pupil_src/shared_modules/calibration_routines/optimization_calibration/bundleCalibration.h deleted file mode 100644 index 992a1b63eb..0000000000 --- a/pupil_src/shared_modules/calibration_routines/optimization_calibration/bundleCalibration.h +++ /dev/null @@ -1,162 +0,0 @@ -/* -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -*/ - - -#include "common.h" -#include -#include -#include - -#include -#include -#include "ceres/Fixed3DNormParametrization.h" -#include "ceres/EigenQuaternionParameterization.h" -#include "ceres/CeresUtils.h" -#include "math/distance.h" -#include "common/types.h" - -using ceres::AutoDiffCostFunction; -using ceres::NumericDiffCostFunction; -using ceres::CauchyLoss; -using ceres::CostFunction; -using ceres::LossFunction; -using ceres::Problem; -using ceres::Solve; -using ceres::Solver; - - -struct ReprojectionError { - ReprojectionError( ::Vector3 observed_point) - : observed_point(observed_point) {} - - template - bool operator()(const T* const orientation, - const T* const translation, - const T* const point, - T* residuals) const { - - T p[3]; - - - // convetional order rot and then trans - // ceres::AngleAxisRotatePoint(orientation, point, p); - // // pose[3,4,5] are the translation. - // p[0] += translation[0]; - // p[1] += translation[1]; - // p[2] += translation[2]; - - - // unconvetional reverse order trans then rot. - T tp[3]; - // pose[3,4,5] are the translation. - tp[0] = point[0] + translation[0]; - tp[1] = point[1] + translation[1]; - tp[2] = point[2] + translation[2]; - ceres::AngleAxisRotatePoint(orientation, tp, p); - - // Normalize / project back to unit sphere - T s = sqrt( p[0]*p[0] + p[1]*p[1] + p[2]*p[2] ); - p[0] /= s; - p[1] /= s; - p[2] /= s; - - - // The error is the difference between the predicted and observed position. - residuals[0] = p[0] - T(observed_point[0]); - residuals[1] = p[1] - T(observed_point[1]); - residuals[2] = p[2] - T(observed_point[2]); - - return true; - } - -// Factory to hide the construction of the CostFunction object from - // the client code. - static ceres::CostFunction* Create(const ::Vector3 observed_point ) { - return (new ceres::AutoDiffCostFunction( - new ReprojectionError(observed_point))); - } - - ::Vector3 observed_point; -}; - -double bundleAdjustCalibration( std::vector& observers, std::vector<::Vector3>& points,bool fix_points) -{ - - - Problem problem; - - for( auto& observer : observers ){ - - double* pose = observer.pose.data(); - - int index = 0; - for( auto& observation : observer.observations){ - - // Each Residual block takes a point and a pose as input and outputs a 2 - // dimensional residual. Internally, the cost function stores the observed - // image location and compares the reprojection against the observation. - ceres::CostFunction* cost_function = - ReprojectionError::Create(observation); - - problem.AddResidualBlock(cost_function, - NULL /* squared loss */, - pose, - pose+3, - points[index].data() ); - index++; - - } - if(observer.fix_rotation == 1){ - problem.SetParameterBlockConstant(pose); - } - if(observer.fix_translation == 1){ - problem.SetParameterBlockConstant(pose+3); - } - } - - if(fix_points == true){ - int index = 0; - for(auto o : observers[0].observations){ - problem.SetParameterBlockConstant(points[index].data()); - index++; - } - } - - - // Build and solve the problem. - Solver::Options options; - options.max_num_iterations = 1000; - options.linear_solver_type = ceres::DENSE_SCHUR; - - // options.parameter_tolerance = 1e-35; - // options.function_tolerance = 1e-35; - options.gradient_tolerance = 1e-35; - // options.minimizer_progress_to_stdout = true; - //options.logging_type = ceres::SILENT; - // options.check_gradients = true; - - - Solver::Summary summary; - Solve(options, &problem, &summary); - - - std::cout << summary.BriefReport() << "\n"; - // std::cout << summary.FullReport() << "\n"; - - if( summary.termination_type != ceres::TerminationType::CONVERGENCE ){ - std::cout << "Termination Error: " << ceres::TerminationTypeToString(summary.termination_type) << std::endl; - return -1; - } - - return summary.final_cost; - -} - diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/bundle_adjustment.py b/pupil_src/shared_modules/calibration_routines/optimization_calibration/bundle_adjustment.py new file mode 100644 index 0000000000..877b6d30e0 --- /dev/null +++ b/pupil_src/shared_modules/calibration_routines/optimization_calibration/bundle_adjustment.py @@ -0,0 +1,240 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" + +import cv2 +import numpy as np +from scipy import optimize as scipy_optimize, sparse as scipy_sparse + +from calibration_routines.optimization_calibration import utils + + +# BundleAdjustment is a class instead of functions, since passing all the parameters +# would be inefficient. (especially true for _compute_residuals as a callback) +class BundleAdjustment: + def __init__(self, fix_gaze_targets): + self._fix_gaze_targets = bool(fix_gaze_targets) + self._opt_items = None + self._n_spherical_cameras = None + self._n_poses_variables = None + self._gaze_targets_size = None + self._indices = None + self._current_values = None + self._rotation_size = None + + self._row_ind = None + self._col_ind = None + + @staticmethod + def _toarray(arr): + return np.asarray(arr, dtype=np.float64) + + def calculate(self, initial_spherical_cameras, initial_gaze_targets): + initial_rotation = self._toarray( + [o.rotation for o in initial_spherical_cameras] + ) + initial_translation = self._toarray( + [o.translation for o in initial_spherical_cameras] + ) + all_observations = self._toarray( + [o.observations for o in initial_spherical_cameras] + ) + initial_gaze_targets = self._toarray(initial_gaze_targets) + + opt_rot = [not o.fix_rotation for o in initial_spherical_cameras] + opt_trans = [not o.fix_translation for o in initial_spherical_cameras] + self._opt_items = np.array(opt_rot + opt_trans, dtype=bool) + self._n_poses_variables = 3 * np.sum(self._opt_items) + self._rotation_size = initial_rotation.size + self._gaze_targets_size = initial_gaze_targets.size + self._n_spherical_cameras = len(initial_spherical_cameras) + self._indices = self._get_indices() + initial_guess = self._get_initial_guess( + initial_rotation, initial_translation, initial_gaze_targets + ) + self._row_ind, self._col_ind = self._get_ind_for_jacobian_matrix() + + result = self._least_squares(initial_guess, all_observations) + return self._get_final_output(result) + + def _get_indices(self): + """Get the indices of the parameters for the optimization""" + + to_be_opt = np.repeat(self._opt_items, 3) + if not self._fix_gaze_targets: + to_be_opt = np.append( + to_be_opt, np.ones(self._gaze_targets_size, dtype=bool) + ) + return np.where(to_be_opt)[0] + + def _get_initial_guess( + self, initial_rotation, initial_translation, initial_gaze_targets + ): + self._current_values = np.append( + initial_rotation.ravel(), initial_translation.ravel() + ) + self._current_values = np.append( + self._current_values, initial_gaze_targets.ravel() + ) + return self._current_values[self._indices] + + def _get_ind_for_jacobian_matrix(self): + def get_mat_pose(i): + mat_pose = np.ones((self._gaze_targets_size, 3), dtype=bool) + row, col = np.where(mat_pose) + row += (i % self._n_spherical_cameras) * self._gaze_targets_size + col += 3 * np.sum(self._opt_items[:i]) + return row, col + + try: + row_ind, col_ind = np.concatenate( + [ + get_mat_pose(i) + for i in range(len(self._opt_items)) + if self._opt_items[i] + ], + axis=1, + ) + except ValueError: + row_ind, col_ind = np.where([[]]) + + if not self._fix_gaze_targets: + _row = np.repeat( + np.arange(self._gaze_targets_size).reshape(-1, 3), 3, axis=0 + ).ravel() + ind_row = [ + _row + self._gaze_targets_size * i + for i in range(self._n_spherical_cameras) + ] + ind_row = np.asarray(ind_row).ravel() + ind_col = np.tile( + np.repeat(np.arange(self._gaze_targets_size), 3), + self._n_spherical_cameras, + ) + row_ind = np.append(row_ind, ind_row) + col_ind = np.append(col_ind, ind_col + self._n_poses_variables) + + return row_ind, col_ind + + def _calculate_jacobian_matrix(self, variables, all_observations): + def get_jac_rot(normals, rotation): + jacobian = cv2.Rodrigues(rotation)[1].reshape(3, 3, 3) + return np.einsum("mk,ijk->mji", normals, jacobian) + + def get_jac_trans(translation): + vectors = gaze_targets - translation + norms = np.linalg.norm(vectors, axis=1) + block = -np.einsum("ki,kj->kij", vectors, vectors) + block /= (norms ** 3)[:, np.newaxis, np.newaxis] + ones = np.eye(3)[np.newaxis] / norms[:, np.newaxis, np.newaxis] + block += ones + return block + + rotations, translations, gaze_targets = self._decompose_variables(variables) + + data_rot = [ + get_jac_rot(normals, rotation) + for normals, rotation, opt in zip( + all_observations, + rotations, + self._opt_items[: self._n_spherical_cameras], + ) + if opt + ] + data_rot = self._toarray(data_rot).ravel() + data_trans = [ + get_jac_trans(translation) + for translation, opt in zip( + translations, self._opt_items[self._n_spherical_cameras :] + ) + if opt + ] + data_trans = self._toarray(data_trans).ravel() + data = np.append(data_rot, data_trans) + + if not self._fix_gaze_targets: + data_targets = [-get_jac_trans(translation) for translation in translations] + data_targets = self._toarray(data_targets).ravel() + data = np.append(data, data_targets) + + n_residuals = self._gaze_targets_size * self._n_spherical_cameras + n_variables = len(self._indices) + jacobian_matrix = scipy_sparse.csc_matrix( + (data, (self._row_ind, self._col_ind)), shape=(n_residuals, n_variables) + ) + return jacobian_matrix + + def _least_squares(self, initial_guess, all_observations, tol=1e-8, max_nfev=100): + x_scale = np.ones(self._n_poses_variables) + if not self._fix_gaze_targets: + x_scale = np.append(x_scale, np.ones(self._gaze_targets_size) * 500) / 20 + + result = scipy_optimize.least_squares( + fun=self._compute_residuals, + x0=initial_guess, + args=(all_observations,), + jac=self._calculate_jacobian_matrix, + ftol=tol, + xtol=tol, + gtol=tol, + x_scale=x_scale, + max_nfev=max_nfev, + verbose=1, + ) + return result + + def _compute_residuals(self, variables, all_observations): + rotations, translations, gaze_targets = self._decompose_variables(variables) + + all_observations_world = self._transform_all_observations_to_world( + rotations, all_observations + ) + projected_gaze_targets = self._project_gaze_targets(translations, gaze_targets) + residuals = all_observations_world - projected_gaze_targets + return residuals.ravel() + + def _transform_all_observations_to_world(self, rotations, all_observations): + rotation_matrices = [cv2.Rodrigues(r)[0] for r in rotations] + all_observations_world = [ + np.dot(matrix, observations.T).T + for matrix, observations in zip(rotation_matrices, all_observations) + ] + return self._toarray(all_observations_world) + + @staticmethod + def _project_gaze_targets(translations, gaze_targets): + """Project gaze targets onto the spherical cameras + (where projection simply means normalization) + """ + + directions = gaze_targets[np.newaxis] - translations[:, np.newaxis] + norms = np.linalg.norm(directions, axis=2)[:, :, np.newaxis] + projected_gaze_targets = directions / norms + return projected_gaze_targets + + def _decompose_variables(self, variables): + self._current_values[self._indices] = variables + rotations = self._current_values[: self._rotation_size].reshape(-1, 3) + translations = self._current_values[ + self._rotation_size : -self._gaze_targets_size + ].reshape(-1, 3) + gaze_targets = self._current_values[-self._gaze_targets_size :].reshape(-1, 3) + return rotations, translations, gaze_targets + + def _get_final_output(self, result): + residual = result.cost + rotations, translations, final_gaze_targets = self._decompose_variables( + result.x + ) + final_poses = [ + utils.merge_extrinsic(rotation, translation) + for rotation, translation in zip(rotations, translations) + ] + return residual, final_poses, final_gaze_targets diff --git a/pupil_src/shared_modules/calibration_routines/calibrate.py b/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibrate_2d.py similarity index 65% rename from pupil_src/shared_modules/calibration_routines/calibrate.py rename to pupil_src/shared_modules/calibration_routines/optimization_calibration/calibrate_2d.py index e99d601457..3a8ece00ae 100644 --- a/pupil_src/shared_modules/calibration_routines/calibrate.py +++ b/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibrate_2d.py @@ -9,12 +9,10 @@ ---------------------------------------------------------------------------~(*) """ -import numpy as np -import cv2 - -# logging import logging +import numpy as np + logger = logging.getLogger(__name__) @@ -48,15 +46,15 @@ def calibrate_2d_polynomial( "first iteration. root-mean-square residuals: {}, in pixel".format(err_rms) ) logger.info( - "second iteration: ignoring outliers. root-mean-square residuals: {} in pixel".format( - new_err_rms - ) + "second iteration: ignoring outliers. root-mean-square residuals: {} " + "in pixel".format(new_err_rms) ) used_num = cal_pt_cloud[err_dist <= threshold].shape[0] complete_num = cal_pt_cloud.shape[0] logger.info( - "used {} data points out of the full dataset {}: subset is {:.2f} percent".format( + "used {} data points out of the full dataset {}: " + "subset is {:.2f} percent".format( used_num, complete_num, 100 * float(used_num) / complete_num ) ) @@ -67,12 +65,12 @@ def calibrate_2d_polynomial( ([p.tolist() for p in cx], [p.tolist() for p in cy], model_n), ) - else: # did disregard all points. The data cannot be represented by the model in a meaningful way: + else: # did disregard all points. The data cannot be represented by the model in + # a meaningful way: map_fn = make_map_function(cx, cy, model_n) logger.error( - "First iteration. root-mean-square residuals: {} in pixel, this is bad!".format( - err_rms - ) + "First iteration. root-mean-square residuals: {} in pixel, " + "this is bad!".format(err_rms) ) logger.error( "The data cannot be represented by the model in a meaningfull way." @@ -108,15 +106,6 @@ def fit_error_screen(err_x, err_y, screen_pos): return err_dist, err_mean, err_rms -def fit_error_angle(err_x, err_y): - err_x *= 2.0 * np.pi - err_y *= 2.0 * np.pi - err_dist = np.sqrt(err_x * err_x + err_y * err_y) - err_mean = np.sum(err_dist) / len(err_dist) - err_rms = np.sqrt(np.sum(err_dist * err_dist) / len(err_dist)) - return err_dist, err_mean, err_rms - - def make_model(cal_pt_cloud, n=7): n_points = cal_pt_cloud.shape[0] @@ -396,177 +385,3 @@ def fn(pt_0, pt_1): raise Exception("ERROR: unsopported number of coefficiants.") return fn - - -def closest_matches_binocular(ref_pts, pupil_pts, max_dispersion=1 / 15.0): - """ - get pupil positions closest in time to ref points. - return list of dict with matching ref, pupil0 and pupil1 data triplets. - """ - pupil0 = [p for p in pupil_pts if p["id"] == 0] - pupil1 = [p for p in pupil_pts if p["id"] == 1] - - pupil0_ts = np.array([p["timestamp"] for p in pupil0]) - pupil1_ts = np.array([p["timestamp"] for p in pupil1]) - - def find_nearest_idx(array, value): - idx = np.searchsorted(array, value, side="left") - try: - if abs(value - array[idx - 1]) < abs(value - array[idx]): - return idx - 1 - else: - return idx - except IndexError: - return idx - 1 - - matched = [] - - if pupil0 and pupil1: - for r in ref_pts: - closest_p0_idx = find_nearest_idx(pupil0_ts, r["timestamp"]) - closest_p0 = pupil0[closest_p0_idx] - closest_p1_idx = find_nearest_idx(pupil1_ts, r["timestamp"]) - closest_p1 = pupil1[closest_p1_idx] - - dispersion = max( - closest_p0["timestamp"], closest_p1["timestamp"], r["timestamp"] - ) - min(closest_p0["timestamp"], closest_p1["timestamp"], r["timestamp"]) - if dispersion < max_dispersion: - matched.append({"ref": r, "pupil": closest_p0, "pupil1": closest_p1}) - else: - logger.debug( - "Binocular match rejected due to time dispersion criterion" - ) - return matched - - -def closest_matches_monocular(ref_pts, pupil_pts, max_dispersion=1 / 15.0): - """ - get pupil positions closest in time to ref points. - return list of dict with matching ref and pupil datum. - - if your data is binocular use: - pupil0 = [p for p in pupil_pts if p['id']==0] - pupil1 = [p for p in pupil_pts if p['id']==1] - to get the desired eye and pass it as pupil_pts - """ - - ref = ref_pts - pupil0 = pupil_pts - pupil0_ts = np.array([p["timestamp"] for p in pupil0]) - - def find_nearest_idx(array, value): - idx = np.searchsorted(array, value, side="left") - try: - if abs(value - array[idx - 1]) < abs(value - array[idx]): - return idx - 1 - else: - return idx - except IndexError: - return idx - 1 - - matched = [] - if pupil0: - for r in ref_pts: - closest_p0_idx = find_nearest_idx(pupil0_ts, r["timestamp"]) - closest_p0 = pupil0[closest_p0_idx] - dispersion = max(closest_p0["timestamp"], r["timestamp"]) - min( - closest_p0["timestamp"], r["timestamp"] - ) - if dispersion < max_dispersion: - matched.append({"ref": r, "pupil": closest_p0}) - else: - pass - return matched - - -def preprocess_2d_data_monocular(matched_data): - cal_data = [ - (*pair["pupil"]["norm_pos"], *pair["ref"]["norm_pos"]) for pair in matched_data - ] - return cal_data - - -def preprocess_2d_data_binocular(matched_data): - cal_data = [ - ( - *triplet["pupil"]["norm_pos"], - *triplet["pupil1"]["norm_pos"], - *triplet["ref"]["norm_pos"], - ) - for triplet in matched_data - ] - return cal_data - - -def preprocess_3d_data(matched_data, g_pool): - pupil0_processed = [ - dp["pupil"]["circle_3d"]["normal"] - for dp in matched_data - if "circle_3d" in dp["pupil"] - ] - - pupil1_processed = [ - dp["pupil1"]["circle_3d"]["normal"] - for dp in matched_data - if "pupil1" in dp and "circle_3d" in dp["pupil1"] - ] - - ref = np.array([dp["ref"]["screen_pos"] for dp in matched_data]) - ref_processed = g_pool.capture.intrinsics.unprojectPoints(ref, normalize=True) - - return ref_processed, pupil0_processed, pupil1_processed - - -def find_rigid_transform(A, B): - # we expect the shape to be of length 2 - assert len(A.shape) == len(B.shape) == 2 - assert A.shape[0] == B.shape[0] - - centroid_A = np.mean(A, axis=0) - centroid_B = np.mean(B, axis=0) - - # centre the points - A -= centroid_A - B -= centroid_B - - # dot is matrix multiplication for array - H = A.T @ B - U, S, Vt = np.linalg.svd(H) - R = Vt.T @ U.T - # special reflection case - if np.linalg.det(R) < 0: - logger.info("Reflection detected") - Vt[2, :] *= -1 - R = Vt.T * U.T - - t = -R @ centroid_A.T + centroid_B.T - - return R, t.reshape(3) - - -def calculate_residual_3D_Points(ref_points, gaze_points, eye_to_world_matrix): - - average_distance = 0.0 - distance_variance = 0.0 - transformed_gaze_points = [] - - for p in gaze_points: - point = np.zeros(4) - point[:3] = p - point[3] = 1.0 - point = eye_to_world_matrix.dot(point) - point = np.squeeze(np.asarray(point)) - transformed_gaze_points.append(point[:3]) - - for (a, b) in zip(ref_points, transformed_gaze_points): - average_distance += np.linalg.norm(a - b) - - average_distance /= len(ref_points) - - for (a, b) in zip(ref_points, transformed_gaze_points): - distance_variance += (np.linalg.norm(a - b) - average_distance) ** 2 - - distance_variance /= len(ref_points) - - return average_distance, distance_variance diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibrate_3d.py b/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibrate_3d.py new file mode 100644 index 0000000000..42257561cc --- /dev/null +++ b/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibrate_3d.py @@ -0,0 +1,218 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" + +import cv2 +import numpy as np + +from calibration_routines.optimization_calibration import utils, bundle_adjustment + +# Fixed eyeball positions are assumed for all users +eye0_hardcoded_translation = np.array([20, 15, -20]) +eye1_hardcoded_translation = np.array([-40, 15, -20]) + +residual_threshold = 1e3 + + +class SphericalCamera: + def __init__( + self, observations, rotation, translation, fix_rotation, fix_translation + ): + self.observations = observations + self.rotation = rotation + self.translation = translation + self.fix_rotation = bool(fix_rotation) + self.fix_translation = bool(fix_translation) + + +def calibrate_binocular( + unprojected_ref_points, pupil0_normals, pupil1_normals, initial_depth +): + """Determine the poses of the eyes and 3d gaze points by solving a specific + least-squares minimization + + :param unprojected_ref_points: the unprojection of the observed 2d reference points + at unit distance in world camera coordinates + :param pupil0_normals: eye0's pupil normals in eye0 camera coordinates + :param pupil1_normals: eye1's pupil normals in eye1 camera coordinates + :param initial_depth: initial guess of the depth of the gaze targets + :return: optimized poses and 3d gaze targets in world camera coordinates + """ + # Binocular calibration strategy: + # Take world cam as the origin and express everything in world cam coordinates. + # Minimize reprojection-type errors by moving the 3d gaze targets and + # adjusting the orientation of the eyes while fixing their positions. + + # Find initial guess for the poses in world coordinates + initial_rotation0 = utils.get_initial_eye_camera_rotation( + pupil0_normals, unprojected_ref_points + ) + initial_rotation1 = utils.get_initial_eye_camera_rotation( + pupil1_normals, unprojected_ref_points + ) + initial_translation0 = eye0_hardcoded_translation + initial_translation1 = eye1_hardcoded_translation + + # world cam and eyes are viewed as spherical cameras of unit radius + world = SphericalCamera( + observations=unprojected_ref_points, + rotation=np.zeros(3), + translation=np.zeros(3), + fix_rotation=True, + fix_translation=True, + ) + eye0 = SphericalCamera( + observations=pupil0_normals, + rotation=initial_rotation0, + translation=initial_translation0, + fix_rotation=False, + fix_translation=True, + ) + eye1 = SphericalCamera( + observations=pupil1_normals, + rotation=initial_rotation1, + translation=initial_translation1, + fix_rotation=False, + fix_translation=True, + ) + + initial_spherical_cameras = world, eye0, eye1 + initial_gaze_targets = unprojected_ref_points * initial_depth + + ba = bundle_adjustment.BundleAdjustment(fix_gaze_targets=False) + residual, poses_in_world, gaze_targets_in_world = ba.calculate( + initial_spherical_cameras, initial_gaze_targets + ) + + success = residual < residual_threshold + return success, poses_in_world, gaze_targets_in_world + + +def calibrate_monocular(unprojected_ref_points, pupil_normals, pupil_id, initial_depth): + """Determine the poses of the eyes and 3d gaze points by solving a specific + least-squares minimization + + :param unprojected_ref_points: the unprojection of the observed 2d reference points + at unit distance in world camera coordinates + :param pupil_normals: eye's pupil normals in eye camera coordinates + :param pupil_id: pupil id (0 or 1) + :param initial_depth: initial guess of the depth of the gaze targets + :return: optimized poses and 3d gaze targets in world camera coordinates + """ + # Monocular calibration strategy: + # Take eye as the origin and express everything in eye coordinates. + # Minimize reprojection-type errors by moving world cam + # while fixing the 3d gaze targets. + + # Find initial guess for the poses in eye coordinates + initial_rotation_matrix, _ = utils.find_rigid_transform( + unprojected_ref_points, pupil_normals + ) + hardcoded_translation = ( + eye0_hardcoded_translation if pupil_id == 0 else eye1_hardcoded_translation + ) + initial_rotation = cv2.Rodrigues(initial_rotation_matrix)[0].ravel() + initial_translation = -np.dot(initial_rotation_matrix, hardcoded_translation) + + # world cam and eye are viewed as spherical cameras of unit radius + world = SphericalCamera( + observations=unprojected_ref_points, + rotation=initial_rotation, + translation=initial_translation, + fix_rotation=False, + fix_translation=False, + ) + eye = SphericalCamera( + observations=pupil_normals, + rotation=np.zeros(3), + translation=np.zeros(3), + fix_rotation=True, + fix_translation=True, + ) + + initial_spherical_cameras = world, eye + initial_gaze_targets = pupil_normals * initial_depth + + ba = bundle_adjustment.BundleAdjustment(fix_gaze_targets=True) + residual, poses_in_eye, gaze_targets_in_eye = ba.calculate( + initial_spherical_cameras, initial_gaze_targets + ) + + world_pose_in_eye, eye_pose_in_eye = poses_in_eye + + # Transform everything from eye coordinates to world coordinates + eye_pose_in_world = utils.inverse_extrinsic(world_pose_in_eye) + poses_in_world = [np.zeros(6), eye_pose_in_world] + gaze_targets_in_world = utils.transform_points_by_pose( + gaze_targets_in_eye, world_pose_in_eye + ) + + success = residual < residual_threshold + return success, poses_in_world, gaze_targets_in_world + + +def calibrate_hmd(ref_points_3d, pupil0_normals, pupil1_normals, eye_translations): + """Determine the poses of the eyes and 3d gaze points by solving a specific + least-squares minimization + + :param ref_points_3d: the observed 3d reference points in world camera coordinates + :param pupil0_normals: eye0's pupil normals in eye0 camera coordinates + :param pupil1_normals: eye1's pupil normals in eye1 camera coordinates + :param eye_translations: eyeballs position in world coordinates + :return: optimized poses and 3d gaze targets in world camera coordinates + """ + # HMD calibration strategy: + # Take world cam as the origin and express everything in world cam coordinates. + # Minimize reprojection-type errors by adjusting the orientation of the eyes + # while fixing their positions and the 3d gaze targets. + + initial_translation0, initial_translation1 = np.asarray(eye_translations) + + smallest_residual = 1000 + scales = list(np.linspace(0.7, 10, 50)) + for s in scales: + scaled_ref_points_3d = ref_points_3d * (1, -1, s) + + # Find initial guess for the poses in eye coordinates + initial_rotation0 = utils.get_initial_eye_camera_rotation( + pupil0_normals, scaled_ref_points_3d + ) + initial_rotation1 = utils.get_initial_eye_camera_rotation( + pupil1_normals, scaled_ref_points_3d + ) + + eye0 = SphericalCamera( + observations=pupil0_normals, + rotation=initial_rotation0, + translation=initial_translation0, + fix_rotation=False, + fix_translation=True, + ) + eye1 = SphericalCamera( + observations=pupil1_normals, + rotation=initial_rotation1, + translation=initial_translation1, + fix_rotation=False, + fix_translation=True, + ) + + initial_spherical_cameras = eye0, eye1 + initial_gaze_targets = scaled_ref_points_3d + + ba = bundle_adjustment.BundleAdjustment(fix_gaze_targets=True) + residual, poses_in_world, gaze_targets_in_world = ba.calculate( + initial_spherical_cameras, initial_gaze_targets + ) + if residual <= smallest_residual: + smallest_residual = residual + scales[-1] = s + + success = residual < residual_threshold + return success, poses_in_world, gaze_targets_in_world diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibration_methods.pxd b/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibration_methods.pxd deleted file mode 100644 index 2a43034ef0..0000000000 --- a/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibration_methods.pxd +++ /dev/null @@ -1,73 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -from libcpp.vector cimport vector - -cdef extern from '' namespace 'Eigen': - cdef cppclass Matrix21d "Eigen::Matrix": # eigen defaults to column major layout - Matrix21d() except + - double * data() - double& operator[](size_t) - - cdef cppclass Matrix31d "Eigen::Matrix": # eigen defaults to column major layout - Matrix31d() except + - Matrix31d(double x, double y, double z) - double * data() - double& operator[](size_t) - bint isZero() - - cdef cppclass Matrix41d "Eigen::Matrix": # eigen defaults to column major layout - Matrix41d() except + - Matrix41d(double w, double a, double b, double c) - double * data() - double& operator[](size_t) - bint isZero() - - - cdef cppclass Matrix4d "Eigen::Matrix": # eigen defaults to column major layout - Matrix4d() except + - double& operator()(size_t,size_t) - - cdef cppclass Quaterniond "Eigen::Quaterniond": # eigen defaults to column major layout - Quaterniond() except + - Quaterniond( double w, double x, double y, double z) - double w() - double x() - double y() - double z() - -cdef extern from 'common.h' namespace "" : - - #typdefs - ctypedef Matrix31d Vector3 - ctypedef Matrix21d Vector2 - ctypedef Matrix41d Vector4 - - - cdef cppclass Observer "Observer": # eigen defaults to column major layout - Observer() except + - vector[Vector3] observations - vector[double] pose - int fix_rotation - int fix_translation - -cdef extern from 'ceres/rotation.h' namespace 'ceres': - #template - #AngleAxisToQuaternion(T const* angle_axis, T* quaternion); - #template - #QuaternionToAngleAxis(T const* quaternion, T* angle_axis); - void AngleAxisToQuaternion(const double * angle_axis, double * quaternion); - void QuaternionToAngleAxis(const double * quaternion, double * angle_axis); - void AngleAxisRotatePoint(const double * angle_axis, const double * pt,double * result) - -cdef extern from 'bundleCalibration.h': - - double bundleAdjustCalibration( vector[Observer]& obsevers, vector[Vector3]& points,bint fix_points) diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibration_methods.py b/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibration_methods.py new file mode 100644 index 0000000000..2e8e420a66 --- /dev/null +++ b/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibration_methods.py @@ -0,0 +1,218 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" + +import numpy as np + +from calibration_routines.optimization_calibration import ( + calibrate_3d, + calibrate_2d, + utils, +) + + +def calibrate_3d_binocular( + unprojected_ref_points, + pupil0_normals, + pupil1_normals, + last_pupil0, + last_pupil1, + initial_depth=500, +): + method = "binocular 3d model" + + res = calibrate_3d.calibrate_binocular( + unprojected_ref_points, pupil0_normals, pupil1_normals, initial_depth + ) + success, poses_in_world, gaze_targets_in_world = res + if not success: + return method, None + + world_pose, eye0_pose, eye1_pose = poses_in_world + + sphere_pos0 = last_pupil0["sphere"]["center"] + sphere_pos1 = last_pupil1["sphere"]["center"] + eye0_cam_pose_in_world = utils.get_eye_cam_pose_in_world(eye0_pose, sphere_pos0) + eye1_cam_pose_in_world = utils.get_eye_cam_pose_in_world(eye1_pose, sphere_pos1) + + all_observations = [unprojected_ref_points, pupil0_normals, pupil1_normals] + nearest_points = utils.calculate_nearest_points_to_targets( + all_observations, poses_in_world, gaze_targets_in_world + ) + nearest_points_world, nearest_points_eye0, nearest_points_eye1 = nearest_points + + mapper = "Binocular_Vector_Gaze_Mapper" + args = { + "eye_camera_to_world_matrix0": eye0_cam_pose_in_world.tolist(), + "eye_camera_to_world_matrix1": eye1_cam_pose_in_world.tolist(), + "cal_points_3d": gaze_targets_in_world.tolist(), + "cal_ref_points_3d": nearest_points_world.tolist(), + "cal_gaze_points0_3d": nearest_points_eye0.tolist(), + "cal_gaze_points1_3d": nearest_points_eye1.tolist(), + } + result = {"subject": "start_plugin", "name": mapper, "args": args} + return method, result + + +def calibrate_3d_monocular( + unprojected_ref_points, pupil_normals, last_pupil, initial_depth=500 +): + method = "monocular 3d model" + + pupil_id = last_pupil["id"] + res = calibrate_3d.calibrate_monocular( + unprojected_ref_points, pupil_normals, pupil_id, initial_depth + ) + success, poses_in_world, gaze_targets_in_world = res + if not success: + return method, None + + world_pose, eye_pose = poses_in_world + + sphere_pos = last_pupil["sphere"]["center"] + eye_cam_pose_in_world = utils.get_eye_cam_pose_in_world(eye_pose, sphere_pos) + + all_observations = [unprojected_ref_points, pupil_normals] + nearest_points = utils.calculate_nearest_points_to_targets( + all_observations, poses_in_world, gaze_targets_in_world + ) + nearest_points_world, nearest_points_eye = nearest_points + + mapper = "Vector_Gaze_Mapper" + args = { + "eye_camera_to_world_matrix": eye_cam_pose_in_world.tolist(), + "cal_points_3d": gaze_targets_in_world.tolist(), + "cal_ref_points_3d": nearest_points_world.tolist(), + "cal_gaze_points_3d": nearest_points_eye.tolist(), + "gaze_distance": initial_depth, + } + result = {"subject": "start_plugin", "name": mapper, "args": args} + return method, result + + +def calibrate_2d_binocular( + g_pool, cal_pt_cloud_binocular, cal_pt_cloud0, cal_pt_cloud1 +): + method = "binocular polynomial regression" + + map_fn, inliers, params = calibrate_2d.calibrate_2d_polynomial( + cal_pt_cloud_binocular, g_pool.capture.frame_size, binocular=True + ) + if not inliers.any(): + return method, None + + map_fn, inliers, params_eye0 = calibrate_2d.calibrate_2d_polynomial( + cal_pt_cloud0, g_pool.capture.frame_size, binocular=False + ) + if not inliers.any(): + return method, None + + map_fn, inliers, params_eye1 = calibrate_2d.calibrate_2d_polynomial( + cal_pt_cloud1, g_pool.capture.frame_size, binocular=False + ) + if not inliers.any(): + return method, None + + mapper = "Binocular_Gaze_Mapper" + args = {"params": params, "params_eye0": params_eye0, "params_eye1": params_eye1} + result = {"subject": "start_plugin", "name": mapper, "args": args} + return method, result + + +def calibrate_2d_monocular(g_pool, cal_pt_cloud): + method = "monocular polynomial regression" + + map_fn, inliers, params = calibrate_2d.calibrate_2d_polynomial( + cal_pt_cloud, g_pool.capture.frame_size, binocular=False + ) + if not inliers.any(): + return method, None + + mapper = "Monocular_Gaze_Mapper" + args = {"params": params} + result = {"subject": "start_plugin", "name": mapper, "args": args} + return method, result + + +def calibrate_3d_hmd( + ref_points_3d, + pupil0_normals, + pupil1_normals, + last_pupil0, + last_pupil1, + eye_translations, +): + method = "hmd binocular 3d model" + + res = calibrate_3d.calibrate_hmd( + ref_points_3d, pupil0_normals, pupil1_normals, eye_translations + ) + success, poses_in_world, gaze_targets_in_world = res + if not success: + return method, None + + eye0_pose, eye1_pose = poses_in_world + + sphere_pos0 = last_pupil0["sphere"]["center"] + sphere_pos1 = last_pupil1["sphere"]["center"] + eye0_cam_pose_in_world = utils.get_eye_cam_pose_in_world(eye0_pose, sphere_pos0) + eye1_cam_pose_in_world = utils.get_eye_cam_pose_in_world(eye1_pose, sphere_pos1) + + all_observations = [gaze_targets_in_world, pupil0_normals, pupil1_normals] + nearest_points = utils.calculate_nearest_points_to_targets( + all_observations, [np.zeros(6), *poses_in_world], gaze_targets_in_world + ) + nearest_points_world, nearest_points_eye0, nearest_points_eye1 = nearest_points + + mapper = "Binocular_Vector_Gaze_Mapper" + args = { + "eye_camera_to_world_matrix0": eye0_cam_pose_in_world.tolist(), + "eye_camera_to_world_matrix1": eye1_cam_pose_in_world.tolist(), + "cal_points_3d": gaze_targets_in_world.tolist(), + "cal_ref_points_3d": nearest_points_world.tolist(), + "cal_gaze_points0_3d": nearest_points_eye0.tolist(), + "cal_gaze_points1_3d": nearest_points_eye1.tolist(), + } + result = {"subject": "start_plugin", "name": mapper, "args": args} + return method, result + + +def calibrate_2d_hmd(hmd_video_frame_size, cal_pt_cloud0, cal_pt_cloud1): + params0, params1 = None, None + + if cal_pt_cloud0: + map_fn0, inliers0, params0 = calibrate_2d.calibrate_2d_polynomial( + cal_pt_cloud0, hmd_video_frame_size, binocular=False + ) + if not inliers0.any(): + return None, None + if cal_pt_cloud1: + map_fn1, inliers1, params1 = calibrate_2d.calibrate_2d_polynomial( + cal_pt_cloud1, hmd_video_frame_size, binocular=False + ) + if not inliers1.any(): + return None, None + + if params0 and params1: + method = "dual monocular polynomial regression" + mapper = "Dual_Monocular_Gaze_Mapper" + args = {"params0": params0, "params1": params1} + elif params0 or params1: + method = "monocular polynomial regression" + mapper = "Monocular_Gaze_Mapper" + args = {"params": params0 if params0 else params1} + else: + # This case should not happen. + # If cal_pt_cloud0 and cal_pt_cloud1 are both empty lists, + # not_enough_data_error_msg should have been shown. + raise RuntimeError + + result = {"subject": "start_plugin", "name": mapper, "args": args} + return method, result diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibration_methods.pyx b/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibration_methods.pyx deleted file mode 100644 index 249c4367e3..0000000000 --- a/pupil_src/shared_modules/calibration_routines/optimization_calibration/calibration_methods.pyx +++ /dev/null @@ -1,119 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -from libcpp.vector cimport vector - -from calibration_methods cimport * -import numpy as np - - - - -def bundle_adjust_calibration( initial_observers, initial_points,fix_points = True): - - - cdef vector[Observer] cpp_observers; - cdef Observer cpp_observer - cdef vector[double] cpp_pose - cdef vector[Vector3] cpp_observations - cdef vector[Vector3] cpp_points - - cdef Vector4 rotation_quaternion - cdef Vector3 rotation_angle_axis - cdef Vector3 cpp_translation - - for o in initial_observers: - observations = o["observations"] - translation = o["translation"] - rotation = o["rotation"] - cpp_pose.resize(6) - - - rotation_quaternion = Vector4(rotation[0],rotation[1],rotation[2],rotation[3]) - #angle axis rotation: https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation - QuaternionToAngleAxis(rotation_quaternion.data(),rotation_angle_axis.data()) - - - #we need to invert the pose of the observer - #we will use this rotation translation to tranform the observed points in the cost fn - cpp_translation = Vector3(-translation[0],-translation[1],-translation[2]) - - #invert rotation - rotation_angle_axis[0] *= -1 - rotation_angle_axis[1] *= -1 - rotation_angle_axis[2] *= -1 - - #we have swapped to order rot/trans in the cost fn so we dont need to apply the line below - #AngleAxisRotatePoint(rotation_angle_axis.data(),cpp_translation.data(),cpp_translation.data()) - - - - - #first three is rotation - cpp_pose[0] = rotation_angle_axis[0] - cpp_pose[1] = rotation_angle_axis[1] - cpp_pose[2] = rotation_angle_axis[2] - - #last three is translation - cpp_pose[3] = cpp_translation[0] - cpp_pose[4] = cpp_translation[1] - cpp_pose[5] = cpp_translation[2] - - cpp_observations.clear() - for p in observations: - cpp_observations.push_back(Vector3(p[0],p[1],p[2])) - - cpp_observer = Observer() - cpp_observer.observations = cpp_observations - cpp_observer.pose = cpp_pose - cpp_observer.fix_rotation = 1*bool('rotation' in o['fix']) - cpp_observer.fix_translation = 1*bool('translation' in o['fix']) - cpp_observers.push_back( cpp_observer ) - - for p in initial_points: - cpp_points.push_back( Vector3(p[0],p[1],p[2]) ) - - - ## optimized values are written to cpp_orientation and cpp_translation - cdef double final_cost = bundleAdjustCalibration(cpp_observers, cpp_points,fix_points) - - - observers = [] - for cpp_observer in cpp_observers: - observer = {} - - #invert translation rotation back to get the pose - rotation_angle_axis = Vector3(cpp_observer.pose[0],cpp_observer.pose[1],cpp_observer.pose[2]) - cpp_translation = Vector3(-cpp_observer.pose[3],-cpp_observer.pose[4],-cpp_observer.pose[5]) - - rotation_angle_axis[0] *=-1 - rotation_angle_axis[1] *=-1 - rotation_angle_axis[2] *=-1 - - #we have swapped to order rot/trans in the cost fn so we dont need to apply the line below - #AngleAxisRotatePoint(rotation_angle_axis.data(),cpp_translation.data(),cpp_translation.data()) - - AngleAxisToQuaternion(rotation_angle_axis.data(),rotation_quaternion.data()) - - observer['rotation'] = rotation_quaternion[0],rotation_quaternion[1],rotation_quaternion[2],rotation_quaternion[3] - observer['translation'] = cpp_translation[0],cpp_translation[1],cpp_translation[2] - observers.append(observer) - - for final,inital in zip(observers,initial_observers): - final['observations'] = inital['observations'] - - - points = [] - cdef Vector3 cpp_p - for cpp_p in cpp_points: - points.append( (cpp_p[0],cpp_p[1],cpp_p[2]) ) - - return final_cost != -1,final_cost, observers, points diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/common.h b/pupil_src/shared_modules/calibration_routines/optimization_calibration/common.h deleted file mode 100644 index a3ab9e6c6d..0000000000 --- a/pupil_src/shared_modules/calibration_routines/optimization_calibration/common.h +++ /dev/null @@ -1,32 +0,0 @@ -/* -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -*/ - -#ifndef COMMON_H__ -#define COMMON_H__ - - -#include - - -typedef Eigen::Matrix Vector2; -typedef Eigen::Matrix Vector3; -typedef Eigen::Matrix Vector4; - - -struct Observer{ - std::vector observations; - std::vector pose; - int fix_rotation; - int fix_translation; - -}; - -#endif /* end of include guard: COMMON_H__ */ diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/setup.py b/pupil_src/shared_modules/calibration_routines/optimization_calibration/setup.py deleted file mode 100644 index 1628a6fa77..0000000000 --- a/pupil_src/shared_modules/calibration_routines/optimization_calibration/setup.py +++ /dev/null @@ -1,145 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -# # monkey-patch for parallel compilation -# def parallelCCompile(self, sources, output_dir=None, macros=None, include_dirs=None, debug=0, extra_preargs=None, extra_postargs=None, depends=None): -# # those lines are copied from distutils.ccompiler.CCompiler directly -# macros, objects, extra_postargs, pp_opts, build = self._setup_compile(output_dir, macros, include_dirs, sources, depends, extra_postargs) -# cc_args = self._get_cc_args(pp_opts, debug, extra_preargs) -# # parallel code -# N=4 # number of parallel compilations -# import multiprocessing.pool -# def _single_compile(obj): -# try: src, ext = build[obj] -# except KeyError: return -# self._compile(obj, src, ext, cc_args, extra_postargs, pp_opts) -# # convert to list, imap is evaluated on-demand -# list(multiprocessing.pool.ThreadPool(N).imap(_single_compile,objects)) -# return objects -# import distutils.ccompiler -# distutils.ccompiler.CCompiler.compile=parallelCCompile - -from distutils.core import setup -from distutils.extension import Extension -from Cython.Build import cythonize -import numpy as np -import os, sys, platform - - -dependencies = [] -# include all header files, to recognize changes -for dirpath, dirnames, filenames in os.walk("."): - for filename in [f for f in filenames if f.endswith(".h")]: - dependencies.append(os.path.join(dirpath, filename)) - -shared_cpp_include_path = "../../../shared_cpp/include" - -if platform.system() == "Windows": - libs = [] - library_dirs = [] - lib_spec = [ - [np.get_include(), ""], - [ - "C:\\work\\opencv\\build\\include", - "C:\\work\\opencv\\build\\x64\\vc14\\lib\\opencv_world345.lib", - ], - ["C:\\work\\ceres-windows\\Eigen", ""], - [ - "C:\\work\\ceres-windows\\ceres-solver\\include", - "C:\\work\\ceres-windows\\x64\\Release\\ceres_static.lib", - ], - [ - "C:\\work\\ceres-windows\\glog\\src\\windows", - "C:\\work\\ceres-windows\\x64\\Release\\libglog_static.lib", - ], - ["C:\\work\\ceres-windows", ""], - ] - - include_dirs = [spec[0] for spec in lib_spec] - include_dirs.append(shared_cpp_include_path) - xtra_obj = [spec[1] for spec in lib_spec] - -else: - # opencv3 - highgui module has been split into parts: imgcodecs, videoio, and highgui itself - opencv_libraries = [ - "opencv_core", - "opencv_highgui", - "opencv_videoio", - "opencv_imgcodecs", - "opencv_imgproc", - "opencv_video", - ] - - # explicit lib and include dirs for homebrew installed opencv - opencv_library_dirs = [ - "/usr/local/opt/opencv/lib", # old opencv brew (v3) - "/usr/local/opt/opencv@3/lib", # new opencv@3 brew - "/usr/local/lib", # new opencv brew (v4) - ] - opencv_include_dirs = [ - "/usr/local/opt/opencv/include", # old opencv brew (v3) - "/usr/local/opt/opencv@3/include", # new opencv@3 brew - "/usr/local/include/opencv4", # new opencv brew (v4) - ] - opencv_core_found = any( - os.path.isfile(path + "/libopencv_core.so") for path in opencv_library_dirs - ) - if not opencv_core_found: - ros_dists = ["kinetic", "jade", "indigo"] - for ros_dist in ros_dists: - ros_candidate_path = "/opt/ros/" + ros_dist + "/lib" - if os.path.isfile(ros_candidate_path + "/libopencv_core3.so"): - opencv_library_dirs = [ros_candidate_path] - opencv_include_dirs = [ - "/opt/ros/" + ros_dist + "/include/opencv-3.1.0-dev" - ] - opencv_libraries = [lib + "3" for lib in opencv_libraries] - break - include_dirs = [ - np.get_include(), - "/usr/local/include/eigen3", - "/usr/include/eigen3", - shared_cpp_include_path, - ] + opencv_include_dirs - libs = ["ceres"] + opencv_libraries - xtra_obj = [] - library_dirs = opencv_library_dirs - -extensions = [ - Extension( - name="calibration_routines.optimization_calibration.calibration_methods", - sources=["calibration_methods.pyx"], - include_dirs=include_dirs, - libraries=libs, - library_dirs=library_dirs, - extra_link_args=[], # '-WL,-R/usr/local/lib' - extra_compile_args=[ - "-D_USE_MATH_DEFINES", - "-std=c++11", - "-w", - "-O2", - ], # -w hides warnings - extra_objects=xtra_obj, - depends=dependencies, - language="c++", - ) -] - -if __name__ == "__main__": - setup( - name="calibration_routines.optimization_calibration", - version="0.1", - url="https://github.com/pupil-labs/pupil", - author="Pupil Labs", - author_email="info@pupil-labs.com", - license="GNU", - ext_modules=cythonize(extensions, quiet=True, nthreads=8), - ) diff --git a/pupil_src/shared_modules/calibration_routines/optimization_calibration/utils.py b/pupil_src/shared_modules/calibration_routines/optimization_calibration/utils.py new file mode 100644 index 0000000000..eb0da970bf --- /dev/null +++ b/pupil_src/shared_modules/calibration_routines/optimization_calibration/utils.py @@ -0,0 +1,153 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" + +import cv2 +import numpy as np + + +def transform_points_by_extrinsic(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) + """ + + rotation_cam2_cam1, translation_cam2_cam1 = split_extrinsic(extrinsic_cam2_cam1) + points_3d_cam1 = np.asarray(points_3d_cam1, dtype=np.float64) + points_3d_cam1.shape = -1, 3 + rotation_matrix_cam2_cam1 = cv2.Rodrigues(rotation_cam2_cam1)[0] + points_3d_cam2 = ( + np.dot(rotation_matrix_cam2_cam1, points_3d_cam1.T).T + translation_cam2_cam1 + ) + return points_3d_cam2 + + +def transform_points_by_pose(points_3d_cam1, pose_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 pose_cam2_cam1: camera pose of cam2 in cam1 coordinate, shape: (6,) + :return: 3d points in cam2 coordinate, shape: (N x 3) + """ + + rotation_cam2_cam1, translation_cam2_cam1 = split_extrinsic(pose_cam2_cam1) + points_3d_cam1 = np.asarray(points_3d_cam1, dtype=np.float64) + points_3d_cam1.shape = -1, 3 + + rotation_matrix_cam2_cam1 = cv2.Rodrigues(rotation_cam2_cam1)[0] + rotation_matrix_cam1_cam2 = rotation_matrix_cam2_cam1.T + translation_cam1_cam2 = np.dot(-rotation_matrix_cam1_cam2, translation_cam2_cam1) + points_3d_cam2 = ( + np.dot(rotation_matrix_cam1_cam2, points_3d_cam1.T).T + translation_cam1_cam2 + ) + return points_3d_cam2 + + +def inverse_extrinsic(extrinsic): + rotation_ext, translation_ext = split_extrinsic(extrinsic) + rotation_inv = -rotation_ext + translation_inv = np.dot(-cv2.Rodrigues(rotation_inv)[0], translation_ext) + return merge_extrinsic(rotation_inv, translation_inv) + + +def split_extrinsic(extrinsic): + extrinsic = np.asarray(extrinsic, dtype=np.float64) + assert extrinsic.size == 6 + rotation = extrinsic.ravel()[0:3] + translation = extrinsic.ravel()[3:6] + return rotation, translation + + +def merge_extrinsic(rotation, translation): + assert rotation.size == 3 and translation.size == 3 + extrinsic = np.concatenate((rotation.ravel(), translation.ravel())) + return extrinsic + + +def find_rigid_transform(A, B): + """Calculates the transformation between two coordinate systems using SVD. + This function determines the rotation matrix (R) and the translation vector + (L) for a rigid body after the following transformation [1]_, [2]_: + B = R*A + L + err, where A and B represents the rigid body in different instants + and err is an aleatory noise (which should be zero for a perfect rigid body). + + Adapted from: https://github.com/demotu/BMC/blob/master/functions/svdt.py + """ + + assert A.shape == B.shape and A.ndim == 2 and A.shape[1] == 3 + + A_centroid = np.mean(A, axis=0) + B_centroid = np.mean(B, axis=0) + M = np.dot((B - B_centroid).T, (A - A_centroid)) + U, S, Vt = np.linalg.svd(M) + + rotation_matrix = np.dot( + U, np.dot(np.diag([1, 1, np.linalg.det(np.dot(U, Vt))]), Vt) + ) + + translation_vector = B_centroid - np.dot(rotation_matrix, A_centroid) + return rotation_matrix, translation_vector + + +def get_initial_eye_camera_rotation(pupil_normals, gaze_targets): + initial_rotation_matrix, _ = find_rigid_transform(pupil_normals, gaze_targets) + initial_rotation = cv2.Rodrigues(initial_rotation_matrix)[0].ravel() + return initial_rotation + + +def get_eye_cam_pose_in_world(eye_pose, sphere_pos): + """ + :param eye_pose: eye pose in world coordinates + :param sphere_pos: eye ball center in eye cam coordinates + :return: the eye cam pose in world coordinates + """ + + eye_cam_position_in_eye = -np.asarray(sphere_pos) + world_extrinsic = eye_pose + eye_cam_position_in_world = transform_points_by_extrinsic( + eye_cam_position_in_eye, world_extrinsic + ) + + rotation, translation = split_extrinsic(eye_pose) + eye_cam_rotation_in_world = cv2.Rodrigues(rotation)[0] + + eye_cam_pose_in_world = np.eye(4) + eye_cam_pose_in_world[0:3, 0:3] = eye_cam_rotation_in_world + eye_cam_pose_in_world[0:3, 3] = eye_cam_position_in_world + return eye_cam_pose_in_world + + +def calculate_nearest_linepoints_to_points(ref_points, lines): + p1, p2 = lines + direction = p2 - p1 + denom = np.linalg.norm(direction, axis=1) + denom[denom == 0] = 1 + delta = np.diag(np.dot(ref_points - p1, direction.T)) / (denom * denom) + nearest_linepoints = p1 + direction * delta[:, np.newaxis] + return nearest_linepoints + + +def calculate_nearest_points_to_targets( + all_observations, poses_in_world, gaze_targets_in_world +): + all_nearest_points = [] + for observations, pose in zip(all_observations, poses_in_world): + lines_start = transform_points_by_extrinsic(np.zeros(3), pose) + lines_end = transform_points_by_extrinsic(observations, pose) + nearest_points = calculate_nearest_linepoints_to_points( + gaze_targets_in_world, (lines_start, lines_end) + ) + all_nearest_points.append(nearest_points) + + return all_nearest_points diff --git a/pupil_src/shared_modules/calibration_routines/screen_marker_calibration.py b/pupil_src/shared_modules/calibration_routines/screen_marker_calibration.py index 6576490215..6a9db155e8 100644 --- a/pupil_src/shared_modules/calibration_routines/screen_marker_calibration.py +++ b/pupil_src/shared_modules/calibration_routines/screen_marker_calibration.py @@ -138,6 +138,11 @@ def get_monitors_idx_list(): ui.Info_Text("Calibrate gaze parameters using a screen based animation.") ) self.menu.append( + # TODO: potential race condition through selection_getter. Should ensure + # that current selection will always be present in the list returned by the + # selection_getter. Highly unlikely though as this needs to happen between + # having clicked the Selector and the next redraw. + # See https://github.com/pupil-labs/pyglui/pull/112/commits/587818e9556f14bfedd8ff8d093107358745c29b ui.Selector( "monitor_idx", self, diff --git a/pupil_src/shared_modules/calibration_routines/single_marker_calibration.py b/pupil_src/shared_modules/calibration_routines/single_marker_calibration.py index 4437959e23..aeb89d8ad5 100644 --- a/pupil_src/shared_modules/calibration_routines/single_marker_calibration.py +++ b/pupil_src/shared_modules/calibration_routines/single_marker_calibration.py @@ -122,6 +122,11 @@ def get_monitors_idx_list(): ) ) self.menu.append( + # TODO: potential race condition through selection_getter. Should ensure + # that current selection will always be present in the list returned by the + # selection_getter. Highly unlikely though as this needs to happen between + # having clicked the Selector and the next redraw. + # See https://github.com/pupil-labs/pyglui/pull/112/commits/587818e9556f14bfedd8ff8d093107358745c29b ui.Selector( "monitor_idx", self, @@ -162,9 +167,14 @@ def open_window(self, title="new_window"): ) self.monitor_idx = 0 monitor = glfwGetMonitors()[self.monitor_idx] - width, height, redBits, blueBits, greenBits, refreshRate = glfwGetVideoMode( - monitor - ) + ( + width, + height, + redBits, + blueBits, + greenBits, + refreshRate, + ) = glfwGetVideoMode(monitor) else: monitor = None width, height = 640, 360 @@ -374,9 +384,7 @@ def map_value(value, in_range=(0, 1), out_range=(0, 1)): map_value(self.display_pos[0], out_range=(pad, p_window_size[0] - pad)), map_value(self.display_pos[1], out_range=(p_window_size[1] - pad, pad)), ) - alpha = ( - 1.0 - ) # interp_fn(self.screen_marker_state,0.,1.,float(self.sample_duration+self.lead_in+self.lead_out),float(self.lead_in),float(self.sample_duration+self.lead_in)) + alpha = 1.0 # interp_fn(self.screen_marker_state,0.,1.,float(self.sample_duration+self.lead_in+self.lead_out),float(self.lead_in),float(self.sample_duration+self.lead_in)) r2 = 2 * r draw_points( diff --git a/pupil_src/shared_modules/camera_intrinsics_estimation.py b/pupil_src/shared_modules/camera_intrinsics_estimation.py index 29796f4adc..5ab0f01352 100644 --- a/pupil_src/shared_modules/camera_intrinsics_estimation.py +++ b/pupil_src/shared_modules/camera_intrinsics_estimation.py @@ -119,6 +119,11 @@ def get_monitors_idx_list(): self.menu.append(ui.Button("show Pattern", self.open_window)) self.menu.append( + # TODO: potential race condition through selection_getter. Should ensure + # that current selection will always be present in the list returned by the + # selection_getter. Highly unlikely though as this needs to happen between + # having clicked the Selector and the next redraw. + # See https://github.com/pupil-labs/pyglui/pull/112/commits/587818e9556f14bfedd8ff8d093107358745c29b ui.Selector( "monitor_idx", self, diff --git a/pupil_src/shared_modules/cython_methods/.gitignore b/pupil_src/shared_modules/cython_methods/.gitignore deleted file mode 100644 index cd92b3c4a6..0000000000 --- a/pupil_src/shared_modules/cython_methods/.gitignore +++ /dev/null @@ -1 +0,0 @@ -*.cpp \ No newline at end of file diff --git a/pupil_src/shared_modules/cython_methods/build.py b/pupil_src/shared_modules/cython_methods/build.py deleted file mode 100644 index 9c1522eb6d..0000000000 --- a/pupil_src/shared_modules/cython_methods/build.py +++ /dev/null @@ -1,39 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -import logging - -logger = logging.getLogger(__name__) - - -def build_cpp_extension(): - import subprocess as sp - import os, sys - - src_loc = os.path.dirname(os.path.realpath(__file__)) - install_loc = os.path.split(src_loc)[0] - cwd = os.getcwd() - os.chdir(src_loc) - logger.info("Building extension modules...") - build_cmd = [ - sys.executable, - "setup.py", - "install", - "--install-lib={}".format(install_loc), - ] - ret = sp.check_output(build_cmd).decode(sys.stdout.encoding) - logger.debug("Build log:\n{}".format(ret)) - os.chdir(cwd) - - -if __name__ == "__main__": - logging.basicConfig(level=logging.DEBUG) - build_cpp_extension() diff --git a/pupil_src/shared_modules/cython_methods/methods.pyx b/pupil_src/shared_modules/cython_methods/methods.pyx deleted file mode 100644 index 043e3d733d..0000000000 --- a/pupil_src/shared_modules/cython_methods/methods.pyx +++ /dev/null @@ -1,46 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -import numpy as np -cimport numpy as np - - -def cumhist_color_map16(np.ndarray[np.uint16_t, ndim=2] depth_buffer): - cdef int r, c, i, f, width, height - cdef np.uint16_t d - height = depth_buffer.shape[0] - width = depth_buffer.shape[1] - - cdef np.ndarray[np.uint32_t, ndim=1] cumhist = np.zeros(0x10000, dtype=np.uint32) - cdef np.ndarray[np.uint8_t, ndim=3] rgb_image = np.empty((height, width, 3), dtype=np.uint8) - - for r in range(height): - for c in range(width): - cumhist[depth_buffer[r, c]] += 1 - - for i in range(2, 0x10000): - cumhist[i] += cumhist[i-1] - - for r in range(height): - for c in range(width): - d = depth_buffer[r, c] - if d != 0: - # 0-255 based on histogram location - f = cumhist[d] * 255 / cumhist[0xFFFF] - rgb_image[r, c, 0] = f - rgb_image[r, c, 1] = 0 - rgb_image[r, c, 2] = 255 - f - else: - rgb_image[r, c, 0] = 0 - rgb_image[r, c, 1] = 5 - rgb_image[r, c, 2] = 20 - - return rgb_image diff --git a/pupil_src/shared_modules/cython_methods/setup.py b/pupil_src/shared_modules/cython_methods/setup.py deleted file mode 100644 index a1a9e0acf7..0000000000 --- a/pupil_src/shared_modules/cython_methods/setup.py +++ /dev/null @@ -1,42 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -from distutils.core import setup -from distutils.extension import Extension -from Cython.Build import cythonize -import numpy as np - - -extensions = [ - Extension( - name="cython_methods.methods", - sources=["methods.pyx"], - include_dirs=[np.get_include()], - libraries=[], - library_dirs=[], - extra_link_args=[], - extra_compile_args=["-D_USE_MATH_DEFINES", "-std=c++11", "-w", "-O2"], - extra_objects=[], - depends=[], - language="c++", - ) -] - -if __name__ == "__main__": - setup( - name="cython_methods", - version="0.1", - url="https://github.com/pupil-labs/pupil", - author="Pupil Labs", - author_email="info@pupil-labs.com", - license="GNU", - ext_modules=cythonize(extensions, quiet=True, nthreads=8), - ) diff --git a/pupil_src/shared_modules/data_changed.py b/pupil_src/shared_modules/data_changed.py index 4fe0ceb352..5f1d69f114 100644 --- a/pupil_src/shared_modules/data_changed.py +++ b/pupil_src/shared_modules/data_changed.py @@ -67,18 +67,18 @@ def __init__(self, topic, rec_dir, plugin): self._current_token = None plugin.add_observer("on_notify", self._on_notify) - def announce_new(self): + def announce_new(self, delay=None, token_data=None): """ Announce that new data is available for the topic. New means that is has never been broadcasted before (not even in a previous run of the software). """ - token = _create_new_token() - self._notify_all(token) + token = _normalize_token(token_data) + self._notify_all(token, delay=delay) _write_token_to_file( token, self._plugin_role, self._topic, self._plugin_name, self._rec_dir ) - def announce_existing(self): + def announce_existing(self, delay=None): """ Announce that data for a topic is available, which was already announced some time ago (the exact same data). @@ -93,15 +93,16 @@ def announce_existing(self): if read_token is not None: self._current_token = read_token else: - self.announce_new() + self.announce_new(delay=delay) return - self._notify_all(self._current_token) + self._notify_all(self._current_token, delay=delay) - def _notify_all(self, token): + def _notify_all(self, token, delay=None): self._plugin().notify_all( { "subject": "data_changed.{}.announce_token".format(self._topic), "token": token, + "delay": delay, } ) @@ -173,6 +174,14 @@ def _on_notify(self, notification): self.on_data_changed() +def _normalize_token(token_data): + if token_data is None: + return _create_new_token() + if isinstance(token_data, str): + return token_data + return str(hash(token_data)) + + def _create_new_token(): """ Returns: A random string like e.g. "04bfd332" diff --git a/pupil_src/shared_modules/eye_movement/eye_movement_detector_offline.py b/pupil_src/shared_modules/eye_movement/eye_movement_detector_offline.py index 9d6cdb0dc5..9a391da8ad 100644 --- a/pupil_src/shared_modules/eye_movement/eye_movement_detector_offline.py +++ b/pupil_src/shared_modules/eye_movement/eye_movement_detector_offline.py @@ -137,17 +137,7 @@ def get_init_dict(self): return {"show_segmentation": self.menu_content.show_segmentation} def on_notify(self, notification): - if notification["subject"] == "gaze_positions_changed": - # TODO: Remove when gaze_positions will be announced with `data_changed.Announcer` - note = notification.copy() - note["subject"] = "data_changed.{}.announce_token".format( - self._gaze_changed_listener._topic - ) - note["token"] = notification.get( - "token", "{:0>8x}".format(random.getrandbits(32)) - ) - self._gaze_changed_listener._on_notify(note) - elif notification["subject"] in ( + if notification["subject"] in ( Notification_Subject.SHOULD_RECALCULATE, Notification_Subject.MIN_DATA_CONFIDENCE_CHANGED, ): diff --git a/pupil_src/shared_modules/file_methods.py b/pupil_src/shared_modules/file_methods.py index 583b019650..09e9f78aae 100644 --- a/pupil_src/shared_modules/file_methods.py +++ b/pupil_src/shared_modules/file_methods.py @@ -11,6 +11,7 @@ import collections import collections.abc +import copy import logging import os import pickle @@ -310,6 +311,9 @@ def copy(self): self._deser() return self._data.copy() + def __deepcopy__(self, memo=None): + return _recursive_deep_copy(self) + def has_key(self, k): self._deser() return k in self._data @@ -345,6 +349,21 @@ def __iter__(self): return iter(self._data) +def _recursive_deep_copy(item): + + if isinstance(item, collections.abc.Mapping): + _item_dict = {k: _recursive_deep_copy(v) for k, v in item.items()} + if isinstance(item, types.MappingProxyType): + return _item_dict + else: + return type(item)(_item_dict) + + if isinstance(item, collections.abc.Sequence) and not isinstance(item, str): + return type(item)([_recursive_deep_copy(el) for el in item]) + + return copy.deepcopy(item) + + def bench_save(): import time diff --git a/pupil_src/shared_modules/fixation_detector.py b/pupil_src/shared_modules/fixation_detector.py index d74593fe3f..6bd6803eaf 100644 --- a/pupil_src/shared_modules/fixation_detector.py +++ b/pupil_src/shared_modules/fixation_detector.py @@ -44,7 +44,9 @@ from scipy.spatial.distance import pdist import background_helper as bh +import data_changed import file_methods as fm +from observable import Observable import player_methods as pm from eye_movement.utils import can_use_3d_gaze_mapping from methods import denormalize @@ -242,7 +244,7 @@ def detect_fixations( yield "Fixation detection complete", () -class Offline_Fixation_Detector(Fixation_Detector_Base): +class Offline_Fixation_Detector(Observable, Fixation_Detector_Base): """Dispersion-duration-based fixation detector. This plugin detects fixations based on a dispersion threshold in terms of @@ -279,6 +281,12 @@ def __init__( self.prev_index = -1 self.bg_task = None self.status = "" + self._gaze_changed_listener = data_changed.Listener( + "gaze_positions", g_pool.rec_dir, plugin=self + ) + self._gaze_changed_listener.add_observer( + "on_data_changed", self._classify + ) self.notify_all( {"subject": "fixation_detector.should_recalculate", "delay": 0.5} ) @@ -428,9 +436,6 @@ def get_init_dict(self): } def on_notify(self, notification): - if notification["subject"] == "gaze_positions_changed": - logger.info("Gaze postions changed. Recalculating.") - self._classify() if notification["subject"] == "min_data_confidence_changed": logger.info("Minimal data confidence changed. Recalculating.") self._classify() diff --git a/pupil_src/shared_modules/gaze_producer/gaze_from_offline_calibration.py b/pupil_src/shared_modules/gaze_producer/gaze_from_offline_calibration.py index e7b1b0d308..987f7796d3 100644 --- a/pupil_src/shared_modules/gaze_producer/gaze_from_offline_calibration.py +++ b/pupil_src/shared_modules/gaze_producer/gaze_from_offline_calibration.py @@ -20,7 +20,7 @@ # IMPORTANT: GazeProducerBase needs to be THE LAST in the list of bases, otherwise # uniqueness by base class does not work -class GazeFromOfflineCalibration(Observable, GazeProducerBase): +class GazeFromOfflineCalibration(GazeProducerBase): pretty_class_name = "Gaze From Offline Calibration" icon_chr = chr(0xEC14) icon_font = "pupil_icons" @@ -180,7 +180,7 @@ def init_ui(self): def _publish_gaze(self, gaze_bisector): self.g_pool.gaze_positions = gaze_bisector - self.notify_all({"subject": "gaze_positions_changed", "delay": 1}) + self._gaze_changed_announcer.announce_new(delay=1) def _seek_to_frame(self, frame_index): self.notify_all({"subject": "seek_control.should_seek", "index": frame_index}) diff --git a/pupil_src/shared_modules/gaze_producer/gaze_from_recording.py b/pupil_src/shared_modules/gaze_producer/gaze_from_recording.py index 8d45b45b8b..bd56ae909d 100644 --- a/pupil_src/shared_modules/gaze_producer/gaze_from_recording.py +++ b/pupil_src/shared_modules/gaze_producer/gaze_from_recording.py @@ -21,7 +21,7 @@ class GazeFromRecording(GazeProducerBase): def __init__(self, g_pool): super().__init__(g_pool) self.g_pool.gaze_positions = self._load_gaze_data() - self.notify_all({"subject": "gaze_positions_changed"}) + self._gaze_changed_announcer.announce_existing() def _load_gaze_data(self): gaze = fm.load_pldata_file(self.g_pool.rec_dir, "gaze") diff --git a/pupil_src/shared_modules/gaze_producer/gaze_producer_base.py b/pupil_src/shared_modules/gaze_producer/gaze_producer_base.py index 80f0263bd1..29604aa338 100644 --- a/pupil_src/shared_modules/gaze_producer/gaze_producer_base.py +++ b/pupil_src/shared_modules/gaze_producer/gaze_producer_base.py @@ -11,16 +11,24 @@ from pyglui import ui +import data_changed +from observable import Observable import player_methods as pm from plugin import Producer_Plugin_Base -class GazeProducerBase(Producer_Plugin_Base): +class GazeProducerBase(Observable, Producer_Plugin_Base): uniqueness = "by_base_class" order = 0.02 icon_chr = chr(0xEC14) icon_font = "pupil_icons" + def __init__(self, g_pool): + super().__init__(g_pool) + self._gaze_changed_announcer = data_changed.Announcer( + "gaze_positions", g_pool.rec_dir, plugin=self + ) + def init_ui(self): self.add_menu() self.menu_icon.order = 0.3 diff --git a/pupil_src/shared_modules/gaze_producer/worker/create_calibration.py b/pupil_src/shared_modules/gaze_producer/worker/create_calibration.py index e12e3107bc..0b1e286d2d 100644 --- a/pupil_src/shared_modules/gaze_producer/worker/create_calibration.py +++ b/pupil_src/shared_modules/gaze_producer/worker/create_calibration.py @@ -14,7 +14,9 @@ import player_methods as pm import tasklib.background -from calibration_routines.finish_calibration import select_calibration_method +from calibration_routines.finish_calibration import ( + select_method_and_perform_calibration, +) from gaze_producer import model from methods import normalize @@ -48,11 +50,7 @@ def create_task(calibration, all_reference_locations): args = (fake_gpool, ref_dicts_in_calib_range, pupil_pos_in_calib_range) name = "Create calibration {}".format(calibration.name) - return tasklib.background.create( - name, - _create_calibration, - args=args, - ) + return tasklib.background.create(name, _create_calibration, args=args) def _create_ref_dict(ref): @@ -80,7 +78,7 @@ def _setup_fake_gpool( def _create_calibration(fake_gpool, ref_dicts_in_calib_range, pupil_pos_in_calib_range): - method, result = select_calibration_method( + method, result = select_method_and_perform_calibration( fake_gpool, pupil_pos_in_calib_range, ref_dicts_in_calib_range ) diff --git a/pupil_src/shared_modules/methods.py b/pupil_src/shared_modules/methods.py index 41dedaf464..cc9faec692 100644 --- a/pupil_src/shared_modules/methods.py +++ b/pupil_src/shared_modules/methods.py @@ -54,54 +54,6 @@ def get_dt(): yield dt -class Roi(object): - """this is a simple 2D Region of Interest class - it is applied on numpy arrays for convenient slicing - like this: - - roi_array_slice = full_array[r.view] - # do something with roi_array_slice - - this creates a view, no data copying done - """ - - def __init__(self, array_shape): - self.array_shape = array_shape - self.lX = 0 - self.lY = 0 - self.uX = array_shape[1] - self.uY = array_shape[0] - self.nX = 0 - self.nY = 0 - - @property - def view(self): - return slice(self.lY, self.uY), slice(self.lX, self.uX) - - def add_vector(self, vector): - """ - adds the roi offset to a len2 vector - """ - x, y = vector - return (self.lX + x, self.lY + y) - - def sub_vector(self, vector): - """ - subs the roi offset to a len2 vector - """ - x, y = vector - return (x - self.lX, y - self.lY) - - def set(self, vals): - if vals is not None and len(vals) is 5: - self.lX, self.lY, self.uX, self.uY, self.array_shape = vals - elif vals is not None and len(vals) is 4: - self.lX, self.lY, self.uX, self.uY = vals - - def get(self): - return self.lX, self.lY, self.uX, self.uY, self.array_shape - - def project_distort_pts( pts_xyz, camera_matrix, diff --git a/pupil_src/shared_modules/plugin.py b/pupil_src/shared_modules/plugin.py index 928b79d718..1d6193b795 100644 --- a/pupil_src/shared_modules/plugin.py +++ b/pupil_src/shared_modules/plugin.py @@ -322,6 +322,9 @@ def __init__(self, g_pool, plugin_initializers): expanded_initializers.append((plugin_by_name[name], name, args)) except KeyError: logger.debug(f"Plugin {name} failed to load, not available for import.") + + expanded_initializers.sort(key=lambda data: data[0].order) + # only add plugins that won't be replaced by newer plugins for i, (plugin, name, args) in enumerate(expanded_initializers): for new_plugin, new_name, _ in expanded_initializers[i + 1 :]: diff --git a/pupil_src/shared_modules/pupil_detector_plugins/detector_2d_plugin.py b/pupil_src/shared_modules/pupil_detector_plugins/detector_2d_plugin.py index b8efae7eb8..23e9c1b40a 100644 --- a/pupil_src/shared_modules/pupil_detector_plugins/detector_2d_plugin.py +++ b/pupil_src/shared_modules/pupil_detector_plugins/detector_2d_plugin.py @@ -47,17 +47,8 @@ def __init__( self.proxy = PropertyProxy(self.detector_2d) def detect(self, frame): - roi = Roi(*self.g_pool.u_r.get()[:4]) - if ( - not 0 <= roi.x_min <= roi.x_max < frame.width - or not 0 <= roi.y_min <= roi.y_max < frame.height - ): - # TODO: Invalid ROIs can occur when switching camera resolutions, because we - # adjust the roi only after all plugin recent_events() have been called. - # Optimally we make a plugin out of the ROI and call its recent_events() - # immediately after the backend, before the detection. - logger.debug(f"Invalid Roi {roi} for img {frame.width}x{frame.height}!") - return None + # convert roi-plugin to detector roi + roi = Roi(*self.g_pool.roi.bounds) debug_img = frame.bgr if self.g_pool.display_mode == "algorithm" else None result = self.detector_2d.detect( diff --git a/pupil_src/shared_modules/pupil_detector_plugins/detector_3d_plugin.py b/pupil_src/shared_modules/pupil_detector_plugins/detector_3d_plugin.py index c9461d4179..655410f244 100644 --- a/pupil_src/shared_modules/pupil_detector_plugins/detector_3d_plugin.py +++ b/pupil_src/shared_modules/pupil_detector_plugins/detector_3d_plugin.py @@ -49,17 +49,8 @@ def __init__( self.debugVisualizer3D = Eye_Visualizer(g_pool, self.detector_3d.focal_length()) def detect(self, frame): - roi = Roi(*self.g_pool.u_r.get()[:4]) - if ( - not 0 <= roi.x_min <= roi.x_max < frame.width - or not 0 <= roi.y_min <= roi.y_max < frame.height - ): - # TODO: Invalid ROIs can occur when switching camera resolutions, because we - # adjust the roi only after all plugin recent_events() have been called. - # Optimally we make a plugin out of the ROI and call its recent_events() - # immediately after the backend, before the detection. - logger.debug(f"Invalid Roi {roi} for img {frame.width}x{frame.height}!") - return None + # convert roi-plugin to detector roi + roi = Roi(*self.g_pool.roi.bounds) debug_img = frame.bgr if self.g_pool.display_mode == "algorithm" else None result = self.detector_3d.detect( diff --git a/pupil_src/shared_modules/pupil_detector_plugins/detector_base_plugin.py b/pupil_src/shared_modules/pupil_detector_plugins/detector_base_plugin.py index 2706949a33..783038d4d4 100644 --- a/pupil_src/shared_modules/pupil_detector_plugins/detector_base_plugin.py +++ b/pupil_src/shared_modules/pupil_detector_plugins/detector_base_plugin.py @@ -54,6 +54,7 @@ def __init__(self, g_pool): "pupil_detector.broadcast_properties": self.handle_broadcast_properties_notification, "pupil_detector.set_property": self.handle_set_property_notification, } + self._last_frame_size = None def recent_events(self, event): frame = event.get("frame") @@ -61,6 +62,12 @@ def recent_events(self, event): self._recent_detection_result = None return + frame_size = (frame.width, frame.height) + if frame_size != self._last_frame_size: + if self._last_frame_size is not None: + self.on_resolution_change(self._last_frame_size, frame_size) + self._last_frame_size = frame_size + detection_result = self.detect(frame=frame) event["pupil_detection_result"] = detection_result self._recent_detection_result = detection_result @@ -105,27 +112,35 @@ def handle_set_property_notification(self, notification): ) elif property_name == "roi": # Modify the ROI with the values sent over network + try: - minX, maxX, minY, maxY = property_value + minX, minY, maxX, maxY = property_value except (ValueError, TypeError) as err: # NOTE: ValueError gets throws when length of the tuple does not # match. TypeError gets thrown when it is not a tuple. raise ValueError( - "ROI needs to be 4 integers: (minX, maxX, minY, maxY)" + "ROI needs to be 4 integers: (minX, minY, maxX, maxY)" ) from err - if minX > maxX or minY > maxY: - raise ValueError("ROI malformed: minX > maxX or minY > maxY!") - ui_roi = self.g_pool.u_r - ui_roi.lX = max(ui_roi.min_x, int(minX)) - ui_roi.lY = max(ui_roi.min_y, int(minY)) - ui_roi.uX = min(ui_roi.max_x, int(maxX)) - ui_roi.uY = min(ui_roi.max_y, int(maxY)) + + # Apply very strict error checking here, although roi deal with invalid + # values, so the user gets immediate feedback and does not wonder why + # something did not work as expected. + width, height = self.g_pool.roi.frame_size + if not ((0 <= minX < maxX < width) and (0 <= minY < maxY <= height)): + raise ValueError( + "Received ROI with invalid dimensions!" + f" (minX={minX}, minY={minY}, maxX={maxX}, maxY={maxY})" + f" for frame size ({width} x {height})" + ) + + self.g_pool.roi.bounds = (minX, minY, maxX, maxY) + else: raise KeyError( "Notification subject does not " "specifiy detector type nor modify ROI." ) - logger.debug(f"`{property_name}` property set to {property_value}") + logger.debug(f"'{property_name}' property set to {property_value}") except KeyError: logger.error("Malformed notification received") logger.debug(traceback.format_exc()) diff --git a/pupil_src/shared_modules/pupil_recording/update/old_style.py b/pupil_src/shared_modules/pupil_recording/update/old_style.py index de94c63da7..e605e6e38c 100644 --- a/pupil_src/shared_modules/pupil_recording/update/old_style.py +++ b/pupil_src/shared_modules/pupil_recording/update/old_style.py @@ -120,7 +120,7 @@ def _update_recording_to_old_style_v1_16(rec_dir): elif rec_version >= VersionFormat("0.3"): update_recording_v03_to_v074(rec_dir) else: - logger.Error("This recording is too old. Sorry.") + logger.error("This recording is too old. Sorry.") return # Incremental format updates diff --git a/pupil_src/shared_modules/raw_data_exporter.py b/pupil_src/shared_modules/raw_data_exporter.py index a21b36723b..a46d804d44 100644 --- a/pupil_src/shared_modules/raw_data_exporter.py +++ b/pupil_src/shared_modules/raw_data_exporter.py @@ -10,19 +10,17 @@ """ import abc -import collections import csv import logging import os import typing -import csv_utils from pyglui import ui +import csv_utils import player_methods as pm from plugin import Analysis_Plugin_Base - # logging logger = logging.getLogger(__name__) @@ -175,7 +173,7 @@ def export_data(self, export_window, export_dir): positions_bisector=self.g_pool.gaze_positions, timestamps=self.g_pool.timestamps, export_window=export_window, - export_dir=export_dir + export_dir=export_dir, ) if self.should_export_field_info: @@ -186,7 +184,6 @@ def export_data(self, export_window, export_dir): class _Base_Positions_Exporter(abc.ABC): - @classmethod @abc.abstractmethod def csv_export_filename(cls) -> str: @@ -199,19 +196,19 @@ def csv_export_labels(cls) -> typing.Tuple[csv_utils.CSV_EXPORT_LABEL_TYPE, ...] @classmethod @abc.abstractmethod - def dict_export(cls, raw_value: csv_utils.CSV_EXPORT_RAW_TYPE, world_index: int) -> dict: + def dict_export( + cls, raw_value: csv_utils.CSV_EXPORT_RAW_TYPE, world_index: int + ) -> dict: pass - def csv_export_write(self, positions_bisector, timestamps, export_window, export_dir): + def csv_export_write( + self, positions_bisector, timestamps, export_window, export_dir + ): export_file = type(self).csv_export_filename() export_path = os.path.join(export_dir, export_file) - export_section = positions_bisector.init_dict_for_window( - export_window - ) - export_world_idc = pm.find_closest( - timestamps, export_section["data_ts"] - ) + export_section = positions_bisector.init_dict_for_window(export_window) + export_world_idc = pm.find_closest(timestamps, export_section["data_ts"]) with open(export_path, "w", encoding="utf-8", newline="") as csvfile: csv_header = type(self).csv_export_labels() @@ -226,7 +223,6 @@ def csv_export_write(self, positions_bisector, timestamps, export_window, export class Pupil_Positions_Exporter(_Base_Positions_Exporter): - @classmethod def csv_export_filename(cls) -> str: return "pupil_positions.csv" @@ -274,7 +270,9 @@ def csv_export_labels(cls) -> typing.Tuple[csv_utils.CSV_EXPORT_LABEL_TYPE, ...] ) @classmethod - def dict_export(cls, raw_value: csv_utils.CSV_EXPORT_RAW_TYPE, world_index: int) -> dict: + def dict_export( + cls, raw_value: csv_utils.CSV_EXPORT_RAW_TYPE, world_index: int + ) -> dict: # 2d data pupil_timestamp = str(raw_value["timestamp"]) eye_id = raw_value["id"] @@ -366,7 +364,6 @@ def dict_export(cls, raw_value: csv_utils.CSV_EXPORT_RAW_TYPE, world_index: int) class Gaze_Positions_Exporter(_Base_Positions_Exporter): - @classmethod def csv_export_filename(cls) -> str: return "gaze_positions.csv" @@ -398,7 +395,9 @@ def csv_export_labels(cls) -> typing.Tuple[csv_utils.CSV_EXPORT_LABEL_TYPE, ...] ) @classmethod - def dict_export(cls, raw_value: csv_utils.CSV_EXPORT_RAW_TYPE, world_index: int) -> dict: + def dict_export( + cls, raw_value: csv_utils.CSV_EXPORT_RAW_TYPE, world_index: int + ) -> dict: gaze_timestamp = str(raw_value["timestamp"]) confidence = raw_value["confidence"] @@ -424,12 +423,28 @@ def dict_export(cls, raw_value: csv_utils.CSV_EXPORT_RAW_TYPE, world_index: int) eye_centers0_3d = raw_value["eye_centers_3d"].get(0, [None, None, None]) eye_centers1_3d = raw_value["eye_centers_3d"].get(1, [None, None, None]) # - gaze_normals0_3d = raw_value["gaze_normals_3d"].get(0, [None, None, None]) - gaze_normals1_3d = raw_value["gaze_normals_3d"].get(1, [None, None, None]) + gaze_normals0_3d = raw_value["gaze_normals_3d"].get( + 0, [None, None, None] + ) + gaze_normals1_3d = raw_value["gaze_normals_3d"].get( + 1, [None, None, None] + ) # monocular elif raw_value.get("eye_center_3d", None) is not None: - eye_centers0_3d = raw_value["eye_center_3d"] - gaze_normals0_3d = raw_value["gaze_normal_3d"] + try: + eye_id = raw_value["base_data"][0]["id"] + except (KeyError, IndexError): + logger.warning( + f"Unexpected raw base_data for monocular gaze!" + f" Data: {raw_value.get('base_data', None)}" + ) + else: + if str(eye_id) == "0": + eye_centers0_3d = raw_value["eye_center_3d"] + gaze_normals0_3d = raw_value["gaze_normal_3d"] + elif str(eye_id) == "1": + eye_centers1_3d = raw_value["eye_center_3d"] + gaze_normals1_3d = raw_value["gaze_normal_3d"] return { "gaze_timestamp": gaze_timestamp, diff --git a/pupil_src/shared_modules/remote_recorder.py b/pupil_src/shared_modules/remote_recorder.py index 80f3c10d11..d6fa0f2fd3 100644 --- a/pupil_src/shared_modules/remote_recorder.py +++ b/pupil_src/shared_modules/remote_recorder.py @@ -18,6 +18,11 @@ logger = logging.getLogger(__name__) +# Suppress pyre debug logs (except beacon) +logger.debug("Suppressing pyre debug logs (except zbeacon)") +logging.getLogger("pyre").setLevel(logging.WARNING) +logging.getLogger("pyre.zbeacon").setLevel(logging.DEBUG) + class Remote_Recording_State: __slots__ = ["sensor"] diff --git a/pupil_src/shared_modules/roi.py b/pupil_src/shared_modules/roi.py new file mode 100644 index 0000000000..e8369e1802 --- /dev/null +++ b/pupil_src/shared_modules/roi.py @@ -0,0 +1,405 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +import logging +import typing as T +from enum import Enum + +import numpy as np +from OpenGL.GL import GL_LINE_LOOP +from pyglui.cygl.utils import RGBA as cygl_rgba +from pyglui.cygl.utils import draw_points as cygl_draw_points +from pyglui.cygl.utils import draw_polyline as cygl_draw_polyline + +import glfw +from methods import denormalize, normalize +from observable import Observable +from plugin import Plugin + +logger = logging.getLogger(__name__) + + +# Type aliases +# Note that this version of Vec2 is immutable! We don't need mutability here. +Vec2 = T.Tuple[int, int] +Bounds = T.Tuple[int, int, int, int] +ChangeCallback = T.Callable[[], None] + + +class RoiModel(Observable): + """Model for ROI masks on an image frame. + + The mask has 2 primary properties: + - frame_size: width, height + - bounds: minx, miny, maxx, maxy + + Some notes on behavior: + - Modifying bounds will always confine to the frame size and keep and area of >= 1. + - Changing the frame size will scale the bounds to the same relative area. + - If any frame dimension is <= 0, the ROI becomes invalid. + - Setting the frame size of an invalid ROI to a valid size re-initializes the ROI. + """ + + def __init__(self, frame_size: Vec2) -> None: + """Create a new RoiModel with bounds set to the full frame.""" + self._change_callbacks: T.List[ChangeCallback] = [] + self._set_to_full_frame(frame_size) + + def _set_to_full_frame(self, frame_size: Vec2) -> None: + """Initialize to full frame for given frame_size.""" + width, height = (int(v) for v in frame_size) + self._frame_width = width + self._frame_height = height + self._minx = 0 + self._miny = 0 + self._maxx = width - 1 + self._maxy = height - 1 + self.on_changed() + + def is_invalid(self) -> bool: + """Returns true if the frame size has 0 dimension.""" + return self._frame_width <= 0 or self._frame_height <= 0 + + def set_invalid(self) -> None: + """Set frame size to (0, 0).""" + self._frame_width = 0 + self._frame_height = 0 + self.on_changed() + + @property + def frame_size(self) -> Vec2: + return self._frame_width, self._frame_height + + @frame_size.setter + def frame_size(self, value: Vec2) -> None: + """Set frame_size. + + Marks ROI as invalid, if size has 0 dimension. + If old and new size are valid, scales the bounds to the same relative area. + """ + width, height = (int(v) for v in value) + if (width, height) == self.frame_size: + return + + if width <= 0 or height <= 0: + self.set_invalid() + return + + # if we are recovering from invalid, just re-initialize + if self.is_invalid(): + self._set_to_full_frame(value) + return + + # calculate scale factor for scaling bounds + fx: float = width / self._frame_width + fy: float = height / self._frame_height + self._frame_width = width + self._frame_height = height + + # scale bounds + minx = int(round(self._minx * fx)) + miny = int(round(self._miny * fy)) + maxx = int(round(self._maxx * fx)) + maxy = int(round(self._maxy * fy)) + # set bounds (to also apply contrainsts) + self.bounds = minx, miny, maxx, maxy + + self.on_changed() + logger.debug(f"Roi changed frame_size, now: {self}") + + @property + def bounds(self) -> Bounds: + return self._minx, self._miny, self._maxx, self._maxy + + @bounds.setter + def bounds(self, value: Bounds) -> None: + # convert to ints + minx, miny, maxx, maxy = (int(v) for v in value) + + # ensure all 0 <= all bounds < dimension + minx = min(max(minx, 0), self._frame_width - 1) + miny = min(max(miny, 0), self._frame_height - 1) + maxx = min(max(maxx, 0), self._frame_width - 1) + maxy = min(max(maxy, 0), self._frame_height - 1) + + # ensure min < max + # tries move max behind min first, otherwise moves min before max + if maxx <= minx: + if minx < self._frame_width - 1: + maxx = minx + 1 + else: + minx = maxx - 1 + if maxy <= miny: + if miny < self._frame_height - 1: + maxy = miny + 1 + else: + miny = maxy - 1 + + self._minx, self._miny, self._maxx, self._maxy = minx, miny, maxx, maxy + + self.on_changed() + + def __str__(self): + return f"Roi(frame={self.frame_size}, bounds={self.bounds})" + + def on_change(self, callback: ChangeCallback) -> None: + """Register callback to be called when model changes.""" + self._change_callbacks.append(callback) + + def on_changed(self) -> None: + """Called when the model changes. + + Observe this method to be notified of any changes. + """ + pass + + +class Handle(Enum): + """Enum for the 4 handles of the ROI UI.""" + + NONE = -1 + TOPLEFT = 0 + TOPRIGHT = 1 + BOTTOMRIGHT = 2 + BOTTOMLEFT = 3 + + +class Roi(Plugin): + """Plugin for managing a ROI on the frame.""" + + # Needs to be after base_backend and before detector_base_plugin! + order = 0.05 + + # style definitions + handle_size = 35 + handle_size_shadow = 45 + handle_size_active = 45 + handle_size_shadow_active = 65 + handle_color = cygl_rgba(0.5, 0.5, 0.9, 0.9) + handle_color_active = cygl_rgba(0.5, 0.9, 0.9, 0.9) + handle_color_shadow = cygl_rgba(0.0, 0.0, 0.0, 0.5) + outline_color = cygl_rgba(0.8, 0.8, 0.8, 0.9) + + def __init__( + self, g_pool, frame_size: Vec2 = (0, 0), bounds: Bounds = (0, 0, 0, 0), + ) -> None: + super().__init__(g_pool) + self.model = RoiModel(frame_size) + self.model.bounds = bounds + self._active_handle = Handle.NONE + self.reset_points() + self.model.add_observer("on_changed", self.reset_points) + + # Need to keep track of whether we have a valid frame to work with. Otherwise + # don't render UI. + self.has_frame = False + + # Expose roi model to outside. + self.g_pool.roi = self.model + + def get_init_dict(self) -> T.Dict[str, T.Any]: + return { + "frame_size": self.model.frame_size, + "bounds": self.model.bounds, + } + + def reset_points(self) -> None: + """Refresh cached points from underlying model.""" + if self.model.is_invalid(): + return + # all points are in image coordinates + # NOTE: for right/bottom points, we need to draw 1 behind the actual value. This + # is because the outline is supposed to visually contain all pixels that are + # masked. + minx, miny, maxx, maxy = self.model.bounds + self._all_points = { + Handle.TOPLEFT: (minx, miny), + Handle.TOPRIGHT: (maxx + 1, miny), + Handle.BOTTOMRIGHT: (maxx + 1, maxy + 1), + Handle.BOTTOMLEFT: (minx, maxy + 1), + } + self._active_points = [] + self._inactive_points = [] + for handle, point in self._all_points.items(): + if handle == self.active_handle: + self._active_points.append(point) + else: + self._inactive_points.append(point) + + @property + def active_handle(self) -> Handle: + return self._active_handle + + @active_handle.setter + def active_handle(self, value: Handle) -> None: + """Set active handle. Will reset points when changed.""" + if value == self._active_handle: + return + self._active_handle = value + self.reset_points() + + def get_handle_at(self, pos: Vec2) -> Handle: + """Returns which handle is rendered at that position.""" + for handle in self._all_points.keys(): + if self.is_point_on_handle(handle, pos): + return handle + return Handle.NONE + + def is_point_on_handle(self, handle: Handle, point: Vec2) -> bool: + """Returns if point is within the rendered handle.""" + # NOTE: point and all stored points are in image coordinates. The render sizes + # for the handles are in display coordinates! So we need to convert the points + # in order for the distances to be correct. + point_display = self.image_to_display_coordinates(point) + center = self._all_points[handle] + center_display = self.image_to_display_coordinates(center) + distance = np.linalg.norm( + (center_display[0] - point_display[0], center_display[1] - point_display[1]) + ) + handle_radius = self.g_pool.gui.scale * self.handle_size_shadow_active / 2 + return distance <= handle_radius + + def image_to_display_coordinates(self, point: Vec2) -> Vec2: + """Convert image coordinates to display coordinates.""" + norm_pos = normalize(point, self.g_pool.capture.frame_size) + return denormalize(norm_pos, self.g_pool.camera_render_size) + + # --- inherited from Plugin base class --- + + def recent_events(self, events: T.Dict[str, T.Any]) -> None: + frame = events.get("frame") + if frame is None: + self.has_frame = False + return + + self.has_frame = True + self.model.frame_size = (frame.width, frame.height) + + def on_click(self, pos: Vec2, button: int, action: int) -> bool: + if action == glfw.GLFW_PRESS: + clicked_handle = self.get_handle_at(pos) + if clicked_handle != self.active_handle: + self.active_handle = clicked_handle + return True + elif action == glfw.GLFW_RELEASE: + if self.active_handle != Handle.NONE: + self.active_handle = Handle.NONE + return True + return False + + def gl_display(self) -> None: + if not self.has_frame or self.model.is_invalid(): + return + + cygl_draw_polyline( + self._all_points.values(), + color=self.outline_color, + thickness=1, + line_type=GL_LINE_LOOP, + ) + + # only display rest of the UI when we're in ROI mode + if self.g_pool.display_mode != "roi": + return + + ui_scale = self.g_pool.gui.scale + + # draw inactive + cygl_draw_points( + self._inactive_points, + size=ui_scale * self.handle_size_shadow, + color=self.handle_color_shadow, + sharpness=0.3, + ) + cygl_draw_points( + self._inactive_points, + size=ui_scale * self.handle_size, + color=self.handle_color, + sharpness=0.9, + ) + + # draw active + if self._active_points: + cygl_draw_points( + self._active_points, + size=ui_scale * self.handle_size_shadow_active, + color=self.handle_color_shadow, + sharpness=0.3, + ) + cygl_draw_points( + self._active_points, + size=ui_scale * self.handle_size_active, + color=self.handle_color_active, + sharpness=0.9, + ) + + def on_pos(self, pos: Vec2) -> None: + if not self.has_frame or self.model.is_invalid(): + return + + if self.active_handle == Handle.NONE: + return + + x, y = pos + minx, miny, maxx, maxy = self.model.bounds + + # Try to ensure that the roi has min_size in both dimensions. This is important + # because otherwise the handles might overlap and the user cannot control the + # ROI anymore. Keep in mind that we cannot assume that the ROI had min_size + # before, since you can also modify from the network. This is purely a UI issue. + # You can test this by e.g. setting the ROI over the network to (0, 0, 1, 1) or + # to (190, 190, 191, 191) for the 192x192 image. + # For every point we: + # 1. Set corresponding coordinate pair + # 2. Push other coordinate pair away to ensure min_size + # 3. Other pair might have been pushed to frame bounds, if so push current + # pair back into the other direction. + # If the frame_size is greater than min_size, we ensure a min_size ROI, + # otherwise we ensure that ROI is the full frame_size. + min_size = 45 + width, height = self.model.frame_size + if self.active_handle == Handle.TOPLEFT: + # 1. + minx, miny = x, y + # 2. + maxx = max(maxx, min(minx + min_size, width - 1)) + maxy = max(maxy, min(miny + min_size, height - 1)) + # 3. + minx = min(minx, max(maxx - min_size, 0)) + miny = min(miny, max(maxy - min_size, 0)) + elif self.active_handle == Handle.TOPRIGHT: + # 1. + maxx, miny = x, y + # 2. + minx = min(minx, max(maxx - min_size, 0)) + maxy = max(maxy, min(miny + min_size, height - 1)) + # 3. + maxx = max(maxx, min(minx + min_size, width - 1)) + miny = min(miny, max(maxy - min_size, 0)) + elif self.active_handle == Handle.BOTTOMRIGHT: + # 1. + maxx, maxy = x, y + # 2. + minx = min(minx, max(maxx - min_size, 0)) + miny = min(miny, max(maxy - min_size, 0)) + # 3. + maxx = max(maxx, min(minx + min_size, width - 1)) + maxy = max(maxy, min(miny + min_size, height - 1)) + elif self.active_handle == Handle.BOTTOMLEFT: + # 1. + minx, maxy = x, y + # 2. + maxx = max(maxx, min(minx + min_size, width - 1)) + miny = min(miny, max(maxy - min_size, 0)) + # 3. + minx = min(minx, max(maxx - min_size, 0)) + maxy = max(maxy, min(miny + min_size, height - 1)) + + self.model.bounds = minx, miny, maxx, maxy diff --git a/pupil_src/shared_modules/cython_methods/__init__.py b/pupil_src/shared_modules/scan_path/__init__.py similarity index 62% rename from pupil_src/shared_modules/cython_methods/__init__.py rename to pupil_src/shared_modules/scan_path/__init__.py index 53dcb12205..db2e213703 100644 --- a/pupil_src/shared_modules/cython_methods/__init__.py +++ b/pupil_src/shared_modules/scan_path/__init__.py @@ -8,11 +8,4 @@ See COPYING and COPYING.LESSER for license details. ---------------------------------------------------------------------------~(*) """ - -try: - from .methods import * -except ModuleNotFoundError: - # when running from source compile cpp extension if necessary. - from .build import build_cpp_extension - build_cpp_extension() - from .methods import * \ No newline at end of file +from .controller import ScanPathController diff --git a/pupil_src/shared_modules/scan_path/algorithm.py b/pupil_src/shared_modules/scan_path/algorithm.py new file mode 100644 index 0000000000..16dc02b3c2 --- /dev/null +++ b/pupil_src/shared_modules/scan_path/algorithm.py @@ -0,0 +1,125 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +import copy +import functools + +import numpy as np +import cv2 + +import methods as m +import file_methods as fm + +from .utils import ( + scan_path_numpy_array_from, + scan_path_zeros_numpy_array, + np_denormalize, + np_normalize, +) + + +class ScanPathAlgorithm: + def __init__(self, timeframe: float): + assert timeframe + + # user settings + self.timeframe = timeframe + + # algorithm working data + self.reset() + + def reset(self): + self._prev_frame_index = -1 + self._prev_gray_image = None + self._prev_gaze_datums = scan_path_zeros_numpy_array() + + def update_from_frame(self, frame, preprocessed_data): + if frame.is_fake: + self.reset() + return scan_path_numpy_array_from(preprocessed_data) + + width, height = frame.width, frame.height + return self.update_from_raw_data( + frame_index=frame.index, + preprocessed_data=preprocessed_data, + image_size=(width, height), + gray_image=frame.gray, + ) + + def update_from_raw_data( + self, frame_index, preprocessed_data, image_size, gray_image + ): + if self._prev_frame_index + 1 != frame_index: + self.reset() + + # lets update past gaze using optical flow: this is like sticking the gaze points onto the pixels of the img. + if len(self._prev_gaze_datums) > 0: + prev_gaze_points = np.zeros( + (self._prev_gaze_datums.shape[0], 2), dtype=np.float32 + ) + prev_gaze_points[:, 0] = self._prev_gaze_datums["norm_x"] + prev_gaze_points[:, 1] = self._prev_gaze_datums["norm_y"] + prev_gaze_points = np_denormalize(prev_gaze_points, size=image_size) + + new_gaze_points, status, err = cv2.calcOpticalFlowPyrLK( + self._prev_gray_image, + gray_image, + prev_gaze_points, + None, + **self._lk_params, + ) + + new_gaze_points = np_normalize(new_gaze_points, size=image_size) + + new_gaze_data = scan_path_zeros_numpy_array(new_gaze_points.shape[0]) + new_gaze_data["timestamp"] = self._prev_gaze_datums["timestamp"] + new_gaze_data["norm_x"] = new_gaze_points[:, 0] + new_gaze_data["norm_y"] = new_gaze_points[:, 1] + + # Only keep gaze data where the status is 1 + status = np.array(status, dtype=bool) + status.shape = -1 # flatten to keep dimensionality below + new_gaze_data = new_gaze_data[status] + else: + new_gaze_data = scan_path_zeros_numpy_array() + + # trim gaze that is too old + if len(preprocessed_data) > 0: + now = preprocessed_data[0]["timestamp"] + cutoff = now - self.timeframe + new_gaze_data = new_gaze_data[new_gaze_data["timestamp"] > cutoff] + + # inject the scan path gaze points into recent_gaze_positions + all_gaze_datums = np.concatenate([new_gaze_data, preprocessed_data]) + all_gaze_datums["frame_index"] = frame_index + all_gaze_datums = np_sort_by_named_columns(all_gaze_datums, ["timestamp"]) + + # update info for next frame. + self._prev_gray_image = gray_image + self._prev_frame_index = frame_index + self._prev_gaze_datums = all_gaze_datums + + return all_gaze_datums + + # Private + + # vars for calcOpticalFlowPyrLK + _lk_params = dict( + winSize=(90, 90), + maxLevel=3, + criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 20, 0.03), + minEigThreshold=0.005, + ) + + +def np_sort_by_named_columns(array, colums_by_priority): + for col_name in reversed(colums_by_priority): + array = array[array[col_name].argsort(kind="mergesort")] + return array diff --git a/pupil_src/shared_modules/scan_path/controller.py b/pupil_src/shared_modules/scan_path/controller.py new file mode 100644 index 0000000000..dc769d277f --- /dev/null +++ b/pupil_src/shared_modules/scan_path/controller.py @@ -0,0 +1,210 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +import os +import abc +import logging + +import numpy as np + +from file_methods import Persistent_Dict +from observable import Observable +from plugin import Plugin + +from .storage import ScanPathStorage +from .tasks import ScanPathBackgroundTask, ScanPathPreprocessingTask + + +logger = logging.getLogger(__name__) + + +class ScanPathController(Observable): + """ + Enables previous gaze history to be visible for the timeframe specified by the user. + """ + + min_timeframe = 0.0 + max_timeframe = 3.0 + timeframe_step = 0.05 + + def __init__(self, g_pool, **kwargs): + self.g_pool = g_pool + + self._params = ScanPathParams(**kwargs) + assert self.min_timeframe <= self.timeframe <= self.max_timeframe + + self._status_str = "" + + self._preproc = ScanPathPreprocessingTask(g_pool) + self._preproc.add_observer("on_started", self._on_preproc_started) + self._preproc.add_observer("on_updated", self._on_preproc_updated) + self._preproc.add_observer("on_failed", self._on_preproc_failed) + self._preproc.add_observer("on_canceled", self._on_preproc_canceled) + self._preproc.add_observer("on_completed", self._on_preproc_completed) + + self._bg_task = ScanPathBackgroundTask(g_pool) + self._bg_task.add_observer("on_started", self._on_bg_task_started) + self._bg_task.add_observer("on_updated", self._on_bg_task_updated) + self._bg_task.add_observer("on_failed", self._on_bg_task_failed) + self._preproc.add_observer("on_canceled", self._on_bg_task_canceled) + self._bg_task.add_observer("on_completed", self._on_bg_task_completed) + + self._gaze_data_store = ScanPathStorage(g_pool.rec_dir) + self._gaze_data_store.load_from_disk() + + def get_init_dict(self): + return self._params.copy() + + @property + def timeframe(self) -> float: + return self._params["timeframe"] + + @timeframe.setter + def timeframe(self, value: float): + self._params["timeframe"] = value + + @property + def is_active(self) -> bool: + return self._preproc.is_active or self._bg_task.is_active + + @property + def progress(self) -> float: + if self.is_active: + ratio = 0.85 + return ( + 1.0 - ratio + ) * self._preproc.progress + ratio * self._bg_task.progress + else: + return 0.0 # idle + + @property + def status_string(self) -> str: + return self._status_str + + def process(self): + self._preproc.process() + self._bg_task.process() + + def scan_path_gaze_for_frame(self, frame): + if self.timeframe == 0.0: + return None + + if not self._gaze_data_store.is_valid or not self._gaze_data_store.is_complete: + if not self.is_active: + self._trigger_immediate_scan_path_calculation() + return None + + timestamp_cutoff = frame.timestamp - self.timeframe + + gaze_data = self._gaze_data_store.gaze_data + gaze_data = gaze_data[gaze_data.frame_index == frame.index] + gaze_data = gaze_data[gaze_data.timestamp > timestamp_cutoff] + + return gaze_data + + def cleanup(self): + self._preproc.cleanup() + self._bg_task.cleanup() + self._params.cleanup() + + def on_gaze_data_changed(self): + self._preproc.cancel() + self._bg_task.cancel() + self._gaze_data_store.mark_invalid() + + def on_update_ui(self): + pass + + # Private - helpers + + def _trigger_immediate_scan_path_calculation(self): + # Cancel old tasks + self._preproc.cancel() + self._bg_task.cancel() + # Start new tasks + self._preproc.start() + + # Private - preprocessing callbacks + + def _on_preproc_started(self): + logger.debug("ScanPathController._on_preproc_started") + self._status_str = "Preprocessing started..." + self.on_update_ui() + + def _on_preproc_updated(self, gaze_datum): + logger.debug("ScanPathController._on_preproc_updated") + self._status_str = f"Preprocessing {int(self._preproc.progress * 100)}%..." + self.on_update_ui() + + def _on_preproc_failed(self, error): + logger.debug("ScanPathController._on_preproc_failed") + logger.error(f"Scan path preprocessing failed: {error}") + self._status_str = "Preprocessing failed" + self.on_update_ui() + + def _on_preproc_canceled(self): + logger.debug("ScanPathController._on_preproc_canceled") + self._status_str = "Preprocessing canceled" + self.on_update_ui() + + def _on_preproc_completed(self, gaze_data): + logger.debug("ScanPathController._on_preproc_completed") + self._status_str = "Preprocessing completed" + # Start the background task with max_timeframe + # The current timeframe will be used only for visualization + self._bg_task.start(self.max_timeframe, gaze_data) + self.on_update_ui() + + # Private - calculation callbacks + + def _on_bg_task_started(self): + logger.debug("ScanPathController._on_bg_task_started") + self._status_str = "Calculation started..." + self.on_update_ui() + + def _on_bg_task_updated(self, update_data): + logger.debug("ScanPathController._on_bg_task_updated") + self._status_str = f"Calculation {int(self._bg_task.progress * 100)}%..." + # TODO: Save intermediary data + self.on_update_ui() + + def _on_bg_task_failed(self, error): + logger.debug("ScanPathController._on_bg_task_failed") + logger.error(f"Scan path calculation failed: {error}") + self._status_str = "Calculation failed" + self.on_update_ui() + + def _on_bg_task_canceled(self): + logger.debug("ScanPathController._on_bg_task_canceled") + self._status_str = "Calculation canceled" + self.on_update_ui() + + def _on_bg_task_completed(self, complete_data): + logger.debug("ScanPathController._on_bg_task_completed") + self._gaze_data_store.gaze_data = complete_data + self._gaze_data_store.mark_complete() + self._status_str = "Calculation completed" + self.on_update_ui() + + +class ScanPathParams(dict): + + version = 1 + + default_params = { + "timeframe": ScanPathController.min_timeframe + } + + def __init__(self, **kwargs): + super().__init__(**self.default_params) + self.update(**kwargs) + + def cleanup(self): + pass diff --git a/pupil_src/shared_modules/scan_path/storage.py b/pupil_src/shared_modules/scan_path/storage.py new file mode 100644 index 0000000000..9b4cf4b9fd --- /dev/null +++ b/pupil_src/shared_modules/scan_path/storage.py @@ -0,0 +1,118 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +import os +import logging +import threading +import contextlib + +import numpy as np + +from .utils import scan_path_zeros_numpy_array, SCAN_PATH_GAZE_DATUM_DTYPE + + +logger = logging.getLogger(__name__) + + +class ScanPathStorage: + + version = 1 + + def __init__(self, rec_dir, gaze_data=...): + self.__lock = threading.RLock() + self.rec_dir = rec_dir + if gaze_data is ...: + self.gaze_data = None + else: + self.gaze_data = gaze_data + + @property + def gaze_data(self): + with self._locked(): + return self._gaze_data + + @gaze_data.setter + def gaze_data(self, gaze_data): + with self._locked(): + if gaze_data is not None: + self._validate_gaze_data(gaze_data) + gaze_data = gaze_data.view(np.recarray) + self._gaze_data = gaze_data + + @property + def is_valid(self) -> bool: + return self._gaze_data is not None + + @property + def is_complete(self) -> bool: + return self._is_complete + + def mark_invalid(self): + with self._locked(): + self._gaze_data = None + self._is_complete = False + self.__remove_from_disk() + + def mark_complete(self): + with self._locked(): + self._is_complete = True + self.__save_to_disk() + + def load_from_disk(self): + with self._locked(): + self.__load_from_disk() + + @staticmethod + def empty_gaze_data(): + gaze_data = scan_path_zeros_numpy_array() + ScanPathStorage._validate_gaze_data(gaze_data) + return gaze_data + + @staticmethod + def _validate_gaze_data(gaze_data): + assert isinstance(gaze_data, np.ndarray) + assert gaze_data.dtype == SCAN_PATH_GAZE_DATUM_DTYPE + assert len(gaze_data.shape) == 1 + + @contextlib.contextmanager + def _locked(self): + self.__lock.acquire() + try: + yield + finally: + self.__lock.release() + + # Filesystem + + @property + def __file_path(self) -> str: + rec_dir = self.rec_dir + filename = f"scan_path_cache_v{self.version}.npy" + return os.path.join(rec_dir, "offline_data", filename) + + def __load_from_disk(self): + try: + gaze_data = np.load(self.__file_path) + except IOError: + return + self.gaze_data = gaze_data + # TODO: Figure out if gaze_data is complete + self._is_complete = self.is_valid + + def __save_to_disk(self): + if not self.is_valid: + return + np.save(self.__file_path, self._gaze_data) + + def __remove_from_disk(self): + try: + os.remove(self.__file_path) + except FileNotFoundError: + pass diff --git a/pupil_src/shared_modules/scan_path/tasks/__init__.py b/pupil_src/shared_modules/scan_path/tasks/__init__.py new file mode 100644 index 0000000000..e7963a3437 --- /dev/null +++ b/pupil_src/shared_modules/scan_path/tasks/__init__.py @@ -0,0 +1,12 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +from .background import ScanPathBackgroundTask +from .preprocessing import ScanPathPreprocessingTask diff --git a/pupil_src/shared_modules/scan_path/tasks/background.py b/pupil_src/shared_modules/scan_path/tasks/background.py new file mode 100644 index 0000000000..193fde44fa --- /dev/null +++ b/pupil_src/shared_modules/scan_path/tasks/background.py @@ -0,0 +1,97 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +from collections import namedtuple + +import numpy as np + +from observable import Observable +from background_helper import IPC_Logging_Task_Proxy + +from .base import _BaseTask +from scan_path.algorithm import ScanPathAlgorithm +from scan_path.utils import ( + FakeGPool, + generate_frames, + SCAN_PATH_GAZE_DATUM_DTYPE, + scan_path_zeros_numpy_array, + scan_path_numpy_array_from, +) + + +class ScanPathBackgroundTask(Observable, _BaseTask): + def __init__(self, g_pool): + self.g_pool = g_pool + self._bg_task = None + self._progress = 0.0 + self._gaze_data = None + + # _BaseTask + + @property + def progress(self) -> float: + return self._progress + + @property + def is_active(self) -> bool: + return self._bg_task is not None + + def start(self, timeframe, preprocessed_data): + if self.is_active: + return + + g_pool = FakeGPool(self.g_pool) + + self._gaze_data = scan_path_zeros_numpy_array() + + self._bg_task = IPC_Logging_Task_Proxy( + "Scan path", + generate_frames_with_corrected_gaze, + args=(g_pool, timeframe, preprocessed_data), + ) + + def process(self): + if self._bg_task: + try: + task_data = self._bg_task.fetch() + except Exception as err: + self._bg_task.cancel() + self._bg_task = None + self.on_failed(err) + + for progress, gaze_data in task_data: + gaze_data = scan_path_numpy_array_from(gaze_data) + self._gaze_data = np.append(self._gaze_data, gaze_data) + self._progress = progress + self.on_updated(gaze_data) + + if self._bg_task.completed: + self._bg_task = None + self._gaze_data = scan_path_numpy_array_from(self._gaze_data) + self.on_completed(self._gaze_data) + + def cancel(self): + if self._bg_task is not None: + self._bg_task.cancel() + self._bg_task = None + self.on_canceled() + self._progress = 0.0 + + def cleanup(self): + self.cancel() + + +def generate_frames_with_corrected_gaze(g_pool, timeframe, preprocessed_data): + sp = ScanPathAlgorithm(timeframe) + + for progress, frame in generate_frames(g_pool): + gaze_data = preprocessed_data[preprocessed_data.frame_index == frame.index] + gaze_data = sp.update_from_frame(frame, gaze_data) + yield progress, gaze_data diff --git a/pupil_src/shared_modules/scan_path/tasks/base.py b/pupil_src/shared_modules/scan_path/tasks/base.py new file mode 100644 index 0000000000..e6893ae805 --- /dev/null +++ b/pupil_src/shared_modules/scan_path/tasks/base.py @@ -0,0 +1,54 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +import abc + + +class _BaseTask: + @property + @abc.abstractmethod + def progress(self) -> float: + pass + + @property + @abc.abstractmethod + def is_active(self) -> bool: + pass + + @abc.abstractmethod + def start(self): + pass + + @abc.abstractmethod + def process(self): + pass + + @abc.abstractmethod + def cancel(self): + pass + + @abc.abstractmethod + def cleanup(self): + pass + + def on_started(self): + pass + + def on_updated(self, data): + pass + + def on_canceled(self): + pass + + def on_failed(self, error): + pass + + def on_completed(self, data): + pass diff --git a/pupil_src/shared_modules/scan_path/tasks/preprocessing.py b/pupil_src/shared_modules/scan_path/tasks/preprocessing.py new file mode 100644 index 0000000000..3acee2f2d3 --- /dev/null +++ b/pupil_src/shared_modules/scan_path/tasks/preprocessing.py @@ -0,0 +1,119 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +from collections import namedtuple +from types import SimpleNamespace + +import numpy as np + +from observable import Observable + +from .base import _BaseTask +from scan_path.utils import ( + timestamp_ns, + sec_to_ns, + generate_frame_indices_with_deserialized_gaze, + SCAN_PATH_GAZE_DATUM_DTYPE, + scan_path_zeros_numpy_array, + scan_path_numpy_array_from, +) + + +class _BaseState: + def __init__(self, g_pool): + self.g_pool = g_pool + + +class UninitializedState(_BaseState): + pass + + +class StartedState(_BaseState): + pass + + +class ActiveState(_BaseState): + def __init__(self, g_pool): + super().__init__(g_pool) + self.generator = generate_frame_indices_with_deserialized_gaze(g_pool) + + +class CompletedState(_BaseState): + pass + + +class CanceledState(_BaseState): + pass + + +class ScanPathPreprocessingTask(Observable, _BaseTask): + def __init__(self, g_pool): + self.g_pool = g_pool + self._progress = 0.0 + self._gaze_data = None + self._state = UninitializedState(self.g_pool) + + # _BaseTask + + @property + def is_active(self) -> bool: + return isinstance(self._state, ActiveState) + + @property + def progress(self) -> float: + return self._progress + + def start(self): + self._progress = 0.0 + self._state = StartedState(self.g_pool) + self.on_started() + + def process(self, time_limit_sec: float = 0.01): + time_limit_ns = sec_to_ns(time_limit_sec) + + if isinstance(self._state, (UninitializedState, CompletedState, CanceledState)): + return + + if isinstance(self._state, StartedState): + self._state = ActiveState(self.g_pool) + self._gaze_data = scan_path_zeros_numpy_array() + + assert isinstance(self._state, ActiveState) + + generator_is_done = True + start_time_ns = timestamp_ns() + + for progress, gaze_data in self._state.generator: + generator_is_done = False + + self._gaze_data = np.append(self._gaze_data, gaze_data) + + self._progress = progress + self.on_updated(gaze_data) + + time_diff_ns = timestamp_ns() - start_time_ns + if time_diff_ns > time_limit_ns: + break + + if generator_is_done: + self._progress = 1.0 + self._state = CompletedState(self.g_pool) + self._gaze_data = scan_path_numpy_array_from(self._gaze_data) + self.on_completed(self._gaze_data) + self._gaze_data = None + + def cancel(self): + if isinstance(self._state, ActiveState): + self._state = CanceledState(self.g_pool) + self.on_canceled() + self._progress = 0.0 + + def cleanup(self): + self.cancel() diff --git a/pupil_src/shared_modules/scan_path/utils.py b/pupil_src/shared_modules/scan_path/utils.py new file mode 100644 index 0000000000..af8bc3fab3 --- /dev/null +++ b/pupil_src/shared_modules/scan_path/utils.py @@ -0,0 +1,167 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +from time import monotonic +from types import SimpleNamespace + +import numpy as np + +from pupil_recording import PupilRecording +from video_capture.utils import VideoSet +from video_capture.file_backend import File_Source, EndofVideoError +from gaze_producer.gaze_from_recording import GazeFromRecording +import methods as m +import player_methods as pm + + +SCAN_PATH_GAZE_DATUM_DTYPE = np.dtype( + [ + ("frame_index", np.int64), + ("timestamp", np.float64), + ("norm_x", np.float32), + ("norm_y", np.float32), + ] +) + + +def np_normalize(array, size, flip_y=True): + """ + normalize return as float + """ + if len(array) == 0: + return array + assert len(array.shape) == 2 + assert array.shape[1] == 2 + width, height = size + array[:, 0] /= float(width) + array[:, 1] /= float(height) + if flip_y: + array[:, 1] = 1 - array[:, 1] + return array + + +def np_denormalize(array, size, flip_y=True): + """ + denormalize + """ + if len(array) == 0: + return array + assert len(array.shape) == 2 + assert array.shape[1] == 2 + width, height = size + if flip_y: + array[:, 1] = 1 - array[:, 1] + array[:, 0] *= width + array[:, 1] *= height + return array + + +def scan_path_zeros_numpy_array(n=0): + new_array = np.zeros(n, dtype=SCAN_PATH_GAZE_DATUM_DTYPE) + new_array = new_array.view(np.recarray) + return new_array + + +def scan_path_numpy_array_from(it): + if len(it) == 0: + return scan_path_zeros_numpy_array() + + array = np.asarray(it) + + if array.dtype == SCAN_PATH_GAZE_DATUM_DTYPE: + return array.view(np.recarray) + + assert len(array.shape) == 2 + assert array.shape[1] == len(SCAN_PATH_GAZE_DATUM_DTYPE) + + new_array = scan_path_zeros_numpy_array(array.shape[0]) + + new_array["frame_index"] = array[:, 0] + new_array["timestamp"] = array[:, 1] + new_array["norm_x"] = array[:, 2] + new_array["norm_y"] = array[:, 3] + + return new_array + + +class FakeGPool: + def __init__(self, g_pool): + self.rec_dir = g_pool.rec_dir + self.app = g_pool.app + self.min_data_confidence = g_pool.min_data_confidence + self.timestamps = g_pool.timestamps + + +def timestamp_ns() -> int: + """ + Returns a monotonic timestamp in nanoseconds. + """ + return sec_to_ns(monotonic()) + + +def sec_to_ns(sec: float) -> int: + return int(sec * 10e9) + + +def ns_to_sec(ns: int) -> float: + return float(ns) / 10e9 + + +def generate_frame_indices_with_deserialized_gaze(g_pool): + recording = PupilRecording(g_pool.rec_dir) + video_name = recording.files().world().videos()[0].stem + + videoset = VideoSet(rec=g_pool.rec_dir, name=video_name, fill_gaps=True) + videoset.load_or_build_lookup() + + frame_indices = np.flatnonzero(videoset.lookup.container_idx > -1) + frame_count = len(frame_indices) + + for frame_index in frame_indices: + progress = (frame_index + 1) / frame_count + frame_ts_window = pm.enclosing_window(g_pool.timestamps, frame_index) + gaze_data = g_pool.gaze_positions.by_ts_window(frame_ts_window) + gaze_data = [ + (frame_index, g["timestamp"], g["norm_pos"][0], g["norm_pos"][1]) + for g in gaze_data + if g["confidence"] >= g_pool.min_data_confidence + ] + gaze_data = scan_path_numpy_array_from(gaze_data) + yield progress, gaze_data + + +def generate_frames_with_gaze(g_pool): + for progress, current_frame in generate_frames(g_pool): + frame_ts_window = pm.enclosing_window(g_pool.timestamps, current_frame.index) + gaze_datums = g_pool.gaze_positions.by_ts_window(frame_ts_window) + gaze_datums = [ + g for g in gaze_datums if g["confidence"] >= g_pool.min_data_confidence + ] + + yield progress, current_frame, gaze_datums + + +def generate_frames(g_pool): + recording = PupilRecording(g_pool.rec_dir) + video_path = recording.files().world().videos()[0] + + fs = File_Source(g_pool, source_path=video_path, fill_gaps=True) + + total_frame_count = fs.get_frame_count() + + while True: + try: + current_frame = fs.get_frame() + except EndofVideoError: + break + + progress = current_frame.index / total_frame_count + + yield progress, current_frame diff --git a/pupil_src/shared_modules/surface_tracker/surface_tracker_offline.py b/pupil_src/shared_modules/surface_tracker/surface_tracker_offline.py index a1870f9ac2..ea0786799c 100644 --- a/pupil_src/shared_modules/surface_tracker/surface_tracker_offline.py +++ b/pupil_src/shared_modules/surface_tracker/surface_tracker_offline.py @@ -22,8 +22,10 @@ import pyglui import pyglui.cygl.utils as pyglui_utils +import data_changed import file_methods import gl_utils +from observable import Observable from plugin import Analysis_Plugin_Base from . import background_tasks, offline_utils @@ -49,7 +51,7 @@ mp_context = multiprocessing.get_context() -class Surface_Tracker_Offline(Surface_Tracker, Analysis_Plugin_Base): +class Surface_Tracker_Offline(Observable, Surface_Tracker, Analysis_Plugin_Base): """ The Surface_Tracker_Offline does marker based AOI tracking in a recording. All marker and surface detections are calculated in the background and cached to reduce @@ -81,6 +83,13 @@ def __init__(self, g_pool, *args, **kwargs): self._heatmap_update_requests = set() self.export_proxies = set() + self._gaze_changed_listener = data_changed.Listener( + "gaze_positions", g_pool.rec_dir, plugin=self + ) + self._gaze_changed_listener.add_observer( + "on_data_changed", self._on_gaze_positions_changed + ) + @property def Surface_Class(self): return Surface_Offline @@ -513,18 +522,18 @@ def on_notify(self, notification): ) self.export_proxies.add(proxy) - elif notification["subject"] == "gaze_positions_changed": - for surface in self.surfaces: - self._heatmap_update_requests.add(surface) - surface.within_surface_heatmap = surface.get_placeholder_heatmap() - self._fill_gaze_on_surf_buffer() - elif ( notification["subject"] == "surface_tracker_offline._should_fill_gaze_on_surf_buffer" ): self._fill_gaze_on_surf_buffer() + def _on_gaze_positions_changed(self): + for surface in self.surfaces: + self._heatmap_update_requests.add(surface) + surface.within_surface_heatmap = surface.get_placeholder_heatmap() + self._fill_gaze_on_surf_buffer() + def on_surface_change(self, surface): self.save_surface_definitions_to_file() self._heatmap_update_requests.add(surface) diff --git a/pupil_src/shared_modules/ui_roi.py b/pupil_src/shared_modules/ui_roi.py deleted file mode 100644 index 06cfdceece..0000000000 --- a/pupil_src/shared_modules/ui_roi.py +++ /dev/null @@ -1,139 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -from methods import Roi -from pyglui.cygl.utils import draw_points as cygl_draw_points -from pyglui.cygl.utils import RGBA as cygl_rgba -from pyglui.cygl.utils import draw_polyline as cygl_draw_polyline -from OpenGL.GL import GL_LINE_LOOP - - -class UIRoi(Roi): - """ - this object inherits from ROI and adds some UI helper functions - """ - - def __init__(self, array_shape): - super().__init__(array_shape) - self.max_x = array_shape[1] - 1 - self.min_x = 1 - self.max_y = array_shape[0] - 1 - self.min_y = 1 - - # enforce contraints - self.lX = max(self.min_x, self.lX) - self.uX = min(self.max_x, self.uX) - self.lY = max(self.min_y, self.lY) - self.uY = min(self.max_y, self.uY) - - self.handle_size = 35 - self.active_edit_pt = False - self.active_pt_idx = None - self.handle_color = cygl_rgba(0.5, 0.5, 0.9, 0.9) - self.handle_color_selected = cygl_rgba(0.5, 0.9, 0.9, 0.9) - self.handle_color_shadow = cygl_rgba(0.0, 0.0, 0.0, 0.5) - - @property - def rect(self): - return [ - [self.lX, self.lY], - [self.uX, self.lY], - [self.uX, self.uY], - [self.lX, self.uY], - ] - - def move_vertex(self, vert_idx, pt): - x, y = pt - x, y = int(x), int(y) - x, y = min(self.max_x, x), min(self.max_y, y) - x, y = max(self.min_x, x), max(self.min_y, y) - thresh = 45 - if vert_idx == 0: - x = min(x, self.uX - thresh) - y = min(y, self.uY - thresh) - self.lX, self.lY = x, y - if vert_idx == 1: - x = max(x, self.lX + thresh) - y = min(y, self.uY - thresh) - self.uX, self.lY = x, y - if vert_idx == 2: - x = max(x, self.lX + thresh) - y = max(y, self.lY + thresh) - self.uX, self.uY = x, y - if vert_idx == 3: - x = min(x, self.uX - thresh) - y = max(y, self.lY + thresh) - self.lX, self.uY = x, y - - def mouse_over_center(self, edit_pt, mouse_pos, w, h): - return ( - edit_pt[0] - w / 2 <= mouse_pos[0] <= edit_pt[0] + w / 2 - and edit_pt[1] - h / 2 <= mouse_pos[1] <= edit_pt[1] + h / 2 - ) - - def mouse_over_edit_pt(self, mouse_pos, w, h): - for p, i in zip(self.rect, range(4)): - if self.mouse_over_center(p, mouse_pos, w, h): - self.active_pt_idx = i - self.active_edit_pt = True - return True - - def draw(self, ui_scale=1): - cygl_draw_polyline( - self.rect, - color=cygl_rgba(0.8, 0.8, 0.8, 0.9), - thickness=1, - line_type=GL_LINE_LOOP, - ) - - def draw_points(self, ui_scale=1): - if self.active_edit_pt: - inactive_pts = ( - self.rect[: self.active_pt_idx] + self.rect[self.active_pt_idx + 1 :] - ) - active_pt = [self.rect[self.active_pt_idx]] - cygl_draw_points( - inactive_pts, - size=(self.handle_size + 10) * ui_scale, - color=self.handle_color_shadow, - sharpness=0.3, - ) - cygl_draw_points( - inactive_pts, - size=self.handle_size * ui_scale, - color=self.handle_color, - sharpness=0.9, - ) - cygl_draw_points( - active_pt, - size=(self.handle_size + 30) * ui_scale, - color=self.handle_color_shadow, - sharpness=0.3, - ) - cygl_draw_points( - active_pt, - size=(self.handle_size + 10) * ui_scale, - color=self.handle_color_selected, - sharpness=0.9, - ) - else: - cygl_draw_points( - self.rect, - size=(self.handle_size + 10) * ui_scale, - color=self.handle_color_shadow, - sharpness=0.3, - ) - cygl_draw_points( - self.rect, - size=self.handle_size * ui_scale, - color=self.handle_color, - sharpness=0.9, - ) diff --git a/pupil_src/shared_modules/video_capture/__init__.py b/pupil_src/shared_modules/video_capture/__init__.py index c4e1767528..4c075bddc8 100644 --- a/pupil_src/shared_modules/video_capture/__init__.py +++ b/pupil_src/shared_modules/video_capture/__init__.py @@ -37,7 +37,6 @@ InitialisationError, StreamError, ) -from .fake_backend import Fake_Manager, Fake_Source from .file_backend import File_Manager, File_Source, FileSeekError from .hmd_streaming import HMD_Streaming_Source from .uvc_backend import UVC_Manager, UVC_Source @@ -45,8 +44,8 @@ logger = logging.getLogger(__name__) -source_classes = [File_Source, UVC_Source, Fake_Source, HMD_Streaming_Source] -manager_classes = [File_Manager, UVC_Manager, Fake_Manager] +source_classes = [File_Source, UVC_Source, HMD_Streaming_Source] +manager_classes = [File_Manager, UVC_Manager] try: from .ndsi_backend import NDSI_Source, NDSI_Manager @@ -55,21 +54,3 @@ else: source_classes.append(NDSI_Source) manager_classes.append(NDSI_Manager) - -try: - from .realsense_backend import Realsense_Source, Realsense_Manager -except ImportError: - logger.debug("Install pyrealsense to use the Intel RealSense backend") -else: - source_classes.append(Realsense_Source) - manager_classes.append(Realsense_Manager) - -try: - from .realsense2_backend import Realsense2_Source, Realsense2_Manager -except ImportError: - logger.debug( - "Install pyrealsense2 to use the Intel RealSense backend for D400 series cameras" - ) -else: - source_classes.append(Realsense2_Source) - manager_classes.append(Realsense2_Manager) diff --git a/pupil_src/shared_modules/video_capture/base_backend.py b/pupil_src/shared_modules/video_capture/base_backend.py index c70e283b35..8a805fa739 100644 --- a/pupil_src/shared_modules/video_capture/base_backend.py +++ b/pupil_src/shared_modules/video_capture/base_backend.py @@ -10,10 +10,12 @@ """ import logging +import typing as T +from enum import IntEnum, auto from time import monotonic, sleep import numpy as np -from pyglui import cygl +from pyglui import cygl, ui import gl_utils from plugin import Plugin @@ -37,6 +39,12 @@ class NoMoreVideoError(Exception): pass +class SourceMode(IntEnum): + # NOTE: IntEnum is serializable with msgpack + AUTO = auto() + MANUAL = auto() + + class Base_Source(Plugin): """Abstract source class @@ -63,22 +71,150 @@ class Base_Source(Plugin): icon_chr = chr(0xE412) icon_font = "pupil_icons" - def __init__(self, g_pool): + @property + def pretty_class_name(self): + return "Video Source" + + def __init__( + self, g_pool, *, source_mode: T.Optional[SourceMode] = None, **kwargs, + ): super().__init__(g_pool) self.g_pool.capture = self self._recent_frame = None self._intrinsics = None + # Three relevant cases for initializing source_mode: + # - Plugin started at runtime: use existing source mode in g_pool + # - Fresh start without settings: initialize to auto + # - Start with settings: will be passed as parameter, use those + if not hasattr(self.g_pool, "source_mode"): + self.g_pool.source_mode = source_mode or SourceMode.AUTO + + if not hasattr(self.g_pool, "source_managers"): + # If for some reason no manager is loaded, we initialize this ourselves. + self.g_pool.source_managers = [] + def add_menu(self): super().add_menu() self.menu_icon.order = 0.2 + def init_ui(self): + self.add_menu() + self.menu.label = "Video Source" + self.update_menu() + + def deinit_ui(self): + self.remove_menu() + + def source_list(self): + source_type = "Camera" if self.manual_mode else "Device" + entries = [(None, f"Activate {source_type}")] + + for manager in self.g_pool.source_managers: + if self.manual_mode: + sources = manager.get_cameras() + else: + sources = manager.get_devices() + + for info in sources: + entries.append((info, info.label)) + + if len(entries) == 1: + entries.append((None, f"No {source_type}s Found!")) + + return zip(*entries) + + def activate_source(self, source_info): + if source_info is not None: + source_info.activate() + + @property + def manual_mode(self) -> bool: + return self.g_pool.source_mode == SourceMode.MANUAL + + @manual_mode.setter + def manual_mode(self, enable) -> None: + new_mode = SourceMode.MANUAL if enable else SourceMode.AUTO + if new_mode != self.g_pool.source_mode: + logger.debug(f"Setting source mode: {new_mode.name}") + self.notify_all({"subject": "backend.change_mode", "mode": new_mode}) + + def on_notify(self, notification): + subject = notification["subject"] + + if subject == "backend.change_mode": + mode = SourceMode(notification["mode"]) + if mode != self.g_pool.source_mode: + self.g_pool.source_mode = mode + # redraw menu to close potentially open (and now incorrect) dropdown + self.update_menu() + + elif subject == "eye_process.started": + # Make sure to broadcast current source mode once to newly started eyes so + # they are always in sync! + if self.g_pool.app == "capture" and self.g_pool.process == "world": + self.notify_all( + {"subject": "backend.change_mode", "mode": self.g_pool.source_mode} + ) + + def update_menu(self) -> None: + """Update the UI for the source. + + Do not overwrite this in inherited classes. Use ui_elements() instead. + """ + + del self.menu[:] + + if self.manual_mode: + self.menu.append( + ui.Info_Text("Select a camera to use as input for this window.") + ) + else: + self.menu.append( + ui.Info_Text( + "Select a Pupil Core headset from the list." + " Cameras will be automatically selected for world and eye windows." + ) + ) + + self.menu.append( + ui.Selector( + "selected_source", + selection_getter=self.source_list, + getter=lambda: None, + setter=self.activate_source, + label=" ", # TODO: pyglui does not allow using no label at all + ) + ) + + if not self.manual_mode: + self.menu.append( + ui.Info_Text( + "Enable manual camera selection to choose a specific camera" + " as input for every window." + ) + ) + + self.menu.append( + ui.Switch("manual_mode", self, label="Enable Manual Camera Selection") + ) + + source_settings = self.ui_elements() + if source_settings: + settings_menu = ui.Growing_Menu(f"Settings") + settings_menu.extend(source_settings) + self.menu.append(settings_menu) + + def ui_elements(self) -> T.List[ui.UI_element]: + """Returns a list of ui elements with info and settings for the source.""" + return [] + def recent_events(self, events): """Returns None Adds events['frame']=Frame(args) Frame: Object containing image and time information of the current - source frame. See `fake_source.py` for a minimal implementation. + source frame. """ raise NotImplementedError() @@ -110,7 +246,7 @@ def name(self): raise NotImplementedError() def get_init_dict(self): - return {} + return {"source_mode": self.g_pool.source_mode} @property def frame_size(self): @@ -120,10 +256,6 @@ def frame_size(self): """ raise NotImplementedError() - @frame_size.setter - def frame_size(self, new_size): - raise NotImplementedError() - @property def frame_rate(self): """ @@ -132,10 +264,6 @@ def frame_rate(self): """ raise NotImplementedError() - @frame_rate.setter - def frame_rate(self, new_rate): - pass - @property def jpeg_support(self): """ @@ -164,116 +292,56 @@ def intrinsics(self, model): class Base_Manager(Plugin): """Abstract base class for source managers. - Managers are plugins that enumerate and load accessible sources from - different backends, e.g. locally USB-connected cameras. + Managers are plugins that enumerate and load accessible sources from different + backends, e.g. locally USB-connected cameras. - Attributes: - gui_name (str): String used for manager selector labels + Supported sources can be either single cameras or whole devices. Identification and + activation of sources works via SourceInfo (see below). """ - uniqueness = "by_base_class" - gui_name = "Base Manager" - icon_chr = chr(0xEC01) - icon_font = "pupil_icons" + # backend managers are always loaded and need to be loaded before the sources + order = -1 def __init__(self, g_pool): super().__init__(g_pool) - from . import manager_classes + # register all instances in g_pool.source_managers list + if not hasattr(g_pool, "source_managers"): + g_pool.source_managers = [] - self.manager_classes = {m.__name__: m for m in manager_classes} + if self not in g_pool.source_managers: + g_pool.source_managers.append(self) - def on_notify(self, notification): - """ - Reacts to notification: - ``backend.auto_select_manager``: Changes the current Manager to one that's emitted - ``backend.auto_activate_source``: Activates the current source via self.auto_activate_source() - - Emmits notifications (indirectly): - ``start_plugin``: For world thread - ``start_eye_plugin``: For eye thread - ``backend.auto_activate_source`` - """ - - if notification["subject"].startswith("backend.auto_select_manager"): - target_manager_class = self.manager_classes[notification["name"]] - self.replace_backend_manager(target_manager_class, auto_activate=True) - if ( - notification["subject"].startswith("backend.auto_activate_source") - and notification["proc_name"] == self.g_pool.process - ): - self.auto_activate_source() - - def replace_backend_manager(self, manager_class, auto_activate=False): - if not isinstance(self, manager_class): - if self.g_pool.process.startswith("eye"): - self.notify_all( - { - "subject": "start_eye_plugin", - "target": self.g_pool.process, - "name": manager_class.__name__, - } - ) - else: - self.notify_all( - {"subject": "start_plugin", "name": manager_class.__name__} - ) - if auto_activate: - self.notify_all( - { - "subject": "backend.auto_activate_source.{}".format( - self.g_pool.process - ), - "proc_name": self.g_pool.process, - "delay": 0.5, - } - ) + def get_devices(self) -> T.Sequence["SourceInfo"]: + """Return source infos for all devices that the backend supports.""" + return [] - def auto_activate_source(self): - """This function should be implemented in *_Manager classes - to activate the corresponding source with following preferences: - eye0: Pupil Cam1/2/3 ID0 - eye1: Pupil Cam1/2/3 ID1 - world: Pupil Cam1 ID2 + def get_cameras(self) -> T.Sequence["SourceInfo"]: + """Return source infos for all cameras that the backend supports.""" + return [] - See issue #1278 for more details. - """ + def activate(self, key: T.Any) -> None: + """Activate a source (device or camera) by key from source info.""" pass - def auto_select_manager(self): - self.notify_all( - {"subject": "backend.auto_select_manager", "name": self.class_name} - ) - def add_auto_select_button(self): - from pyglui import ui +class SourceInfo: + """SourceInfo is a proxy for a source (camera or device) from a manager. - self.menu.append( - ui.Button("Start with default devices", self.auto_select_manager) - ) - - def add_menu(self): - super().add_menu() - from . import manager_classes - from pyglui import ui + Managers hand out source infos that can be activated from other places in the code. + A manager needs to identify a source uniquely by a key. + """ - self.menu_icon.order = 0.1 + def __init__(self, label: str, manager: Base_Manager, key: T.Any): + self.label = label + self.manager = manager + self.key = key - # We add the capture selection menu - manager_classes.sort(key=lambda x: x.gui_name) - self.menu.append( - ui.Selector( - "capture_manager", - setter=self.replace_backend_manager, - getter=lambda: self.__class__, - selection=manager_classes, - labels=[b.gui_name for b in manager_classes], - label="Manager", - ) - ) + def activate(self) -> None: + self.manager.activate(self.key) - # here is where you add all your menu entries. - self.menu.label = "Backend Manager" + def __str__(self) -> str: + return f"{self.label} - {self.manager.class_name}({self.key})" class Playback_Source(Base_Source): @@ -286,7 +354,7 @@ def __init__(self, g_pool, timing="own", *args, **kwargs): most appropriate frame; does not wait on its own None: Simply returns next frame as fast as possible; used for detectors """ - super().__init__(g_pool) + super().__init__(g_pool, *args, **kwargs) assert timing in ( "external", "own", @@ -321,3 +389,6 @@ def wait(self, timestamp): sleep(target_wait_time) self._recent_wait_ts = timestamp self.finished_sleep = monotonic() + + def get_init_dict(self): + return dict(**super().get_init_dict(), timing=self.timing) diff --git a/pupil_src/shared_modules/video_capture/fake_backend.py b/pupil_src/shared_modules/video_capture/fake_backend.py deleted file mode 100644 index cdf6a3149c..0000000000 --- a/pupil_src/shared_modules/video_capture/fake_backend.py +++ /dev/null @@ -1,384 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" -from .base_backend import Base_Source, Playback_Source, Base_Manager, EndofVideoError - -import os -import cv2 -import numpy as np -from time import time, sleep -from pyglui import ui -from camera_models import Dummy_Camera -from file_methods import load_object - -# logging -import logging - -logger = logging.getLogger(__name__) - - -class Frame(object): - """docstring of Frame""" - - def __init__(self, timestamp, img, index): - self.timestamp = timestamp - self._img = img - self.bgr = img - self.height, self.width, _ = img.shape - self._gray = None - self.index = index - # indicate that the frame does not have a native yuv or jpeg buffer - self.yuv_buffer = None - self.jpeg_buffer = None - - @property - def img(self): - return self._img - - @property - def gray(self): - if self._gray is None: - self._gray = cv2.cvtColor(self._img, cv2.COLOR_BGR2GRAY) - return self._gray - - def copy(self): - return Frame(self.timestamp, self._img.copy(), self.index) - - -class Fake_Source(Playback_Source, Base_Source): - """Simple source which shows random, static image. - - It is used as falback in case the original source fails. `preferred_source` - contains the necessary information to recover to the original source if - it becomes accessible again. - - Attributes: - current_frame_idx (int): Sequence counter - frame_rate (int) - frame_size (tuple) - """ - - def __init__( - self, - g_pool, - source_path=None, - frame_size=None, - frame_rate=None, - name="Fake Source", - *args, - **kwargs - ): - super().__init__(g_pool, *args, **kwargs) - if self.timing == "external": - self.recent_events = self.recent_events_external_timing - else: - self.recent_events = self.recent_events_own_timing - - if source_path: - meta = load_object(source_path) - frame_size = meta["frame_size"] - frame_rate = meta["frame_rate"] - self.timestamps = np.load( - os.path.splitext(source_path)[0] + "_timestamps.npy" - ) - else: - self.timestamps = None - - self.fps = frame_rate - self._name = name - self.make_img(tuple(frame_size)) - self.source_path = source_path - self.current_frame_idx = 0 - self.target_frame_idx = 0 - - def init_ui(self): - self.add_menu() - self.menu.label = "Static Image Source" - - text = ui.Info_Text("This plugin displays a static image.") - self.menu.append(text) - - self.menu.append( - ui.Text_Input( - "frame_size", - label="Frame size", - setter=lambda x: None, - getter=lambda: "{} x {}".format(*self.frame_size), - ) - ) - - self.menu.append( - ui.Text_Input( - "frame_rate", - label="Frame rate", - setter=lambda x: None, - getter=lambda: "{:.0f} FPS".format(self.frame_rate), - ) - ) - - if self.g_pool.app == "player": - # get_frame_count() is not constant in Capture and would trigger - # a redraw on each frame - self.menu.append( - ui.Text_Input( - "frame_num", - label="Number of frames", - setter=lambda x: None, - getter=lambda: self.get_frame_count(), - ) - ) - - def deinit_ui(self): - self.remove_menu() - - def make_img(self, size): - # Generate Pupil Labs colored gradient - self._img = np.zeros((size[1], size[0], 3), dtype=np.uint8) - self._img[:, :, 0] += np.linspace(91, 157, self.frame_size[0], dtype=np.uint8) - self._img[:, :, 1] += np.linspace(165, 161, self.frame_size[0], dtype=np.uint8) - self._img[:, :, 2] += np.linspace(35, 112, self.frame_size[0], dtype=np.uint8) - - self._intrinsics = Dummy_Camera(size, self.name) - - def recent_events_external_timing(self, events): - try: - last_index = self._recent_frame.index - except AttributeError: - # called once on start when self._recent_frame is None - last_index = -1 - - frame = None - pbt = self.g_pool.seek_control.current_playback_time - ts_idx = self.g_pool.seek_control.ts_idx_from_playback_time(pbt) - if ts_idx == last_index: - frame = self._recent_frame.copy() - if self.play and ts_idx == self.get_frame_count() - 1: - logger.info("Recording has ended.") - self.g_pool.seek_control.play = False - elif ts_idx < last_index or ts_idx > last_index + 1: - # time to seek - self.seek_to_frame(ts_idx) - - # Only call get_frame() if the next frame is actually needed - frame = frame or self.get_frame() - self._recent_frame = frame - events["frame"] = frame - self.g_pool.seek_control.end_of_seek() - - def recent_events_own_timing(self, events): - try: - frame = self.get_frame() - except IndexError: - logger.info("Recording has ended.") - self.play = False - else: - if self.timing: - self.wait(frame.timestamp) - self._recent_frame = frame - events["frame"] = frame - - def get_frame(self): - try: - timestamp = self.timestamps[self.target_frame_idx] - except IndexError: - raise EndofVideoError("Reached end of timestamps list.") - except TypeError: - timestamp = self._recent_wait_ts + 1 / self.fps - - frame = Frame(timestamp, self._img.copy(), self.target_frame_idx) - - frame_txt_font_name = cv2.FONT_HERSHEY_SIMPLEX - frame_txt_font_scale = 1.0 - frame_txt_thickness = 1 - - # first line: frame index - frame_txt = "Fake source frame {}".format(frame.index) - frame_txt_size = cv2.getTextSize( - frame_txt, frame_txt_font_name, frame_txt_font_scale, frame_txt_thickness - )[0] - - frame_txt_loc = ( - self.frame_size[0] // 2 - frame_txt_size[0] // 2, - self.frame_size[1] // 2 - frame_txt_size[1], - ) - - cv2.putText( - frame.img, - frame_txt, - frame_txt_loc, - frame_txt_font_name, - frame_txt_font_scale, - (255, 255, 255), - thickness=frame_txt_thickness, - lineType=cv2.LINE_8, - ) - - # second line: resolution @ fps - frame_txt = "{}x{} @ {} fps".format(*self.frame_size, self.frame_rate) - frame_txt_size = cv2.getTextSize( - frame_txt, frame_txt_font_name, frame_txt_font_scale, frame_txt_thickness - )[0] - - frame_txt_loc = ( - self.frame_size[0] // 2 - frame_txt_size[0] // 2, - self.frame_size[1] // 2 + frame_txt_size[1], - ) - - cv2.putText( - frame.img, - frame_txt, - frame_txt_loc, - frame_txt_font_name, - frame_txt_font_scale, - (255, 255, 255), - thickness=frame_txt_thickness, - lineType=cv2.LINE_8, - ) - - self.current_frame_idx = self.target_frame_idx - self.target_frame_idx += 1 - - return frame - - def get_frame_count(self): - try: - return len(self.timestamps) - except TypeError: - return self.current_frame_idx + 1 - - def seek_to_frame(self, frame_idx): - self.target_frame_idx = max(0, min(frame_idx, self.get_frame_count() - 1)) - self.finished_sleep = 0 - - def get_frame_index(self): - return self.current_frame_idx - - @property - def name(self): - return self._name - - @property - def settings(self): - return {"frame_size": self.frame_size, "frame_rate": self.frame_rate} - - @settings.setter - def settings(self, settings): - self.frame_size = settings.get("frame_size", self.frame_size) - self.frame_rate = settings.get("frame_rate", self.frame_rate) - - @property - def frame_size(self): - return self._img.shape[1], self._img.shape[0] - - @frame_size.setter - def frame_size(self, new_size): - # closest match for size - sizes = [abs(r[0] - new_size[0]) for r in self.frame_sizes] - best_size_idx = sizes.index(min(sizes)) - size = self.frame_sizes[best_size_idx] - if size != new_size: - logger.warning( - "%s resolution capture mode not available. Selected %s." - % (new_size, size) - ) - self.make_img(size) - - @property - def frame_rates(self): - return (30, 60, 90, 120) - - @property - def frame_sizes(self): - return ((640, 480), (1280, 720), (1920, 1080)) - - @property - def frame_rate(self): - return self.fps - - @frame_rate.setter - def frame_rate(self, new_rate): - rates = [abs(r - new_rate) for r in self.frame_rates] - best_rate_idx = rates.index(min(rates)) - rate = self.frame_rates[best_rate_idx] - if rate != new_rate: - logger.warning( - "%sfps capture mode not available at (%s) on 'Fake Source'. Selected %sfps. " - % (new_rate, self.frame_size, rate) - ) - self.fps = rate - - @property - def jpeg_support(self): - return False - - @property - def online(self): - return True - - def get_init_dict(self): - if self.g_pool.app == "capture": - d = super().get_init_dict() - d["frame_size"] = self.frame_size - d["frame_rate"] = self.frame_rate - d["name"] = self.name - return d - else: - raise NotImplementedError() - - -class Fake_Manager(Base_Manager): - """Simple manager to explicitly activate a fake source""" - - gui_name = "Test image" - - def __init__(self, g_pool): - super().__init__(g_pool) - - def init_ui(self): - self.add_menu() - from pyglui import ui - - self.add_auto_select_button() - text = ui.Info_Text("Convenience manager to select a fake source explicitly.") - - activation_button = ui.Button("Activate Fake Capture", self.activate) - self.menu.extend([text, activation_button]) - - def activate(self): - # a capture leaving is a must stop for recording. - self.notify_all({"subject": "recording.should_stop"}) - settings = {} - settings["timing"] = "own" - settings["frame_rate"] = self.g_pool.capture.frame_rate - settings["frame_size"] = self.g_pool.capture.frame_size - settings["name"] = "Fake Source" - # if the user set fake capture, we dont want it to auto jump back to the old capture. - if self.g_pool.process == "world": - self.notify_all( - {"subject": "start_plugin", "name": "Fake_Source", "args": settings} - ) - else: - self.notify_all( - { - "subject": "start_eye_plugin", - "target": self.g_pool.process, - "name": "Fake_Source", - "args": settings, - } - ) - - def auto_activate_source(self): - self.activate() - - def deinit_ui(self): - self.remove_menu() - - def recent_events(self, events): - pass diff --git a/pupil_src/shared_modules/video_capture/file_backend.py b/pupil_src/shared_modules/video_capture/file_backend.py index a5a1437f7c..d20cab7dbc 100644 --- a/pupil_src/shared_modules/video_capture/file_backend.py +++ b/pupil_src/shared_modules/video_capture/file_backend.py @@ -13,22 +13,22 @@ import logging import os import os.path -import av -import numpy as np import typing as T - -from multiprocessing import cpu_count from abc import ABC, abstractmethod +from multiprocessing import cpu_count from time import sleep + +import av +import numpy as np +from pyglui import ui + from camera_models import load_intrinsics -from .utils import VideoSet +from pupil_recording import PupilRecording -import player_methods as pm from .base_backend import Base_Manager, Base_Source, EndofVideoError, Playback_Source -from pupil_recording import PupilRecording +from .utils import VideoSet logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) av.logging.set_level(av.logging.ERROR) logging.getLogger("libav").setLevel(logging.ERROR) @@ -190,9 +190,14 @@ def get_frame_iterator(self): yield frame +# NOTE:Base_Source is included as base class for uniqueness:by_base_class to work +# correctly with other Source plugins. class File_Source(Playback_Source, Base_Source): """Simple file capture. + Note that File_Source is special since it is usually not intended to be used as + capture plugin. Therefore it hides it's UI by default. + Playback_Source arguments: timing (str): "external", "own" (default), None @@ -201,6 +206,7 @@ class File_Source(Playback_Source, Base_Source): loop (bool): loop video set if timing!="external" buffered_decoding (bool): use buffered decode fill_gaps (bool): fill gaps with static frames + show_plugin_menu (bool): enable to show regular capture UI with source selection """ def __init__( @@ -210,6 +216,7 @@ def __init__( loop=False, buffered_decoding=False, fill_gaps=False, + show_plugin_menu=False, *args, **kwargs, ): @@ -239,6 +246,8 @@ def __init__( self.reset_video() self._intrinsics = load_intrinsics(rec, set_name, self.frame_size) + self.show_plugin_menu = show_plugin_menu + def get_rec_set_name(self, source_path): """ Return dir and set name by source_path @@ -324,6 +333,10 @@ def _get_streams(self, container, should_buffer): def initialised(self): return not self.videoset.is_empty() + @property + def online(self): + return not self.videoset.is_empty() + @property def frame_size(self): return self.video_stream.frame_size @@ -332,21 +345,30 @@ def frame_size(self): def frame_rate(self): return self._frame_rate + def init_ui(self): + if self.show_plugin_menu: + super().init_ui() + + def deinit_ui(self): + if self.show_plugin_menu: + super().deinit_ui() + def get_init_dict(self): - if self.g_pool.app == "capture": - settings = super().get_init_dict() - settings["source_path"] = self.source_path - settings["loop"] = self.loop - return settings - else: - raise NotImplementedError() + return dict( + **super().get_init_dict(), + source_path=self.source_path, + loop=self.loop, + buffered_decoding=self.buffering, + fill_gaps=self.fill_gaps, + show_plugin_menu=self.show_plugin_menu, + ) @property def name(self): if self.source_path: return os.path.splitext(self.source_path)[0] else: - return "File source in ghost mode" + return "File source (no file loaded)" def get_frame_index(self): return int(self.current_frame_idx) @@ -493,6 +515,7 @@ def seek_to_frame(self, seek_pos): self.target_frame_idx = seek_pos def on_notify(self, notification): + super().on_notify(notification) if ( notification["subject"] == "file_source.seek" and notification.get("source_path") == self.source_path @@ -509,32 +532,33 @@ def on_notify(self, notification): ): self.play = False - def init_ui(self): - self.add_menu() - self.menu.label = "File Source: {}".format(os.path.split(self.source_path)[-1]) - from pyglui import ui - - self.menu.append( - ui.Info_Text( - "The file source plugin loads and " - + "displays video from a given file." - ) + def ui_elements(self): + ui_elements = [] + ui_elements.append( + ui.Info_Text(f"File Source: {os.path.split(self.source_path)[-1]}") ) - if self.g_pool.app == "capture": + if not self.online: + ui_elements.append( + ui.Info_Text( + "Could not playback file! Check if file exists and if" + " corresponding timestamps file is present." + ) + ) + return ui_elements - def toggle_looping(val): - self.loop = val - if val: - self.play = True + def toggle_looping(val): + self.loop = val + if val: + self.play = True - self.menu.append(ui.Switch("loop", self, setter=toggle_looping)) + ui_elements.append(ui.Switch("loop", self, setter=toggle_looping)) - self.menu.append( + ui_elements.append( ui.Text_Input("source_path", self, label="Full path", setter=lambda x: None) ) - self.menu.append( + ui_elements.append( ui.Text_Input( "frame_size", label="Frame size", @@ -543,7 +567,7 @@ def toggle_looping(val): ) ) - self.menu.append( + ui_elements.append( ui.Text_Input( "frame_rate", label="Frame rate", @@ -552,7 +576,7 @@ def toggle_looping(val): ) ) - self.menu.append( + ui_elements.append( ui.Text_Input( "frame_num", label="Number of frames", @@ -560,9 +584,7 @@ def toggle_looping(val): getter=lambda: self.get_frame_count(), ) ) - - def deinit_ui(self): - self.remove_menu() + return ui_elements def cleanup(self): try: @@ -593,68 +615,10 @@ class File_Manager(Base_Manager): Attributes: file_exts (list): File extensions to filter displayed files - root_folder (str): Folder path, which includes file sources """ - gui_name = "Video File Source" file_exts = [".mp4", ".mkv", ".mov", ".mjpeg"] - def __init__(self, g_pool, root_folder=None): - super().__init__(g_pool) - base_dir = self.g_pool.user_dir.rsplit(os.path.sep, 1)[0] - default_rec_dir = os.path.join(base_dir, "recordings") - self.root_folder = root_folder or default_rec_dir - - def init_ui(self): - self.add_menu() - from pyglui import ui - - self.add_auto_select_button() - self.menu.append( - ui.Info_Text( - "Enter a folder to enumerate all eligible video files. " - + "Be aware that entering folders with a lot of files can " - + "slow down Pupil Capture." - ) - ) - - def set_root(folder): - if not os.path.isdir(folder): - logger.error("`%s` is not a valid folder path." % folder) - else: - self.root_folder = folder - - self.menu.append( - ui.Text_Input("root_folder", self, label="Source Folder", setter=set_root) - ) - - def split_enumeration(): - eligible_files = self.enumerate_folder(self.root_folder) - eligible_files.insert(0, (None, "Select to activate")) - return zip(*eligible_files) - - self.menu.append( - ui.Selector( - "selected_file", - selection_getter=split_enumeration, - getter=lambda: None, - setter=self.activate, - label="Video File", - ) - ) - - def deinit_ui(self): - self.remove_menu() - - def activate(self, full_path): - if not full_path: - return - settings = {"source_path": full_path, "timing": "own"} - self.activate_source(settings) - - def auto_activate_source(self): - self.activate(None) - def on_drop(self, paths): for p in paths: if os.path.splitext(p)[-1] in self.file_exts: @@ -662,25 +626,15 @@ def on_drop(self, paths): return True return False - def enumerate_folder(self, path): - eligible_files = [] - is_eligible = lambda f: os.path.splitext(f)[-1] in self.file_exts - path = os.path.abspath(os.path.expanduser(path)) - for root, dirs, files in os.walk(path): - - def root_split(file): - full_p = os.path.join(root, file) - disp_p = full_p.replace(path, "") - return (full_p, disp_p) - - eligible_files.extend(map(root_split, filter(is_eligible, files))) - eligible_files.sort(key=lambda x: x[1]) - return eligible_files - - def get_init_dict(self): - return {"root_folder": self.root_folder} + def activate(self, full_path): + if not full_path: + return - def activate_source(self, settings={}): + settings = { + "source_path": full_path, + "timing": "own", + "show_plugin_menu": True, + } if self.g_pool.process == "world": self.notify_all( {"subject": "start_plugin", "name": "File_Source", "args": settings} @@ -694,6 +648,3 @@ def activate_source(self, settings={}): "args": settings, } ) - - def recent_events(self, events): - pass diff --git a/pupil_src/shared_modules/video_capture/hmd_streaming.py b/pupil_src/shared_modules/video_capture/hmd_streaming.py index 6648f0d60b..809850bc02 100644 --- a/pupil_src/shared_modules/video_capture/hmd_streaming.py +++ b/pupil_src/shared_modules/video_capture/hmd_streaming.py @@ -15,8 +15,8 @@ from pyglui import ui import zmq_tools -from camera_models import Radial_Dist_Camera, Dummy_Camera -from video_capture.base_backend import Base_Manager, Base_Source +from camera_models import Dummy_Camera, Radial_Dist_Camera +from video_capture.base_backend import Base_Source logger = logging.getLogger(__name__) @@ -52,17 +52,6 @@ def __init__(self, g_pool, *args, **kwargs): topics=("hmd_streaming.world",), ) - # def get_init_dict(self): - - def init_ui(self): # was gui - self.add_menu() - self.menu.label = "HMD Streaming" - text = ui.Info_Text("HMD Streaming Info") - self.menu.append(text) - - def deinit_ui(self): - self.remove_menu() - def cleanup(self): self.frame_sub = None @@ -139,42 +128,7 @@ def intrinsics(self, model): "HMD Streaming backend does not support setting intrinsics manually" ) - -class HMD_Streaming_Manager(Base_Manager): - """Simple manager to explicitly activate a fake source""" - - gui_name = "HMD Streaming" - - def __init__(self, g_pool): - super().__init__(g_pool) - - # Initiates the UI for starting the webcam. - def init_ui(self): - self.add_menu() - from pyglui import ui - - self.menu.append(ui.Info_Text("Backend for HMD Streaming")) - self.menu.append(ui.Button("Activate HMD Streaming", self.activate_source)) - - def activate_source(self): - settings = {} - # if the user set fake capture, we dont want it to auto jump back to the old capture. - if self.g_pool.process == "world": - self.notify_all( - { - "subject": "start_plugin", - "name": "HMD_Streaming_Source", - "args": settings, - } - ) - else: - logger.warning("HMD Streaming backend is not supported in the eye process.") - - def deinit_ui(self): - self.remove_menu() - - def recent_events(self, events): - pass - - def get_init_dict(self): - return {} + def ui_elements(self): + ui_elements = [] + ui_elements.append(ui.Info_Text(f"HMD Streaming")) + return ui_elements diff --git a/pupil_src/shared_modules/video_capture/ndsi_backend.py b/pupil_src/shared_modules/video_capture/ndsi_backend.py index 886ec48e33..3d8f839c05 100644 --- a/pupil_src/shared_modules/video_capture/ndsi_backend.py +++ b/pupil_src/shared_modules/video_capture/ndsi_backend.py @@ -9,15 +9,17 @@ ---------------------------------------------------------------------------~(*) """ -import time import logging -from packaging.version import Version +import time import ndsi +from packaging.version import Version +from pyglui import ui -from .base_backend import Base_Source, Base_Manager from camera_models import load_intrinsics +from .base_backend import Base_Manager, Base_Source, SourceInfo + try: from ndsi import __version__ @@ -25,8 +27,15 @@ from ndsi import __protocol_version__ except (ImportError, AssertionError): raise Exception("pyndsi version is to old. Please upgrade") from None + + logger = logging.getLogger(__name__) +# Suppress pyre debug logs (except beacon) +logger.debug("Suppressing pyre debug logs (except zbeacon)") +logging.getLogger("pyre").setLevel(logging.WARNING) +logging.getLogger("pyre.zbeacon").setLevel(logging.DEBUG) + class NDSI_Source(Base_Source): """Pupil Mobile video source @@ -41,42 +50,45 @@ def __init__( g_pool, frame_size, frame_rate, - network=None, source_id=None, host_name=None, sensor_name=None, + *args, + **kwargs, ): - super().__init__(g_pool) + super().__init__(g_pool, *args, **kwargs) self.sensor = None self._source_id = source_id self._sensor_name = sensor_name self._host_name = host_name self._frame_size = frame_size self._frame_rate = frame_rate - self.has_ui = False + self.ui_initialized = False self.control_id_ui_mapping = {} self.get_frame_timeout = 100 # ms self.ghost_mode_timeout = 10 # sec self._initial_refresh = True self.last_update = self.g_pool.get_timestamp() + manager = next((p for p in g_pool.plugins if isinstance(p, NDSI_Manager)), None) + if not manager: + logger.error("Error connecting to Pupil Mobile: NDSI Manager not found!") + return + + network = manager.network if not network: - logger.debug( - "No network reference provided. Capture is started " - + "in ghost mode. No images will be supplied." - ) + logger.error("Error connecting to Pupil Mobile: No NDSI network!") return self.recover(network) if not self.sensor or not self.sensor.supports_data_subscription: - logger.error( - "Init failed. Capture is started in ghost mode. " - + "No images will be supplied." - ) + logger.error("Could not connect to device! No images will be supplied.") self.cleanup() - logger.debug("NDSI Source Sensor: %s" % self.sensor) + logger.warning( + "Make sure to enable the Time_Sync plugin for recording with Pupil Mobile!" + ) def recover(self, network): logger.debug( @@ -142,6 +154,13 @@ def recent_events(self, events): ) except ndsi.StreamError: frame = None + except ndsi.sensor.NotDataSubSupportedError: + # NOTE: This (most likely) is a race-condition in NDSI initialization + # that is waiting to be fixed for Pupil Mobile. It happens rarely and + # can be solved by simply reconnecting the headset to the mobile phone. + # Preventing traceback logfloods here and displaying more helpful + # message to the user. + logger.warning("Connection problem! Please reconnect headset to phone!") except Exception: frame = None import traceback @@ -156,13 +175,13 @@ def recent_events(self, events): elif ( self.g_pool.get_timestamp() - self.last_update > self.ghost_mode_timeout ): - logger.info("Entering ghost mode") + logger.info("Device disconnected.") if self.online: self.sensor.unlink() self.sensor = None self._source_id = None self._initial_refresh = True - self.update_control_menu() + self.update_menu() self.last_update = self.g_pool.get_timestamp() else: time.sleep(self.get_frame_timeout / 1e3) @@ -179,15 +198,16 @@ def on_notification(self, sensor, event): logger.warning("Error {}".format(event["error_str"])) if "control_id" in event and event["control_id"] in self.sensor.controls: logger.debug(str(self.sensor.controls[event["control_id"]])) - elif self.has_ui and ( + elif self.ui_initialized and ( event["control_id"] not in self.control_id_ui_mapping or event["changes"].get("dtype") == "strmapping" or event["changes"].get("dtype") == "intmapping" ): - self.update_control_menu() + self.update_menu() # local notifications def on_notify(self, notification): + super().on_notify(notification) subject = notification["subject"] if subject.startswith("remote_recording.") and self.online: if "should_start" in subject and self.online: @@ -246,21 +266,12 @@ def get_init_dict(self): return settings def init_ui(self): - self.add_menu() - self.menu.label = "NDSI Source: {} @ {}".format( - self._sensor_name, self._host_name - ) - - from pyglui import ui - - self.has_ui = True - self.uvc_menu = ui.Growing_Menu("UVC Controls") - self.update_control_menu() + super().init_ui() + self.ui_initialized = True def deinit_ui(self): - self.uvc_menu = None - self.remove_menu() - self.has_ui = False + self.ui_initialized = False + super().deinit_ui() def add_controls_to_menu(self, menu, controls): from pyglui import ui @@ -327,26 +338,18 @@ def initiate_value_change(val): import traceback as tb tb.print_exc() - if len(menu) == 0: - menu.append(ui.Info_Text("No {} settings found".format(menu.label))) return menu - def update_control_menu(self): - if not self.has_ui: - return - from pyglui import ui + def ui_elements(self): + + ui_elements = [] + ui_elements.append( + ui.Info_Text(f"Camera: {self._sensor_name} @ {self._host_name}") + ) - del self.menu[:] - del self.uvc_menu[:] - self.control_id_ui_mapping = {} if not self.sensor: - self.menu.append( - ui.Info_Text( - ("Sensor %s @ %s not available. " + "Running in ghost mode.") - % (self._sensor_name, self._host_name) - ) - ) - return + ui_elements.append(ui.Info_Text("Camera disconnected!")) + return ui_elements uvc_controls = [] other_controls = [] @@ -356,14 +359,22 @@ def update_control_menu(self): else: other_controls.append(entry) - self.add_controls_to_menu(self.menu, other_controls) - self.add_controls_to_menu(self.uvc_menu, uvc_controls) - self.menu.append(self.uvc_menu) + uvc_menu = ui.Growing_Menu("UVC Controls") + self.control_id_ui_mapping = {} + if other_controls: + self.add_controls_to_menu(ui_elements, other_controls) + if uvc_controls: + self.add_controls_to_menu(uvc_menu, uvc_controls) + else: + uvc_menu.append(ui.Info_Text("No UVC settings found.")) + ui_elements.append(uvc_menu) - self.menu.append( + ui_elements.append( ui.Button("Reset to default values", self.sensor.reset_all_control_values) ) + return ui_elements + def cleanup(self): if self.online: self.sensor.unlink() @@ -371,115 +382,58 @@ def cleanup(self): class NDSI_Manager(Base_Manager): - """Enumerates and activates Pupil Mobile video sources - - Attributes: - network (ndsi.Network): NDSI Network backend - selected_host (unicode): Selected host uuid - """ - - gui_name = "Pupil Mobile" + """Enumerates and activates NDSI video sources""" def __init__(self, g_pool): super().__init__(g_pool) self.network = ndsi.Network( - formats={ndsi.DataFormat.V3, ndsi.DataFormat.V4}, - callbacks=(self.on_event,) + formats={ndsi.DataFormat.V3, ndsi.DataFormat.V4}, callbacks=(self.on_event,) ) self.network.start() - self.selected_host = None self._recover_in = 3 self._rejoin_in = 400 - self.should_select_host = None self.cam_selection_lut = { "eye0": ["ID0", "PI right"], "eye1": ["ID1", "PI left"], "world": ["ID2", "Logitech", "PI world"], } - logger.warning("Make sure the `time_sync` plugin is loaded!") def cleanup(self): self.network.stop() - def init_ui(self): - self.add_menu() - self.re_build_ndsi_menu() - - def deinit_ui(self): - self.remove_menu() - - def view_host(self, host_uuid): - if self.selected_host != host_uuid: - self.selected_host = host_uuid - self.re_build_ndsi_menu() - - def host_selection_list(self): - devices = { - s["host_uuid"]: s["host_name"] # removes duplicates + def get_devices(self): + # store hosts in dict to remove duplicates from multiple sensors + active_hosts = { + s["host_uuid"]: s["host_name"] for s in self.network.sensors.values() + if s["sensor_type"] == "video" } - - if devices: - return list(devices.keys()), list(devices.values()) - else: - return [None], ["No hosts found"] - - def source_selection_list(self): - default = (None, "Select to activate") - sources = [default] + [ - (s["sensor_uuid"], s["sensor_name"]) - for s in self.network.sensors.values() - if (s["sensor_type"] == "video" and s["host_uuid"] == self.selected_host) + return [ + SourceInfo(label=host_name, manager=self, key=f"host.{host_uuid}") + for host_uuid, host_name in active_hosts.items() ] - return zip(*sources) - - def re_build_ndsi_menu(self): - del self.menu[1:] - from pyglui import ui - ui_elements = [] - ui_elements.append(ui.Info_Text("Remote Pupil Mobile sources")) - ui_elements.append( - ui.Info_Text("Pupil Mobile Commspec v{}".format(__protocol_version__)) - ) - - host_sel, host_sel_labels = self.host_selection_list() - ui_elements.append( - ui.Selector( - "selected_host", - self, - selection=host_sel, - labels=host_sel_labels, - setter=self.view_host, - label="Remote host", + def get_cameras(self): + return [ + SourceInfo( + label=f"{s['sensor_name']} @ PM {s['host_name']}", + manager=self, + key=f"sensor.{s['sensor_uuid']}", ) - ) - - self.menu.extend(ui_elements) - self.add_auto_select_button() - - if not self.selected_host: - return - ui_elements = [] + for s in self.network.sensors.values() + if s["sensor_type"] == "video" + ] - host_menu = ui.Growing_Menu("Remote Host Information") - ui_elements.append(host_menu) - - src_sel, src_sel_labels = self.source_selection_list() - host_menu.append( - ui.Selector( - "selected_source", - selection=src_sel, - labels=src_sel_labels, - getter=lambda: None, - setter=self.activate, - label="Source", + def activate(self, key): + source_type, uid = key.split(".", maxsplit=1) + if source_type == "host": + self.notify_all( + {"subject": "backend.ndsi.auto_activate_source", "host_uid": uid} ) - ) + elif source_type == "sensor": + self.activate_source(source_uid=uid) - self.menu.extend(ui_elements) - - def activate(self, source_uid): + def activate_source(self, source_uid): if not source_uid: return settings = { @@ -487,40 +441,44 @@ def activate(self, source_uid): "frame_rate": self.g_pool.capture.frame_rate, "source_id": source_uid, } - self.activate_source(settings) - - def auto_select_manager(self): - super().auto_select_manager() - self.notify_all( - { - "subject": "backend.ndsi_do_select_host", - "target_host": self.selected_host, - "delay": 0.4, - } - ) + if self.g_pool.process == "world": + self.notify_all( + {"subject": "start_plugin", "name": "NDSI_Source", "args": settings} + ) + else: + self.notify_all( + { + "subject": "start_eye_plugin", + "target": self.g_pool.process, + "name": "NDSI_Source", + "args": settings, + } + ) - def auto_activate_source(self): - if not self.selected_host: - return + def auto_activate_source(self, host_uid): + host_sensors = [ + sensor + for sensor in self.network.sensors.values() + if (sensor["sensor_type"] == "video" and sensor["host_uuid"] == host_uid) + ] - src_sel, src_sel_labels = self.source_selection_list() - if len(src_sel) < 2: # "Select to Activate" is always presenet as first element - logger.warning("No device is available on the remote host.") + if not host_sensors: + logger.warning("No devices available on the remote host.") return - cam_ids = self.cam_selection_lut[self.g_pool.process] + name_patterns = self.cam_selection_lut[self.g_pool.process] + matching_cams = [ + sensor + for sensor in host_sensors + if any(pattern in sensor["sensor_name"] for pattern in name_patterns) + ] - for cam_id in cam_ids: - try: - source_id = next( - src_sel[i] for i, lab in enumerate(src_sel_labels) if cam_id in lab - ) - self.activate(source_id) - break - except StopIteration: - source_id = None - else: + if not matching_cams: logger.warning("The default device was not found on the remote host.") + return + + cam = matching_cams[0] + self.activate_source(cam["sensor_uuid"]) def poll_events(self): while self.network.has_events: @@ -542,6 +500,7 @@ def recent_events(self, events): if self._rejoin_in <= 0: logger.debug("Rejoining network...") self.network.rejoin() + # frame-timeout independent timer self._rejoin_in = int(10 * 1e3 / self.g_pool.capture.get_frame_timeout) else: self._rejoin_in -= 1 @@ -550,42 +509,27 @@ def on_event(self, caller, event): if event["subject"] == "detach": logger.debug("detached: %s" % event) sensors = [s for s in self.network.sensors.values()] - if self.selected_host == event["host_uuid"]: - if sensors: - self.selected_host = sensors[0]["host_uuid"] - else: - self.selected_host = None - self.re_build_ndsi_menu() elif event["subject"] == "attach": if event["sensor_type"] == "video": logger.debug("attached: {}".format(event)) self.notify_all({"subject": "backend.ndsi_source_found"}) - if not self.selected_host and not self.should_select_host: - self.selected_host = event["host_uuid"] - elif self.should_select_host and event["sensor_type"] == "video": - self.select_host(self.should_select_host) - - self.re_build_ndsi_menu() - - def activate_source(self, settings={}): - settings["network"] = self.network - self.g_pool.plugins.add(NDSI_Source, args=settings) - def recover(self): - self.g_pool.capture.recover(self.network) + if isinstance(self.g_pool.capture, NDSI_Source): + self.g_pool.capture.recover(self.network) def on_notify(self, n): - """Provides UI for the capture selection + """Starts appropriate NDSI sources. Reacts to notification: ``backend.ndsi_source_found``: Check if recovery is possible - ``backend.ndsi_do_select_host``: Switches to selected host from other process + ``backend.ndsi.auto_activate_source``: Auto activate best source for process Emmits notifications: - ``backend.ndsi_source_found`` - ``backend.ndsi_do_select_host` + ``backend.ndsi_source_found``: New NDSI source available + ``backend.ndsi.auto_activate_source``: All NDSI managers should auto activate a source + ``start_(eye_)plugin``: Starts NDSI sources """ super().on_notify(n) @@ -597,20 +541,5 @@ def on_notify(self, n): ): self.recover() - if n["subject"].startswith("backend.ndsi_do_select_host"): - self.select_host(n["target_host"]) - - def select_host(self, selected_host): - host_sel, _ = self.host_selection_list() - if selected_host in host_sel: - self.view_host(selected_host) - self.should_select_host = None - self.re_build_ndsi_menu() - src_sel, _ = self.source_selection_list() - # "Select to Activate" is always presenet as first element - if len(src_sel) >= 2: - self.auto_activate_source() - - else: - self.should_select_host = selected_host - + if n["subject"] == "backend.ndsi.auto_activate_source": + self.auto_activate_source(n["host_uid"]) diff --git a/pupil_src/shared_modules/video_capture/realsense2_backend.py b/pupil_src/shared_modules/video_capture/realsense2_backend.py deleted file mode 100755 index 903c28db95..0000000000 --- a/pupil_src/shared_modules/video_capture/realsense2_backend.py +++ /dev/null @@ -1,916 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -import logging -import time -import cv2 -import os - -import pyrealsense2 as rs - -from version_utils import VersionFormat -from .base_backend import Base_Source, Base_Manager -from av_writer import MPEG_Writer -from camera_models import load_intrinsics - -import glfw -import gl_utils -from OpenGL.GL import * -from OpenGL.GLU import * -from pyglui import cygl -import cython_methods -import numpy as np -from ctypes import * - -# check versions for our own depedencies as they are fast-changing -# assert VersionFormat(rs.__version__) >= VersionFormat("2.2") # FIXME - -# logging -logger = logging.getLogger(__name__) -logger.setLevel(logging.DEBUG) - - -TIMEOUT = 500 # ms FIXME -DEFAULT_COLOR_SIZE = (1280, 720) -DEFAULT_COLOR_FPS = 30 -DEFAULT_DEPTH_SIZE = (640, 480) -DEFAULT_DEPTH_FPS = 30 - -# very thin wrapper for rs.frame objects -class ColorFrame(object): - def __init__(self, data, timestamp, index): - self.timestamp = timestamp - self.index = index - - self.data = data[:, :, np.newaxis].view(dtype=np.uint8) - total_size = self.data.size - y_plane = total_size // 2 - u_plane = y_plane // 2 - self._yuv = np.empty(total_size, dtype=np.uint8) - self._yuv[:y_plane] = self.data[:, :, 0].ravel() - self._yuv[y_plane : y_plane + u_plane] = self.data[:, ::2, 1].ravel() - self._yuv[y_plane + u_plane :] = self.data[:, 1::2, 1].ravel() - self._shape = self.data.shape[:2] - - self._bgr = None - self._gray = None - - @property - def height(self): - return self._shape[0] - - @property - def width(self): - return self._shape[1] - - @property - def yuv_buffer(self): - return self._yuv - - @property - def yuv422(self): - Y = self._yuv[: self._yuv.size // 2] - U = self._yuv[self._yuv.size // 2 : 3 * self._yuv.size // 4] - V = self._yuv[3 * self._yuv.size // 4 :] - - Y.shape = self._shape - U.shape = self._shape[0], self._shape[1] // 2 - V.shape = self._shape[0], self._shape[1] // 2 - - return Y, U, V - - @property - def bgr(self): - if self._bgr is None: - self._bgr = cv2.cvtColor(self.data, cv2.COLOR_YUV2BGR_YUYV) - return self._bgr - - @property - def img(self): - return self.bgr - - @property - def gray(self): - if self._gray is None: - self._gray = self._yuv[: self._yuv.size // 2] - self._gray.shape = self._shape - return self._gray - - -class DepthFrame(object): - def __init__(self, data, timestamp, index): - self.timestamp = timestamp - self.index = index - - self._bgr = None - self._gray = None - self.depth = data - self.yuv_buffer = None - - @property - def height(self): - return self.depth.shape[0] - - @property - def width(self): - return self.depth.shape[1] - - @property - def bgr(self): - if self._bgr is None: - self._bgr = cython_methods.cumhist_color_map16(self.depth) - return self._bgr - - @property - def img(self): - return self.bgr - - @property - def gray(self): - if self._gray is None: - self._gray = cv2.cvtColor(self.bgr, cv2.cv2.COLOR_BGR2GRAY) - return self._gray - - -class Realsense2_Source(Base_Source): - def __init__( - self, - g_pool, - device_id=None, - frame_size=DEFAULT_COLOR_SIZE, - frame_rate=DEFAULT_COLOR_FPS, - depth_frame_size=DEFAULT_DEPTH_SIZE, - depth_frame_rate=DEFAULT_DEPTH_FPS, - preview_depth=False, - device_options=(), - record_depth=True, - ): - logger.debug("_init_ started") - super().__init__(g_pool) - self._intrinsics = None - self.color_frame_index = 0 - self.depth_frame_index = 0 - self.context = rs.context() - self.pipeline = rs.pipeline(self.context) - self.pipeline_profile = None - self.preview_depth = preview_depth - self.record_depth = record_depth - self.depth_video_writer = None - self._needs_restart = False - self.frame_size_backup = DEFAULT_COLOR_SIZE - self.depth_frame_size_backup = DEFAULT_DEPTH_SIZE - self.frame_rate_backup = DEFAULT_COLOR_FPS - self.depth_frame_rate_backup = DEFAULT_DEPTH_FPS - - self._initialize_device( - device_id, - frame_size, - frame_rate, - depth_frame_size, - depth_frame_rate, - device_options, - ) - logger.debug("_init_ completed") - - def _initialize_device( - self, - device_id, - color_frame_size, - color_fps, - depth_frame_size, - depth_fps, - device_options=(), - ): - self.stop_pipeline() - self.last_color_frame_ts = None - self.last_depth_frame_ts = None - self._recent_frame = None - self._recent_depth_frame = None - - if device_id is None: - device_id = self.device_id - - if device_id is None: # FIXME these two if blocks look ugly. - return - - # use default streams to filter modes by rs_stream and rs_format - self._available_modes = self._enumerate_formats(device_id) - logger.debug( - "device_id: {} self._available_modes: {}".format( - device_id, str(self._available_modes) - ) - ) - - if ( - color_frame_size is not None - and depth_frame_size is not None - and color_fps is not None - and depth_fps is not None - ): - color_frame_size = tuple(color_frame_size) - depth_frame_size = tuple(depth_frame_size) - - logger.debug( - "Initialize with Color {}@{}\tDepth {}@{}".format( - color_frame_size, color_fps, depth_frame_size, depth_fps - ) - ) - - # make sure the frame rates are compatible with the given frame sizes - color_fps = self._get_valid_frame_rate( - rs.stream.color, color_frame_size, color_fps - ) - depth_fps = self._get_valid_frame_rate( - rs.stream.depth, depth_frame_size, depth_fps - ) - - self.frame_size_backup = color_frame_size - self.depth_frame_size_backup = depth_frame_size - self.frame_rate_backup = color_fps - self.depth_frame_rate_backup = depth_fps - - config = self._prep_configuration( - color_frame_size, color_fps, depth_frame_size, depth_fps - ) - else: - config = self._get_default_config() - self.frame_size_backup = DEFAULT_COLOR_SIZE - self.depth_frame_size_backup = DEFAULT_DEPTH_SIZE - self.frame_rate_backup = DEFAULT_COLOR_FPS - self.depth_frame_rate_backup = DEFAULT_DEPTH_FPS - - try: - self.pipeline_profile = self.pipeline.start(config) - except RuntimeError as re: - logger.error("Cannot start pipeline! " + str(re)) - self.pipeline_profile = None - else: - self.stream_profiles = { - s.stream_type(): s.as_video_stream_profile() - for s in self.pipeline_profile.get_streams() - } - logger.debug("Pipeline started for device " + device_id) - logger.debug("Stream profiles: " + str(self.stream_profiles)) - - self._intrinsics = load_intrinsics( - self.g_pool.user_dir, self.name, self.frame_size - ) - self.update_menu() - self._needs_restart = False - - def _prep_configuration( - self, - color_frame_size=None, - color_fps=None, - depth_frame_size=None, - depth_fps=None, - ): - config = rs.config() - - # only use these two formats - color_format = rs.format.yuyv - depth_format = rs.format.z16 - - config.enable_stream( - rs.stream.depth, - depth_frame_size[0], - depth_frame_size[1], - depth_format, - depth_fps, - ) - - config.enable_stream( - rs.stream.color, - color_frame_size[0], - color_frame_size[1], - color_format, - color_fps, - ) - - return config - - def _get_default_config(self): - config = rs.config() # default config is RGB8, we want YUYV - config.enable_stream( - rs.stream.color, - DEFAULT_COLOR_SIZE[0], - DEFAULT_COLOR_SIZE[1], - rs.format.yuyv, - DEFAULT_COLOR_FPS, - ) - config.enable_stream( - rs.stream.depth, - DEFAULT_DEPTH_SIZE[0], - DEFAULT_DEPTH_SIZE[1], - rs.format.z16, - DEFAULT_DEPTH_FPS, - ) - return config - - def _get_valid_frame_rate(self, stream_type, frame_size, fps): - assert stream_type == rs.stream.color or stream_type == rs.stream.depth - - if not self._available_modes or stream_type not in self._available_modes: - logger.warning( - "_get_valid_frame_rate: self._available_modes not set yet. Returning default fps." - ) - if stream_type == rs.stream.color: - return DEFAULT_COLOR_FPS - elif stream_type == rs.stream.depth: - return DEFAULT_DEPTH_FPS - else: - raise ValueError("Unexpected `stream_type`: {}".format(stream_type)) - - if frame_size not in self._available_modes[stream_type]: - logger.error( - "Frame size not supported for {}: {}. Returning default fps".format( - stream_type, frame_size - ) - ) - if stream_type == rs.stream.color: - return DEFAULT_COLOR_FPS - elif stream_type == rs.stream.depth: - return DEFAULT_DEPTH_FPS - - if fps not in self._available_modes[stream_type][frame_size]: - old_fps = fps - rates = [ - abs(r - fps) for r in self._available_modes[stream_type][frame_size] - ] - best_rate_idx = rates.index(min(rates)) - fps = self._available_modes[stream_type][frame_size][best_rate_idx] - logger.warning( - "{} fps is not supported for ({}) for Color Stream. Fallback to {} fps".format( - old_fps, frame_size, fps - ) - ) - - return fps - - def _enumerate_formats(self, device_id): - """Enumerate formats into hierachical structure: - - streams: - resolutions: - framerates - """ - formats = {} - - if self.context is None: - return formats - - devices = self.context.query_devices() - current_device = None - - for d in devices: - try: - serial = d.get_info(rs.camera_info.serial_number) - except RuntimeError as re: - logger.error("Device no longer available " + str(re)) - else: - if device_id == serial: - current_device = d - - if current_device is None: - return formats - logger.debug("Found the current device: " + device_id) - - sensors = current_device.query_sensors() - for s in sensors: - stream_profiles = s.get_stream_profiles() - for sp in stream_profiles: - vp = sp.as_video_stream_profile() - stream_type = vp.stream_type() - - if stream_type not in (rs.stream.color, rs.stream.depth): - continue - elif vp.format() not in (rs.format.z16, rs.format.yuyv): - continue - - formats.setdefault(stream_type, {}) - stream_resolution = (vp.width(), vp.height()) - formats[stream_type].setdefault(stream_resolution, []).append(vp.fps()) - - return formats - - def stop_pipeline(self): - if self.online: - try: - self.pipeline_profile = None - self.stream_profiles = None - self.pipeline.stop() - logger.debug("Pipeline stopped.") - except RuntimeError as re: - logger.error("Cannot stop the pipeline: " + str(re)) - - def cleanup(self): - if self.depth_video_writer is not None: - self.stop_depth_recording() - self.stop_pipeline() - - def get_init_dict(self): - return { - "frame_size": self.frame_size, - "frame_rate": self.frame_rate, - "depth_frame_size": self.depth_frame_size, - "depth_frame_rate": self.depth_frame_rate, - "preview_depth": self.preview_depth, - "record_depth": self.record_depth, - } - - def get_frames(self): - if self.online: - try: - frames = self.pipeline.wait_for_frames(TIMEOUT) - except RuntimeError as e: - logger.error("get_frames: Timeout!") - raise RuntimeError(e) - else: - current_time = self.g_pool.get_timestamp() - - color = None - # if we're expecting color frames - if rs.stream.color in self.stream_profiles: - color_frame = frames.get_color_frame() - last_color_frame_ts = color_frame.get_timestamp() - if self.last_color_frame_ts != last_color_frame_ts: - self.last_color_frame_ts = last_color_frame_ts - color = ColorFrame( - np.asanyarray(color_frame.get_data()), - current_time, - self.color_frame_index, - ) - self.color_frame_index += 1 - - depth = None - # if we're expecting depth frames - if rs.stream.depth in self.stream_profiles: - depth_frame = frames.get_depth_frame() - last_depth_frame_ts = depth_frame.get_timestamp() - if self.last_depth_frame_ts != last_depth_frame_ts: - self.last_depth_frame_ts = last_depth_frame_ts - depth = DepthFrame( - np.asanyarray(depth_frame.get_data()), - current_time, - self.depth_frame_index, - ) - self.depth_frame_index += 1 - - return color, depth - return None, None - - def recent_events(self, events): - if self._needs_restart or not self.online: - logger.debug("recent_events -> restarting device") - self.restart_device() - time.sleep(0.01) - return - - try: - color_frame, depth_frame = self.get_frames() - except RuntimeError as re: - logger.warning("Realsense failed to provide frames." + str(re)) - self._recent_frame = None - self._recent_depth_frame = None - self._needs_restart = True - else: - if color_frame is not None: - self._recent_frame = color_frame - events["frame"] = color_frame - - if depth_frame is not None: - self._recent_depth_frame = depth_frame - events["depth_frame"] = depth_frame - - if self.depth_video_writer is not None: - self.depth_video_writer.write_video_frame(depth_frame) - - def deinit_ui(self): - self.remove_menu() - - def init_ui(self): - self.add_menu() - self.menu.label = "Local USB Video Source" - self.update_menu() - - def update_menu(self): - logger.debug("update_menu") - try: - del self.menu[:] - except AttributeError: - return - - from pyglui import ui - - if not self.online: - self.menu.append(ui.Info_Text("Capture initialization failed.")) - return - - self.menu.append(ui.Switch("record_depth", self, label="Record Depth Stream")) - self.menu.append(ui.Switch("preview_depth", self, label="Preview Depth")) - - if self._available_modes is not None: - - def frame_size_selection_getter(): - if self.device_id: - frame_size = sorted( - self._available_modes[rs.stream.color], reverse=True - ) - labels = ["({}, {})".format(t[0], t[1]) for t in frame_size] - return frame_size, labels - else: - return [self.frame_size_backup], [str(self.frame_size_backup)] - - selector = ui.Selector( - "frame_size", - self, - selection_getter=frame_size_selection_getter, - label="Color Resolution", - ) - self.menu.append(selector) - - def frame_rate_selection_getter(): - if self.device_id: - avail_fps = [ - fps - for fps in self._available_modes[rs.stream.color][ - self.frame_size - ] - ] - return avail_fps, [str(fps) for fps in avail_fps] - else: - return [self.frame_rate_backup], [str(self.frame_rate_backup)] - - selector = ui.Selector( - "frame_rate", - self, - selection_getter=frame_rate_selection_getter, - label="Color Frame Rate", - ) - self.menu.append(selector) - - def depth_frame_size_selection_getter(): - if self.device_id: - depth_sizes = sorted( - self._available_modes[rs.stream.depth], reverse=True - ) - labels = ["({}, {})".format(t[0], t[1]) for t in depth_sizes] - return depth_sizes, labels - else: - return ( - [self.depth_frame_size_backup], - [str(self.depth_frame_size_backup)], - ) - - selector = ui.Selector( - "depth_frame_size", - self, - selection_getter=depth_frame_size_selection_getter, - label="Depth Resolution", - ) - self.menu.append(selector) - - def depth_frame_rate_selection_getter(): - if self.device_id: - avail_fps = [ - fps - for fps in self._available_modes[rs.stream.depth][ - self.depth_frame_size - ] - ] - return avail_fps, [str(fps) for fps in avail_fps] - else: - return ( - [self.depth_frame_rate_backup], - [str(self.depth_frame_rate_backup)], - ) - - selector = ui.Selector( - "depth_frame_rate", - self, - selection_getter=depth_frame_rate_selection_getter, - label="Depth Frame Rate", - ) - self.menu.append(selector) - - def reset_options(): - logger.debug("reset_options") - self.reset_device(self.device_id) - - sensor_control = ui.Growing_Menu(label="Sensor Settings") - sensor_control.append( - ui.Button("Reset device options to default", reset_options) - ) - self.menu.append(sensor_control) - else: - logger.debug("update_menu: self._available_modes is None") - - def gl_display(self): - - if self.preview_depth and self._recent_depth_frame is not None: - self.g_pool.image_tex.update_from_ndarray(self._recent_depth_frame.bgr) - gl_utils.glFlush() - gl_utils.make_coord_system_norm_based() - self.g_pool.image_tex.draw() - elif self._recent_frame is not None: - self.g_pool.image_tex.update_from_yuv_buffer( - self._recent_frame.yuv_buffer, - self._recent_frame.width, - self._recent_frame.height, - ) - gl_utils.glFlush() - gl_utils.make_coord_system_norm_based() - self.g_pool.image_tex.draw() - - if not self.online: - super().gl_display() - - gl_utils.make_coord_system_pixel_based( - (self.frame_size[1], self.frame_size[0], 3) - ) - - def reset_device(self, device_id): - logger.debug("reset_device") - if device_id is None: - device_id = self.device_id - - self.notify_all( - { - "subject": "realsense2_source.restart", - "device_id": device_id, - "color_frame_size": None, - "color_fps": None, - "depth_frame_size": None, - "depth_fps": None, - "device_options": [], # FIXME - } - ) - - def restart_device( - self, - color_frame_size=None, - color_fps=None, - depth_frame_size=None, - depth_fps=None, - device_options=None, - ): - if color_frame_size is None: - color_frame_size = self.frame_size - if color_fps is None: - color_fps = self.frame_rate - if depth_frame_size is None: - depth_frame_size = self.depth_frame_size - if depth_fps is None: - depth_fps = self.depth_frame_rate - if device_options is None: - device_options = [] # FIXME - - self.notify_all( - { - "subject": "realsense2_source.restart", - "device_id": None, - "color_frame_size": color_frame_size, - "color_fps": color_fps, - "depth_frame_size": depth_frame_size, - "depth_fps": depth_fps, - "device_options": device_options, - } - ) - logger.debug("self.restart_device --> self.notify_all") - - def on_notify(self, notification): - logger.debug( - 'self.on_notify, notification["subject"]: ' + notification["subject"] - ) - if notification["subject"] == "realsense2_source.restart": - kwargs = notification.copy() - del kwargs["subject"] - del kwargs["topic"] - self._initialize_device(**kwargs) - elif notification["subject"] == "recording.started": - self.start_depth_recording(notification["rec_path"], notification["start_time_synced"]) - elif notification["subject"] == "recording.stopped": - self.stop_depth_recording() - - def start_depth_recording(self, rec_loc, start_time_synced): - if not self.record_depth: - return - - if self.depth_video_writer is not None: - logger.warning("Depth video recording has been started already") - return - - video_path = os.path.join(rec_loc, "depth.mp4") - self.depth_video_writer = MPEG_Writer(video_path, start_time_synced) - - def stop_depth_recording(self): - if self.depth_video_writer is None: - logger.warning("Depth video recording was not running") - return - - self.depth_video_writer.close() - self.depth_video_writer = None - - @property - def device_id(self): - if self.online: # already running - return self.pipeline_profile.get_device().get_info( - rs.camera_info.serial_number - ) - else: - # set the first available device - devices = self.context.query_devices() - if devices: - logger.info("device_id: first device by default.") - return devices[0].get_info(rs.camera_info.serial_number) - else: - logger.debug("device_id: No device connected.") - return None - - @property - def frame_size(self): - try: - stream_profile = self.stream_profiles[rs.stream.color] - # TODO check width & height is in self.available modes - return stream_profile.width(), stream_profile.height() - except AttributeError: - return self.frame_size_backup - except KeyError: - return self.frame_size_backup - except TypeError: - return self.frame_size_backup - - @frame_size.setter - def frame_size(self, new_size): - if new_size != self.frame_size: - self.restart_device(color_frame_size=new_size) - - @property - def frame_rate(self): - try: - stream_profile = self.stream_profiles[rs.stream.color] - # TODO check FPS is in self.available modes - return stream_profile.fps() - except AttributeError: - return self.frame_rate_backup - except KeyError: - return self.frame_rate_backup - except TypeError: - return self.frame_rate_backup - - @frame_rate.setter - def frame_rate(self, new_rate): - if new_rate != self.frame_rate: - self.restart_device(color_fps=new_rate) - - @property - def depth_frame_size(self): - try: - stream_profile = self.stream_profiles[rs.stream.depth] - # TODO check width & height is in self.available modes - return stream_profile.width(), stream_profile.height() - except AttributeError: - return self.depth_frame_size_backup - except KeyError: - return self.depth_frame_size_backup - except TypeError: - return self.depth_frame_size_backup - - @depth_frame_size.setter - def depth_frame_size(self, new_size): - if new_size != self.depth_frame_size: - self.restart_device(depth_frame_size=new_size) - - @property - def depth_frame_rate(self): - try: - stream_profile = self.stream_profiles[rs.stream.depth] - return stream_profile.fps() - except AttributeError: - return self.depth_frame_rate_backup - except KeyError: - return self.depth_frame_rate_backup - except TypeError: - return self.depth_frame_rate_backup - - @depth_frame_rate.setter - def depth_frame_rate(self, new_rate): - if new_rate != self.depth_frame_rate: - self.restart_device(depth_fps=new_rate) - - @property - def jpeg_support(self): - return False - - @property - def online(self): - return self.pipeline_profile is not None and self.pipeline is not None - - @property - def name(self): - if self.online: - return self.pipeline_profile.get_device().get_info(rs.camera_info.name) - else: - logger.debug( - "self.name: Realsense2 not online. Falling back to Ghost capture" - ) - return "Ghost capture" - - -class Realsense2_Manager(Base_Manager): - """Manages Intel RealSense D400 sources - - Attributes: - check_intervall (float): Intervall in which to look for new UVC devices - """ - - gui_name = "RealSense D400" - - def get_init_dict(self): - return {} - - def init_ui(self): - self.add_menu() - from pyglui import ui - - self.menu.append(ui.Info_Text("Intel RealSense D400 sources")) - - def is_streaming(device_id): - try: - c = rs.config() - c.enable_device(device_id) # device_id is in fact the serial_number - p = rs.pipeline() - p.start(c) - p.stop() - return False - except RuntimeError: - return True - - def get_device_info(d): - name = d.get_info(rs.camera_info.name) # FIXME is camera in use? - device_id = d.get_info(rs.camera_info.serial_number) - - fmt = "- " if is_streaming(device_id) else "" - fmt += name - - return device_id, fmt - - def dev_selection_list(): - default = (None, "Select to activate") - try: - ctx = rs.context() # FIXME cannot use "with rs.context() as ctx:" - # got "AttributeError: __enter__" - # see https://stackoverflow.com/questions/5093382/object-becomes-none-when-using-a-context-manager - dev_pairs = [default] + [get_device_info(d) for d in ctx.devices] - except Exception: # FIXME - dev_pairs = [default] - - return zip(*dev_pairs) - - def activate(source_uid): - if source_uid is None: - return - - settings = { - "frame_size": self.g_pool.capture.frame_size, - "frame_rate": self.g_pool.capture.frame_rate, - "device_id": source_uid, - } - if self.g_pool.process == "world": - self.notify_all( - { - "subject": "start_plugin", - "name": "Realsense2_Source", - "args": settings, - } - ) - else: - self.notify_all( - { - "subject": "start_eye_plugin", - "target": self.g_pool.process, - "name": "Realsense2_Source", - "args": settings, - } - ) - - self.menu.append( - ui.Selector( - "selected_source", - selection_getter=dev_selection_list, - getter=lambda: None, - setter=activate, - label="Activate source", - ) - ) - - def deinit_ui(self): - self.remove_menu() diff --git a/pupil_src/shared_modules/video_capture/realsense_backend.py b/pupil_src/shared_modules/video_capture/realsense_backend.py deleted file mode 100755 index 7b64befc72..0000000000 --- a/pupil_src/shared_modules/video_capture/realsense_backend.py +++ /dev/null @@ -1,935 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -import logging -import time -import cv2 -import os - -import pyrealsense as pyrs -from pyrealsense.stream import ColorStream, DepthStream, DACStream, PointStream -from pyrealsense.constants import rs_stream, rs_option, rs_preset -from pyrealsense.extlib import rsutilwrapper - -from version_utils import VersionFormat -from .base_backend import Base_Source, Base_Manager -from av_writer import MPEG_Writer -from camera_models import load_intrinsics - -import glfw -import gl_utils -from OpenGL.GL import * -from OpenGL.GLU import * -from pyglui import cygl -import cython_methods -import numpy as np -from ctypes import * - -# check versions for our own depedencies as they are fast-changing -assert VersionFormat(pyrs.__version__) >= VersionFormat("2.2") - -# logging -logging.getLogger("pyrealsense").setLevel(logging.ERROR + 1) -logger = logging.getLogger(__name__) -logger.setLevel(logging.DEBUG) - - -class ColorFrame(object): - def __init__(self, device): - # we need to keep this since there is no cv2 conversion for our planar format - self._yuv422 = device.color - self._shape = self._yuv422.shape[:2] - self._yuv = np.empty(self._yuv422.size, dtype=np.uint8) - y_plane = self._yuv422.size // 2 - u_plane = y_plane // 2 - self._yuv[:y_plane] = self._yuv422[:, :, 0].flatten() - self._yuv[y_plane : y_plane + u_plane] = self._yuv422[:, ::2, 1].flatten() - self._yuv[y_plane + u_plane :] = self._yuv422[:, 1::2, 1].flatten() - self._bgr = None - self._gray = None - - @property - def height(self): - return self._shape[0] - - @property - def width(self): - return self._shape[1] - - @property - def yuv_buffer(self): - return self._yuv - - @property - def yuv422(self): - Y = self._yuv[: self._yuv.size // 2] - U = self._yuv[self._yuv.size // 2 : 3 * self._yuv.size // 4] - V = self._yuv[3 * self._yuv.size // 4 :] - - Y.shape = self._shape - U.shape = self._shape[0], self._shape[1] // 2 - V.shape = self._shape[0], self._shape[1] // 2 - - return Y, U, V - - @property - def bgr(self): - if self._bgr is None: - self._bgr = cv2.cvtColor(self._yuv422, cv2.COLOR_YUV2BGR_YUYV) - return self._bgr - - @property - def img(self): - return self.bgr - - @property - def gray(self): - if self._gray is None: - self._gray = self._yuv[: self._yuv.size // 2] - self._gray.shape = self._shape - return self._gray - - -class DepthFrame(object): - def __init__(self, device): - self._bgr = None - self._gray = None - self.depth = device.depth - self.yuv_buffer = None - - @property - def height(self): - return self.depth.shape[0] - - @property - def width(self): - return self.depth.shape[1] - - @property - def bgr(self): - if self._bgr is None: - self._bgr = cython_methods.cumhist_color_map16(self.depth) - return self._bgr - - @property - def img(self): - return self.bgr - - @property - def gray(self): - if self._gray is None: - self._gray = cv2.cvtColor(self.bgr, cv2.cv2.COLOR_BGR2GRAY) - return self._gray - - -class Control(object): - def __init__(self, device, opt_range, value): - self._dev = device - self._value = value - self.range = opt_range - self.label = rs_option.name_for_value[opt_range.option] - self.label = self.label.replace("RS_OPTION_", "") - self.label = self.label.replace("R200_", "") - self.label = self.label.replace("_", " ") - self.label = self.label.title() - self.description = self._dev.get_device_option_description(opt_range.option) - - @property - def value(self): - return self._value - - @value.setter - def value(self, val): - try: - self._dev.set_device_option(self.range.option, val) - except pyrs.RealsenseError as err: - logger.error('Setting option "{}" failed'.format(self.label)) - logger.debug("Reason: {}".format(err)) - else: - self._value = val - - def refresh(self): - self._value = self._dev.get_device_option(self.range.option) - - -class Realsense_Controls(dict): - def __init__(self, device, presets=()): - if not device: - super().__init__() - return - if presets: - # presets: list of (option, value)-tuples - try: - device.set_device_options(*zip(*presets)) - except pyrs.RealsenseError as err: - logger.error("Setting device option presets failed") - logger.debug("Reason: {}".format(err)) - controls = {} - for opt_range, value in device.get_available_options(): - if opt_range.min < opt_range.max: - controls[opt_range.option] = Control(device, opt_range, value) - super().__init__(controls) - - def export_presets(self): - return [(opt, ctrl.value) for opt, ctrl in self.items()] - - def refresh(self): - for ctrl in self.values(): - ctrl.refresh() - - -class Realsense_Source(Base_Source): - """ - Camera Capture is a class that encapsualtes pyrs.Device: - """ - - def __init__( - self, - g_pool, - device_id=0, - frame_size=(1920, 1080), - frame_rate=30, - depth_frame_size=(640, 480), - depth_frame_rate=60, - align_streams=False, - preview_depth=False, - device_options=(), - record_depth=True, - stream_preset=None, - ): - super().__init__(g_pool) - self._intrinsics = None - self.color_frame_index = 0 - self.depth_frame_index = 0 - self.device = None - self.service = pyrs.Service() - self.align_streams = align_streams - self.preview_depth = preview_depth - self.record_depth = record_depth - self.depth_video_writer = None - self.controls = None - self.pitch = 0 - self.yaw = 0 - self.mouse_drag = False - self.last_pos = (0, 0) - self.depth_window = None - self._needs_restart = False - self.stream_preset = stream_preset - self._initialize_device( - device_id, - frame_size, - frame_rate, - depth_frame_size, - depth_frame_rate, - device_options, - ) - - def _initialize_device( - self, - device_id, - color_frame_size, - color_fps, - depth_frame_size, - depth_fps, - device_options=(), - ): - devices = tuple(self.service.get_devices()) - color_frame_size = tuple(color_frame_size) - depth_frame_size = tuple(depth_frame_size) - - self.streams = [ColorStream(), DepthStream(), PointStream()] - self.last_color_frame_ts = None - self.last_depth_frame_ts = None - self._recent_frame = None - self._recent_depth_frame = None - - if not devices: - if not self._needs_restart: - logger.error("Camera failed to initialize. No cameras connected.") - self.device = None - self.update_menu() - return - - if self.device is not None: - self.device.stop() # only call Device.stop() if its context - - if device_id >= len(devices): - logger.error( - "Camera with id {} not found. Initializing default camera.".format( - device_id - ) - ) - device_id = 0 - - # use default streams to filter modes by rs_stream and rs_format - self._available_modes = self._enumerate_formats(device_id) - - # make sure that given frame sizes and rates are available - color_modes = self._available_modes[rs_stream.RS_STREAM_COLOR] - if color_frame_size not in color_modes: - # automatically select highest resolution - color_frame_size = sorted(color_modes.keys(), reverse=True)[0] - - if color_fps not in color_modes[color_frame_size]: - # automatically select highest frame rate - color_fps = color_modes[color_frame_size][0] - - depth_modes = self._available_modes[rs_stream.RS_STREAM_DEPTH] - if self.align_streams: - depth_frame_size = color_frame_size - else: - if depth_frame_size not in depth_modes: - # automatically select highest resolution - depth_frame_size = sorted(depth_modes.keys(), reverse=True)[0] - - if depth_fps not in depth_modes[depth_frame_size]: - # automatically select highest frame rate - depth_fps = depth_modes[depth_frame_size][0] - - colorstream = ColorStream( - width=color_frame_size[0], - height=color_frame_size[1], - fps=color_fps, - color_format="yuv", - preset=self.stream_preset, - ) - depthstream = DepthStream( - width=depth_frame_size[0], - height=depth_frame_size[1], - fps=depth_fps, - preset=self.stream_preset, - ) - pointstream = PointStream( - width=depth_frame_size[0], height=depth_frame_size[1], fps=depth_fps - ) - - self.streams = [colorstream, depthstream, pointstream] - if self.align_streams: - dacstream = DACStream( - width=depth_frame_size[0], height=depth_frame_size[1], fps=depth_fps - ) - dacstream.name = "depth" # rename data accessor - self.streams.append(dacstream) - - # update with correctly initialized streams - # always initiliazes color + depth, adds rectified/aligned versions as necessary - - self.device = self.service.Device(device_id, streams=self.streams) - self.controls = Realsense_Controls(self.device, device_options) - self._intrinsics = load_intrinsics( - self.g_pool.user_dir, self.name, self.frame_size - ) - - self.update_menu() - self._needs_restart = False - - def _enumerate_formats(self, device_id): - """Enumerate formats into hierachical structure: - - streams: - resolutions: - framerates - """ - formats = {} - # only lists modes for native streams (RS_STREAM_COLOR/RS_STREAM_DEPTH) - for mode in self.service.get_device_modes(device_id): - if mode.stream in (rs_stream.RS_STREAM_COLOR, rs_stream.RS_STREAM_DEPTH): - # check if frame size dict is available - if mode.stream not in formats: - formats[mode.stream] = {} - stream_obj = next((s for s in self.streams if s.stream == mode.stream)) - if mode.format == stream_obj.format: - size = mode.width, mode.height - # check if framerate list is already available - if size not in formats[mode.stream]: - formats[mode.stream][size] = [] - formats[mode.stream][size].append(mode.fps) - - if self.align_streams: - depth_sizes = formats[rs_stream.RS_STREAM_DEPTH].keys() - color_sizes = formats[rs_stream.RS_STREAM_COLOR].keys() - # common_sizes = depth_sizes & color_sizes - discarded_sizes = depth_sizes ^ color_sizes - for size in discarded_sizes: - for sizes in formats.values(): - if size in sizes: - del sizes[size] - - return formats - - def cleanup(self): - if self.depth_video_writer is not None: - self.stop_depth_recording() - if self.device is not None: - self.device.stop() - self.service.stop() - - def get_init_dict(self): - return { - "device_id": self.device.device_id if self.device is not None else 0, - "frame_size": self.frame_size, - "frame_rate": self.frame_rate, - "depth_frame_size": self.depth_frame_size, - "depth_frame_rate": self.depth_frame_rate, - "preview_depth": self.preview_depth, - "record_depth": self.record_depth, - "align_streams": self.align_streams, - "device_options": self.controls.export_presets() - if self.controls is not None - else (), - "stream_preset": self.stream_preset, - } - - def get_frames(self): - if self.device: - self.device.wait_for_frames() - current_time = self.g_pool.get_timestamp() - - last_color_frame_ts = self.device.get_frame_timestamp( - self.streams[0].stream - ) - if self.last_color_frame_ts != last_color_frame_ts: - self.last_color_frame_ts = last_color_frame_ts - color = ColorFrame(self.device) - color.timestamp = current_time - color.index = self.color_frame_index - self.color_frame_index += 1 - else: - color = None - - last_depth_frame_ts = self.device.get_frame_timestamp( - self.streams[1].stream - ) - if self.last_depth_frame_ts != last_depth_frame_ts: - self.last_depth_frame_ts = last_depth_frame_ts - depth = DepthFrame(self.device) - depth.timestamp = current_time - depth.index = self.depth_frame_index - self.depth_frame_index += 1 - else: - depth = None - - return color, depth - return None, None - - def recent_events(self, events): - if self._needs_restart: - self.restart_device() - time.sleep(0.05) - elif not self.online: - time.sleep(0.05) - return - - try: - color_frame, depth_frame = self.get_frames() - except (pyrs.RealsenseError, TimeoutError) as err: - logger.warning("Realsense failed to provide frames. Attempting to reinit.") - self._recent_frame = None - self._recent_depth_frame = None - self._needs_restart = True - else: - if color_frame and depth_frame: - self._recent_frame = color_frame - events["frame"] = color_frame - - if depth_frame: - self._recent_depth_frame = depth_frame - events["depth_frame"] = depth_frame - - if self.depth_video_writer is not None: - self.depth_video_writer.write_video_frame(depth_frame) - - def deinit_ui(self): - self.remove_menu() - - def init_ui(self): - self.add_menu() - self.menu.label = "Local USB Video Source" - self.update_menu() - - def update_menu(self): - try: - del self.menu[:] - except AttributeError: - return - - from pyglui import ui - - if self.device is None: - self.menu.append(ui.Info_Text("Capture initialization failed.")) - return - - def align_and_restart(val): - self.align_streams = val - self.restart_device() - - self.menu.append(ui.Switch("record_depth", self, label="Record Depth Stream")) - self.menu.append(ui.Switch("preview_depth", self, label="Preview Depth")) - self.menu.append( - ui.Switch( - "align_streams", self, label="Align Streams", setter=align_and_restart - ) - ) - - def toggle_depth_display(): - def on_depth_mouse_button(window, button, action, mods): - if button == glfw.GLFW_MOUSE_BUTTON_LEFT and action == glfw.GLFW_PRESS: - self.mouse_drag = True - if ( - button == glfw.GLFW_MOUSE_BUTTON_LEFT - and action == glfw.GLFW_RELEASE - ): - self.mouse_drag = False - - if self.depth_window is None: - self.pitch = 0 - self.yaw = 0 - - win_size = glfw.glfwGetWindowSize(self.g_pool.main_window) - self.depth_window = glfw.glfwCreateWindow( - win_size[0], win_size[1], "3D Point Cloud" - ) - glfw.glfwSetMouseButtonCallback( - self.depth_window, on_depth_mouse_button - ) - active_window = glfw.glfwGetCurrentContext() - glfw.glfwMakeContextCurrent(self.depth_window) - gl_utils.basic_gl_setup() - gl_utils.make_coord_system_norm_based() - - # refresh speed settings - glfw.glfwSwapInterval(0) - - glfw.glfwMakeContextCurrent(active_window) - - native_presets = [ - ("None", None), - ("Best Quality", rs_preset.RS_PRESET_BEST_QUALITY), - ("Largest image", rs_preset.RS_PRESET_LARGEST_IMAGE), - ("Highest framerate", rs_preset.RS_PRESET_HIGHEST_FRAMERATE), - ] - - def set_stream_preset(val): - if self.stream_preset != val: - self.stream_preset = val - self.restart_device() - - self.menu.append( - ui.Selector( - "stream_preset", - self, - setter=set_stream_preset, - labels=[preset[0] for preset in native_presets], - selection=[preset[1] for preset in native_presets], - label="Stream preset", - ) - ) - color_sizes = sorted( - self._available_modes[rs_stream.RS_STREAM_COLOR], reverse=True - ) - selector = ui.Selector( - "frame_size", - self, - # setter=, - selection=color_sizes, - label="Resolution" if self.align_streams else "Color Resolution", - ) - selector.read_only = self.stream_preset is not None - self.menu.append(selector) - - def color_fps_getter(): - avail_fps = [ - fps - for fps in self._available_modes[rs_stream.RS_STREAM_COLOR][ - self.frame_size - ] - if self.depth_frame_rate % fps == 0 - ] - return avail_fps, [str(fps) for fps in avail_fps] - - selector = ui.Selector( - "frame_rate", - self, - # setter=, - selection_getter=color_fps_getter, - label="Color Frame Rate", - ) - selector.read_only = self.stream_preset is not None - self.menu.append(selector) - - if not self.align_streams: - depth_sizes = sorted( - self._available_modes[rs_stream.RS_STREAM_DEPTH], reverse=True - ) - selector = ui.Selector( - "depth_frame_size", - self, - # setter=, - selection=depth_sizes, - label="Depth Resolution", - ) - selector.read_only = self.stream_preset is not None - self.menu.append(selector) - - def depth_fps_getter(): - avail_fps = [ - fps - for fps in self._available_modes[rs_stream.RS_STREAM_DEPTH][ - self.depth_frame_size - ] - if fps % self.frame_rate == 0 - ] - return avail_fps, [str(fps) for fps in avail_fps] - - selector = ui.Selector( - "depth_frame_rate", - self, - selection_getter=depth_fps_getter, - label="Depth Frame Rate", - ) - selector.read_only = self.stream_preset is not None - self.menu.append(selector) - - def reset_options(): - if self.device: - try: - self.device.reset_device_options_to_default(self.controls.keys()) - except pyrs.RealsenseError as err: - logger.info("Resetting some device options failed") - logger.debug("Reason: {}".format(err)) - finally: - self.controls.refresh() - - self.menu.append(ui.Button("Point Cloud Window", toggle_depth_display)) - sensor_control = ui.Growing_Menu(label="Sensor Settings") - sensor_control.append( - ui.Button("Reset device options to default", reset_options) - ) - for ctrl in sorted(self.controls.values(), key=lambda x: x.range.option): - # sensor_control.append(ui.Info_Text(ctrl.description)) - if ( - ctrl.range.min == 0.0 - and ctrl.range.max == 1.0 - and ctrl.range.step == 1.0 - ): - sensor_control.append( - ui.Switch("value", ctrl, label=ctrl.label, off_val=0.0, on_val=1.0) - ) - else: - sensor_control.append( - ui.Slider( - "value", - ctrl, - label=ctrl.label, - min=ctrl.range.min, - max=ctrl.range.max, - step=ctrl.range.step, - ) - ) - self.menu.append(sensor_control) - - def gl_display(self): - from math import floor - - if self.depth_window is not None and glfw.glfwWindowShouldClose( - self.depth_window - ): - glfw.glfwDestroyWindow(self.depth_window) - self.depth_window = None - - if self.depth_window is not None and self._recent_depth_frame is not None: - active_window = glfw.glfwGetCurrentContext() - glfw.glfwMakeContextCurrent(self.depth_window) - - win_size = glfw.glfwGetFramebufferSize(self.depth_window) - gl_utils.adjust_gl_view(win_size[0], win_size[1]) - pos = glfw.glfwGetCursorPos(self.depth_window) - if self.mouse_drag: - self.pitch = np.clip(self.pitch + (pos[1] - self.last_pos[1]), -80, 80) - self.yaw = np.clip(self.yaw - (pos[0] - self.last_pos[0]), -120, 120) - self.last_pos = pos - - glClearColor(0, 0, 0, 0) - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) - glMatrixMode(GL_PROJECTION) - glLoadIdentity() - gluPerspective(60, win_size[0] / win_size[1], 0.01, 20.0) - glMatrixMode(GL_MODELVIEW) - glLoadIdentity() - gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0) - glTranslatef(0, 0, 0.5) - glRotated(self.pitch, 1, 0, 0) - glRotated(self.yaw, 0, 1, 0) - glTranslatef(0, 0, -0.5) - - # glPointSize(2) - glEnable(GL_DEPTH_TEST) - extrinsics = self.device.get_device_extrinsics( - rs_stream.RS_STREAM_DEPTH, rs_stream.RS_STREAM_COLOR - ) - depth_frame = self._recent_depth_frame - color_frame = self._recent_frame - depth_scale = self.device.depth_scale - - glEnableClientState(GL_VERTEX_ARRAY) - - pointcloud = self.device.pointcloud - glVertexPointer(3, GL_FLOAT, 0, pointcloud) - glEnableClientState(GL_COLOR_ARRAY) - depth_to_color = np.zeros( - depth_frame.height * depth_frame.width * 3, np.uint8 - ) - rsutilwrapper.project_pointcloud_to_pixel( - depth_to_color, - self.device.depth_intrinsics, - self.device.color_intrinsics, - extrinsics, - pointcloud, - self._recent_frame.bgr, - ) - glColorPointer(3, GL_UNSIGNED_BYTE, 0, depth_to_color) - glDrawArrays(GL_POINTS, 0, depth_frame.width * depth_frame.height) - gl_utils.glFlush() - glDisable(GL_DEPTH_TEST) - # gl_utils.make_coord_system_norm_based() - glfw.glfwSwapBuffers(self.depth_window) - glfw.glfwMakeContextCurrent(active_window) - - if self.preview_depth and self._recent_depth_frame is not None: - self.g_pool.image_tex.update_from_ndarray(self._recent_depth_frame.bgr) - gl_utils.glFlush() - gl_utils.make_coord_system_norm_based() - self.g_pool.image_tex.draw() - elif self._recent_frame is not None: - self.g_pool.image_tex.update_from_yuv_buffer( - self._recent_frame.yuv_buffer, - self._recent_frame.width, - self._recent_frame.height, - ) - gl_utils.glFlush() - gl_utils.make_coord_system_norm_based() - self.g_pool.image_tex.draw() - - if not self.online: - super().gl_display() - - gl_utils.make_coord_system_pixel_based( - (self.frame_size[1], self.frame_size[0], 3) - ) - - def restart_device( - self, - device_id=None, - color_frame_size=None, - color_fps=None, - depth_frame_size=None, - depth_fps=None, - device_options=None, - ): - if device_id is None: - if self.device is not None: - device_id = self.device.device_id - else: - device_id = 0 - if color_frame_size is None: - color_frame_size = self.frame_size - if color_fps is None: - color_fps = self.frame_rate - if depth_frame_size is None: - depth_frame_size = self.depth_frame_size - if depth_fps is None: - depth_fps = self.depth_frame_rate - if device_options is None: - device_options = self.controls.export_presets() - if self.device is not None: - self.device.stop() - self.device = None - self.service.stop() - self.service.start() - self.notify_all( - { - "subject": "realsense_source.restart", - "device_id": device_id, - "color_frame_size": color_frame_size, - "color_fps": color_fps, - "depth_frame_size": depth_frame_size, - "depth_fps": depth_fps, - "device_options": device_options, - } - ) - - def on_click(self, pos, button, action): - if button == glfw.GLFW_MOUSE_BUTTON_LEFT and action == glfw.GLFW_PRESS: - self.mouse_drag = True - if button == glfw.GLFW_MOUSE_BUTTON_LEFT and action == glfw.GLFW_RELEASE: - self.mouse_drag = False - - def on_notify(self, notification): - if notification["subject"] == "realsense_source.restart": - kwargs = notification.copy() - del kwargs["subject"] - del kwargs["topic"] - self._initialize_device(**kwargs) - elif notification["subject"] == "recording.started": - self.start_depth_recording(notification["rec_path"], notification["start_time_synced"]) - elif notification["subject"] == "recording.stopped": - self.stop_depth_recording() - - def start_depth_recording(self, rec_loc, start_time_synced): - if not self.record_depth: - return - - if self.depth_video_writer is not None: - logger.warning("Depth video recording has been started already") - return - - video_path = os.path.join(rec_loc, "depth.mp4") - self.depth_video_writer = MPEG_Writer(video_path, start_time_synced) - - def stop_depth_recording(self): - if self.depth_video_writer is None: - logger.warning("Depth video recording was not running") - return - - self.depth_video_writer.close() - self.depth_video_writer = None - - @property - def frame_size(self): - stream = self.streams[0] - return stream.width, stream.height - - @frame_size.setter - def frame_size(self, new_size): - if self.device is not None and new_size != self.frame_size: - self.restart_device(color_frame_size=new_size) - - @property - def frame_rate(self): - return self.streams[0].fps - - @frame_rate.setter - def frame_rate(self, new_rate): - if self.device is not None and new_rate != self.frame_rate: - self.restart_device(color_fps=new_rate) - - @property - def depth_frame_size(self): - stream = self.streams[1] - return stream.width, stream.height - - @depth_frame_size.setter - def depth_frame_size(self, new_size): - if self.device is not None and new_size != self.depth_frame_size: - self.restart_device(depth_frame_size=new_size) - - @property - def depth_frame_rate(self): - return self.streams[1].fps - - @depth_frame_rate.setter - def depth_frame_rate(self, new_rate): - if self.device is not None and new_rate != self.depth_frame_rate: - self.restart_device(depth_fps=new_rate) - - @property - def jpeg_support(self): - return False - - @property - def online(self): - return self.device and self.device.is_streaming() - - @property - def name(self): - # not the same as `if self.device:`! - if self.device is not None: - return self.device.name - else: - return "Ghost capture" - - -class Realsense_Manager(Base_Manager): - """Manages Intel RealSense R200 sources - - Attributes: - check_intervall (float): Intervall in which to look for new UVC devices - """ - - gui_name = "RealSense R200" - - def get_init_dict(self): - return {} - - def init_ui(self): - self.add_menu() - from pyglui import ui - - self.menu.append(ui.Info_Text("Intel RealSense R200 sources")) - - def pair(d): - fmt = "- " if d["is_streaming"] else "" - fmt += d["name"] - return d["id"], fmt - - def dev_selection_list(): - default = (None, "Select to activate") - try: - with pyrs.Service() as service: - dev_pairs = [default] + [pair(d) for d in service.get_devices()] - except pyrs.RealsenseError: - dev_pairs = [default] - - return zip(*dev_pairs) - - def activate(source_uid): - if source_uid is None: - return - - # with pyrs.Service() as service: - # if not service.is_device_streaming(source_uid): - # logger.error("The selected camera is already in use or blocked.") - # return - settings = { - "frame_size": self.g_pool.capture.frame_size, - "frame_rate": self.g_pool.capture.frame_rate, - "device_id": source_uid, - } - if self.g_pool.process == "world": - self.notify_all( - { - "subject": "start_plugin", - "name": "Realsense_Source", - "args": settings, - } - ) - else: - self.notify_all( - { - "subject": "start_eye_plugin", - "target": self.g_pool.process, - "name": "Realsense_Source", - "args": settings, - } - ) - - self.menu.append( - ui.Selector( - "selected_source", - selection_getter=dev_selection_list, - getter=lambda: None, - setter=activate, - label="Activate source", - ) - ) - - def deinit_ui(self): - self.remove_menu() diff --git a/pupil_src/shared_modules/video_capture/uvc_backend.py b/pupil_src/shared_modules/video_capture/uvc_backend.py index d623eee9d5..58e119af66 100644 --- a/pupil_src/shared_modules/video_capture/uvc_backend.py +++ b/pupil_src/shared_modules/video_capture/uvc_backend.py @@ -16,14 +16,14 @@ import time import numpy as np -from pyglui import cygl +from pyglui import cygl, ui import gl_utils import uvc from camera_models import load_intrinsics from version_utils import VersionFormat -from .base_backend import Base_Manager, Base_Source, InitialisationError +from .base_backend import Base_Manager, Base_Source, InitialisationError, SourceInfo from .utils import Check_Frame_Stripes, Exposure_Time # check versions for our own depedencies as they are fast-changing @@ -31,7 +31,7 @@ # logging logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) +logger.setLevel(logging.DEBUG) class TJSAMP(enum.IntEnum): @@ -61,9 +61,11 @@ def __init__( uvc_controls={}, check_stripes=True, exposure_mode="manual", + *args, + **kwargs, ): - super().__init__(g_pool) + super().__init__(g_pool, *args, **kwargs) self.uvc_capture = None self._restart_in = 3 assert name or preferred_names or uid @@ -124,9 +126,7 @@ def __init__( # check if we were sucessfull if not self.uvc_capture: - logger.error( - "Init failed. Capture is started in ghost mode. No images will be supplied." - ) + logger.error("Could not connect to device! No images will be supplied.") self.name_backup = preferred_names self.frame_size_backup = frame_size self.frame_rate_backup = frame_rate @@ -382,12 +382,13 @@ def _restart_logic(self): ) except (InitialisationError, uvc.InitError): time.sleep(0.02) - self.update_menu() self._restart_in = int(5 / 0.02) else: self._restart_in -= 1 def recent_events(self, events): + was_online = self.online + try: frame = self.uvc_capture.get_frame(0.05) @@ -422,10 +423,24 @@ def recent_events(self, events): # c930 timestamps need to be set here. The camera does not provide valid pts from device frame.timestamp = uvc.get_time_monotonic() + self.ts_offset frame.timestamp -= self.g_pool.timebase.value + + if ( + self._recent_frame is not None + and frame.timestamp <= self._recent_frame.timestamp + ): + logger.debug( + "Received non-monotonic timestamps from UVC! Dropping frame." + f" Last: {self._recent_frame.timestamp}, current: {frame.timestamp}" + ) + return + self._recent_frame = frame events["frame"] = frame self._restart_in = 3 + if was_online != self.online: + self.update_menu() + def _get_uvc_controls(self): d = {} if self.uvc_capture: @@ -451,7 +466,7 @@ def name(self): if self.uvc_capture: return self.uvc_capture.name else: - return "Ghost capture" + return "(disconnected)" @property def frame_size(self): @@ -563,19 +578,14 @@ def jpeg_support(self): def online(self): return bool(self.uvc_capture) - def deinit_ui(self): - self.remove_menu() - - def init_ui(self): - self.add_menu() - self.menu.label = "Local USB Source: {}".format(self.name) - self.update_menu() + def ui_elements(self): + ui_elements = [] - def update_menu(self): - del self.menu[:] - from pyglui import ui + if self.uvc_capture is None: + ui_elements.append(ui.Info_Text("Local USB: camera disconnected!")) + return ui_elements - ui_elements = [] + ui_elements.append(ui.Info_Text(f"Camera: {self.name} @ Local USB")) # lets define some helper functions: def gui_load_defaults(): @@ -596,12 +606,6 @@ def set_frame_rate(new_rate): self.frame_rate = new_rate self.update_menu() - if self.uvc_capture is None: - ui_elements.append(ui.Info_Text("Capture initialization failed.")) - self.menu.extend(ui_elements) - return - - ui_elements.append(ui.Info_Text("{} Controls".format(self.name))) sensor_control = ui.Growing_Menu(label="Sensor Settings") sensor_control.append( ui.Info_Text("Do not change these during calibration or recording!") @@ -626,6 +630,11 @@ def frame_rate_getter(): [str(fr) for fr in self.uvc_capture.frame_rates], ) + # TODO: potential race condition through selection_getter. Should ensure that + # current selection will always be present in the list returned by the + # selection_getter. Highly unlikely though as this needs to happen between + # having clicked the Selector and the next redraw. + # See https://github.com/pupil-labs/pyglui/pull/112/commits/587818e9556f14bfedd8ff8d093107358745c29b sensor_control.append( ui.Selector( "frame_rate", @@ -660,16 +669,13 @@ def set_exposure_mode(exposure_mode): ) self.update_menu() - def exposure_mode_getter(): - return ["manual", "auto"], ["manual mode", "auto mode"] - sensor_control.append( ui.Selector( "exposure_mode", self, setter=set_exposure_mode, - selection_getter=exposure_mode_getter, - selection=self.exposure_mode, + selection=["manual", "auto"], + labels=["manual mode", "auto mode"], label="Exposure Mode", ) ) @@ -789,7 +795,8 @@ def set_check_stripes(enable_stripe_checks): label="Check Stripes", ) ) - self.menu.extend(ui_elements) + + return ui_elements def cleanup(self): self.devices.cleanup() @@ -828,13 +835,7 @@ def gl_display(self): class UVC_Manager(Base_Manager): - """Manages local USB sources - - Attributes: - check_intervall (float): Intervall in which to look for new UVC devices - """ - - gui_name = "Local USB" + """Manages local USB sources.""" def __init__(self, g_pool): super().__init__(g_pool) @@ -845,40 +846,33 @@ def __init__(self, g_pool): "world": ["ID2", "Logitech"], } - def get_init_dict(self): - return {} + def get_devices(self): + self.devices.update() + if len(self.devices) == 0: + return [] + else: + return [SourceInfo(label="Local USB", manager=self, key="usb")] - def init_ui(self): - self.add_menu() + def get_cameras(self): + self.devices.update() + return [ + SourceInfo( + label=f"{device['name']} @ Local USB", + manager=self, + key=f"cam.{device['uid']}", + ) + for device in self.devices + ] - from pyglui import ui + def activate(self, key): + if key == "usb": + self.notify_all({"subject": "backend.uvc.auto_activate_source"}) + return - self.add_auto_select_button() - ui_elements = [] - ui_elements.append(ui.Info_Text("Local UVC sources")) - - def dev_selection_list(): - default = (None, "Select to activate") - self.devices.update() - dev_pairs = [default] + [ - (d["uid"], d["name"]) - for d in self.devices - if "RealSense" not in d["name"] - ] - return zip(*dev_pairs) + source_uid = key[4:] + self.activate_source(source_uid) - ui_elements.append( - ui.Selector( - "selected_source", - selection_getter=dev_selection_list, - getter=lambda: None, - setter=self.activate, - label="Activate source", - ) - ) - self.menu.extend(ui_elements) - - def activate(self, source_uid): + def activate_source(self, source_uid): if not source_uid: return @@ -909,29 +903,45 @@ def activate(self, source_uid): } ) + def on_notify(self, notification): + """Starts appropriate UVC sources. + + Emits notifications: + ``backend.uvc.auto_activate_source``: All UVC managers should auto activate a source + ``start_(eye_)plugin``: Starts UVC sources + + Reacts to notifications: + ``backend.uvc.auto_activate_source``: Auto activate best source for process + """ + if notification["subject"] == "backend.uvc.auto_activate_source": + self.auto_activate_source() + def auto_activate_source(self): + logger.debug("Auto activating USB source.") + self.devices.update() if not self.devices or len(self.devices) == 0: logger.warning("No default device is available.") return - cam_ids = self.cam_selection_lut[self.g_pool.process] + name_patterns = self.cam_selection_lut[self.g_pool.process] + matching_cams = [ + device + for device in self.devices + if any(pattern in device["name"] for pattern in name_patterns) + ] - for cam_id in cam_ids: - try: - source_id = next(d["uid"] for d in self.devices if cam_id in d["name"]) - self.activate(source_id) - break - except StopIteration: - source_id = None - else: - logger.warning("The default device is not found.") + if not matching_cams: + logger.warning("Could not find default device.") + return - def deinit_ui(self): - self.remove_menu() + # Sorting cams by bus_number increases chances of selecting only cams from the + # same headset when having multiple headsets connected. Note that two headsets + # might have the same bus_number when they share an internal USB bus. + cam = min( + matching_cams, key=lambda device: device.get("bus_number", float("inf")) + ) + self.activate_source(cam["uid"]) def cleanup(self): self.devices.cleanup() self.devices = None - - def recent_events(self, events): - pass diff --git a/pupil_src/shared_modules/video_export/plugins/eye_video_exporter.py b/pupil_src/shared_modules/video_export/plugins/eye_video_exporter.py index 56b7f0e3f9..39fcbebe65 100644 --- a/pupil_src/shared_modules/video_export/plugins/eye_video_exporter.py +++ b/pupil_src/shared_modules/video_export/plugins/eye_video_exporter.py @@ -36,7 +36,9 @@ def __init__(self, g_pool, render_pupil=True): def customize_menu(self): self.menu.label = "Eye Video Exporter" - self.menu.append(ui.Switch("render_pupil", self, label="Render detected pupil")) + self.menu.append( + ui.Switch("render_pupil", self, label="Visualize Pupil Detection") + ) super().customize_menu() def _export_eye_video(self, export_range, export_dir, eye_id): diff --git a/pupil_src/shared_modules/video_export/plugins/world_video_exporter.py b/pupil_src/shared_modules/video_export/plugins/world_video_exporter.py index f8ab8524b2..32c3de9a9c 100644 --- a/pupil_src/shared_modules/video_export/plugins/world_video_exporter.py +++ b/pupil_src/shared_modules/video_export/plugins/world_video_exporter.py @@ -124,7 +124,6 @@ def _export_world_video( from vis_cross import Vis_Cross from vis_light_points import Vis_Light_Points from vis_polyline import Vis_Polyline - from vis_scan_path import Vis_Scan_Path from vis_watermark import Vis_Watermark PID = str(os.getpid()) @@ -141,7 +140,6 @@ def _export_world_video( Vis_Polyline, Vis_Light_Points, Vis_Watermark, - Vis_Scan_Path, Eye_Overlay, Video_Overlay, ], diff --git a/pupil_src/shared_modules/video_overlay/ui/management.py b/pupil_src/shared_modules/video_overlay/ui/management.py index 50006a1b42..2a3593adb6 100644 --- a/pupil_src/shared_modules/video_overlay/ui/management.py +++ b/pupil_src/shared_modules/video_overlay/ui/management.py @@ -125,7 +125,9 @@ def _add_menu_with_general_elements(self): ) ) self._parent_menu.append( - ui.Switch("value", self.plugin().show_ellipses, label="Visualize Ellipses") + ui.Switch( + "value", self.plugin().show_ellipses, label="Visualize Pupil Detection" + ) ) def _add_overlay_menu(self, overlay): diff --git a/pupil_src/shared_modules/video_overlay/utils/image_manipulation.py b/pupil_src/shared_modules/video_overlay/utils/image_manipulation.py index 7883e626a7..73d9f36641 100644 --- a/pupil_src/shared_modules/video_overlay/utils/image_manipulation.py +++ b/pupil_src/shared_modules/video_overlay/utils/image_manipulation.py @@ -83,6 +83,24 @@ def render_pupil(self, image, pupil_position): thickness=-1, ) + eye_ball = pupil_position.get("projected_sphere", None) + if eye_ball is not None: + try: + cv2.ellipse( + image, + center=tuple(int(v) for v in eye_ball["center"]), + axes=tuple(int(v / 2) for v in eye_ball["axes"]), + angle=int(eye_ball["angle"]), + startAngle=0, + endAngle=360, + color=(26, 230, 0, 255 * pupil_position["model_confidence"]), + thickness=2, + ) + except ValueError: + # Happens when converting 'nan' to int + # TODO: Investigate why results are sometimes 'nan' + pass + @staticmethod def get_ellipse_points(e, num_pts=10): c1 = e[0][0] diff --git a/pupil_src/shared_modules/vis_polyline.py b/pupil_src/shared_modules/vis_polyline.py index f403ce86f9..f2af24e42f 100644 --- a/pupil_src/shared_modules/vis_polyline.py +++ b/pupil_src/shared_modules/vis_polyline.py @@ -15,69 +15,235 @@ import cv2 from pyglui import ui +from observable import Observable from methods import denormalize +from data_changed import Listener +from scan_path import ScanPathController +from scan_path.utils import np_denormalize -class Vis_Polyline(Visualizer_Plugin_Base): + +class Vis_Polyline(Visualizer_Plugin_Base, Observable): + order = 0.9 uniqueness = "not_unique" icon_chr = chr(0xE922) icon_font = "pupil_icons" - def __init__(self, g_pool, color=(1.0, 0.0, 0.4, 1.0), thickness=2): + def __init__( + self, g_pool, polyline_style_init_dict={}, scan_path_init_dict={}, **kwargs + ): super().__init__(g_pool) - self.order = 0.9 - self.menu = None - self.r = color[0] - self.g = color[1] - self.b = color[2] - self.a = color[3] - self.thickness = thickness + self.polyline_style_controller = PolylineStyleController( + **polyline_style_init_dict + ) - def recent_events(self, events): - frame = events.get("frame") - if not frame: - return - pts = [ - denormalize(pt["norm_pos"], frame.img.shape[:-1][::-1], flip_y=True) - for pt in events.get("gaze", []) - if pt["confidence"] >= self.g_pool.min_data_confidence - ] - bgra = (self.b * 255, self.g * 255, self.r * 255, self.a * 255) - if pts: - pts = np.array([pts], dtype=np.int32) - cv2.polylines( - frame.img, - pts, - isClosed=False, - color=bgra, - thickness=self.thickness, - lineType=cv2.LINE_AA, - ) + self.scan_path_controller = ScanPathController(g_pool, **scan_path_init_dict) + self.scan_path_controller.add_observer( + "on_update_ui", self._update_scan_path_ui + ) + + self._gaze_changed_listener = Listener( + plugin=self, topic="gaze_positions", rec_dir=g_pool.rec_dir + ) + self._gaze_changed_listener.add_observer( + method_name="on_data_changed", + observer=self.scan_path_controller.on_gaze_data_changed, + ) + + def get_init_dict(self): + return { + "polyline_style_init_dict": self.polyline_style_controller.get_init_dict(), + "scan_path_init_dict": self.scan_path_controller.get_init_dict(), + } def init_ui(self): - self.add_menu() - self.menu.label = "Gaze Polyline" - self.menu.append( - ui.Slider("thickness", self, min=1, step=1, max=15, label="Line thickness") + + polyline_style_thickness_slider = ui.Slider( + "thickness", + self.polyline_style_controller, + min=self.polyline_style_controller.thickness_min, + max=self.polyline_style_controller.thickness_max, + step=self.polyline_style_controller.thickness_step, + label="Line thickness", ) - color_menu = ui.Growing_Menu("Color") - color_menu.collapsed = True - color_menu.append(ui.Info_Text("Set RGB color component values.")) - color_menu.append( - ui.Slider("r", self, min=0.0, step=0.05, max=1.0, label="Red") + polyline_style_color_info_text = ui.Info_Text("Set RGB color component values.") + + polyline_style_color_r_slider = ui.Slider( + "r", + self.polyline_style_controller, + min=self.polyline_style_controller.rgba_min, + max=self.polyline_style_controller.rgba_max, + step=self.polyline_style_controller.rgba_step, + label="Red", ) - color_menu.append( - ui.Slider("g", self, min=0.0, step=0.05, max=1.0, label="Green") + polyline_style_color_g_slider = ui.Slider( + "g", + self.polyline_style_controller, + min=self.polyline_style_controller.rgba_min, + max=self.polyline_style_controller.rgba_max, + step=self.polyline_style_controller.rgba_step, + label="Green", ) - color_menu.append( - ui.Slider("b", self, min=0.0, step=0.05, max=1.0, label="Blue") + polyline_style_color_b_slider = ui.Slider( + "b", + self.polyline_style_controller, + min=self.polyline_style_controller.rgba_min, + max=self.polyline_style_controller.rgba_max, + step=self.polyline_style_controller.rgba_step, + label="Blue", + ) + + scan_path_timeframe_range = ui.Slider( + "timeframe", + self.scan_path_controller, + min=self.scan_path_controller.min_timeframe, + max=self.scan_path_controller.max_timeframe, + step=self.scan_path_controller.timeframe_step, + label="Duration", ) - self.menu.append(color_menu) + + scan_path_doc = ui.Info_Text("Duration of past gaze to include in polyline.") + scan_path_status = ui.Info_Text("") + + polyline_style_color_menu = ui.Growing_Menu("Color") + polyline_style_color_menu.collapsed = True + polyline_style_color_menu.append(polyline_style_color_info_text) + polyline_style_color_menu.append(polyline_style_color_r_slider) + polyline_style_color_menu.append(polyline_style_color_g_slider) + polyline_style_color_menu.append(polyline_style_color_b_slider) + + scan_path_menu = ui.Growing_Menu("Gaze History") + scan_path_menu.collapsed = False + scan_path_menu.append(scan_path_doc) + scan_path_menu.append(scan_path_timeframe_range) + scan_path_menu.append(scan_path_status) + + self.add_menu() + self.menu.label = "Gaze Polyline" + self.menu.append(polyline_style_thickness_slider) + self.menu.append(polyline_style_color_menu) + self.menu.append(scan_path_menu) + + self.scan_path_timeframe_range = scan_path_timeframe_range + self.scan_path_status = scan_path_status + + self._update_scan_path_ui() def deinit_ui(self): self.remove_menu() + self.scan_path_timeframe_range = None + self.scan_path_status = None + + def recent_events(self, events): + self.scan_path_controller.process() + + frame = events.get("frame") + if not frame: + return + + self._draw_polyline_path(frame, events) + # self._draw_scan_path_debug(frame, events) + + def cleanup(self): + self.scan_path_controller.cleanup() + + def _update_scan_path_ui(self): + if self.menu_icon: + self.menu_icon.indicator_stop = self.scan_path_controller.progress + if self.scan_path_status: + self.scan_path_status.text = self.scan_path_controller.status_string + + def _polyline_points(self, image_size, base_gaze_data, scan_path_gaze_data): + if scan_path_gaze_data is not None: + points_fields = ["norm_x", "norm_y"] + gaze_points = scan_path_gaze_data[points_fields] + gaze_points = np.array( + gaze_points.tolist(), dtype=gaze_points.dtype[0] + ) # FIXME: This is a workaround + gaze_points = gaze_points.reshape((-1, len(points_fields))) + gaze_points = np_denormalize(gaze_points, image_size, flip_y=True) + return gaze_points.tolist() + else: + return [ + denormalize(datum["norm_pos"], image_size, flip_y=True) + for datum in base_gaze_data + if datum["confidence"] >= self.g_pool.min_data_confidence + ] + + def _draw_polyline_path(self, frame, events): + pts = self._polyline_points( + image_size=frame.img.shape[:-1][::-1], + base_gaze_data=events.get("gaze", []), + scan_path_gaze_data=self.scan_path_controller.scan_path_gaze_for_frame( + frame + ), + ) + + if not pts: + return + + pts = np.array([pts], dtype=np.int32) + cv2.polylines( + frame.img, + pts, + isClosed=False, + color=self.polyline_style_controller.cv2_bgra, + thickness=self.polyline_style_controller.thickness, + lineType=cv2.LINE_AA, + ) + + def _draw_scan_path_debug(self, frame, events): + from methods import denormalize + from player_methods import transparent_circle + + gaze_data = self.scan_path_controller.scan_path_gaze_for_frame(frame) + + if gaze_data is None: + return + + points_to_draw_count = len(gaze_data) + image_size = frame.img.shape[:-1][::-1] + + for idx, datum in enumerate(gaze_data): + point = (datum["norm_x"], datum["norm_y"]) + point = denormalize(point, image_size, flip_y=True) + + gray = float(idx) / points_to_draw_count + transparent_circle( + frame.img, point, radius=20, color=(gray, gray, gray, 0.9), thickness=2 + ) + + +class PolylineStyleController: + + rgba_min = 0.0 + rgba_max = 1.0 + rgba_step = 0.05 + + thickness_min = 1 + thickness_max = 15 + thickness_step = 1 + + def __init__(self, rgba=(1.0, 0.0, 0.4, 1.0), thickness=2): + self.rgba = rgba + self.thickness = thickness + + @property + def rgba(self): + return (self.r, self.g, self.b, self.a) + + @rgba.setter + def rgba(self, rgba): + self.r = rgba[0] + self.g = rgba[1] + self.b = rgba[2] + self.a = rgba[3] def get_init_dict(self): - return {"color": (self.r, self.g, self.b, self.a), "thickness": self.thickness} + return {"rgba": self.rgba, "thickness": self.thickness} + + @property + def cv2_bgra(self): + return (self.b * 255, self.g * 255, self.r * 255, self.a * 255) diff --git a/pupil_src/shared_modules/vis_scan_path.py b/pupil_src/shared_modules/vis_scan_path.py deleted file mode 100644 index 867ce05bd4..0000000000 --- a/pupil_src/shared_modules/vis_scan_path.py +++ /dev/null @@ -1,144 +0,0 @@ -""" -(*)~--------------------------------------------------------------------------- -Pupil - eye tracking platform -Copyright (C) 2012-2020 Pupil Labs - -Distributed under the terms of the GNU -Lesser General Public License (LGPL v3.0). -See COPYING and COPYING.LESSER for license details. ----------------------------------------------------------------------------~(*) -""" - -import cv2 -from plugin import Analysis_Plugin_Base -import numpy as np -from pyglui import ui -from methods import denormalize, normalize -import logging - -logger = logging.getLogger(__name__) -from copy import deepcopy - - -class Vis_Scan_Path(Analysis_Plugin_Base): - """docstring - using this plugin will extend the recent_gaze_positions by x extra dots from previous frames. - lock recent gaze points onto pixels. - """ - - icon_chr = chr(0xE422) - icon_font = "pupil_icons" - - def __init__(self, g_pool, timeframe=0.5): - super().__init__(g_pool) - # let the plugin work after most other plugins. - self.order = 0.1 - self.menu = None - - # user settings - self.timeframe = timeframe - - # algorithm working data - self.prev_frame_idx = -1 - self.past_gaze_positions = [] - self.prev_gray = None - self.gaze_changed = False - - def on_notify(self, notification): - if notification["subject"] == "gaze_positions_changed": - self.gaze_changed = True - - def recent_events(self, events): - frame = events.get("frame") - if not frame: - return - img = frame.img - img_shape = img.shape[:-1][::-1] # width,height - - succeeding_frame = frame.index - self.prev_frame_idx == 1 - same_frame = frame.index == self.prev_frame_idx - gray_img = frame.gray - - # vars for calcOpticalFlowPyrLK - lk_params = dict( - winSize=(90, 90), - maxLevel=3, - criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 20, 0.03), - ) - - updated_past_gaze = [] - - # lets update past gaze using optical flow: this is like sticking the gaze points onto the pixels of the img. - if self.past_gaze_positions and succeeding_frame: - past_screen_gaze = np.array( - [ - denormalize(ng["norm_pos"], img_shape, flip_y=True) - for ng in self.past_gaze_positions - ], - dtype=np.float32, - ) - new_pts, status, err = cv2.calcOpticalFlowPyrLK( - self.prev_gray, - gray_img, - past_screen_gaze, - None, - minEigThreshold=0.005, - **lk_params - ) - for gaze, new_gaze_pt, s, e in zip( - self.past_gaze_positions, new_pts, status, err - ): - if s: - # print "norm,updated",gaze['norm_gaze'], normalize(new_gaze_pt,img_shape[:-1],flip_y=True) - gaze["norm_pos"] = normalize(new_gaze_pt, img_shape, flip_y=True) - updated_past_gaze.append(gaze) - # logger.debug("updated gaze") - - else: - # logger.debug("dropping gaze") - # Since we will replace self.past_gaze_positions later, - # not appedning tu updated_past_gaze is like deliting this data point. - pass - else: - # we must be seeking, do not try to do optical flow, or pausing: see below. - pass - - if same_frame and not self.gaze_changed: - # paused - # re-use last result - events["gaze"] = self.past_gaze_positions[:] - else: - # trim gaze that is too old - if events["gaze"]: - now = events["gaze"][0]["timestamp"] - cutoff = now - self.timeframe - updated_past_gaze = [ - g for g in updated_past_gaze if g["timestamp"] > cutoff - ] - - # inject the scan path gaze points into recent_gaze_positions - events["gaze"] = updated_past_gaze + events["gaze"] - events["gaze"].sort( - key=lambda x: x["timestamp"] - ) # this may be redundant... - - # update info for next frame. - self.gaze_changed = False - self.prev_gray = gray_img - self.prev_frame_idx = frame.index - self.past_gaze_positions = deepcopy(events["gaze"]) - - def init_ui(self): - self.add_menu() - self.menu.label = "Scan Path" - self.menu.append( - ui.Slider( - "timeframe", self, min=0, step=0.1, max=5, label="duration in sec" - ) - ) - - def deinit_ui(self): - self.remove_menu() - - def get_init_dict(self): - return {"timeframe": self.timeframe} diff --git a/pupil_src/tests/test_roi.py b/pupil_src/tests/test_roi.py new file mode 100644 index 0000000000..f4727caabf --- /dev/null +++ b/pupil_src/tests/test_roi.py @@ -0,0 +1,109 @@ +""" +(*)~--------------------------------------------------------------------------- +Pupil - eye tracking platform +Copyright (C) 2012-2020 Pupil Labs + +Distributed under the terms of the GNU +Lesser General Public License (LGPL v3.0). +See COPYING and COPYING.LESSER for license details. +---------------------------------------------------------------------------~(*) +""" +import pytest + +from roi import RoiModel + + +@pytest.fixture +def invalid_model(): + return RoiModel((0, 0)) + + +def test_fixture_invalid_model(invalid_model): + # ensure fixture is invalid + assert invalid_model.is_invalid() + + +@pytest.fixture +def valid_model(): + return RoiModel((300, 200)) + + +def test_fixture_valid_model(valid_model): + # ensure fixture is valid + assert not valid_model.is_invalid() + + +def test_model_init_validity(): + model = RoiModel((300, 200)) + assert not model.is_invalid() + + # any dimension <= 0 should be invalid + model = RoiModel((0, 0)) + assert model.is_invalid() + model = RoiModel((1, 0)) + assert model.is_invalid() + model = RoiModel((0, 1)) + assert model.is_invalid() + model = RoiModel((1, -100)) + assert model.is_invalid() + model = RoiModel((-100, 1)) + assert model.is_invalid() + + +def test_model_invalidation_by_set(valid_model: RoiModel): + # set_invalid should make the model invalid + valid_model.set_invalid() + assert valid_model.is_invalid() + + +def test_model_invalidation_by_frame_size(valid_model: RoiModel): + # setting invalid frame_size should make the model invalid + valid_model.frame_size = (-1, 100) + assert valid_model.is_invalid() + + +def test_model_revalidation(invalid_model: RoiModel): + # setting valid frame_size should revalidate model + invalid_model.frame_size = (200, 400) + assert not invalid_model.is_invalid() + + +def test_model_init_bounds(): + # initial bounds should be full frame + model = RoiModel((100, 200)) + assert model.bounds == (0, 0, 99, 199) + + +def test_model_revalidation_bounds(invalid_model): + # revalidation should set bounds to full frame + invalid_model.frame_size = (100, 200) + assert invalid_model.bounds == (0, 0, 99, 199) + + +def test_bounds_cutoff(): + model = RoiModel((100, 200)) + + # model bounds should stay within full frame + model.bounds = (-1, -100, 200, 300) + assert model.bounds == (0, 0, 99, 199) + + # model bounds should always have area > 0 + model.bounds = (500, 500, 400, 400) + minx, miny, maxx, maxy = model.bounds + assert 0 <= minx < maxx < 100 and 0 <= miny < maxy < 200 + + # model bounds should always have area > 0 + model.bounds = (-100, -200, -400, -300) + minx, miny, maxx, maxy = model.bounds + assert 0 <= minx < maxx < 100 and 0 <= miny < maxy < 200 + + +def test_frame_size_bounds_scaling(): + model = RoiModel((400, 800)) + model.bounds = (100, 200, 300, 400) + + # bounds should be scaled by frame_size changes + model.frame_size = (800, 400) + assert model.bounds == (200, 100, 600, 200) + model.frame_size = (400, 800) + assert model.bounds == (100, 200, 300, 400) diff --git a/update_license_header.py b/update_license_header.py index dba116b7c0..8b11e34e55 100644 --- a/update_license_header.py +++ b/update_license_header.py @@ -45,7 +45,6 @@ "glfw.py", "version_utils.py", "update_license_header.py", - "shared_cpp*", ] # transform glob patterns to regular expressions