From b0e22e955d566b2a981cb43ae477f2646c0c978d Mon Sep 17 00:00:00 2001 From: Stéphane Lenclud Date: Wed, 27 Mar 2019 21:18:39 +0100 Subject: Create Points Tracker based on original Point Tracker. --- tracker-points/CMakeLists.txt | 8 + tracker-points/FTNoIR_PT_Controls.ui | 1430 +++++++++++++++++++++++++++ tracker-points/Resources/Logo_IR.png | Bin 0 -> 10386 bytes tracker-points/Resources/cap_front.png | Bin 0 -> 1164 bytes tracker-points/Resources/cap_side.png | Bin 0 -> 1733 bytes tracker-points/Resources/clip_front.png | Bin 0 -> 571 bytes tracker-points/Resources/clip_side.png | Bin 0 -> 2677 bytes tracker-points/doc/index.htm | 262 +++++ tracker-points/doc/logo.png | Bin 0 -> 10386 bytes tracker-points/doc/ptrack.ico | Bin 0 -> 4286 bytes tracker-points/doc/settings1.png | Bin 0 -> 25013 bytes tracker-points/doc/settings2.png | Bin 0 -> 26841 bytes tracker-points/doc/settings3.png | Bin 0 -> 29547 bytes tracker-points/doc/style.css | 131 +++ tracker-points/ftnoir_tracker_pt.cpp | 229 +++++ tracker-points/ftnoir_tracker_pt.h | 81 ++ tracker-points/ftnoir_tracker_pt_dialog.cpp | 282 ++++++ tracker-points/ftnoir_tracker_pt_dialog.h | 55 ++ tracker-points/lang/nl_NL.ts | 290 ++++++ tracker-points/lang/ru_RU.ts | 295 ++++++ tracker-points/lang/stub.ts | 290 ++++++ tracker-points/lang/zh_CN.ts | 290 ++++++ tracker-points/module/CMakeLists.txt | 6 + tracker-points/module/Resources/Logo_IR.png | Bin 0 -> 10386 bytes tracker-points/module/camera.cpp | 152 +++ tracker-points/module/camera.h | 55 ++ tracker-points/module/export.hpp | 11 + tracker-points/module/frame.cpp | 80 ++ tracker-points/module/frame.hpp | 44 + tracker-points/module/lang/nl_NL.ts | 11 + tracker-points/module/lang/ru_RU.ts | 11 + tracker-points/module/lang/stub.ts | 11 + tracker-points/module/lang/zh_CN.ts | 11 + tracker-points/module/module.cpp | 72 ++ tracker-points/module/module.hpp | 20 + tracker-points/module/point_extractor.cpp | 387 ++++++++ tracker-points/module/point_extractor.h | 58 ++ tracker-points/module/tracker_pt.qrc | 5 + tracker-points/point_tracker.cpp | 364 +++++++ tracker-points/point_tracker.h | 88 ++ tracker-points/pt-api.cpp | 54 + tracker-points/pt-api.hpp | 123 +++ tracker-points/pt-settings.hpp | 73 ++ tracker-points/tracker_pt_base.qrc | 8 + 44 files changed, 5287 insertions(+) create mode 100644 tracker-points/CMakeLists.txt create mode 100644 tracker-points/FTNoIR_PT_Controls.ui create mode 100644 tracker-points/Resources/Logo_IR.png create mode 100644 tracker-points/Resources/cap_front.png create mode 100644 tracker-points/Resources/cap_side.png create mode 100644 tracker-points/Resources/clip_front.png create mode 100644 tracker-points/Resources/clip_side.png create mode 100644 tracker-points/doc/index.htm create mode 100644 tracker-points/doc/logo.png create mode 100644 tracker-points/doc/ptrack.ico create mode 100644 tracker-points/doc/settings1.png create mode 100644 tracker-points/doc/settings2.png create mode 100644 tracker-points/doc/settings3.png create mode 100644 tracker-points/doc/style.css create mode 100644 tracker-points/ftnoir_tracker_pt.cpp create mode 100644 tracker-points/ftnoir_tracker_pt.h create mode 100644 tracker-points/ftnoir_tracker_pt_dialog.cpp create mode 100644 tracker-points/ftnoir_tracker_pt_dialog.h create mode 100644 tracker-points/lang/nl_NL.ts create mode 100644 tracker-points/lang/ru_RU.ts create mode 100644 tracker-points/lang/stub.ts create mode 100644 tracker-points/lang/zh_CN.ts create mode 100644 tracker-points/module/CMakeLists.txt create mode 100644 tracker-points/module/Resources/Logo_IR.png create mode 100644 tracker-points/module/camera.cpp create mode 100644 tracker-points/module/camera.h create mode 100644 tracker-points/module/export.hpp create mode 100644 tracker-points/module/frame.cpp create mode 100644 tracker-points/module/frame.hpp create mode 100644 tracker-points/module/lang/nl_NL.ts create mode 100644 tracker-points/module/lang/ru_RU.ts create mode 100644 tracker-points/module/lang/stub.ts create mode 100644 tracker-points/module/lang/zh_CN.ts create mode 100644 tracker-points/module/module.cpp create mode 100644 tracker-points/module/module.hpp create mode 100644 tracker-points/module/point_extractor.cpp create mode 100644 tracker-points/module/point_extractor.h create mode 100644 tracker-points/module/tracker_pt.qrc create mode 100644 tracker-points/point_tracker.cpp create mode 100644 tracker-points/point_tracker.h create mode 100644 tracker-points/pt-api.cpp create mode 100644 tracker-points/pt-api.hpp create mode 100644 tracker-points/pt-settings.hpp create mode 100644 tracker-points/tracker_pt_base.qrc (limited to 'tracker-points') diff --git a/tracker-points/CMakeLists.txt b/tracker-points/CMakeLists.txt new file mode 100644 index 00000000..b9fcca9e --- /dev/null +++ b/tracker-points/CMakeLists.txt @@ -0,0 +1,8 @@ +find_package(OpenCV QUIET) +if(OpenCV_FOUND) + otr_module(tracker-points-base STATIC) + target_include_directories(${self} SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS}) + target_link_libraries(${self} opencv_imgproc opentrack-cv opencv_core opentrack-video) + #set_property(TARGET ${self} PROPERTY OUTPUT_NAME "points-base") +endif() +add_subdirectory(module) diff --git a/tracker-points/FTNoIR_PT_Controls.ui b/tracker-points/FTNoIR_PT_Controls.ui new file mode 100644 index 00000000..061f5351 --- /dev/null +++ b/tracker-points/FTNoIR_PT_Controls.ui @@ -0,0 +1,1430 @@ + + + UICPTClientControls + + + Qt::NonModal + + + + 0 + 0 + 418 + 724 + + + + + 0 + 0 + + + + PointTracker Settings + + + + :/Resources/Logo_IR.png:/Resources/Logo_IR.png + + + Qt::LeftToRight + + + false + + + + QLayout::SetFixedSize + + + + + + 0 + 0 + + + + Status + + + + + + + 0 + 0 + + + + Extracted Points: + + + + + + + + 0 + 0 + + + + Camera Info: + + + + + + + + 0 + 0 + + + + + + + + + + + + 0 + 0 + + + + + + + + + + + + + + + 0 + 0 + + + + + + + 0 + + + + Camera + + + + + + + 0 + 0 + + + + Camera settings + + + + + + + 0 + 0 + + + + 10 + + + + + + + + 0 + 0 + + + + ° + + + + + + 10 + + + 90 + + + + + + + + 0 + 0 + + + + Diagonal field of view + + + + + + + + 0 + 0 + + + + Width + + + + + + + + 0 + 0 + + + + FPS + + + fps_spin + + + + + + + + 0 + 0 + + + + Desired capture height + + + px + + + 2000 + + + 10 + + + + + + + + 0 + 0 + + + + Dynamic pose timeout + + + + + + + + 0 + 0 + + + + Desired capture framerate + + + Hz + + + 2000 + + + + + + + + 0 + 0 + + + + + + + + + + + + 0 + 0 + + + + Desired capture width + + + px + + + 2000 + + + 10 + + + + + + + + 0 + 0 + + + + Height + + + + + + + + 0 + 0 + + + + ms + + + 50 + + + 5000 + + + + + + + + 0 + 0 + + + + Device + + + camdevice_combo + + + + + + + + 0 + 0 + + + + Open + + + + + + + + 0 + 0 + + + + Camera settings (when available) + + + + + + + + 0 + 0 + + + + Color channels used + + + + + + + + 0 + 0 + + + + + Average + + + + + Natural + + + + + Red only + + + + + Green only + + + + + Blue only + + + + + + + + + 0 + 0 + + + + Dynamic pose (for caps only, never clips) + + + + + + + + + + + 0 + 0 + + + + Point extraction + + + + + + + 0 + 0 + + + + Threshold + + + threshold_slider + + + + + + + + 0 + 0 + + + + Min size + + + mindiam_spin + + + + + + + + 0 + 0 + + + + Max size + + + maxdiam_spin + + + + + + + + 0 + 0 + + + + Intensity threshold for point extraction + + + 255 + + + 1 + + + 127 + + + Qt::Horizontal + + + QSlider::TicksBothSides + + + 25 + + + + + + + + 0 + 0 + + + + Enable, slider sets point size + + + + + + + + 0 + 0 + + + + Automatic threshold + + + + + + + + 0 + 0 + + + + Maximum point diameter + + + px + + + 1 + + + 0.100000000000000 + + + + + + + + 0 + 0 + + + + Minimum point diameter + + + px + + + 1 + + + 0.100000000000000 + + + + + + + + 0 + 0 + + + + + + + + + + + Value + + + + + + + + + + + Model + + + + + + + 0 + 0 + + + + QTabWidget::Rounded + + + 0 + + + false + + + false + + + false + + + + Clip + + + + + + + 0 + 0 + + + + + 331 + 208 + + + + Model Dimensions + + + + + 70 + 35 + 100 + 22 + + + + mm + + + -65535 + + + 65535 + + + + + + 150 + 130 + 100 + 22 + + + + mm + + + -65535 + + + 65535 + + + + + + 65 + 55 + 71 + 111 + + + + + + + :/Resources/clip_side.png + + + + + + 20 + 40 + 46 + 13 + + + + Side + + + + + + 50 + 160 + 100 + 22 + + + + mm + + + -65535 + + + 65535 + + + + + + 150 + 70 + 100 + 22 + + + + mm + + + -65535 + + + 65535 + + + + + + 290 + 40 + 46 + 13 + + + + Front + + + + + + 300 + 70 + 21 + 111 + + + + + + + :/Resources/clip_front.png + + + + + + + + + Cap + + + + + + + 331 + 208 + + + + Model Dimensions + + + + + 100 + 60 + 111 + 81 + + + + + + + :/Resources/cap_side.png + + + + + + 20 + 40 + 46 + 13 + + + + Side + + + + + + 90 + 40 + 101 + 22 + + + + mm + + + -65535 + + + 65535 + + + + + + 220 + 100 + 81 + 81 + + + + + + + :/Resources/cap_front.png + + + + + + 240 + 70 + 81 + 22 + + + + mm + + + -65535 + + + 65535 + + + + + + 240 + 40 + 46 + 13 + + + + Front + + + + + + 20 + 90 + 81 + 22 + + + + mm + + + -65535 + + + 65535 + + + + + + + + + Custom + + + + + + Model Dimensions + + + + + + + 0 + 0 + + + + z: + + + + + + + mm + + + -65535 + + + 65535 + + + + + + + + 0 + 0 + + + + x: + + + + + + + mm + + + -65535 + + + 65535 + + + + + + + mm + + + -65535 + + + 65535 + + + + + + + mm + + + -65535 + + + 65535 + + + + + + + mm + + + -65535 + + + 65535 + + + + + + + + 0 + 0 + + + + <html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p><p>Use any units you want, not necessarily centimeters.</p></body></html> + + + + + + + mm + + + -65535 + + + 65535 + + + + + + + + 0 + 0 + + + + y: + + + + + + + + 0 + 0 + + + + x: + + + + + + + + 0 + 0 + + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">3</span></p></body></html> + + + + + + + + 0 + 0 + + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">2</span></p></body></html> + + + + + + + + 0 + 0 + + + + z: + + + + + + + + 0 + 0 + + + + y: + + + + + + + + + + + + + + + 0 + 0 + + + + Model position + + + + + + QFrame::NoFrame + + + QFrame::Raised + + + + + + mm + + + -65535 + + + 65536 + + + + + + + + 0 + 0 + + + + z: + + + + + + + mm + + + -65535 + + + 65536 + + + + + + + + 0 + 0 + + + + x: + + + + + + + mm + + + -65535 + + + 65536 + + + + + + + + 0 + 0 + + + + y: + + + + + + + + + + QFrame::NoFrame + + + QFrame::Raised + + + + + + Use only yaw and pitch while calibrating. +Don't roll or change position. + + + Qt::AlignCenter + + + true + + + false + + + + + + + + 0 + 0 + + + + + + + true + + + + + + + false + + + Start calibration + + + true + + + + + + + + + + + + + + About + + + + + + <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 1.1</span></p><p><span style=" font-weight:600;">by Patrick Ruoff</span></p><p><a href="http://ftnoirpt.sourceforge.net/"><span style=" font-weight:600; text-decoration: underline; color:#0000ff;">Manual (external)</span></a></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + + + + + :/Resources/Logo_IR.png + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + + + + + + + + + + 0 + 0 + + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok + + + + + + + tabWidget + camdevice_combo + res_x_spin + res_y_spin + fps_spin + fov + dynamic_pose + init_phase_timeout + camera_settings + blob_color + auto_threshold + threshold_slider + mindiam_spin + maxdiam_spin + model_tabs + clip_tlength_spin + clip_theight_spin + clip_bheight_spin + clip_blength_spin + cap_length_spin + cap_height_spin + cap_width_spin + m1x_spin + m1y_spin + m1z_spin + m2x_spin + m2y_spin + m2z_spin + tx_spin + ty_spin + tz_spin + tcalib_button + + + + + + + + + + + + + + + + startEngineClicked() + stopEngineClicked() + cameraSettingsClicked() + + diff --git a/tracker-points/Resources/Logo_IR.png b/tracker-points/Resources/Logo_IR.png new file mode 100644 index 00000000..95032a25 Binary files /dev/null and b/tracker-points/Resources/Logo_IR.png differ diff --git a/tracker-points/Resources/cap_front.png b/tracker-points/Resources/cap_front.png new file mode 100644 index 00000000..14207a67 Binary files /dev/null and b/tracker-points/Resources/cap_front.png differ diff --git a/tracker-points/Resources/cap_side.png b/tracker-points/Resources/cap_side.png new file mode 100644 index 00000000..5ad4ee65 Binary files /dev/null and b/tracker-points/Resources/cap_side.png differ diff --git a/tracker-points/Resources/clip_front.png b/tracker-points/Resources/clip_front.png new file mode 100644 index 00000000..04880138 Binary files /dev/null and b/tracker-points/Resources/clip_front.png differ diff --git a/tracker-points/Resources/clip_side.png b/tracker-points/Resources/clip_side.png new file mode 100644 index 00000000..72667ac7 Binary files /dev/null and b/tracker-points/Resources/clip_side.png differ diff --git a/tracker-points/doc/index.htm b/tracker-points/doc/index.htm new file mode 100644 index 00000000..87b7356f --- /dev/null +++ b/tracker-points/doc/index.htm @@ -0,0 +1,262 @@ + + + + + FTNoIR PointTracker Help + + + + + + + + + + + + + + +
+

FaceTrackNoIR PointTracker Plugin

PointTracker Plugin Logo
+ + +

About

+
+

+PointTracker is a plugin for the free head tracking software FaceTrackNoIR +which introduces the capability to track a (typically IR-) point model comprising 3 bright points to FaceTrackNoIR, +much like the popular free tracking software Freetrack does.
+It was created as a stable modular alternative to Freetrack, which has some stability issues with newer systems and seems to be no longer actively developped. +

+
+ + +

Settings

+
+

+This section desribes the various settings of the PointTracker plugin in detail. +

+ +Settings Pane 1 +
+
Show VideoWidget
Whether the video widget is updated or not. It may save some performance to turn this off when not needed
+
Sleep time
Time the tracking thread sleeps after each processed image. It's inverse should be below the framefrate you want to achieve. +(check the framerate in the status region when tracker is active, in case the sleep time is too high, the framerate will decrease). +Low values will result in more CPU-load.
+
Dynamic Pose Resolution
Whether the point correspondence and pose ambiquity is resolved using a more sophisticated dynamic algorithm (constant velocity prediction) or a simple static resolution. +Dynamic pose resolution can capture more extreme poses but may occasionally get stuck in a wrong pose estimates so that a reset of the internal state becomes neccessary.
+
Auto-reset time
If no valid tracking result can be found when using dynamic pose resolution, the tracker will automatically reset its internal state (used for resolving the pose ambiguity and point correspondence) +and return to a fail-safe initialization phase that assumes a neutral pose after this time. +Decrease this time, if you get stuck in a wrong pose too often.
+
Reset
Manually reset the trackers internal state used for dynamic pose resolution and return to a fail-safe initialization phase that assumes a neutral pose. +You may use this in case you get stuck in a wrong pose.
+
Enable Axis ...
Which axis to use for FTNoIR.
+
+ +Settings Pane 2 +
+
Device
The camera used for tracking.
+
Resolution
The desired capture resolution. If your camera does not support the entered resolution the true output resolution may be different or even invalid. +You may check the true capture resolution in the status area while the tracker is running. A higher resolution results in more accurate point positions and will increase the +stability of the tracking result, as long as the signal/noise ratio is sufficiently high.
+
FPS
The desired capture framerate. Again, if your camera does not support the entered framerate, the true caputre framerate may be different or invalid. +You may check the true processing framerate in the status area while the tracker is running.
+
F/W
The focal length of the camera divided by the sensor width (of course in the same units). +In case you don't have access to your camera's specifications, you can measure this yourself by placing a plane object of known width (for example a piece of cardboard) in front of the camera until it fills the whole image width. +Then measure the distance between the object and the camera and divide by the object width.
+
VideoWidget
Shows a resizable stand-alone video widget that shows the same content as the integrated video widget in FTNoIR. +Update rate is only 10 fps and may lag behind a bit. Mainly useful during calibration of the point extraction. Same as for the integrated wiget, to save resources, this widget should only be shown when needed.
+
Roll Pitch Yaw...
The orientation of the camera relative to the reference frame. +If these angles are setup properly, the direction of translations may not be correct. +Roll is treated in a special way since it is implemented as a frame rotation by +/- 90 deg that is transparent to the rest of the processing pipeline. +
+
Threshold
The threshold for point recognition. Areas above the threshold are shown in blue in the VideoWidget. +Since point accuracy is best if the points are as big as possible in pixels, the theshold should be chosen as low as possible (stop before the contour of the points becomes "noisy"). +If small reflections are being falsely classified as points, increasing the minimum point diameter (see below) may help.
+
Min Diameter
Minimum diameter of blobs to be classified as a pointmodel-point.
+
Max Diameter
Maximum diameter of blobs to be classified as a pointmodel-point.
+
Status
The tracker's status is shown in this area while the tracker is running. +The FPS shown here correspond to the framerate of the whole tracker processing chain and may be lower than what your camera is able to provide, when
+1. The processing gets not enough CPU time
+2. The sleep time of the tracking thread is set too high
+
+ +Settings Pane 3 +
+
Model Selection and Dimensions ...
+First select your model type (point, clip, custom), then enter the dimensions of your model in milimeters here.
+For the custom setting, the coordinates of the two remaining model points have to be entered (reference point M0 is at (0,0,0)) in a pose where the model roughly faces the camera. +For orientation, the coordinates for the standard Freetrack clip are (0,40,-30), (0,-70,-80) and the ones for the cap (40,-60,-100), (-40,-60,-100).
+When using a custom point-model configuration, the following restrictions should be observed:
+The plane in which the 3 points lie should never be parallel to the image plane, M0-M1 and M0-M2 should be roughly perpendicular.
+ +
Model Position
The vector from the model to the center of the head in the model frame. Can be calibrated automatically.
+
Calibrate
In order to automatically calibrate the model-head offset, do the following:
Press the Calibrate button, then look around while not moving your shoulder. (i.e. only rotation, no translation). +Do not stay in one pose for too long. The current translation estimate will be updated in real time. As soon as the values stabilized sufficiently, press the Calibrate button again to stop the calibration process.
+
+
+ + +

Filter Setup

+
+

+This section desribes how the FTNoIR filter work and what the recommended settings for PointTracker are. +

+

+Filtering is always a tradeoff between stability, accuracy and responsiveness. +

+

+The Smoothing filter in FTNoIR is just a simple average over the last n samples. +Since this filter produces input lag no matter how fast the head-movements are, it is recommended to turn it off by setting samples to 1. +

+

+In the filter tab, it is recommended to select Accela Filter Mk2. +Accela is a non-linear filter that works as follows:
+It looks at the difference between the new raw values new_val from the tracker and the last filtered value old_val +and maps this difference via the customizable response function f via:
+

+

+new_val = old_val + f(new_val - old_val) / reduction_factor +

+

+So by setting f(x) = reduction_factor * x, one will get no filtering at all.
+If you set lower values for small x, small deviations (usually noise) will get dampened. +This results in a dynamic dead-zone around the current position. +

+

+The last two points are used by accela to extrapolate for large deviations. +So in order to get a fast unfiltered response for large deviations, the line connecting the last two points should have a slope >= reduction_factor. +

+

+More aggressive accela settings than the default FTNoIR accela settings are recommended in order to decrease the filtering lag and fully use the potential of point tracking.
+My current settings are: +

+

+[Accela]
+Reduction=20
+
+[Curves-Accela-Scaling-Rotation]
+point-count=4
+point-0-x=0.1
+point-0-y=0
+point-1-x=1.43
+point-1-y=2.45
+point-2-x=2.0
+point-2-y=5.44
+point-3-x=2.06
+point-3-y=6
+
+

+The curve is not too different from the standard one (except that I like a small dynamic dead zone for steady aiming, that's why the curve has a slope of 0 at the beginning).
+However, the reduction factor is decreased to a value of 20 (compared to the standard value of 100). This implies that each value of the curve is effectively 5 times higher than in standard FTNoIR (see formula above), which means higher responsiveness but can also lead to jitter/shaking.
+Keep in mind that there are no best filter settings. Since filtering is always a compromise it's a matter of personal taste and +playing around with the filter settings is highly recommended. +

+
+ + +

Support

+
+

+For questions/feedback about the plugin, post to the FTNoIR-Forum.
+In case you like this plugin and would like to support the author, you may consider making a donation. +

+
+
+
+ + + + +
+
+
+
+ + +

ChangeLog

+
+

1.1

+
    +
  • Added camera yaw and roll correction (intended for vertically mounted cameras)
    • +
  • Improved point extraction algorithm, thanks to Michael Welter
    • +
  • UI improvements: Select camera by device name, different VideoWidget architecture
    • +
  • Bugfixes: Removed 99 FPS limitation
    • +
+ +

1.0

+
    +
  • Added camera pitch correction
    • +
  • Better communication with FTNoIR: output axis configuration, status report
    • +
+ +

1.0 beta

+
    +
  • Switchted to videoInput library for capture. Desired capture resolution and fps can now be customized
    • +
  • Introduced dynamic point-correspondence and POSIT-ambiguity resolution, which allows for the reconstruction of more extreme poses
    • +
  • More convenient freetrack-like model dimension GUI
    • +
  • Bugfixes: VideoWidget skipping frames, Timer resolution too low for accurate FPS measurement
    • +
+
+ + +

Build Instructions

+
+

+This section describes what you need to do in order to build PointTracker yourself.
+You can find the sources at the project site +or as part of the FTNoIR sources. +

+

The project was created with Visual Studio.

+ +

Dependencies

+
    +
  • Qt 4.8.2 library
    • +
  • Qt plugin for Visual studio
    • +
  • OpenCV 2.4 prebuilt for Windows
    • +
  • Boost 1.47
    • +
+ +

Details

+
+

Common

+
    +
  • setup environment variable "QTDIR" (example value "D:\Devel\Libs\Qt\4.8.2")
    • +
  • add "%QTDIR%\bin" to PATH
    • +
  • setup environment variable "BOOST_DIR" (example value "D:\Devel\Libs\boost_1_47_0")
    • +
  • setup environment variable "OPENCV_DIR" (example value "D:\Devel\Libs\opencv\build")
    • +
+

Debug

+

opencv linked dynamically:

+
    +
  • add "%OPENCV_DIR%\x86\vc9\bin" to PATH
    • +
+

(in case of different Visual studio, change PATH and linker dependencies accordingly)

+

Release

+

opencv linked statically:

+
    +
  • custom build a statically linked version of opencv with the buil-option BUILD_WITH_STATIC_CRT set to OFF!
    • +
  • copy resulting libaries to "%OPENCV_DIR%\x86\vc9\static_lib"
    • +
+

(in case of different Visual studio, change PATH and linker dependencies accordingly)

+
+
+ +
+ + + \ No newline at end of file diff --git a/tracker-points/doc/logo.png b/tracker-points/doc/logo.png new file mode 100644 index 00000000..95032a25 Binary files /dev/null and b/tracker-points/doc/logo.png differ diff --git a/tracker-points/doc/ptrack.ico b/tracker-points/doc/ptrack.ico new file mode 100644 index 00000000..c4b2aedc Binary files /dev/null and b/tracker-points/doc/ptrack.ico differ diff --git a/tracker-points/doc/settings1.png b/tracker-points/doc/settings1.png new file mode 100644 index 00000000..35b84c5c Binary files /dev/null and b/tracker-points/doc/settings1.png differ diff --git a/tracker-points/doc/settings2.png b/tracker-points/doc/settings2.png new file mode 100644 index 00000000..c6cfd1f3 Binary files /dev/null and b/tracker-points/doc/settings2.png differ diff --git a/tracker-points/doc/settings3.png b/tracker-points/doc/settings3.png new file mode 100644 index 00000000..5922403d Binary files /dev/null and b/tracker-points/doc/settings3.png differ diff --git a/tracker-points/doc/style.css b/tracker-points/doc/style.css new file mode 100644 index 00000000..0c3d29a6 --- /dev/null +++ b/tracker-points/doc/style.css @@ -0,0 +1,131 @@ +body { + width: 1000px; + font-size: 13px; + color: #000000; + padding: 0; + margin: 0 auto; + background: #444444; + font-family: verdana,arial; +} + +table { + border-width: 3px; + border-color: #0000FF; + border-style: ridge; + margin-top: 5px; + background-color: #E0E0FF; +} + +table.blind { + border: none; + background-color: #E6E6E6; +} + +fieldset.blind { + border: none; +} + +h1 { font-size: 160%; } +h2 { font-size: 140%; } +h3 { font-size: 115%; } + +.indent { + margin-left: 25px; +} + +p +{ + margin-left: 10px; +} + +li +{ + margin: 10px; +} + + +dl +{ + /*width: 80%;*/ + border-bottom: 1px solid #999; +} + +dt +{ + padding-top: 5px; + font-weight: bold; + border-top: 1px solid #999; +} + +dd +{ + padding: 5px; +} + + +hr { + color: #688938; +} + +a:link, a:visited { + color: #0000BF; +} +a:hover { + color: #0000FF; +} + +a.nav { + position: relative; + top: -30px; + display: block; + visibility: hidden; +} + +#navbar { + width: 1000px; + height: 30px; + background-color:#1a1a1b; + position: fixed; + margin: 0 auto; + padding: 0; +} + +#navbar ul +{ + list-style-type: none; + margin: 0 auto; + padding: 0; + overflow: hidden; +} + +#navbar li +{ + margin: 0 auto; + padding: 5px; + float:left; +} + +#navbar a:link,a:visited +{ + display:block; + width:150px; + font-weight:bold; + color:#e85d02; + text-align:center; + /*padding:4px;*/ + text-decoration:none; + /*text-transform:uppercase;*/ +} + +#navbar a:hover,a:active +{ + color:#ffffff; +} + +#content { + background-color:#ffffff; + padding: 15px; + padding-top: 40px; + padding-right: 40px; + margin: 0 auto; +} diff --git a/tracker-points/ftnoir_tracker_pt.cpp b/tracker-points/ftnoir_tracker_pt.cpp new file mode 100644 index 00000000..ca228a24 --- /dev/null +++ b/tracker-points/ftnoir_tracker_pt.cpp @@ -0,0 +1,229 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2016 Stanislaw Halik + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "ftnoir_tracker_pt.h" +#include "video/video-widget.hpp" +#include "compat/math-imports.hpp" +#include "compat/check-visible.hpp" + +#include "pt-api.hpp" + +#include +#include +#include +#include + +using namespace options; + +namespace pt_impl { + +Tracker_PT::Tracker_PT(pointer const& traits) : + traits { traits }, + s { traits->get_module_name() }, + point_extractor { traits->make_point_extractor() }, + camera { traits->make_camera() }, + frame { traits->make_frame() }, + preview_frame { traits->make_preview(preview_width, preview_height) } +{ + cv::setBreakOnError(true); + cv::setNumThreads(1); + + connect(s.b.get(), &bundle_::saving, this, &Tracker_PT::maybe_reopen_camera, Qt::DirectConnection); + connect(s.b.get(), &bundle_::reloading, this, &Tracker_PT::maybe_reopen_camera, Qt::DirectConnection); + + connect(&s.fov, value_::value_changed(), this, &Tracker_PT::set_fov, Qt::DirectConnection); + set_fov(s.fov); +} + +Tracker_PT::~Tracker_PT() +{ + requestInterruption(); + wait(); + + QMutexLocker l(&camera_mtx); + camera->stop(); +} + +void Tracker_PT::run() +{ + maybe_reopen_camera(); + + while(!isInterruptionRequested()) + { + pt_camera_info info; + bool new_frame = false; + + { + QMutexLocker l(&camera_mtx); + + if (camera) + std::tie(new_frame, info) = camera->get_frame(*frame); + } + + if (new_frame) + { + const bool preview_visible = check_is_visible(); + + if (preview_visible) + *preview_frame = *frame; + + point_extractor->extract_points(*frame, *preview_frame, points); + point_count.store(points.size(), std::memory_order_relaxed); + + const bool success = points.size() >= PointModel::N_POINTS; + + Affine X_CM; + + { + QMutexLocker l(¢er_lock); + + if (success) + { + int dynamic_pose_ms = s.dynamic_pose && s.active_model_panel != PointModel::Clip + ? s.init_phase_timeout + : 0; + + point_tracker.track(points, + PointModel(s), + info, + dynamic_pose_ms); + ever_success.store(true, std::memory_order_relaxed); + } + + QMutexLocker l2(&data_lock); + X_CM = point_tracker.pose(); + } + + if (preview_visible) + { + const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y); + Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z)); + Affine X_GH = X_CM * X_MH; + vec3 p = X_GH.t; // head (center?) position in global space + + if (p[2] > f(.1)) + preview_frame->draw_head_center((p[0] * fx) / p[2], (p[1] * fx) / p[2]); + + widget->update_image(preview_frame->get_bitmap()); + + auto [ w, h ] = widget->preview_size(); + if (w != preview_width || h != preview_height) + { + preview_width = w; preview_height = h; + preview_frame = traits->make_preview(w, h); + } + } + } + } +} + +bool Tracker_PT::maybe_reopen_camera() +{ + QMutexLocker l(&camera_mtx); + + return camera->start(s.camera_name, + s.cam_fps, s.cam_res_x, s.cam_res_y); +} + +void Tracker_PT::set_fov(int value) +{ + QMutexLocker l(&camera_mtx); + camera->set_fov(value); +} + +module_status Tracker_PT::start_tracker(QFrame* video_frame) +{ + //video_frame->setAttribute(Qt::WA_NativeWindow); + + widget = std::make_unique(video_frame); + layout = std::make_unique(video_frame); + layout->setContentsMargins(0, 0, 0, 0); + layout->addWidget(widget.get()); + video_frame->setLayout(layout.get()); + //video_widget->resize(video_frame->width(), video_frame->height()); + video_frame->show(); + + start(QThread::HighPriority); + + return {}; +} + +void Tracker_PT::data(double *data) +{ + if (ever_success.load(std::memory_order_relaxed)) + { + Affine X_CM; + { + QMutexLocker l(&data_lock); + X_CM = point_tracker.pose(); + } + + Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z)); + Affine X_GH(X_CM * X_MH); + + // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame + // -z -> x, y -> z, x -> -y + mat33 R_EG(0, 0,-1, + -1, 0, 0, + 0, 1, 0); + mat33 R(R_EG * X_GH.R * R_EG.t()); + + // get translation(s) + const vec3& t = X_GH.t; + + // extract rotation angles + auto r00 = (double)R(0, 0); + auto r10 = (double)R(1,0), r20 = (double)R(2,0); + auto r21 = (double)R(2,1), r22 = (double)R(2,2); + + double beta = atan2(-r20, sqrt(r21*r21 + r22*r22)); + double alpha = atan2(r10, r00); + double gamma = atan2(r21, r22); + + constexpr double rad2deg = 180/M_PI; + + data[Yaw] = rad2deg * alpha; + data[Pitch] = -rad2deg * beta; + data[Roll] = rad2deg * gamma; + + // convert to cm + data[TX] = (double)t[0] / 10; + data[TY] = (double)t[1] / 10; + data[TZ] = (double)t[2] / 10; + } +} + +bool Tracker_PT::center() +{ + QMutexLocker l(¢er_lock); + + point_tracker.reset_state(); + return false; +} + +int Tracker_PT::get_n_points() +{ + return (int)point_count.load(std::memory_order_relaxed); +} + +bool Tracker_PT::get_cam_info(pt_camera_info& info) +{ + QMutexLocker l(&camera_mtx); + bool ret; + + std::tie(ret, info) = camera->get_info(); + return ret; +} + +Affine Tracker_PT::pose() const +{ + QMutexLocker l(&data_lock); + return point_tracker.pose(); +} + +} // ns pt_impl diff --git a/tracker-points/ftnoir_tracker_pt.h b/tracker-points/ftnoir_tracker_pt.h new file mode 100644 index 00000000..210c6a01 --- /dev/null +++ b/tracker-points/ftnoir_tracker_pt.h @@ -0,0 +1,81 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2016 Stanislaw Halik + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once + +#include "api/plugin-api.hpp" +#include "pt-api.hpp" +#include "point_tracker.h" +#include "cv/numeric.hpp" +#include "video/video-widget.hpp" + +#include +#include +#include + +#include + +#include +#include +#include + +namespace pt_impl { + +class TrackerDialog_PT; + +using namespace numeric_types; + +struct Tracker_PT : QThread, ITracker +{ + friend class TrackerDialog_PT; + + template using pointer = pt_pointer; + + explicit Tracker_PT(pointer const& pt_runtime_traits); + ~Tracker_PT() override; + module_status start_tracker(QFrame* parent_window) override; + void data(double* data) override; + bool center() override; + + int get_n_points(); + [[nodiscard]] bool get_cam_info(pt_camera_info& info); + Affine pose() const; + +private: + void run() override; + + bool maybe_reopen_camera(); + void set_fov(int value); + + pointer traits; + + QMutex camera_mtx; + + PointTracker point_tracker; + + pt_settings s; + + std::unique_ptr layout; + std::vector points; + + int preview_width = 320, preview_height = 240; + + pointer point_extractor; + pointer camera; + pointer widget; + pointer frame; + pointer preview_frame; + + std::atomic point_count { 0 }; + std::atomic ever_success = false; + mutable QMutex center_lock, data_lock; +}; + +} // ns pt_impl + +using Tracker_PT = pt_impl::Tracker_PT; diff --git a/tracker-points/ftnoir_tracker_pt_dialog.cpp b/tracker-points/ftnoir_tracker_pt_dialog.cpp new file mode 100644 index 00000000..edf689a9 --- /dev/null +++ b/tracker-points/ftnoir_tracker_pt_dialog.cpp @@ -0,0 +1,282 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2015 Stanislaw Halik + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "ftnoir_tracker_pt_dialog.h" +#include "compat/math.hpp" +#include "video/camera.hpp" + +#include + +#include +#include +#include + +using namespace options; + +static void init_resources() { Q_INIT_RESOURCE(tracker_pt_base); } + +namespace pt_impl { + +TrackerDialog_PT::TrackerDialog_PT(const QString& module_name) : + s(module_name), + tracker(nullptr), + timer(this), + trans_calib(1, 2) +{ + init_resources(); + + ui.setupUi(this); + + for (const QString& str : video::camera_names()) + ui.camdevice_combo->addItem(str); + + tie_setting(s.camera_name, ui.camdevice_combo); + tie_setting(s.cam_res_x, ui.res_x_spin); + tie_setting(s.cam_res_y, ui.res_y_spin); + tie_setting(s.cam_fps, ui.fps_spin); + + tie_setting(s.threshold_slider, ui.threshold_slider); + + tie_setting(s.min_point_size, ui.mindiam_spin); + tie_setting(s.max_point_size, ui.maxdiam_spin); + + tie_setting(s.clip_by, ui.clip_bheight_spin); + tie_setting(s.clip_bz, ui.clip_blength_spin); + tie_setting(s.clip_ty, ui.clip_theight_spin); + tie_setting(s.clip_tz, ui.clip_tlength_spin); + + tie_setting(s.cap_x, ui.cap_width_spin); + tie_setting(s.cap_y, ui.cap_height_spin); + tie_setting(s.cap_z, ui.cap_length_spin); + + tie_setting(s.m01_x, ui.m1x_spin); + tie_setting(s.m01_y, ui.m1y_spin); + tie_setting(s.m01_z, ui.m1z_spin); + + tie_setting(s.m02_x, ui.m2x_spin); + tie_setting(s.m02_y, ui.m2y_spin); + tie_setting(s.m02_z, ui.m2z_spin); + + tie_setting(s.t_MH_x, ui.tx_spin); + tie_setting(s.t_MH_y, ui.ty_spin); + tie_setting(s.t_MH_z, ui.tz_spin); + + tie_setting(s.fov, ui.fov); + + tie_setting(s.active_model_panel, ui.model_tabs); + + tie_setting(s.dynamic_pose, ui.dynamic_pose); + tie_setting(s.init_phase_timeout, ui.init_phase_timeout); + + tie_setting(s.auto_threshold, ui.auto_threshold); + + connect(ui.tcalib_button,SIGNAL(toggled(bool)), this, SLOT(startstop_trans_calib(bool))); + + connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); + connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); + + connect(ui.camdevice_combo, &QComboBox::currentTextChanged, this, &TrackerDialog_PT::set_camera_settings_available); + set_camera_settings_available(ui.camdevice_combo->currentText()); + connect(ui.camera_settings, &QPushButton::clicked, this, &TrackerDialog_PT::show_camera_settings); + + connect(&timer, &QTimer::timeout, this, &TrackerDialog_PT::poll_tracker_info_impl); + timer.setInterval(250); + + connect(&calib_timer, &QTimer::timeout, this, &TrackerDialog_PT::trans_calib_step); + calib_timer.setInterval(35); + + poll_tracker_info_impl(); + + connect(this, &TrackerDialog_PT::poll_tracker_info, this, &TrackerDialog_PT::poll_tracker_info_impl, Qt::DirectConnection); + + constexpr pt_color_type color_types[] = { + pt_color_average, + pt_color_natural, + pt_color_red_only, + pt_color_green_only, + pt_color_blue_only, + }; + + for (unsigned k = 0; k < std::size(color_types); k++) + ui.blob_color->setItemData(k, int(color_types[k])); + + tie_setting(s.blob_color, ui.blob_color); + + tie_setting(s.threshold_slider, ui.threshold_value_display, [this](const slider_value& val) { + return threshold_display_text(int(val)); + }); + + // refresh threshold display on auto-threshold checkbox state change + tie_setting(s.auto_threshold, + this, + [this](bool) { s.threshold_slider.notify(); }); +} + +QString TrackerDialog_PT::threshold_display_text(int threshold_value) +{ + if (!s.auto_threshold) + return tr("Brightness %1/255").arg(threshold_value); + else + { + pt_camera_info info; + int w = s.cam_res_x, h = s.cam_res_y; + + if (w * h <= 0) + { + w = 640; + h = 480; + } + + if (tracker && tracker->get_cam_info(info) && info.res_x * info.res_y != 0) + { + w = info.res_x; + h = info.res_y; + } + + double value = (double)pt_point_extractor::threshold_radius_value(w, h, threshold_value); + + return tr("LED radius %1 pixels").arg(value, 0, 'f', 2); + } +} + +void TrackerDialog_PT::startstop_trans_calib(bool start) +{ + QMutexLocker l(&calibrator_mutex); + + if (start) + { + qDebug() << "pt: starting translation calibration"; + calib_timer.start(); + trans_calib.reset(); + s.t_MH_x = 0; + s.t_MH_y = 0; + s.t_MH_z = 0; + + ui.sample_count_display->setText(QString()); + } + else + { + calib_timer.stop(); + qDebug() << "pt: stopping translation calibration"; + { + auto [tmp, nsamples] = trans_calib.get_estimate(); + s.t_MH_x = int(tmp[0]); + s.t_MH_y = int(tmp[1]); + s.t_MH_z = int(tmp[2]); + + constexpr int min_yaw_samples = 15; + constexpr int min_pitch_samples = 15; + constexpr int min_samples = min_yaw_samples+min_pitch_samples; + + // Don't bother counting roll samples. Roll calibration is hard enough + // that it's a hidden unsupported feature anyway. + + QString sample_feedback; + if (nsamples[0] < min_yaw_samples) + sample_feedback = tr("%1 yaw samples. Yaw more to %2 samples for stable calibration.").arg(nsamples[0]).arg(min_yaw_samples); + else if (nsamples[1] < min_pitch_samples) + sample_feedback = tr("%1 pitch samples. Pitch more to %2 samples for stable calibration.").arg(nsamples[1]).arg(min_pitch_samples); + else + { + const int nsamples_total = nsamples[0] + nsamples[1]; + sample_feedback = tr("%1 samples. Over %2, good!").arg(nsamples_total).arg(min_samples); + } + + ui.sample_count_display->setText(sample_feedback); + } + } + ui.tx_spin->setEnabled(!start); + ui.ty_spin->setEnabled(!start); + ui.tz_spin->setEnabled(!start); + + if (start) + ui.tcalib_button->setText(tr("Stop calibration")); + else + ui.tcalib_button->setText(tr("Start calibration")); +} + +void TrackerDialog_PT::poll_tracker_info_impl() +{ + pt_camera_info info; + if (tracker && tracker->get_cam_info(info)) + { + ui.caminfo_label->setText(tr("%1x%2 @ %3 FPS").arg(info.res_x).arg(info.res_y).arg(iround(info.fps))); + + // display point info + const int n_points = tracker->get_n_points(); + ui.pointinfo_label->setText((n_points == 3 ? tr("%1 OK!") : tr("%1 BAD!")).arg(n_points)); + } + else + { + ui.caminfo_label->setText(tr("Tracker offline")); + ui.pointinfo_label->setText(QString()); + } +} + +void TrackerDialog_PT::set_camera_settings_available(const QString& /* camera_name */) +{ + ui.camera_settings->setEnabled(true); +} + +void TrackerDialog_PT::show_camera_settings() +{ + if (tracker) + { + QMutexLocker l(&tracker->camera_mtx); + tracker->camera->show_camera_settings(); + } + else + (void)video::show_dialog(s.camera_name); +} + +void TrackerDialog_PT::trans_calib_step() +{ + QMutexLocker l(&calibrator_mutex); + + if (tracker) + { + Affine X_CM = tracker->pose(); + trans_calib.update(X_CM.R, X_CM.t); + } + else + startstop_trans_calib(false); +} + +void TrackerDialog_PT::save() +{ + s.b->save(); +} + +void TrackerDialog_PT::doOK() +{ + save(); + close(); +} + +void TrackerDialog_PT::doCancel() +{ + close(); +} + +void TrackerDialog_PT::register_tracker(ITracker *t) +{ + tracker = static_cast(t); + ui.tcalib_button->setEnabled(true); + poll_tracker_info(); + timer.start(); +} + +void TrackerDialog_PT::unregister_tracker() +{ + tracker = nullptr; + ui.tcalib_button->setEnabled(false); + poll_tracker_info(); + timer.stop(); +} + +} // ns pt_impl diff --git a/tracker-points/ftnoir_tracker_pt_dialog.h b/tracker-points/ftnoir_tracker_pt_dialog.h new file mode 100644 index 00000000..66981ee6 --- /dev/null +++ b/tracker-points/ftnoir_tracker_pt_dialog.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once + +#include "pt-api.hpp" + +#include "ftnoir_tracker_pt.h" +#include "tracker-pt/ui_FTNoIR_PT_Controls.h" +#include "cv/translation-calibrator.hpp" +#include "video/video-widget.hpp" + +#include +#include + +namespace pt_impl { + +class TrackerDialog_PT : public ITrackerDialog +{ + Q_OBJECT +public: + TrackerDialog_PT(const QString& module_name); + void register_tracker(ITracker *tracker) override; + void unregister_tracker() override; + void save(); +public slots: + void doOK(); + void doCancel(); + + void startstop_trans_calib(bool start); + void trans_calib_step(); + void poll_tracker_info_impl(); + void set_camera_settings_available(const QString& camera_name); + void show_camera_settings(); +signals: + void poll_tracker_info(); +protected: + QString threshold_display_text(int threshold_value); + + pt_settings s; + Tracker_PT* tracker; + QTimer timer, calib_timer; + TranslationCalibrator trans_calib; + QMutex calibrator_mutex; + + Ui::UICPTClientControls ui; +}; + +} // ns pt_impl + +using TrackerDialog_PT = pt_impl::TrackerDialog_PT; diff --git a/tracker-points/lang/nl_NL.ts b/tracker-points/lang/nl_NL.ts new file mode 100644 index 00000000..5897a731 --- /dev/null +++ b/tracker-points/lang/nl_NL.ts @@ -0,0 +1,290 @@ + + + + + UICPTClientControls + + PointTracker Settings + + + + Camera + + + + Camera settings + + + + ° + + + + Diagonal field of view + + + + Width + + + + FPS + + + + Desired capture height + + + + px + + + + Dynamic pose timeout + + + + Desired capture framerate + + + + Hz + + + + Desired capture width + + + + Height + + + + ms + + + + Device + + + + Open + + + + Camera settings (when available) + + + + Point extraction + + + + Max size + + + + Threshold + + + + Min size + + + + Intensity threshold for point extraction + + + + Automatic threshold + + + + Enable, slider sets point size + + + + Color channels used + + + + Average + + + + Natural + + + + Red only + + + + Blue only + + + + Dynamic pose (for caps only, never clips) + + + + Maximum point diameter + + + + Minimum point diameter + + + + Value + + + + Model + + + + Clip + + + + Model Dimensions + + + + mm + + + + Side + + + + Front + + + + Cap + + + + Custom + + + + z: + + + + x: + + + + <html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p><p>Use any units you want, not necessarily centimeters.</p></body></html> + + + + y: + + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">3</span></p></body></html> + + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">2</span></p></body></html> + + + + Model position + + + + Use only yaw and pitch while calibrating. +Don't roll or change position. + + + + Start calibration + + + + About + + + + <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 1.1</span></p><p><span style=" font-weight:600;">by Patrick Ruoff</span></p><p><a href="http://ftnoirpt.sourceforge.net/"><span style=" font-weight:600; text-decoration: underline; color:#0000ff;">Manual (external)</span></a></p></body></html> + + + + Status + + + + Extracted Points: + + + + Camera Info: + + + + Green only + + + + + pt_impl::TrackerDialog_PT + + Brightness %1/255 + + + + LED radius %1 pixels + + + + %1 yaw samples. Yaw more to %2 samples for stable calibration. + + + + %1 pitch samples. Pitch more to %2 samples for stable calibration. + + + + %1 samples. Over %2, good! + + + + Stop calibration + + + + Start calibration + + + + %1x%2 @ %3 FPS + + + + %1 OK! + + + + %1 BAD! + + + + Tracker offline + + + + + pt_module::metadata_pt + + Points Tracker 0.1 + + + + diff --git a/tracker-points/lang/ru_RU.ts b/tracker-points/lang/ru_RU.ts new file mode 100644 index 00000000..8a06ebac --- /dev/null +++ b/tracker-points/lang/ru_RU.ts @@ -0,0 +1,295 @@ + + + + + UICPTClientControls + + PointTracker Settings + Настройки PointTracker + + + Camera + Камера + + + Camera settings + Настройка камеры + + + ° + + + + Diagonal field of view + Угол обзора камеры + + + Width + Ширина + + + FPS + FPS (Кадров в секунду) + + + Desired capture height + + + + px + + + + Dynamic pose timeout + Динамическая поза (время ожидания) + + + Desired capture framerate + Желаемая частота кадров + + + Hz + Гц + + + Desired capture width + Желаемая ширина захвата + + + Height + Высота + + + ms + мс + + + Dynamic pose (for caps only, never clips) + Динамическая поза (Только для модели "Кепка") + + + Device + Устройство + + + Open + Открыть + + + Camera settings (when available) + Параметры камеры (если доступно) + + + Point extraction + Извлечение точек + + + Max size + Макс.размер + + + Threshold + Порог + + + Min size + Мин.размер + + + Intensity threshold for point extraction + Порог интенсивности для извлечения точки + + + Automatic threshold + Автоматич. порог + + + Enable, slider sets point size + Полузнок устанавливает размер точек + + + Color channels used + + + + Average + + + + Natural + + + + Red only + + + + Blue only + + + + Maximum point diameter + + + + Minimum point diameter + + + + Value + + + + Model + Модель + + + Clip + Клипса + + + Model Dimensions + Размеры модели + + + mm + мм + + + Side + Сбоку + + + Front + Спереди + + + Cap + Кепка + + + Custom + Свой + + + z: + + + + x: + + + + <html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p><p>Use any units you want, not necessarily centimeters.</p></body></html> + Расположение двух оставшихся точек модели относительно опорной точки в стандартной позе. Возможно исп-ть любые единицы измерения, не обязательно сантиметры. + <html><head/><body><p> Расположение двух оставшихся точек модели<br/>относительно опорной точки в стандартной позе. </p><p>Возможно использовать любые единицы измерения.</p></body></html + + + y: + + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">3</span></p></body></html> + + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">2</span></p></body></html> + + + + Model position + Положение модели + + + Use only yaw and pitch while calibrating. +Don't roll or change position. + Во время калибровки +используйте только оси +YAW и PITCH. +Не используйте оси +ROLL или X/Y-смещения. + + + Start calibration + Начать калибровку + + + About + О программе + + + <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 1.1</span></p><p><span style=" font-weight:600;">by Patrick Ruoff</span></p><p><a href="http://ftnoirpt.sourceforge.net/"><span style=" font-weight:600; text-decoration: underline; color:#0000ff;">Manual (external)</span></a></p></body></html> + <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 1.1</span></p><p><span style=" font-weight:600;">by Patrick Ruoff</span></p><p><a href="http://ftnoirpt.sourceforge.net/"><span style=" font-weight:600; text-decoration: underline; color:#0000ff;">Руководство (PointTracker)</span></a></p></body></html> + + + Status + Статус + + + Extracted Points: + Извлечено точек: + + + Camera Info: + Параметры камеры: + + + Green only + + + + + pt_impl::TrackerDialog_PT + + Brightness %1/255 + + + + LED radius %1 pixels + + + + %1 yaw samples. Yaw more to %2 samples for stable calibration. + По оси YAW выполнено: %1 замер(а/ов). Для стабильного результата необходимо не меньше %2 + + + %1 pitch samples. Pitch more to %2 samples for stable calibration. + По оси Pitch выполнено: %1 замер(а/ов). Для стабильного результата необходимо не меньше %2 + + + %1 samples. Over %2, good! + Получено %1 образца(-ов). Больше %2, отлично!! + + + Stop calibration + Остановить калибровку + + + Start calibration + Начать калибровку + + + %1x%2 @ %3 FPS + + + + %1 OK! + + + + %1 BAD! + + + + Tracker offline + Отслеживание отключено + + + + pt_module::metadata_pt + + Points Tracker 0.1 + + + + diff --git a/tracker-points/lang/stub.ts b/tracker-points/lang/stub.ts new file mode 100644 index 00000000..6c493c14 --- /dev/null +++ b/tracker-points/lang/stub.ts @@ -0,0 +1,290 @@ + + + + + UICPTClientControls + + PointTracker Settings + + + + Camera + + + + Camera settings + + + + ° + + + + Diagonal field of view + + + + Width + + + + FPS + + + + Desired capture height + + + + px + + + + Dynamic pose timeout + + + + Desired capture framerate + + + + Hz + + + + Desired capture width + + + + Height + + + + ms + + + + Device + + + + Open + + + + Camera settings (when available) + + + + Point extraction + + + + Max size + + + + Threshold + + + + Min size + + + + Intensity threshold for point extraction + + + + Automatic threshold + + + + Enable, slider sets point size + + + + Color channels used + + + + Average + + + + Natural + + + + Red only + + + + Blue only + + + + Dynamic pose (for caps only, never clips) + + + + Maximum point diameter + + + + Minimum point diameter + + + + Value + + + + Model + + + + Clip + + + + Model Dimensions + + + + mm + + + + Side + + + + Front + + + + Cap + + + + Custom + + + + z: + + + + x: + + + + <html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p><p>Use any units you want, not necessarily centimeters.</p></body></html> + + + + y: + + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">3</span></p></body></html> + + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">2</span></p></body></html> + + + + Model position + + + + Use only yaw and pitch while calibrating. +Don't roll or change position. + + + + Start calibration + + + + About + + + + <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 1.1</span></p><p><span style=" font-weight:600;">by Patrick Ruoff</span></p><p><a href="http://ftnoirpt.sourceforge.net/"><span style=" font-weight:600; text-decoration: underline; color:#0000ff;">Manual (external)</span></a></p></body></html> + + + + Status + + + + Extracted Points: + + + + Camera Info: + + + + Green only + + + + + pt_impl::TrackerDialog_PT + + Brightness %1/255 + + + + LED radius %1 pixels + + + + %1 yaw samples. Yaw more to %2 samples for stable calibration. + + + + %1 pitch samples. Pitch more to %2 samples for stable calibration. + + + + %1 samples. Over %2, good! + + + + Stop calibration + + + + Start calibration + + + + %1x%2 @ %3 FPS + + + + %1 OK! + + + + %1 BAD! + + + + Tracker offline + + + + + pt_module::metadata_pt + + Points Tracker 0.1 + + + + diff --git a/tracker-points/lang/zh_CN.ts b/tracker-points/lang/zh_CN.ts new file mode 100644 index 00000000..8212de68 --- /dev/null +++ b/tracker-points/lang/zh_CN.ts @@ -0,0 +1,290 @@ + + + + + UICPTClientControls + + PointTracker Settings + PointTracker设置 + + + Camera + 摄像头 + + + Camera settings + 摄像头设置 + + + ° + 度 + + + Diagonal field of view + 对角线 + + + Width + 宽度 + + + FPS + 帧数 + + + Desired capture height + 期望高度 + + + px + 像素点 + + + Dynamic pose timeout + 动态姿态超时时间 + + + Desired capture framerate + 期望帧数 + + + Hz + 赫兹 + + + Desired capture width + 期望宽度 + + + Height + 高度 + + + ms + 毫秒 + + + Device + 设备名称 + + + Open + 打开 + + + Camera settings (when available) + 摄像头设置 （连接时） + + + Point extraction + 跟踪点解析 + + + Max size + 最大 + + + Threshold + 大小门限值 + + + Min size + 最小 + + + Intensity threshold for point extraction + 点密度 + + + Automatic threshold + 自动门限值 + + + Enable, slider sets point size + 激活，滑动，设置跟踪点大小 + + + Maximum point diameter + 最大点直径 + + + Minimum point diameter + 最小点直径 + + + Model + 点模式 + + + Clip + 夹子式 + + + Model Dimensions + 尺寸 + + + mm + 毫米 + + + Side + 侧面 + + + Front + 正面 + + + Cap + 帽子式 + + + Custom + 自定义模式 + + + z: + Z: + + + x: + X: + + + <html><head/><body><p>Location of the two remaining model points<br/>with respect to the reference point in default pose</p><p>Use any units you want, not necessarily centimeters.</p></body></html> + <html><head/><body><p>三点中的两点位置是相对第一个点的</p><p>单位不一定要用厘米</p></body></html> + + + y: + Y: + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">3</span></p></body></html> + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">3</span></p></body></html> + + + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">2</span></p></body></html> + <html><head/><body><p><span style=" font-size:16pt;">P</span><span style=" font-size:16pt; vertical-align:sub;">2</span></p></body></html> + + + Model position + 姿态空间位置 + + + Start calibration + 开始校准 + + + About + 关于 + + + <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 1.1</span></p><p><span style=" font-weight:600;">by Patrick Ruoff</span></p><p><a href="http://ftnoirpt.sourceforge.net/"><span style=" font-weight:600; text-decoration: underline; color:#0000ff;">Manual (external)</span></a></p></body></html> + <html><head/><body><p><span style=" font-weight:600;">FTNoIR PointTracker Plugin<br/>Version 1.1</span></p><p><span style=" font-weight:600;">Patrick Ruoff</span></p><p><a href="http://ftnoirpt.sourceforge.net/"><span style=" font-weight:600; text-decoration: underline; color:#0000ff;">参考手册 (外部链接)</span></a></p></body></html> + + + Status + 状态 + + + Extracted Points: + 解析出的点: + + + Camera Info: + 设备信息： + + + Color channels used + + + + Average + + + + Natural + + + + Red only + + + + Blue only + + + + Dynamic pose (for caps only, never clips) + + + + Value + + + + Use only yaw and pitch while calibrating. +Don't roll or change position. + 用pitch和yaw校准。不要roll或者变换位置 + + + Green only + + + + + pt_impl::TrackerDialog_PT + + Brightness %1/255 + 亮度 %1/255 + + + LED radius %1 pixels + 光源半径 %1 像素 + + + %1 yaw samples. Yaw more to %2 samples for stable calibration. + + + + %1 pitch samples. Pitch more to %2 samples for stable calibration. + + + + %1 samples. Over %2, good! + %1 样本。%2 正常 + + + Stop calibration + 停止校准 + + + Start calibration + 开始校准 + + + %1x%2 @ %3 FPS + %1x%2 @ %3 帧 + + + %1 OK! + %1 正常 + + + %1 BAD! + %1 异常 + + + Tracker offline + 跟踪器脱机 + + + + pt_module::metadata_pt + + Points Tracker 0.1 + + + + diff --git a/tracker-points/module/CMakeLists.txt b/tracker-points/module/CMakeLists.txt new file mode 100644 index 00000000..1eec9616 --- /dev/null +++ b/tracker-points/module/CMakeLists.txt @@ -0,0 +1,6 @@ +find_package(OpenCV QUIET) +if(OpenCV_FOUND) + otr_module(tracker-points) + target_link_libraries(${self} opentrack-tracker-points-base) + target_include_directories(${self} PUBLIC "${CMAKE_SOURCE_DIR}/tracker-pt") +endif() diff --git a/tracker-points/module/Resources/Logo_IR.png b/tracker-points/module/Resources/Logo_IR.png new file mode 100644 index 00000000..95032a25 Binary files /dev/null and b/tracker-points/module/Resources/Logo_IR.png differ diff --git a/tracker-points/module/camera.cpp b/tracker-points/module/camera.cpp new file mode 100644 index 00000000..a70698de --- /dev/null +++ b/tracker-points/module/camera.cpp @@ -0,0 +1,152 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "camera.h" +#include "frame.hpp" + +#include "compat/math-imports.hpp" + +#include + +namespace pt_module { + +Camera::Camera(const QString& module_name) : s { module_name } +{ +} + +QString Camera::get_desired_name() const +{ + return cam_desired.name; +} + +QString Camera::get_active_name() const +{ + return cam_info.name; +} + +void Camera::show_camera_settings() +{ + if (cap) + (void)cap->show_dialog(); +} + +Camera::result Camera::get_info() const +{ + if (cam_info.res_x == 0 || cam_info.res_y == 0) + return { false, pt_camera_info() }; + else + return { true, cam_info }; +} + +Camera::result Camera::get_frame(pt_frame& frame_) +{ + cv::Mat& frame = frame_.as()->mat; + + const bool new_frame = get_frame_(frame); + + if (new_frame) + { + const f dt = (f)t.elapsed_seconds(); + t.start(); + + // measure fps of valid frames + constexpr f RC = f{1}/10; // seconds + const f alpha = dt/(dt + RC); + + if (dt_mean < dt_eps) + dt_mean = dt; + else + dt_mean = (1-alpha) * dt_mean + alpha * dt; + + cam_info.fps = dt_mean > dt_eps ? 1 / dt_mean : 0; + cam_info.res_x = frame.cols; + cam_info.res_y = frame.rows; + cam_info.fov = fov; + + return { true, cam_info }; + } + else + return { false, {} }; +} + +bool Camera::start(const QString& name, int fps, int res_x, int res_y) +{ + if (fps >= 0 && res_x >= 0 && res_y >= 0) + { + if (cam_desired.name != name || + (int)cam_desired.fps != fps || + cam_desired.res_x != res_x || + cam_desired.res_y != res_y || + !cap || !cap->is_open()) + { + stop(); + + cam_desired.name = name; + cam_desired.fps = fps; + cam_desired.res_x = res_x; + cam_desired.res_y = res_y; + cam_desired.fov = fov; + + cap = video::make_camera(name); + + if (!cap) + goto fail; + + camera::info info {}; + info.fps = fps; + info.width = res_x; + info.height = res_y; + + if (!cap->start(info)) + goto fail; + + cam_info = pt_camera_info(); + cam_info.name = name; + dt_mean = 0; + + cv::Mat tmp; + + if (!get_frame_(tmp)) + goto fail; + + t.start(); + } + } + + return true; + +fail: + stop(); + return false; +} + +void Camera::stop() +{ + cap = nullptr; + cam_info = {}; + cam_desired = {}; +} + +bool Camera::get_frame_(cv::Mat& img) +{ + if (cap && cap->is_open()) + { + auto [ frame, ret ] = cap->get_frame(); + if (ret) + { + int stride = frame.stride; + if (stride == 0) + stride = cv::Mat::AUTO_STEP; + img = cv::Mat(frame.height, frame.width, CV_8UC(frame.channels), (void*)frame.data, stride); + return true; + } + } + + return false; +} + +} // ns pt_module diff --git a/tracker-points/module/camera.h b/tracker-points/module/camera.h new file mode 100644 index 00000000..02e2fe4d --- /dev/null +++ b/tracker-points/module/camera.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once + +#include "pt-api.hpp" +#include "compat/timer.hpp" +#include "video/camera.hpp" + +#include + +#include + +#include + +namespace pt_module { + +struct Camera final : pt_camera +{ + Camera(const QString& module_name); + + bool start(const QString& name, int fps, int res_x, int res_y) override; + void stop() override; + + result get_frame(pt_frame& Frame) override; + result get_info() const override; + + pt_camera_info get_desired() const override { return cam_desired; } + QString get_desired_name() const override; + QString get_active_name() const override; + + void set_fov(f value) override { fov = value; } + void show_camera_settings() override; + +private: + using camera = typename video::impl::camera; + + [[nodiscard]] bool get_frame_(cv::Mat& frame); + + f dt_mean = 0, fov = 30; + Timer t; + pt_camera_info cam_info; + pt_camera_info cam_desired; + + std::unique_ptr cap; + pt_settings s; + + static constexpr f dt_eps = f{1}/256; +}; + +} // ns pt_module diff --git a/tracker-points/module/export.hpp b/tracker-points/module/export.hpp new file mode 100644 index 00000000..a733c9fe --- /dev/null +++ b/tracker-points/module/export.hpp @@ -0,0 +1,11 @@ +// generates export.hpp for each module from compat/linkage.hpp + +#pragma once + +#include "compat/linkage-macros.hpp" + +#ifdef BUILD_TRACKER_PT +# define OTR_PT_EXPORT OTR_GENERIC_EXPORT +#else +# define OTR_PT_EXPORT OTR_GENERIC_IMPORT +#endif diff --git a/tracker-points/module/frame.cpp b/tracker-points/module/frame.cpp new file mode 100644 index 00000000..a045b783 --- /dev/null +++ b/tracker-points/module/frame.cpp @@ -0,0 +1,80 @@ +#include "frame.hpp" + +#include "compat/math.hpp" + +#include + +namespace pt_module { + +Preview& Preview::operator=(const pt_frame& frame_) +{ + const cv::Mat& frame = frame_.as_const()->mat; + + if (frame.channels() != 3) + { + eval_once(qDebug() << "tracker/pt: camera frame depth: 3 !=" << frame.channels()); + return *this; + } + + const bool need_resize = frame.cols != frame_out.cols || frame.rows != frame_out.rows; + if (need_resize) + cv::resize(frame, frame_copy, cv::Size(frame_out.cols, frame_out.rows), 0, 0, cv::INTER_NEAREST); + else + frame.copyTo(frame_copy); + + return *this; +} + +Preview::Preview(int w, int h) +{ + ensure_size(frame_out, w, h, CV_8UC4); + ensure_size(frame_copy, w, h, CV_8UC3); + + frame_copy.setTo(cv::Scalar(0, 0, 0)); +} + +QImage Preview::get_bitmap() +{ + int stride = frame_out.step.p[0]; + + if (stride < 64 || stride < frame_out.cols * 4) + { + eval_once(qDebug() << "bad stride" << stride + << "for bitmap size" << frame_copy.cols << frame_copy.rows); + return QImage(); + } + + cv::cvtColor(frame_copy, frame_out, cv::COLOR_BGR2BGRA); + + return QImage((const unsigned char*) frame_out.data, + frame_out.cols, frame_out.rows, + stride, + QImage::Format_ARGB32); +} + +void Preview::draw_head_center(f x, f y) +{ + auto [px_, py_] = to_pixel_pos(x, y, frame_copy.cols, frame_copy.rows); + + int px = iround(px_), py = iround(py_); + + constexpr int len = 9; + + static const cv::Scalar color(0, 255, 255); + cv::line(frame_copy, + cv::Point(px - len, py), + cv::Point(px + len, py), + color, 1); + cv::line(frame_copy, + cv::Point(px, py - len), + cv::Point(px, py + len), + color, 1); +} + +void Preview::ensure_size(cv::Mat& frame, int w, int h, int type) +{ + if (frame.cols != w || frame.rows != h) + frame = cv::Mat(h, w, type); +} + +} // ns pt_module diff --git a/tracker-points/module/frame.hpp b/tracker-points/module/frame.hpp new file mode 100644 index 00000000..89334599 --- /dev/null +++ b/tracker-points/module/frame.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include "pt-api.hpp" + +#include +#include + +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wweak-vtables" +#endif + +namespace pt_module { + +struct Frame final : pt_frame +{ + cv::Mat mat; + + operator const cv::Mat&() const& { return mat; } + operator cv::Mat&() & { return mat; } +}; + +struct Preview final : pt_preview +{ + Preview(int w, int h); + + Preview& operator=(const pt_frame& frame) override; + QImage get_bitmap() override; + void draw_head_center(f x, f y) override; + + operator cv::Mat&() { return frame_copy; } + operator cv::Mat const&() const { return frame_copy; } + +private: + static void ensure_size(cv::Mat& frame, int w, int h, int type); + + cv::Mat frame_copy, frame_out; +}; + +} // ns pt_module + +#ifdef __clang__ +# pragma clang diagnostic pop +#endif diff --git a/tracker-points/module/lang/nl_NL.ts b/tracker-points/module/lang/nl_NL.ts new file mode 100644 index 00000000..aaaead85 --- /dev/null +++ b/tracker-points/module/lang/nl_NL.ts @@ -0,0 +1,11 @@ + + + + + pt_module::metadata_pt + + Points Tracker 0.1 + + + + diff --git a/tracker-points/module/lang/ru_RU.ts b/tracker-points/module/lang/ru_RU.ts new file mode 100644 index 00000000..b71de82c --- /dev/null +++ b/tracker-points/module/lang/ru_RU.ts @@ -0,0 +1,11 @@ + + + + + pt_module::metadata_pt + + Points Tracker 0.1 + + + + diff --git a/tracker-points/module/lang/stub.ts b/tracker-points/module/lang/stub.ts new file mode 100644 index 00000000..9a27c78c --- /dev/null +++ b/tracker-points/module/lang/stub.ts @@ -0,0 +1,11 @@ + + + + + pt_module::metadata_pt + + Points Tracker 0.1 + + + + diff --git a/tracker-points/module/lang/zh_CN.ts b/tracker-points/module/lang/zh_CN.ts new file mode 100644 index 00000000..9a27c78c --- /dev/null +++ b/tracker-points/module/lang/zh_CN.ts @@ -0,0 +1,11 @@ + + + + + pt_module::metadata_pt + + Points Tracker 0.1 + + + + diff --git a/tracker-points/module/module.cpp b/tracker-points/module/module.cpp new file mode 100644 index 00000000..06cd003b --- /dev/null +++ b/tracker-points/module/module.cpp @@ -0,0 +1,72 @@ +#include "ftnoir_tracker_pt.h" + +#include "module.hpp" +#include "camera.h" +#include "frame.hpp" +#include "point_extractor.h" +#include "ftnoir_tracker_pt_dialog.h" + +#include "pt-api.hpp" + +#include + +static const QString module_name = "tracker-pt"; + +#ifdef __clang__ +# pragma clang diagnostic ignored "-Wweak-vtables" +#endif + +namespace pt_module { + +struct pt_module_traits final : pt_runtime_traits +{ + pointer make_camera() const override + { + return pointer(new Camera(module_name)); + } + + pointer make_point_extractor() const override + { + return pointer(new PointExtractor(module_name)); + } + + QString get_module_name() const override + { + return module_name; + } + + pointer make_frame() const override + { + return pointer(new Frame); + } + + pointer make_preview(int w, int h) const override + { + return pointer(new Preview(w, h)); + } +}; + +struct tracker_pt : Tracker_PT +{ + tracker_pt() : Tracker_PT(pointer(new pt_module_traits)) + { + } +}; + +struct dialog_pt : TrackerDialog_PT +{ + dialog_pt(); +}; + +dialog_pt::dialog_pt() : TrackerDialog_PT(module_name) {} + +QString metadata_pt::name() { return tr("Points Tracker 0.1"); } +QIcon metadata_pt::icon() { return QIcon(":/Resources/Logo_IR.png"); } + +} + +// ns pt_module + +using namespace pt_module; + +OPENTRACK_DECLARE_TRACKER(tracker_pt, dialog_pt, metadata_pt) diff --git a/tracker-points/module/module.hpp b/tracker-points/module/module.hpp new file mode 100644 index 00000000..0b3f12cf --- /dev/null +++ b/tracker-points/module/module.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "api/plugin-api.hpp" +#include +#include + +#include "compat/linkage-macros.hpp" + +namespace pt_module +{ + +class OTR_GENERIC_EXPORT metadata_pt : public Metadata +{ + Q_OBJECT + + QString name() override; + QIcon icon() override; +}; + +} // ns pt_module diff --git a/tracker-points/module/point_extractor.cpp b/tracker-points/module/point_extractor.cpp new file mode 100644 index 00000000..1a75a3e3 --- /dev/null +++ b/tracker-points/module/point_extractor.cpp @@ -0,0 +1,387 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2015-2017 Stanislaw Halik + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "point_extractor.h" +#include "point_tracker.h" +#include "frame.hpp" + +#include "cv/numeric.hpp" +#include "compat/math.hpp" + +#undef PREVIEW +//#define PREVIEW + +#if defined PREVIEW +# include +#endif + +#include +#include +#include +#include + +#include + +using namespace numeric_types; + +// meanshift code written by Michael Welter + +/* +http://en.wikipedia.org/wiki/Mean-shift +In this application the idea, is to eliminate any bias of the point estimate +which is introduced by the rather arbitrary thresholded area. One must recognize +that the thresholded area can only move in one pixel increments since it is +binary. Thus, its center of mass might make "jumps" as pixels are added/removed +from the thresholded area. +With mean-shift, a moving "window" or kernel is multiplied with the gray-scale +image, and the COM is calculated of the result. This is iterated where the +kernel center is set the previously computed COM. Thus, peaks in the image intensity +distribution "pull" the kernel towards themselves. Eventually it stops moving, i.e. +then the computed COM coincides with the kernel center. We hope that the +corresponding location is a good candidate for the extracted point. +The idea similar to the window scaling suggested in Berglund et al. "Fast, bias-free +algorithm for tracking single particles with variable size and shape." (2008). +*/ +static vec2 MeanShiftIteration(const cv::Mat1b &frame_gray, const vec2 ¤t_center, f filter_width) +{ + const f s = 1 / filter_width; + + f m = 0; + vec2 com { 0, 0 }; + for (int i = 0; i < frame_gray.rows; i++) + { + uint8_t const* const __restrict frame_ptr = frame_gray.ptr(i); + for (int j = 0; j < frame_gray.cols; j++) + { + f val = frame_ptr[j]; + val = val * val; // taking the square weighs brighter parts of the image stronger. + f dx = (j - current_center[0])*s; + f dy = (i - current_center[1])*s; + f max = std::fmax(f(0), 1 - dx*dx - dy*dy); + val *= max; + m += val; + com[0] += j * val; + com[1] += i * val; + } + } + if (m > f(.1)) + { + com *= 1 / m; + return com; + } + else + return current_center; +} + +namespace pt_module { + +PointExtractor::PointExtractor(const QString& module_name) : s(module_name) +{ + blobs.reserve(max_blobs); +} + +void PointExtractor::ensure_channel_buffers(const cv::Mat& orig_frame) +{ + if (ch[0].rows != orig_frame.rows || ch[0].cols != orig_frame.cols) + for (cv::Mat1b& x : ch) + x = cv::Mat1b(orig_frame.rows, orig_frame.cols); +} + +void PointExtractor::ensure_buffers(const cv::Mat& frame) +{ + const int W = frame.cols, H = frame.rows; + + if (frame_gray.rows != W || frame_gray.cols != H) + { + frame_gray = cv::Mat1b(H, W); + frame_bin = cv::Mat1b(H, W); + frame_gray_unmasked = cv::Mat1b(H, W); + } +} + +void PointExtractor::extract_single_channel(const cv::Mat& orig_frame, int idx, cv::Mat1b& dest) +{ + ensure_channel_buffers(orig_frame); + + const int from_to[] = { + idx, 0, + }; + + cv::mixChannels(&orig_frame, 1, &dest, 1, from_to, 1); +} + +void PointExtractor::color_to_grayscale(const cv::Mat& frame, cv::Mat1b& output) +{ + switch (s.blob_color) + { + case pt_color_green_only: + { + extract_single_channel(frame, 1, output); + break; + } + case pt_color_blue_only: + { + extract_single_channel(frame, 0, output); + break; + } + case pt_color_red_only: + { + extract_single_channel(frame, 2, output); + break; + } + case pt_color_average: + { + const int W = frame.cols, H = frame.rows, sz = W*H; + cv::reduce(frame.reshape(1, sz), + output.reshape(1, sz), + 1, cv::REDUCE_AVG); + break; + } + default: + eval_once(qDebug() << "wrong pt_color_type enum value" << int(s.blob_color)); + [[fallthrough]]; + case pt_color_natural: + cv::cvtColor(frame, output, cv::COLOR_BGR2GRAY); + break; + } +} + +void PointExtractor::threshold_image(const cv::Mat& frame_gray, cv::Mat1b& output) +{ + const int threshold_slider_value = s.threshold_slider.to(); + + if (!s.auto_threshold) + { + cv::threshold(frame_gray, output, threshold_slider_value, 255, cv::THRESH_BINARY); + } + else + { + const int hist_size = 256; + const float ranges_[] = { 0, 256 }; + float const* ranges = (const float*) ranges_; + + cv::calcHist(&frame_gray, + 1, + nullptr, + cv::noArray(), + hist, + 1, + &hist_size, + &ranges); + + const f radius = threshold_radius_value(frame_gray.cols, frame_gray.rows, threshold_slider_value); + + float const* const __restrict ptr = hist.ptr(0); + const unsigned area = uround(3 * pi * radius*radius); + const unsigned sz = unsigned(hist.cols * hist.rows); + constexpr unsigned min_thres = 64; + unsigned thres = min_thres; + for (unsigned i = sz-1, cnt = 0; i > 32; i--) + { + cnt += (unsigned)ptr[i]; + if (cnt >= area) + break; + thres = i; + } + + cv::threshold(frame_gray, output, thres, 255, cv::THRESH_BINARY); + } +} + +static void draw_blobs(cv::Mat& preview_frame, const blob* blobs, unsigned nblobs, const cv::Size& size) +{ + for (unsigned k = 0; k < nblobs; k++) + { + const blob& b = blobs[k]; + + if (b.radius < 0) + continue; + + const f dpi = preview_frame.cols / f(320); + const f offx = 10 * dpi, offy = f(7.5) * dpi; + + const f cx = preview_frame.cols / f(size.width), + cy = preview_frame.rows / f(size.height), + c = std::fmax(f(1), cx+cy)/2; + + constexpr unsigned fract_bits = 8; + constexpr int c_fract(1 << fract_bits); + + cv::Point p(iround(b.pos[0] * cx * c_fract), iround(b.pos[1] * cy * c_fract)); + + auto circle_color = k >= PointModel::N_POINTS + ? cv::Scalar(192, 192, 192) + : cv::Scalar(255, 255, 0); + + const int overlay_size = iround(dpi); + + cv::circle(preview_frame, p, iround((b.radius + f(3.3) * c) * c_fract), + circle_color, overlay_size, + cv::LINE_AA, fract_bits); + + char buf[16]; + buf[sizeof(buf)-1] = '\0'; + std::snprintf(buf, sizeof(buf) - 1, "%.2fpx", (double)b.radius); + + auto text_color = k >= PointModel::N_POINTS + ? cv::Scalar(160, 160, 160) + : cv::Scalar(0, 0, 255); + + cv::Point pos(iround(b.pos[0]*cx+offx), iround(b.pos[1]*cy+offy)); + cv::putText(preview_frame, buf, pos, + cv::FONT_HERSHEY_PLAIN, overlay_size, text_color, + 1); + } +} + +void PointExtractor::extract_points(const pt_frame& frame_, pt_preview& preview_frame_, std::vector& points) +{ + const cv::Mat& frame = frame_.as_const()->mat; + + ensure_buffers(frame); + color_to_grayscale(frame, frame_gray_unmasked); + +#if defined PREVIEW + cv::imshow("capture", frame_gray); + cv::waitKey(1); +#endif + + threshold_image(frame_gray_unmasked, frame_bin); + frame_gray_unmasked.copyTo(frame_gray, frame_bin); + + const f region_size_min = (f)s.min_point_size; + const f region_size_max = (f)s.max_point_size; + + unsigned idx = 0; + + blobs.clear(); + + for (int y=0; y < frame_bin.rows; y++) + { + const unsigned char* __restrict ptr_bin = frame_bin.ptr(y); + for (int x=0; x < frame_bin.cols; x++) + { + if (ptr_bin[x] != 255) + continue; + idx = blobs.size() + 1; + + cv::Rect rect; + cv::floodFill(frame_bin, + cv::Point(x,y), + cv::Scalar(idx), + &rect, + cv::Scalar(0), + cv::Scalar(0), + 4 | cv::FLOODFILL_FIXED_RANGE); + + unsigned cnt = 0; + unsigned norm = 0; + + const int ymax = rect.y+rect.height, + xmax = rect.x+rect.width; + + for (int i=rect.y; i < ymax; i++) + { + unsigned char const* const __restrict ptr_blobs = frame_bin.ptr(i); + unsigned char const* const __restrict ptr_gray = frame_gray.ptr(i); + for (int j=rect.x; j < xmax; j++) + { + if (ptr_blobs[j] != idx) + continue; + + //ptr_blobs[j] = 0; + norm += ptr_gray[j]; + cnt++; + } + } + + const f radius = std::sqrt(cnt / pi); + if (radius > region_size_max || radius < region_size_min) + continue; + + blobs.emplace_back(radius, + vec2(rect.width/f(2), rect.height/f(2)), + std::pow(f(norm), f(1.1))/cnt, + rect); + + if (idx >= max_blobs) + goto end; + + // XXX we could go to the next scanline unless the points are really small. + // i'd expect each point being present on at least one unique scanline + // but it turns out some people are using 2px points -sh 20180110 + //break; + } + } +end: + + const int W = frame_gray.cols; + const int H = frame_gray.rows; + + const unsigned sz = blobs.size(); + + std::sort(blobs.begin(), blobs.end(), [](const blob& b1, const blob& b2) { return b2.brightness < b1.brightness; }); + + for (idx = 0; idx < sz; ++idx) + { + blob& b = blobs[idx]; + cv::Rect rect = b.rect; + + rect.x -= rect.width / 2; + rect.y -= rect.height / 2; + rect.width *= 2; + rect.height *= 2; + rect &= cv::Rect(0, 0, W, H); // crop at frame boundaries + + cv::Mat frame_roi = frame_gray(rect); + + // smaller values mean more changes. 1 makes too many changes while 1.5 makes about .1 + static constexpr f radius_c = f(1.75); + + const f kernel_radius = b.radius * radius_c; + vec2 pos(rect.width/f(2), rect.height/f(2)); // position relative to ROI. + + for (int iter = 0; iter < 10; ++iter) + { + vec2 com_new = MeanShiftIteration(frame_roi, pos, kernel_radius); + vec2 delta = com_new - pos; + pos = com_new; + if (delta.dot(delta) < f(1e-3)) + break; + } + + b.pos[0] = pos[0] + rect.x; + b.pos[1] = pos[1] + rect.y; + } + + draw_blobs(preview_frame_.as()->mat, + blobs.data(), blobs.size(), + frame_gray.size()); + + + // End of mean shift code. At this point, blob positions are updated with hopefully less noisy less biased values. + points.reserve(max_blobs); + points.clear(); + + for (const auto& b : blobs) + { + // note: H/W is equal to fx/fy + + vec2 p; + std::tie(p[0], p[1]) = to_screen_pos(b.pos[0], b.pos[1], W, H); + points.push_back(p); + } +} + +blob::blob(f radius, const vec2& pos, f brightness, const cv::Rect& rect) : + radius(radius), brightness(brightness), pos(pos), rect(rect) +{ + //qDebug() << "radius" << radius << "pos" << pos[0] << pos[1]; +} + +} // ns pt_module diff --git a/tracker-points/module/point_extractor.h b/tracker-points/module/point_extractor.h new file mode 100644 index 00000000..a6103667 --- /dev/null +++ b/tracker-points/module/point_extractor.h @@ -0,0 +1,58 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2015-2016 Stanislaw Halik + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once + +#include "pt-api.hpp" + +#include + +#include +#include + +namespace pt_module { + +using namespace numeric_types; + +struct blob final +{ + f radius, brightness; + vec2 pos; + cv::Rect rect; + + blob(f radius, const vec2& pos, f brightness, const cv::Rect& rect); +}; + +class PointExtractor final : public pt_point_extractor +{ +public: + // extracts points from frame and draws some processing info into frame, if draw_output is set + // dt: time since last call in seconds + void extract_points(const pt_frame& frame, pt_preview& preview_frame, std::vector& points) override; + PointExtractor(const QString& module_name); +private: + static constexpr int max_blobs = 16; + + pt_settings s; + + cv::Mat1b frame_gray_unmasked, frame_bin, frame_gray; + cv::Mat1f hist; + std::vector blobs; + cv::Mat1b ch[3]; + + void ensure_channel_buffers(const cv::Mat& orig_frame); + void ensure_buffers(const cv::Mat& frame); + + void extract_single_channel(const cv::Mat& orig_frame, int idx, cv::Mat1b& dest); + + void color_to_grayscale(const cv::Mat& frame, cv::Mat1b& output); + void threshold_image(const cv::Mat& frame_gray, cv::Mat1b& output); +}; + +} // ns impl + diff --git a/tracker-points/module/tracker_pt.qrc b/tracker-points/module/tracker_pt.qrc new file mode 100644 index 00000000..dfeb7369 --- /dev/null +++ b/tracker-points/module/tracker_pt.qrc @@ -0,0 +1,5 @@ + + + Resources/Logo_IR.png + + diff --git a/tracker-points/point_tracker.cpp b/tracker-points/point_tracker.cpp new file mode 100644 index 00000000..e209938f --- /dev/null +++ b/tracker-points/point_tracker.cpp @@ -0,0 +1,364 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "point_tracker.h" +#include "compat/math-imports.hpp" + +#include +#include +#include + +#include + +namespace pt_impl { + +using namespace numeric_types; + +static void get_row(const mat33& m, int i, vec3& v) +{ + v[0] = m(i,0); + v[1] = m(i,1); + v[2] = m(i,2); +} + +static void set_row(mat33& m, int i, const vec3& v) +{ + m(i,0) = v[0]; + m(i,1) = v[1]; + m(i,2) = v[2]; +} + +PointModel::PointModel(const pt_settings& s) +{ + set_model(s); + // calculate u + u = M01.cross(M02); + u = cv::normalize(u); + + // calculate projection matrix on M01,M02 plane + f s11 = M01.dot(M01); + f s12 = M01.dot(M02); + f s22 = M02.dot(M02); + P = 1/(s11*s22-s12*s12) * mat22(s22, -s12, -s12, s11); +} + +void PointModel::set_model(const pt_settings& s) +{ + switch (s.active_model_panel) + { + default: + eval_once(qDebug() << "pt: wrong model type selected"); + [[fallthrough]]; + case Clip: + M01 = vec3(0, s.clip_ty, -s.clip_tz); + M02 = vec3(0, -s.clip_by, -s.clip_bz); + break; + case Cap: + M01 = vec3(-s.cap_x, -s.cap_y, -s.cap_z); + M02 = vec3(s.cap_x, -s.cap_y, -s.cap_z); + break; + case Custom: + M01 = vec3(s.m01_x, s.m01_y, s.m01_z); + M02 = vec3(s.m02_x, s.m02_y, s.m02_z); + break; + } +} + +void PointModel::get_d_order(const vec2* points, unsigned* d_order, const vec2& d) const +{ + constexpr unsigned cnt = PointModel::N_POINTS; + // fit line to orthographically projected points + using t = std::pair; + t d_vals[cnt]; + // get sort indices with respect to d scalar product + for (unsigned i = 0; i < cnt; ++i) + d_vals[i] = t(d.dot(points[i]), i); + + std::sort(d_vals, + d_vals + 3, + [](const t& a, const t& b) { return a.first < b.first; }); + + for (unsigned i = 0; i < cnt; ++i) + d_order[i] = d_vals[i].second; +} + + +PointTracker::PointTracker() = default; + +PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2* points, + const PointModel& model, + const pt_camera_info& info) +{ + const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y); + PointTracker::PointOrder p; + p[0] = project(vec3(0,0,0), fx); + p[1] = project(model.M01, fx); + p[2] = project(model.M02, fx); + + constexpr unsigned sz = PointModel::N_POINTS; + + // set correspondences by minimum distance to projected model point + bool point_taken[sz] {}; + + for (unsigned i=0; i < sz; ++i) + { + f min_sdist = 0; + unsigned min_idx = 0; + // find closest point to projected model point i + for (unsigned j=0; j < sz; ++j) + { + vec2 d = p[i]-points[j]; + f sdist = d.dot(d); + if (sdist < min_sdist || j == 0) + { + min_idx = j; + min_sdist = sdist; + } + } + + // if one point is closest to more than one model point, fallback + if (point_taken[min_idx]) + { + reset_state(); + return find_correspondences(points, model); + } + point_taken[min_idx] = true; + p[i] = points[min_idx]; + } + + return p; +} + +void PointTracker::track(const std::vector& points, + const PointModel& model, + const pt_camera_info& info, + int init_phase_timeout) +{ + const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y); + PointOrder order; + + if (init_phase || init_phase_timeout <= 0 || t.elapsed_ms() > init_phase_timeout) + { + reset_state(); + order = find_correspondences(points.data(), model); + } + else + order = find_correspondences_previous(points.data(), model, info); + + if (POSIT(model, order, fx) != -1) + { + init_phase = false; + t.start(); + } + else + reset_state(); +} + +PointTracker::PointOrder PointTracker::find_correspondences(const vec2* points, const PointModel& model) +{ + constexpr unsigned cnt = PointModel::N_POINTS; + // We do a simple freetrack-like sorting in the init phase... + unsigned point_d_order[cnt]; + unsigned model_d_order[cnt]; + // calculate d and d_order for simple freetrack-like point correspondence + vec2 d(model.M01[0]-model.M02[0], model.M01[1]-model.M02[1]); + // sort points + model.get_d_order(points, point_d_order, d); + vec2 pts[cnt] { + { 0, 0 }, + { model.M01[0], model.M01[1] }, + { model.M02[0], model.M02[1] }, + }; + model.get_d_order(pts, model_d_order, d); + + // set correspondences + PointOrder p; + for (unsigned i = 0; i < cnt; ++i) + p[model_d_order[i]] = points[point_d_order[i]]; + + return p; +} + +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wfloat-equal" +#endif + +int PointTracker::POSIT(const PointModel& model, const PointOrder& order, f focal_length) +{ + // POSIT algorithm for coplanar points as presented in + // [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] + // we use the same notation as in the paper here + + // The expected rotation used for resolving the ambiguity in POSIT: + // In every iteration step the rotation closer to R_expected is taken + const mat33& R_expected{X_CM_expected.R}; + + // initial pose = last (predicted) pose + vec3 k; + get_row(R_expected, 2, k); + f Z0 = X_CM.t[2] < f(1e-4) ? f(1000) : X_CM.t[2]; + + f old_epsilon_1 = 0; + f old_epsilon_2 = 0; + f epsilon_1, epsilon_2; + + vec3 I0, J0; + vec2 I0_coeff, J0_coeff; + + vec3 I_1, J_1, I_2, J_2; + mat33 R_1, R_2; + mat33* R_current = &R_1; + + constexpr int max_iter = 100; + + int i; + for (i = 1; i < max_iter; ++i) + { + epsilon_1 = k.dot(model.M01)/Z0; + epsilon_2 = k.dot(model.M02)/Z0; + + // vector of scalar products and + vec2 I0_M0i(order[1][0]*(1 + epsilon_1) - order[0][0], + order[2][0]*(1 + epsilon_2) - order[0][0]); + vec2 J0_M0i(order[1][1]*(1 + epsilon_1) - order[0][1], + order[2][1]*(1 + epsilon_2) - order[0][1]); + + // construct projection of I, J onto M0i plane: I0 and J0 + I0_coeff = model.P * I0_M0i; + J0_coeff = model.P * J0_M0i; + I0 = I0_coeff[0]*model.M01 + I0_coeff[1]*model.M02; + J0 = J0_coeff[0]*model.M01 + J0_coeff[1]*model.M02; + + // calculate u component of I, J + f II0 = I0.dot(I0); + f IJ0 = I0.dot(J0); + f JJ0 = J0.dot(J0); + f rho, theta; + // CAVEAT don't change to comparison with an epsilon -sh 20160423 + if (JJ0 == II0) { + rho = sqrt(fabs(2*IJ0)); + theta = -pi/4; + if (IJ0<0) theta *= -1; + } + else { + rho = sqrt(sqrt( (JJ0-II0)*(JJ0-II0) + 4*IJ0*IJ0 )); + theta = atan( -2*IJ0 / (JJ0-II0) ); + // avoid branch misprediction + theta += (JJ0 - II0 < 0) * pi; + theta *= f(.5); + } + + // construct the two solutions + I_1 = I0 + rho*cos(theta)*model.u; + I_2 = I0 - rho*cos(theta)*model.u; + + J_1 = J0 + rho*sin(theta)*model.u; + J_2 = J0 - rho*sin(theta)*model.u; + + f norm_const = (f)(1/cv::norm(I_1)); // all have the same norm + + // create rotation matrices + I_1 *= norm_const; J_1 *= norm_const; + I_2 *= norm_const; J_2 *= norm_const; + + set_row(R_1, 0, I_1); + set_row(R_1, 1, J_1); + set_row(R_1, 2, I_1.cross(J_1)); + + set_row(R_2, 0, I_2); + set_row(R_2, 1, J_2); + set_row(R_2, 2, I_2.cross(J_2)); + + // the single translation solution + Z0 = norm_const * focal_length; + + // pick the rotation solution closer to the expected one + // in simple metric d(A,B) = || I - A * B^T || + f R_1_deviation = (f)(cv::norm(mat33::eye() - R_expected * R_1.t())); + f R_2_deviation = (f)(cv::norm(mat33::eye() - R_expected * R_2.t())); + + if (R_1_deviation < R_2_deviation) + R_current = &R_1; + else + R_current = &R_2; + + get_row(*R_current, 2, k); + + // check for convergence condition + const f delta = fabs(epsilon_1 - old_epsilon_1) + fabs(epsilon_2 - old_epsilon_2); + + if (delta < eps) + break; + + old_epsilon_1 = epsilon_1; + old_epsilon_2 = epsilon_2; + } + + const f t[3] = { + order[0][0] * Z0/focal_length, + order[0][1] * Z0/focal_length, + Z0 + }; + const mat33& r = *R_current; + + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + { + int ret = std::fpclassify(r(i, j)); + if (ret == FP_NAN || ret == FP_INFINITE) + { + qDebug() << "posit nan R"; + return -1; + } + } + + for (unsigned i = 0; i < 3; i++) // NOLINT(modernize-loop-convert) + { + int ret = std::fpclassify(t[i]); + if (ret == FP_NAN || ret == FP_INFINITE) + { + qDebug() << "posit nan T"; + return -1; + } + } + + // apply results + X_CM.R = r; + X_CM.t[0] = t[0]; + X_CM.t[1] = t[1]; + X_CM.t[2] = t[2]; + + X_CM_expected = X_CM; + + //qDebug() << "iter:" << i; + + return i; +} + +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + +vec2 PointTracker::project(const vec3& v_M, f focal_length) +{ + return project(v_M, focal_length, X_CM); +} + +vec2 PointTracker::project(const vec3& v_M, f focal_length, const Affine& X_CM) +{ + vec3 v_C = X_CM * v_M; + return vec2(focal_length*v_C[0]/v_C[2], focal_length*v_C[1]/v_C[2]); +} + +void PointTracker::reset_state() +{ + init_phase = true; + X_CM_expected = {}; +} + +} // ns pt_impl diff --git a/tracker-points/point_tracker.h b/tracker-points/point_tracker.h new file mode 100644 index 00000000..70c7a9fc --- /dev/null +++ b/tracker-points/point_tracker.h @@ -0,0 +1,88 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once + +#include "compat/timer.hpp" +#include "cv/affine.hpp" +#include "cv/numeric.hpp" +#include "pt-api.hpp" + +#include +#include +#include +#include + +#include + +#include + +namespace pt_impl { + +// ---------------------------------------------------------------------------- +// Describes a 3-point model +// nomenclature as in +// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] + +using namespace numeric_types; + +struct PointModel final +{ + static constexpr unsigned N_POINTS = 3; + + vec3 M01; // M01 in model frame + vec3 M02; // M02 in model frame + + vec3 u; // unit vector perpendicular to M01,M02-plane + + mat22 P; + + enum Model { Clip, Cap, Custom }; + + explicit PointModel(const pt_settings& s); + void set_model(const pt_settings& s); + + void get_d_order(const vec2* points, unsigned* d_order, const vec2& d) const; +}; + +// ---------------------------------------------------------------------------- +// Tracks a 3-point model +// implementing the POSIT algorithm for coplanar points as presented in +// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"] +class PointTracker final +{ +public: + PointTracker(); + // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5) + // f : (focal length)/(sensor width) + // dt : time since last call + void track(const std::vector& projected_points, const PointModel& model, const pt_camera_info& info, int init_phase_timeout); + Affine pose() const { return X_CM; } + vec2 project(const vec3& v_M, f focal_length); + vec2 project(const vec3& v_M, f focal_length, const Affine& X_CM); + void reset_state(); + +private: + // the points in model order + using PointOrder = std::array; + + PointOrder find_correspondences(const vec2* projected_points, const PointModel &model); + PointOrder find_correspondences_previous(const vec2* points, const PointModel &model, const pt_camera_info& info); + // The POSIT algorithm, returns the number of iterations + int POSIT(const PointModel& point_model, const PointOrder& order, f focal_length); + + Affine X_CM; // transform from model to camera + Affine X_CM_expected; + PointOrder prev_positions; + Timer t; + bool init_phase = true; +}; + +} // ns pt_impl + +using PointTracker = pt_impl::PointTracker; +using PointModel = pt_impl::PointModel; diff --git a/tracker-points/pt-api.cpp b/tracker-points/pt-api.cpp new file mode 100644 index 00000000..f64d5c9a --- /dev/null +++ b/tracker-points/pt-api.cpp @@ -0,0 +1,54 @@ +#include "pt-api.hpp" +#include "cv/numeric.hpp" + +using namespace numeric_types; + +pt_camera_info::pt_camera_info() = default; + +f pt_camera_info::get_focal_length(f fov, int res_x, int res_y) +{ + const f diag_len = std::sqrt(f(res_x*res_x + res_y*res_y)); + const f aspect_x = res_x / diag_len; + //const double aspect_y = res_y / diag_len; + const f diag_fov = fov * pi/180; + const f fov_x = 2*std::atan(std::tan(diag_fov*f{.5}) * aspect_x); + //const double fov_y = 2*atan(tan(diag_fov*.5) * aspect_y); + const f fx = f{.5} / std::tan(fov_x * f{.5}); + return fx; + //fy = .5 / tan(fov_y * .5); + //static bool once = false; if (!once) { once = true; qDebug() << "f" << ret << "fov" << (fov * 180/M_PI); } +} + +pt_camera::pt_camera() = default; +pt_camera::~pt_camera() = default; +pt_runtime_traits::pt_runtime_traits() = default; +pt_runtime_traits::~pt_runtime_traits() = default; +pt_point_extractor::pt_point_extractor() = default; +pt_point_extractor::~pt_point_extractor() = default; + +f pt_point_extractor::threshold_radius_value(int w, int h, int threshold) +{ + f cx = w / f{640}, cy = h / f{480}; + + const f min_radius = f{1.75} * cx; + const f max_radius = f{15} * cy; + + const f radius = std::fmax(f{0}, (max_radius-min_radius) * threshold / f(255) + min_radius); + + return radius; +} + +std::tuple pt_pixel_pos_mixin::to_pixel_pos(f x, f y, int w, int h) +{ + return std::make_tuple(w*(x+f{.5}), f{.5}*(h - 2*y*w)); +} + +std::tuple pt_pixel_pos_mixin::to_screen_pos(f px, f py, int w, int h) +{ + px *= w/(w-f{1}); py *= h/(h-f{1}); + return std::make_tuple((px - w/f{2})/w, -(py - h/f{2})/w); +} + +pt_frame::pt_frame() = default; + +pt_frame::~pt_frame() = default; diff --git a/tracker-points/pt-api.hpp b/tracker-points/pt-api.hpp new file mode 100644 index 00000000..741576a1 --- /dev/null +++ b/tracker-points/pt-api.hpp @@ -0,0 +1,123 @@ +#pragma once + +#include "pt-settings.hpp" + +#include "cv/numeric.hpp" +#include "options/options.hpp" + +#include +#include +#include + +#include + +#include +#include + +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wweak-vtables" +#endif + +struct pt_camera_info final +{ + using f = numeric_types::f; + + pt_camera_info(); + static f get_focal_length(f fov, int res_x, int res_y); + + f fov = 0; + f fps = 0; + + int res_x = 0; + int res_y = 0; + QString name; +}; + +struct pt_pixel_pos_mixin +{ + using f = numeric_types::f; + + static std::tuple to_pixel_pos(f x, f y, int w, int h); + static std::tuple to_screen_pos(f px, f py, int w, int h); +}; + +struct pt_frame : pt_pixel_pos_mixin +{ + pt_frame(); + virtual ~pt_frame(); + + template + t* as() & + { + return static_cast(this); + } + + template + t const* as_const() const& + { + return static_cast(this); + } +}; + +struct pt_preview : pt_frame +{ + virtual pt_preview& operator=(const pt_frame&) = 0; + virtual QImage get_bitmap() = 0; + virtual void draw_head_center(f x, f y) = 0; +}; + +struct pt_camera +{ + using result = std::tuple; + using f = numeric_types::f; + + pt_camera(); + virtual ~pt_camera(); + + [[nodiscard]] virtual bool start(const QString& name, int fps, int res_x, int res_y) = 0; + virtual void stop() = 0; + + virtual result get_frame(pt_frame& frame) = 0; + virtual result get_info() const = 0; + virtual pt_camera_info get_desired() const = 0; + + virtual QString get_desired_name() const = 0; + virtual QString get_active_name() const = 0; + + virtual void set_fov(f value) = 0; + virtual void show_camera_settings() = 0; +}; + +struct pt_point_extractor : pt_pixel_pos_mixin +{ + using vec2 = numeric_types::vec2; + using f = numeric_types::f; + + pt_point_extractor(); + virtual ~pt_point_extractor(); + virtual void extract_points(const pt_frame& image, pt_preview& preview_frame, std::vector& points) = 0; + + static f threshold_radius_value(int w, int h, int threshold); +}; + +struct pt_runtime_traits +{ + template using pointer = std::shared_ptr; + + pt_runtime_traits(); + virtual ~pt_runtime_traits(); + + virtual pointer make_camera() const = 0; + virtual pointer make_point_extractor() const = 0; + virtual pointer make_frame() const = 0; + virtual pointer make_preview(int w, int h) const = 0; + virtual QString get_module_name() const = 0; +}; + +template +using pt_pointer = typename pt_runtime_traits::pointer; + +#ifdef __clang__ +# pragma clang diagnostic pop +#endif diff --git a/tracker-points/pt-settings.hpp b/tracker-points/pt-settings.hpp new file mode 100644 index 00000000..723ee08d --- /dev/null +++ b/tracker-points/pt-settings.hpp @@ -0,0 +1,73 @@ +#pragma once + +#include "options/options.hpp" + +#include + +enum pt_color_type +{ + // explicit values, gotta preserve the numbering in .ini + // don't reuse when removing some of the modes + pt_color_natural = 2, + pt_color_red_only = 3, + pt_color_average = 5, + pt_color_blue_only = 6, + pt_color_green_only = 7, +}; + +namespace pt_impl { + +using namespace options; + +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wweak-vtables" +#endif + +struct pt_settings final : options::opts +{ + using slider_value = options::slider_value; + + value camera_name { b, "camera-name", "" }; + value cam_res_x { b, "camera-res-width", 640 }, + cam_res_y { b, "camera-res-height", 480 }, + cam_fps { b, "camera-fps", 30 }; + value min_point_size { b, "min-point-size", 2.5 }, + max_point_size { b, "max-point-size", 50 }; + + value m01_x { b, "m_01-x", 0 }, m01_y { b, "m_01-y", 0 }, m01_z { b, "m_01-z", 0 }; + value m02_x { b, "m_02-x", 0 }, m02_y { b, "m_02-y", 0 }, m02_z { b, "m_02-z", 0 }; + + value t_MH_x { b, "model-centroid-x", 0 }, + t_MH_y { b, "model-centroid-y", 0 }, + t_MH_z { b, "model-centroid-z", 0 }; + + value clip_ty { b, "clip-ty", 40 }, + clip_tz { b, "clip-tz", 30 }, + clip_by { b, "clip-by", 70 }, + clip_bz { b, "clip-bz", 80 }; + + value active_model_panel { b, "active-model-panel", 0 }, + cap_x { b, "cap-x", 40 }, + cap_y { b, "cap-y", 60 }, + cap_z { b, "cap-z", 100 }; + + value fov { b, "camera-fov", 56 }; + + value dynamic_pose { b, "dynamic-pose-resolution", false }; + value init_phase_timeout { b, "init-phase-timeout", 250 }; + value auto_threshold { b, "automatic-threshold", true }; + value blob_color { b, "blob-color", pt_color_natural }; + + value threshold_slider { b, "threshold-slider", { 128, 0, 255 } }; + + explicit pt_settings(const QString& name) : opts(name) {} +}; + +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + +} // ns pt_impl + +using pt_settings = pt_impl::pt_settings; diff --git a/tracker-points/tracker_pt_base.qrc b/tracker-points/tracker_pt_base.qrc new file mode 100644 index 00000000..8c270540 --- /dev/null +++ b/tracker-points/tracker_pt_base.qrc @@ -0,0 +1,8 @@ + + + Resources/cap_front.png + Resources/cap_side.png + Resources/clip_front.png + Resources/clip_side.png + + -- cgit v1.2.3