From cd030bc716b9bebcf1e8d4fb32d0cfcbfc7a1479 Mon Sep 17 00:00:00 2001 From: Chadwick Boulay Date: Fri, 13 Feb 2015 16:00:11 -0500 Subject: [PATCH 1/7] updated PS3EYEDriver submodule. --- external/PS3EYEDriver | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/PS3EYEDriver b/external/PS3EYEDriver index 39ee96f1..183a04aa 160000 --- a/external/PS3EYEDriver +++ b/external/PS3EYEDriver @@ -1 +1 @@ -Subproject commit 39ee96f1b8b6071c211bb9e1cef79f08bc207ff0 +Subproject commit 183a04aaf74b55ec08be53c48f379c5e711b9496 From 9fa08177ae5e648f7616ca513b5354b8467b4fef Mon Sep 17 00:00:00 2001 From: Chadwick Boulay Date: Fri, 13 Feb 2015 17:09:14 -0500 Subject: [PATCH 2/7] Fixes autoexposure when using PS3EyeDriver. --- external/PS3EYEDriver | 2 +- src/tracker/platform/camera_control_macosx.c | 4 +++- src/tracker/platform/camera_control_win32.c | 4 +++- src/tracker/psmove_tracker.c | 4 ++-- 4 files changed, 9 insertions(+), 5 deletions(-) diff --git a/external/PS3EYEDriver b/external/PS3EYEDriver index 183a04aa..71176678 160000 --- a/external/PS3EYEDriver +++ b/external/PS3EYEDriver @@ -1 +1 @@ -Subproject commit 183a04aaf74b55ec08be53c48f379c5e711b9496 +Subproject commit 71176678562ade98b2d87a25518e8c7a1d5d0631 diff --git a/src/tracker/platform/camera_control_macosx.c b/src/tracker/platform/camera_control_macosx.c index 0066414a..3d3238f3 100644 --- a/src/tracker/platform/camera_control_macosx.c +++ b/src/tracker/platform/camera_control_macosx.c @@ -27,6 +27,7 @@ **/ #include "../camera_control.h" +#include "../camera_control_private.h" #include "psmove_osxsupport.h" @@ -45,7 +46,8 @@ camera_control_set_parameters(CameraControl* cc, { #if defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER) // TODO: Implement setting those parameters on cc->eye - psmove_WARNING("Unimplemented: Setting of PS3EYEDriver parameters\n"); + ps3eye_set_parameters(cc->eye, autoG > 0, autoWB > 0, gain, exposure, contrast, brightness); + psmove_WARNING("Unimplemented: Only some PS3EYEDriver parameters set.\n"); #else macosx_camera_set_exposure_lock(1); #endif diff --git a/src/tracker/platform/camera_control_win32.c b/src/tracker/platform/camera_control_win32.c index 322e79c9..299aa096 100644 --- a/src/tracker/platform/camera_control_win32.c +++ b/src/tracker/platform/camera_control_win32.c @@ -180,7 +180,9 @@ void camera_control_set_parameters(CameraControl* cc, int autoE, int autoG, int if (cc->capture) { cvReleaseCapture(&cc->capture); } - + + ps3eye_set_parameters(cc->eye, autoG > 0, autoWB > 0, gain, exposure, contrast, brightness); + int width, height; get_metrics(&width, &height); diff --git a/src/tracker/psmove_tracker.c b/src/tracker/psmove_tracker.c index 4ea8139e..4425e90e 100644 --- a/src/tracker/psmove_tracker.c +++ b/src/tracker/psmove_tracker.c @@ -473,7 +473,7 @@ psmove_tracker_set_exposure(PSMoveTracker *tracker, float target_luminance = 0; switch (tracker->exposure_mode) { case Exposure_LOW: - target_luminance = 0; + target_luminance = 1; break; case Exposure_MEDIUM: target_luminance = 25; @@ -1545,7 +1545,7 @@ psmove_tracker_free(PSMoveTracker *tracker) int psmove_tracker_adapt_to_light(PSMoveTracker *tracker, float target_luminance) { - float minimum_exposure = 2051; + float minimum_exposure = 1; // Was 2051 float maximum_exposure = 65535; float current_exposure = (maximum_exposure + minimum_exposure) / 2.; From cf4ecb2ca119d0a1e0de67c82dc4d68e3e606899 Mon Sep 17 00:00:00 2001 From: Chadwick Boulay Date: Sun, 15 Feb 2015 07:49:19 -0500 Subject: [PATCH 3/7] Set exposure for PS3EYEDriver in OSX. --- src/tracker/platform/camera_control_macosx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tracker/platform/camera_control_macosx.c b/src/tracker/platform/camera_control_macosx.c index 3d3238f3..9314e751 100644 --- a/src/tracker/platform/camera_control_macosx.c +++ b/src/tracker/platform/camera_control_macosx.c @@ -28,7 +28,6 @@ #include "../camera_control.h" #include "../camera_control_private.h" - #include "psmove_osxsupport.h" void @@ -47,6 +46,7 @@ camera_control_set_parameters(CameraControl* cc, #if defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER) // TODO: Implement setting those parameters on cc->eye ps3eye_set_parameters(cc->eye, autoG > 0, autoWB > 0, gain, exposure, contrast, brightness); + //cc->eye->eye->setExposure(exposure); psmove_WARNING("Unimplemented: Only some PS3EYEDriver parameters set.\n"); #else macosx_camera_set_exposure_lock(1); From 5ac7978ab9bdb345c13164eb2e38543aa33fae82 Mon Sep 17 00:00:00 2001 From: Chadwick Boulay Date: Sun, 15 Feb 2015 22:27:19 -0500 Subject: [PATCH 4/7] A ton of whitespace and if-format fixes. --- src/tracker/platform/camera_control_win32.c | 252 ++-- src/tracker/psmove_tracker.c | 1183 ++++++++++--------- 2 files changed, 730 insertions(+), 705 deletions(-) diff --git a/src/tracker/platform/camera_control_win32.c b/src/tracker/platform/camera_control_win32.c index 299aa096..aa984e22 100644 --- a/src/tracker/platform/camera_control_win32.c +++ b/src/tracker/platform/camera_control_win32.c @@ -39,92 +39,100 @@ void camera_control_backup_system_settings(CameraControl* cc, const char* file) { #if !defined(CAMERA_CONTROL_USE_CL_DRIVER) && defined(PSMOVE_USE_PSEYE) - HKEY hKey; - DWORD l = sizeof(DWORD); - DWORD AutoAEC = 0; - DWORD AutoAGC = 0; - DWORD AutoAWB = 0; - DWORD Exposure = 0; - DWORD Gain = 0; - DWORD wbB = 0; - DWORD wbG = 0; - DWORD wbR = 0; - char* PATH = CL_DRIVER_REG_PATH; - int err = RegOpenKeyEx(HKEY_CURRENT_USER, PATH, 0, KEY_ALL_ACCESS, &hKey); - if (err != ERROR_SUCCESS) { - printf("Error: %d Unable to open reg-key: [HKCU]\%s!", err, PATH); - return; - } - RegQueryValueEx(hKey, "AutoAEC", NULL, NULL, (LPBYTE) &AutoAEC, &l); - RegQueryValueEx(hKey, "AutoAGC", NULL, NULL, (LPBYTE) &AutoAGC, &l); - RegQueryValueEx(hKey, "AutoAWB", NULL, NULL, (LPBYTE) &AutoAWB, &l); - RegQueryValueEx(hKey, "Exposure", NULL, NULL, (LPBYTE) &Exposure, &l); - RegQueryValueEx(hKey, "Gain", NULL, NULL, (LPBYTE) &Gain, &l); - RegQueryValueEx(hKey, "WhiteBalanceB", NULL, NULL, (LPBYTE) &wbB, &l); - RegQueryValueEx(hKey, "WhiteBalanceG", NULL, NULL, (LPBYTE) &wbG, &l); - RegQueryValueEx(hKey, "WhiteBalanceR", NULL, NULL, (LPBYTE) &wbR, &l); - - dictionary* ini = dictionary_new(0); - iniparser_set(ini, "PSEye", 0); - iniparser_set_int(ini, "PSEye:AutoAEC", AutoAEC); - iniparser_set_int(ini, "PSEye:AutoAGC", AutoAGC); - iniparser_set_int(ini, "PSEye:AutoAWB", AutoAWB); - iniparser_set_int(ini, "PSEye:Exposure", Exposure); - iniparser_set_int(ini, "PSEye:Gain", Gain); - iniparser_set_int(ini, "PSEye:WhiteBalanceB", wbB); - iniparser_set_int(ini, "PSEye:WhiteBalanceG", wbG); - iniparser_set_int(ini, "PSEye:WhiteBalanceR", wbG); - iniparser_save_ini(ini, file); - dictionary_del(ini); + HKEY hKey; + DWORD l = sizeof(DWORD); + DWORD AutoAEC = 0; + DWORD AutoAGC = 0; + DWORD AutoAWB = 0; + DWORD Exposure = 0; + DWORD Gain = 0; + DWORD wbB = 0; + DWORD wbG = 0; + DWORD wbR = 0; + char* PATH = CL_DRIVER_REG_PATH; + int err = RegOpenKeyEx(HKEY_CURRENT_USER, PATH, 0, KEY_ALL_ACCESS, &hKey); + if (err != ERROR_SUCCESS) { + printf("Error: %d Unable to open reg-key: [HKCU]\%s!", err, PATH); + return; + } + RegQueryValueEx(hKey, "AutoAEC", NULL, NULL, (LPBYTE) &AutoAEC, &l); + RegQueryValueEx(hKey, "AutoAGC", NULL, NULL, (LPBYTE) &AutoAGC, &l); + RegQueryValueEx(hKey, "AutoAWB", NULL, NULL, (LPBYTE) &AutoAWB, &l); + RegQueryValueEx(hKey, "Exposure", NULL, NULL, (LPBYTE) &Exposure, &l); + RegQueryValueEx(hKey, "Gain", NULL, NULL, (LPBYTE) &Gain, &l); + RegQueryValueEx(hKey, "WhiteBalanceB", NULL, NULL, (LPBYTE) &wbB, &l); + RegQueryValueEx(hKey, "WhiteBalanceG", NULL, NULL, (LPBYTE) &wbG, &l); + RegQueryValueEx(hKey, "WhiteBalanceR", NULL, NULL, (LPBYTE) &wbR, &l); + + dictionary* ini = dictionary_new(0); + iniparser_set(ini, "PSEye", 0); + iniparser_set_int(ini, "PSEye:AutoAEC", AutoAEC); + iniparser_set_int(ini, "PSEye:AutoAGC", AutoAGC); + iniparser_set_int(ini, "PSEye:AutoAWB", AutoAWB); + iniparser_set_int(ini, "PSEye:Exposure", Exposure); + iniparser_set_int(ini, "PSEye:Gain", Gain); + iniparser_set_int(ini, "PSEye:WhiteBalanceB", wbB); + iniparser_set_int(ini, "PSEye:WhiteBalanceG", wbG); + iniparser_set_int(ini, "PSEye:WhiteBalanceR", wbG); + iniparser_save_ini(ini, file); + dictionary_del(ini); #endif } void camera_control_restore_system_settings(CameraControl* cc, const char* file) { #if !defined(CAMERA_CONTROL_USE_CL_DRIVER) && defined(PSMOVE_USE_PSEYE) - int NOT_FOUND = -1; - int val; - HKEY hKey; - DWORD l = sizeof(DWORD); - - char* PATH = CL_DRIVER_REG_PATH; - int err = RegOpenKeyEx(HKEY_CURRENT_USER, PATH, 0, KEY_ALL_ACCESS, &hKey); - if (err != ERROR_SUCCESS) { - printf("Error: %d Unable to open reg-key: [HKCU]\%s!", err, PATH); - return; - } - - dictionary* ini = iniparser_load(file); - val = iniparser_getint(ini, "PSEye:AutoAEC", NOT_FOUND); - if (val != NOT_FOUND) - RegSetValueExA(hKey, "AutoAEC", 0, REG_DWORD, (CONST BYTE*) &val, l); - - val = iniparser_getint(ini, "PSEye:AutoAGC", NOT_FOUND); - if (val != NOT_FOUND) - RegSetValueExA(hKey, "AutoAGC", 0, REG_DWORD, (CONST BYTE*) &val, l); + int NOT_FOUND = -1; + int val; + HKEY hKey; + DWORD l = sizeof(DWORD); + + char* PATH = CL_DRIVER_REG_PATH; + int err = RegOpenKeyEx(HKEY_CURRENT_USER, PATH, 0, KEY_ALL_ACCESS, &hKey); + if (err != ERROR_SUCCESS) { + printf("Error: %d Unable to open reg-key: [HKCU]\%s!", err, PATH); + return; + } + + dictionary* ini = iniparser_load(file); + val = iniparser_getint(ini, "PSEye:AutoAEC", NOT_FOUND); + if (val != NOT_FOUND){ + RegSetValueExA(hKey, "AutoAEC", 0, REG_DWORD, (CONST BYTE*) &val, l); + } + + val = iniparser_getint(ini, "PSEye:AutoAGC", NOT_FOUND); + if (val != NOT_FOUND){ + RegSetValueExA(hKey, "AutoAGC", 0, REG_DWORD, (CONST BYTE*) &val, l); + } val = iniparser_getint(ini, "PSEye:AutoAWB", NOT_FOUND); - if (val != NOT_FOUND) - RegSetValueExA(hKey, "AutoAWB", 0, REG_DWORD, (CONST BYTE*) &val, l); + if (val != NOT_FOUND){ + RegSetValueExA(hKey, "AutoAWB", 0, REG_DWORD, (CONST BYTE*) &val, l); + } val = iniparser_getint(ini, "PSEye:Exposure", NOT_FOUND); - if (val != NOT_FOUND) - RegSetValueExA(hKey, "Exposure", 0, REG_DWORD, (CONST BYTE*) &val, l); + if (val != NOT_FOUND){ + RegSetValueExA(hKey, "Exposure", 0, REG_DWORD, (CONST BYTE*) &val, l); + } val = iniparser_getint(ini, "PSEye:Gain", NOT_FOUND); - if (val != NOT_FOUND) - RegSetValueExA(hKey, "Gain", 0, REG_DWORD, (CONST BYTE*) &val, l); + if (val != NOT_FOUND){ + RegSetValueExA(hKey, "Gain", 0, REG_DWORD, (CONST BYTE*) &val, l); + } val = iniparser_getint(ini, "PSEye:WhiteBalanceR", NOT_FOUND); - if (val != NOT_FOUND) - RegSetValueExA(hKey, "WhiteBalanceR", 0, REG_DWORD, (CONST BYTE*) &val, l); + if (val != NOT_FOUND){ + RegSetValueExA(hKey, "WhiteBalanceR", 0, REG_DWORD, (CONST BYTE*) &val, l); + } val = iniparser_getint(ini, "PSEye:WhiteBalanceB", NOT_FOUND); - if (val != NOT_FOUND) - RegSetValueExA(hKey, "WhiteBalanceB", 0, REG_DWORD, (CONST BYTE*) &val, l); + if (val != NOT_FOUND){ + RegSetValueExA(hKey, "WhiteBalanceB", 0, REG_DWORD, (CONST BYTE*) &val, l); + } val = iniparser_getint(ini, "PSEye:WhiteBalanceG", NOT_FOUND); - if (val != NOT_FOUND) - RegSetValueExA(hKey, "WhiteBalanceG", 0, REG_DWORD, (CONST BYTE*) &val, l); + if (val != NOT_FOUND){ + RegSetValueExA(hKey, "WhiteBalanceG", 0, REG_DWORD, (CONST BYTE*) &val, l); + } iniparser_freedict(ini); #endif @@ -133,62 +141,70 @@ void camera_control_restore_system_settings(CameraControl* cc, const char* file) void camera_control_set_parameters(CameraControl* cc, int autoE, int autoG, int autoWB, int exposure, int gain, int wbRed, int wbGreen, int wbBlue, int contrast, int brightness) { #if defined(CAMERA_CONTROL_USE_CL_DRIVER) - if (autoE >= 0) - CLEyeSetCameraParameter(cc->camera, CLEYE_AUTO_EXPOSURE, autoE > 0); - if (autoG >= 0) - CLEyeSetCameraParameter(cc->camera, CLEYE_AUTO_GAIN, autoG > 0); - if (autoWB >= 0) - CLEyeSetCameraParameter(cc->camera, CLEYE_AUTO_WHITEBALANCE, autoWB > 0); - if (exposure >= 0) - CLEyeSetCameraParameter(cc->camera, CLEYE_EXPOSURE, round((511 * exposure) / 0xFFFF)); - if (gain >= 0) - CLEyeSetCameraParameter(cc->camera, CLEYE_GAIN, round((79 * gain) / 0xFFFF)); - if (wbRed >= 0) - CLEyeSetCameraParameter(cc->camera, CLEYE_WHITEBALANCE_RED, round((255 * wbRed) / 0xFFFF)); - if (wbGreen >= 0) - CLEyeSetCameraParameter(cc->camera, CLEYE_WHITEBALANCE_GREEN, round((255 * wbGreen) / 0xFFFF)); - if (wbBlue >= 0) - CLEyeSetCameraParameter(cc->camera, CLEYE_WHITEBALANCE_BLUE, round((255 * wbBlue) / 0xFFFF)); + if (autoE >= 0){ + CLEyeSetCameraParameter(cc->camera, CLEYE_AUTO_EXPOSURE, autoE > 0); + } + if (autoG >= 0){ + CLEyeSetCameraParameter(cc->camera, CLEYE_AUTO_GAIN, autoG > 0); + } + if (autoWB >= 0){ + CLEyeSetCameraParameter(cc->camera, CLEYE_AUTO_WHITEBALANCE, autoWB > 0); + } + if (exposure >= 0){ + CLEyeSetCameraParameter(cc->camera, CLEYE_EXPOSURE, round((511 * exposure) / 0xFFFF)); + } + if (gain >= 0){ + CLEyeSetCameraParameter(cc->camera, CLEYE_GAIN, round((79 * gain) / 0xFFFF)); + } + if (wbRed >= 0){ + CLEyeSetCameraParameter(cc->camera, CLEYE_WHITEBALANCE_RED, round((255 * wbRed) / 0xFFFF)); + } + if (wbGreen >= 0){ + CLEyeSetCameraParameter(cc->camera, CLEYE_WHITEBALANCE_GREEN, round((255 * wbGreen) / 0xFFFF)); + } + if (wbBlue >= 0){ + CLEyeSetCameraParameter(cc->camera, CLEYE_WHITEBALANCE_BLUE, round((255 * wbBlue) / 0xFFFF)); + } #elif defined(PSMOVE_USE_PSEYE) - int val; - HKEY hKey; - DWORD l = sizeof(DWORD); - char* PATH = CL_DRIVER_REG_PATH; - int err = RegOpenKeyEx(HKEY_CURRENT_USER, PATH, 0, KEY_ALL_ACCESS, &hKey); - if (err != ERROR_SUCCESS) { - printf("Error: %d Unable to open reg-key: [HKCU]\%s!", err, PATH); - return; - } - val = autoE > 0; - RegSetValueExA(hKey, "AutoAEC", 0, REG_DWORD, (CONST BYTE*) &val, l); - val = autoG > 0; - RegSetValueExA(hKey, "AutoAGC", 0, REG_DWORD, (CONST BYTE*) &val, l); - val = autoWB > 0; - RegSetValueExA(hKey, "AutoAWB", 0, REG_DWORD, (CONST BYTE*) &val, l); - val = round((511 * exposure) / 0xFFFF); - RegSetValueExA(hKey, "Exposure", 0, REG_DWORD, (CONST BYTE*) &val, l); - val = round((79 * gain) / 0xFFFF); - RegSetValueExA(hKey, "Gain", 0, REG_DWORD, (CONST BYTE*) &val, l); - val = round((255 * wbRed) / 0xFFFF); - RegSetValueExA(hKey, "WhiteBalanceR", 0, REG_DWORD, (CONST BYTE*) &val, l); - val = round((255 * wbGreen) / 0xFFFF); - RegSetValueExA(hKey, "WhiteBalanceG", 0, REG_DWORD, (CONST BYTE*) &val, l); - val = round((255 * wbBlue) / 0xFFFF); - RegSetValueExA(hKey, "WhiteBalanceB", 0, REG_DWORD, (CONST BYTE*) &val, l); - - // restart the camera capture with openCv - if (cc->capture) { + int val; + HKEY hKey; + DWORD l = sizeof(DWORD); + char* PATH = CL_DRIVER_REG_PATH; + int err = RegOpenKeyEx(HKEY_CURRENT_USER, PATH, 0, KEY_ALL_ACCESS, &hKey); + if (err != ERROR_SUCCESS) { + printf("Error: %d Unable to open reg-key: [HKCU]\%s!", err, PATH); + return; + } + val = autoE > 0; + RegSetValueExA(hKey, "AutoAEC", 0, REG_DWORD, (CONST BYTE*) &val, l); + val = autoG > 0; + RegSetValueExA(hKey, "AutoAGC", 0, REG_DWORD, (CONST BYTE*) &val, l); + val = autoWB > 0; + RegSetValueExA(hKey, "AutoAWB", 0, REG_DWORD, (CONST BYTE*) &val, l); + val = round((511 * exposure) / 0xFFFF); + RegSetValueExA(hKey, "Exposure", 0, REG_DWORD, (CONST BYTE*) &val, l); + val = round((79 * gain) / 0xFFFF); + RegSetValueExA(hKey, "Gain", 0, REG_DWORD, (CONST BYTE*) &val, l); + val = round((255 * wbRed) / 0xFFFF); + RegSetValueExA(hKey, "WhiteBalanceR", 0, REG_DWORD, (CONST BYTE*) &val, l); + val = round((255 * wbGreen) / 0xFFFF); + RegSetValueExA(hKey, "WhiteBalanceG", 0, REG_DWORD, (CONST BYTE*) &val, l); + val = round((255 * wbBlue) / 0xFFFF); + RegSetValueExA(hKey, "WhiteBalanceB", 0, REG_DWORD, (CONST BYTE*) &val, l); + + // restart the camera capture with openCv + if (cc->capture) { cvReleaseCapture(&cc->capture); } ps3eye_set_parameters(cc->eye, autoG > 0, autoWB > 0, gain, exposure, contrast, brightness); - int width, height; - get_metrics(&width, &height); + int width, height; + get_metrics(&width, &height); - cc->capture = cvCaptureFromCAM(cc->cameraID); - cvSetCaptureProperty(cc->capture, CV_CAP_PROP_FRAME_WIDTH, width); - cvSetCaptureProperty(cc->capture, CV_CAP_PROP_FRAME_HEIGHT, height); + cc->capture = cvCaptureFromCAM(cc->cameraID); + cvSetCaptureProperty(cc->capture, CV_CAP_PROP_FRAME_WIDTH, width); + cvSetCaptureProperty(cc->capture, CV_CAP_PROP_FRAME_HEIGHT, height); #endif } diff --git a/src/tracker/psmove_tracker.c b/src/tracker/psmove_tracker.c index 4425e90e..48dbce53 100644 --- a/src/tracker/psmove_tracker.c +++ b/src/tracker/psmove_tracker.c @@ -50,32 +50,32 @@ # include "platform/psmove_linuxsupport.h" #endif -//#define DEBUG_WINDOWS // shall additional windows be shown -#define ROIS 4 // the number of levels of regions of interest (roi) -#define BLINKS 2 // number of diff images to create during calibration -#define BLINK_DELAY 200 // number of milliseconds to wait between a blink -#define CALIB_MIN_SIZE 50 // minimum size of the estimated glowing sphere during calibration process (in pixel) -#define CALIB_SIZE_STD 10 // maximum standard deviation (in %) of the glowing spheres found during calibration process -#define CALIB_MAX_DIST 30 // maximum displacement of the separate found blobs -#define COLOR_FILTER_RANGE_H 20 // +- H-Range of the hsv-colorfilter -#define COLOR_FILTER_RANGE_S 85 // +- s-Range of the hsv-colorfilter -#define COLOR_FILTER_RANGE_V 85 // +- v-Range of the hsv-colorfilter +//#define DEBUG_WINDOWS // shall additional windows be shown +#define ROIS 4 // the number of levels of regions of interest (roi) +#define BLINKS 2 // number of diff images to create during calibration +#define BLINK_DELAY 200 // number of milliseconds to wait between a blink +#define CALIB_MIN_SIZE 50 // minimum size of the estimated glowing sphere during calibration process (in pixel) +#define CALIB_SIZE_STD 10 // maximum standard deviation (in %) of the glowing spheres found during calibration process +#define CALIB_MAX_DIST 30 // maximum displacement of the separate found blobs +#define COLOR_FILTER_RANGE_H 20 // +- H-Range of the hsv-colorfilter +#define COLOR_FILTER_RANGE_S 85 // +- s-Range of the hsv-colorfilter +#define COLOR_FILTER_RANGE_V 85 // +- v-Range of the hsv-colorfilter /* Thresholds */ -#define ROI_ADJUST_FPS_T 160 // the minimum fps to be reached, if a better roi-center adjusment is to be perfomred -#define CALIBRATION_DIFF_T 20 // during calibration, all grey values in the diff image below this value are set to black +#define ROI_ADJUST_FPS_T 160 // the minimum fps to be reached, if a better roi-center adjusment is to be perfomred +#define CALIBRATION_DIFF_T 20 // during calibration, all grey values in the diff image below this value are set to black // if tracker thresholds not met, sphere is deemed not to be found -#define TRACKER_QUALITY_T1 0.3 // minimum ratio of number of pixels in blob vs pixel of estimated circle. -#define TRACKER_QUALITY_T2 0.7 // maximum allowed change of the radius in percent, compared to the last estimated radius -#define TRACKER_QUALITY_T3 4 // minimum radius -#define TRACKER_ADAPTIVE_XY 1 // specifies to use a adaptive x/y smoothing -#define TRACKER_ADAPTIVE_Z 1 // specifies to use a adaptive z smoothing -#define COLOR_ADAPTION_QUALITY 35 // maximal distance (calculated by 'psmove_tracker_hsvcolor_diff') between the first estimated color and the newly estimated -#define COLOR_UPDATE_RATE 1 // every x seconds adapt to the color, 0 means no adaption +#define TRACKER_QUALITY_T1 0.3 // minimum ratio of number of pixels in blob vs pixel of estimated circle. +#define TRACKER_QUALITY_T2 0.7 // maximum allowed change of the radius in percent, compared to the last estimated radius +#define TRACKER_QUALITY_T3 4 // minimum radius +#define TRACKER_ADAPTIVE_XY 1 // specifies to use a adaptive x/y smoothing +#define TRACKER_ADAPTIVE_Z 1 // specifies to use a adaptive z smoothing +#define COLOR_ADAPTION_QUALITY 35 // maximal distance (calculated by 'psmove_tracker_hsvcolor_diff') between the first estimated color and the newly estimated +#define COLOR_UPDATE_RATE 1 // every x seconds adapt to the color, 0 means no adaption // if color thresholds not met, color is not adapted -#define COLOR_UPDATE_QUALITY_T1 0.8 // minimum ratio of number of pixels in blob vs pixel of estimated circle. -#define COLOR_UPDATE_QUALITY_T2 0.2 // maximum allowed change of the radius in percent, compared to the last estimated radius -#define COLOR_UPDATE_QUALITY_T3 6 // minimum radius +#define COLOR_UPDATE_QUALITY_T1 0.8 // minimum ratio of number of pixels in blob vs pixel of estimated circle. +#define COLOR_UPDATE_QUALITY_T2 0.2 // maximum allowed change of the radius in percent, compared to the last estimated radius +#define COLOR_UPDATE_QUALITY_T3 6 // minimum radius #ifdef WIN32 #define PSEYE_BACKUP_FILE "PSEye_backup_win.ini" #else @@ -112,24 +112,24 @@ struct _TrackedController { /* Assigned RGB color of the controller */ struct PSMove_RGBValue color; - CvScalar eFColor; // first estimated color (BGR) - CvScalar eFColorHSV; // first estimated color (HSV) + CvScalar eFColor; // first estimated color (BGR) + CvScalar eFColorHSV; // first estimated color (HSV) - CvScalar eColor; // estimated color (BGR) - CvScalar eColorHSV; // estimated color (HSV) + CvScalar eColor; // estimated color (BGR) + CvScalar eColorHSV; // estimated color (HSV) - int roi_x, roi_y; // x/y - Coordinates of the ROI - int roi_level; // the current index for the level of ROI + int roi_x, roi_y; // x/y - Coordinates of the ROI + int roi_level; // the current index for the level of ROI enum PSMove_Bool roi_level_fixed; // true if the ROI level should be fixed - float mx, my; // x/y - Coordinates of center of mass of the blob - float x, y, r; // x/y - Coordinates of the controllers sphere and its radius - int search_tile; // current search quadrant when controller is not found (reset to 0 if found) - float rs; // a smoothed variant of the radius + float mx, my; // x/y - Coordinates of center of mass of the blob + float x, y, r; // x/y - Coordinates of the controllers sphere and its radius + int search_tile; // current search quadrant when controller is not found (reset to 0 if found) + float rs; // a smoothed variant of the radius float q1, q2, q3; // Calculated quality criteria from the tracker - int is_tracked; // 1 if tracked 0 otherwise - long last_color_update; // the timestamp when the last color adaption has been performed + int is_tracked; // 1 if tracked 0 otherwise + long last_color_update; // the timestamp when the last color adaption has been performed enum PSMove_Bool auto_update_leds; }; @@ -183,64 +183,64 @@ pseye_distance_parameters = { }; struct _PSMoveTracker { - CameraControl* cc; + CameraControl* cc; - /* Timestamps for performance measurements */ - PSMove_timestamp ts_camera_begin; // when the capture was started - PSMove_timestamp ts_camera_grab; // when the image was grabbed - PSMove_timestamp ts_camera_retrieve; // when the image was retrieved - PSMove_timestamp ts_camera_converted; // when the image was converted + /* Timestamps for performance measurements */ + PSMove_timestamp ts_camera_begin; // when the capture was started + PSMove_timestamp ts_camera_grab; // when the image was grabbed + PSMove_timestamp ts_camera_retrieve; // when the image was retrieved + PSMove_timestamp ts_camera_converted; // when the image was converted - IplImage* frame; // the current frame of the camera - IplImage *frame_rgb; // the frame as tightly packed RGB data - int exposure; // the exposure to use - IplImage* roiI[ROIS]; // array of images for each level of roi (colored) - IplImage* roiM[ROIS]; // array of images for each level of roi (greyscale) - IplConvKernel* kCalib; // kernel used for morphological operations during calibration - CvScalar rHSV; // the range of the color filter + IplImage* frame; // the current frame of the camera + IplImage *frame_rgb; // the frame as tightly packed RGB data + int exposure; // the exposure to use + IplImage* roiI[ROIS]; // array of images for each level of roi (colored) + IplImage* roiM[ROIS]; // array of images for each level of roi (greyscale) + IplConvKernel* kCalib; // kernel used for morphological operations during calibration + CvScalar rHSV; // the range of the color filter - // Parameters for psmove_tracker_distance_from_radius() - struct PSMoveTracker_DistanceParameters distance_parameters; + // Parameters for psmove_tracker_distance_from_radius() + struct PSMoveTracker_DistanceParameters distance_parameters; - enum PSMoveTracker_Exposure exposure_mode; // exposure mode - float dimming_factor; // dimming factor used on LED RGB values + enum PSMoveTracker_Exposure exposure_mode; // exposure mode + float dimming_factor; // dimming factor used on LED RGB values - TrackedController controllers[PSMOVE_TRACKER_MAX_CONTROLLERS]; // controller data + TrackedController controllers[PSMOVE_TRACKER_MAX_CONTROLLERS]; // controller data - struct ColorMappingRingBuffer color_mapping; // remembered color mappings + struct ColorMappingRingBuffer color_mapping; // remembered color mappings - CvMemStorage* storage; // use to store the result of cvFindContour and cvHughCircles - long duration; // duration of tracking operation, in ms + CvMemStorage* storage; // use to store the result of cvFindContour and cvHughCircles + long duration; // duration of tracking operation, in ms - // size of "search" tiles when tracking is lost - int search_tile_width; // width of a single tile - int search_tile_height; // height of a single tile - int search_tiles_horizontal; // number of search tiles per row - int search_tiles_count; // number of search tiles + // size of "search" tiles when tracking is lost + int search_tile_width; // width of a single tile + int search_tile_height; // height of a single tile + int search_tiles_horizontal; // number of search tiles per row + int search_tiles_count; // number of search tiles - // internal variables - int tracker_adaptive_xy; // should adaptive x/y-smoothing be used - int tracker_adaptive_z; // should adaptive z-smoothing be used + // internal variables + int tracker_adaptive_xy; // should adaptive x/y-smoothing be used + int tracker_adaptive_z; // should adaptive z-smoothing be used - int calibration_t; // the threshold used during calibration to create the diff image + int calibration_t; // the threshold used during calibration to create the diff image - // if one is not met, the tracker is regarded as not found (although something has been found) - float tracker_t1; // quality threshold1 for the tracker - float tracker_t2; // quality threshold2 for the tracker - float tracker_t3; // quality threshold3 for the tracker + // if one is not met, the tracker is regarded as not found (although something has been found) + float tracker_t1; // quality threshold1 for the tracker + float tracker_t2; // quality threshold2 for the tracker + float tracker_t3; // quality threshold3 for the tracker - float adapt_t1; // quality threshold for the color adaption to discard its estimation again + float adapt_t1; // quality threshold for the color adaption to discard its estimation again - // if one is not met, the color will not use adaptive color estimation - float color_t1; // quality threshold3 for the color adaption - float color_t2; // quality threshold3 for the color adaption - float color_t3; // quality threshold3 for the color adaption - float color_update_rate; // how often shall the color be adapted (in seconds), 0 means never + // if one is not met, the color will not use adaptive color estimation + float color_t1; // quality threshold3 for the color adaption + float color_t2; // quality threshold3 for the color adaption + float color_t3; // quality threshold3 for the color adaption + float color_update_rate; // how often shall the color be adapted (in seconds), 0 means never - // internal variables (debug) - float debug_fps; // the current FPS achieved by "psmove_tracker_update" + // internal variables (debug) + float debug_fps; // the current FPS achieved by "psmove_tracker_update" - enum PSMove_Bool mirror; // mirror camera image horizontally + enum PSMove_Bool mirror; // mirror camera image horizontally }; // -------- START: internal functions only @@ -288,7 +288,7 @@ psmove_tracker_wait_for_frame(PSMoveTracker *tracker, IplImage **frame, int dela * tracker - the tracker that contains the camera control * move - the PSMove controller to use * rgb - the RGB color to use to lit the sphere - * on - the pre-allocated image to store the captured image when the sphere is lit + * on - the pre-allocated image to store the captured image when the sphere is lit * diff - the pre-allocated image to store the calculated diff-image * delay - the time to wait before taking a picture (in microseconds) **/ @@ -303,8 +303,8 @@ psmove_tracker_get_diff(PSMoveTracker* tracker, PSMove* move, * * tracker - A valid PSMoveTracker * instance * tc - The TrackableController containing the roi to check & fix - * roi_x - the x-part of the coordinate of the roi - * roi_y - the y-part of the coordinate of the roi + * roi_x - the x-part of the coordinate of the roi + * roi_y - the y-part of the coordinate of the roi * roi_width - the width of the roi * roi_height - the height of the roi * cam_width - the width of the camera image @@ -320,10 +320,10 @@ int psmove_tracker_update_controller(PSMoveTracker* tracker, TrackedController * /* * This finds the biggest contour within the given image. * - * img - (in) the binary image to search for contours - * stor - (out) a storage that can be used to save the result of this function - * resContour - (out) points to the biggest contour found within the image - * resSize - (out) the size of that contour in px� + * img - (in) the binary image to search for contours + * stor - (out) a storage that can be used to save the result of this function + * resContour - (out) points to the biggest contour found within the image + * resSize - (out) the size of that contour in px� */ void psmove_tracker_biggest_contour(IplImage* img, CvMemStorage* stor, CvSeq** resContour, float* resSize); @@ -343,10 +343,10 @@ float psmove_tracker_hsvcolor_diff(TrackedController* tc); * It will calcualte the radius by findin the two most distant points * in the contour. And its by choosing the mid point of those two. * - * cont - (in) The contour representing the orb. + * cont - (in) The contour representing the orb. * x - (out) The X coordinate of the center. * y - (out) The Y coordinate of the center. - * radius - (out) The radius of the contour that is calculated here. + * radius - (out) The radius of the contour that is calculated here. */ void psmove_tracker_estimate_circle_from_contour(CvSeq* cont, float *x, float *y, float* radius); @@ -531,23 +531,23 @@ psmove_tracker_get_mirror(PSMoveTracker *tracker) PSMoveTracker * psmove_tracker_new_with_camera(int camera) { - PSMoveTracker* tracker = (PSMoveTracker*) calloc(1, sizeof(PSMoveTracker)); - tracker->rHSV = cvScalar(COLOR_FILTER_RANGE_H, COLOR_FILTER_RANGE_S, COLOR_FILTER_RANGE_V, 0); - tracker->storage = cvCreateMemStorage(0); - - tracker->dimming_factor = 0.; - - tracker->calibration_t = CALIBRATION_DIFF_T; - tracker->tracker_t1 = TRACKER_QUALITY_T1; - tracker->tracker_t2 = TRACKER_QUALITY_T2; - tracker->tracker_t3 = TRACKER_QUALITY_T3; - tracker->tracker_adaptive_xy = TRACKER_ADAPTIVE_XY; - tracker->tracker_adaptive_z = TRACKER_ADAPTIVE_Z; - tracker->adapt_t1 = COLOR_ADAPTION_QUALITY; - tracker->color_t1 = COLOR_UPDATE_QUALITY_T1; - tracker->color_t2 = COLOR_UPDATE_QUALITY_T2; - tracker->color_t3 = COLOR_UPDATE_QUALITY_T3; - tracker->color_update_rate = COLOR_UPDATE_RATE; + PSMoveTracker* tracker = (PSMoveTracker*) calloc(1, sizeof(PSMoveTracker)); + tracker->rHSV = cvScalar(COLOR_FILTER_RANGE_H, COLOR_FILTER_RANGE_S, COLOR_FILTER_RANGE_V, 0); + tracker->storage = cvCreateMemStorage(0); + + tracker->dimming_factor = 0.; + + tracker->calibration_t = CALIBRATION_DIFF_T; + tracker->tracker_t1 = TRACKER_QUALITY_T1; + tracker->tracker_t2 = TRACKER_QUALITY_T2; + tracker->tracker_t3 = TRACKER_QUALITY_T3; + tracker->tracker_adaptive_xy = TRACKER_ADAPTIVE_XY; + tracker->tracker_adaptive_z = TRACKER_ADAPTIVE_Z; + tracker->adapt_t1 = COLOR_ADAPTION_QUALITY; + tracker->color_t1 = COLOR_UPDATE_QUALITY_T1; + tracker->color_t2 = COLOR_UPDATE_QUALITY_T2; + tracker->color_t3 = COLOR_UPDATE_QUALITY_T3; + tracker->color_update_rate = COLOR_UPDATE_RATE; #if defined(__APPLE__) && !defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER) PSMove *move = psmove_connect(); @@ -562,75 +562,73 @@ psmove_tracker_new_with_camera(int camera) { psmove_update_leds(move); #endif - // start the video capture device for tracking - tracker->cc = camera_control_new(camera); + // start the video capture device for tracking + tracker->cc = camera_control_new(camera); - char *intrinsics_xml = psmove_util_get_file_path(INTRINSICS_XML); - char *distortion_xml = psmove_util_get_file_path(DISTORTION_XML); - camera_control_read_calibration(tracker->cc, intrinsics_xml, distortion_xml); - free(intrinsics_xml); - free(distortion_xml); + char *intrinsics_xml = psmove_util_get_file_path(INTRINSICS_XML); + char *distortion_xml = psmove_util_get_file_path(DISTORTION_XML); + camera_control_read_calibration(tracker->cc, intrinsics_xml, distortion_xml); + free(intrinsics_xml); + free(distortion_xml); - // backup the systems settings, if not already backuped - char *filename = psmove_util_get_file_path(PSEYE_BACKUP_FILE); - camera_control_backup_system_settings(tracker->cc, filename); - free(filename); + // backup the systems settings, if not already backuped + char *filename = psmove_util_get_file_path(PSEYE_BACKUP_FILE); + camera_control_backup_system_settings(tracker->cc, filename); + free(filename); #if !defined(__APPLE__) || defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER) - // try to load color mapping data (not on Mac OS X for now, because the - // automatic white balance means we get different colors every time) - filename = psmove_util_get_file_path(COLOR_MAPPING_DAT); - FILE *fp = NULL; - time_t now = time(NULL); - struct stat st; - memset(&st, 0, sizeof(st)); - - if (stat(filename, &st) == 0 && now != (time_t)-1) { - if (st.st_mtime >= (now - COLOR_MAPPING_MAX_AGE)) { - fp = fopen(filename, "rb"); - } else { - printf("%s is too old - not restoring colors.\n", filename); - } + // try to load color mapping data (not on Mac OS X for now, because the + // automatic white balance means we get different colors every time) + filename = psmove_util_get_file_path(COLOR_MAPPING_DAT); + FILE *fp = NULL; + time_t now = time(NULL); + struct stat st; + memset(&st, 0, sizeof(st)); + + if (stat(filename, &st) == 0 && now != (time_t)-1) { + if (st.st_mtime >= (now - COLOR_MAPPING_MAX_AGE)) { + fp = fopen(filename, "rb"); + } else { + printf("%s is too old - not restoring colors.\n", filename); } + } - if (fp) { - if (!fread(&(tracker->color_mapping), - sizeof(struct ColorMappingRingBuffer), - 1, fp)) { - psmove_WARNING("Cannot read data from: %s\n", filename); - } else { - printf("color mappings restored.\n"); - } - - fclose(fp); + if (fp) { + if (!fread(&(tracker->color_mapping), sizeof(struct ColorMappingRingBuffer), 1, fp)) { + psmove_WARNING("Cannot read data from: %s\n", filename); + } else { + printf("color mappings restored.\n"); } - free(filename); + + fclose(fp); + } + free(filename); #endif - // Default to the distance parameters for the PS Eye camera - tracker->distance_parameters = pseye_distance_parameters; + // Default to the distance parameters for the PS Eye camera + tracker->distance_parameters = pseye_distance_parameters; - // use static exposure - psmove_tracker_set_exposure(tracker, Exposure_LOW); + // use static exposure + psmove_tracker_set_exposure(tracker, Exposure_LOW); - // just query a frame so that we know the camera works - IplImage* frame = NULL; - while (!frame) { - frame = camera_control_query_frame(tracker->cc, NULL, NULL); - } + // just query a frame so that we know the camera works + IplImage* frame = NULL; + while (!frame) { + frame = camera_control_query_frame(tracker->cc, NULL, NULL); + } - // prepare ROI data structures + // prepare ROI data structures - /* Define the size of the biggest ROI */ - int size = psmove_util_get_env_int(PSMOVE_TRACKER_ROI_SIZE_ENV); + /* Define the size of the biggest ROI */ + int size = psmove_util_get_env_int(PSMOVE_TRACKER_ROI_SIZE_ENV); - if (size == -1) { - size = MIN(frame->width, frame->height) / 2; - } else { - psmove_DEBUG("Using ROI size: %d\n", size); - } + if (size == -1) { + size = MIN(frame->width, frame->height) / 2; + } else { + psmove_DEBUG("Using ROI size: %d\n", size); + } - int w = size, h = size; + int w = size, h = size; // We need to grab an image from the camera to determine the frame size psmove_tracker_update_image(tracker); @@ -668,19 +666,19 @@ psmove_tracker_new_with_camera(int camera) { } - int i; - for (i = 0; i < ROIS; i++) { - tracker->roiI[i] = cvCreateImage(cvSize(w,h), frame->depth, 3); - tracker->roiM[i] = cvCreateImage(cvSize(w,h), frame->depth, 1); + int i; + for (i = 0; i < ROIS; i++) { + tracker->roiI[i] = cvCreateImage(cvSize(w,h), frame->depth, 3); + tracker->roiM[i] = cvCreateImage(cvSize(w,h), frame->depth, 1); - /* Smaller rois are always square, and 70% of the previous level */ - h = w = MIN(w,h) * 0.7f; - } + /* Smaller rois are always square, and 70% of the previous level */ + h = w = MIN(w,h) * 0.7f; + } - // prepare structure used for erode and dilate in calibration process - int ks = 5; // Kernel Size - int kc = (ks + 1) / 2; // Kernel Center - tracker->kCalib = cvCreateStructuringElementEx(ks, ks, kc, kc, CV_SHAPE_RECT, NULL); + // prepare structure used for erode and dilate in calibration process + int ks = 5; // Kernel Size + int kc = (ks + 1) / 2; // Kernel Center + tracker->kCalib = cvCreateStructuringElementEx(ks, ks, kc, kc, CV_SHAPE_RECT, NULL); #if defined(__APPLE__) && !defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER) printf("Move the controller away and press the Move button\n"); @@ -690,7 +688,7 @@ psmove_tracker_new_with_camera(int camera) { psmove_disconnect(move); #endif - return tracker; + return tracker; } enum PSMoveTracker_Status @@ -802,7 +800,8 @@ psmove_tracker_lookup_color(PSMoveTracker *tracker, struct PSMove_RGBValue rgb, } void -psmove_tracker_remember_color(PSMoveTracker *tracker, struct PSMove_RGBValue rgb, CvScalar color) +psmove_tracker_remember_color(PSMoveTracker *tracker, + struct PSMove_RGBValue rgb, CvScalar color) { unsigned char dimming = 255 * tracker->dimming_factor; @@ -1059,7 +1058,8 @@ psmove_tracker_enable_with_color_internal(PSMoveTracker *tracker, PSMove *move, CvScalar color; CvScalar hsv_color; - if (psmove_tracker_blinking_calibration(tracker, move, rgb, &color, &hsv_color)) { + if (psmove_tracker_blinking_calibration(tracker, move, + rgb, &color, &hsv_color)) { // Find the next free slot to use as TrackedController TrackedController *tc = psmove_tracker_find_controller(tracker, NULL); @@ -1185,7 +1185,7 @@ psmove_tracker_get_status(PSMoveTracker *tracker, PSMove *move) void* psmove_tracker_get_frame(PSMoveTracker *tracker) { - return tracker->frame; + return tracker->frame; } PSMoveTrackerRGBImage @@ -1232,228 +1232,230 @@ void psmove_tracker_update_image(PSMoveTracker *tracker) { int psmove_tracker_update_controller(PSMoveTracker *tracker, TrackedController *tc) { - float x, y; - int i = 0; - int sphere_found = 0; - - if (tc->auto_update_leds) { - unsigned char r, g, b; - psmove_tracker_get_color(tracker, tc->move, &r, &g, &b); - psmove_set_leds(tc->move, r, g, b); - psmove_update_leds(tc->move); + float x, y; + int i = 0; + int sphere_found = 0; + + if (tc->auto_update_leds) { + unsigned char r, g, b; + psmove_tracker_get_color(tracker, tc->move, &r, &g, &b); + psmove_set_leds(tc->move, r, g, b); + psmove_update_leds(tc->move); + } + + // calculate upper & lower bounds for the color filter + CvScalar min = th_scalar_sub(tc->eColorHSV, tracker->rHSV); + CvScalar max = th_scalar_add(tc->eColorHSV, tracker->rHSV); + + // this is the tracking algorithm + while (1) { + // get pointers to data structures for the given ROI-Level + IplImage *roi_i = tracker->roiI[tc->roi_level]; + IplImage *roi_m = tracker->roiM[tc->roi_level]; + + // adjust the ROI, so that the blob is fully visible, but only if we have a reasonable FPS + if (tracker->debug_fps > ROI_ADJUST_FPS_T) { + // TODO: check for validity differently + CvPoint nRoiCenter; + if (psmove_tracker_center_roi_on_controller(tc, tracker, &nRoiCenter)) { + psmove_tracker_set_roi(tracker, tc, nRoiCenter.x, nRoiCenter.y, roi_i->width, roi_i->height); + } + } + + // apply the ROI + cvSetImageROI(tracker->frame, cvRect(tc->roi_x, tc->roi_y, roi_i->width, roi_i->height)); + cvCvtColor(tracker->frame, roi_i, CV_BGR2HSV); + + // apply color filter + cvInRangeS(roi_i, min, max, roi_m); + + #ifdef DEBUG_WINDOWS + cvShowImage("binary:0", roi_m); + cvShowImage("hsv:0", roi_i); + #endif + + // find the biggest contour in the image + float sizeBest = 0; + CvSeq* contourBest = NULL; + psmove_tracker_biggest_contour(roi_m, tracker->storage, &contourBest, &sizeBest); + + if (contourBest) { + CvMoments mu; // ImageMoments are use to calculate the center of mass of the blob + CvRect br = cvBoundingRect(contourBest, 0); + + // restore the biggest contour + cvSet(roi_m, TH_COLOR_BLACK, NULL); + cvDrawContours(roi_m, contourBest, TH_COLOR_WHITE, TH_COLOR_WHITE, -1, CV_FILLED, 8, cvPoint(0, 0)); + // calucalte image-moments + cvMoments(roi_m, &mu, 0); + // calucalte the mass center + CvPoint p = cvPoint(mu.m10 / mu.m00, mu.m01 / mu.m00); + CvPoint oldMCenter = cvPoint(tc->mx, tc->my); + tc->mx = p.x + tc->roi_x; + tc->my = p.y + tc->roi_y; + CvPoint newMCenter = cvPoint(tc->mx, tc->my); + + // remember the old radius and calcutlate the new x/y position and radius of the found contour + float oldRadius = tc->r; + // estimate x/y position and radius of the sphere + psmove_tracker_estimate_circle_from_contour(contourBest, &x, &y, &tc->r); + + // apply radius-smoothing if enabled + if (tracker->tracker_adaptive_z) { + // calculate the difference between calculated radius and the smoothed radius of the past + float rDiff = abs(tc->rs - tc->r); + // calcualte a adaptive smoothing factor + // a big distance leads to no smoothing, a small one to strong smoothing + float rf = MIN(rDiff/4+0.15,1); + + // apply adaptive smoothing of the radius + tc->rs = tc->rs * (1 - rf) + tc->r * rf; + tc->r = tc->rs; + } + + // apply x/y coordinate smoothing if enabled + if (tracker->tracker_adaptive_z) { + // a big distance between the old and new center of mass results in no smoothing + // a little one to strong smoothing + float diff = sqrt(th_dist_squared(oldMCenter, newMCenter)); + float f = MIN(diff / 7 + 0.15, 1); + // apply adaptive smoothing + tc->x = tc->x * (1 - f) + (x + tc->roi_x) * f; + tc->y = tc->y * (1 - f) + (y + tc->roi_y) * f; + } else { + // do NOT apply adaptive smoothing + tc->x = x + tc->roi_x; + tc->y = y + tc->roi_y; + } + + // calculate the quality of the tracking + int pixelInBlob = cvCountNonZero(roi_m); + float pixelInResult = tc->r * tc->r * M_PI; + tc->q1 = 0; + tc->q2 = FLT_MAX; + tc->q3 = tc->r; + + // decrease TQ1 by half if below 20px (gives better results if controller is far away) + if (pixelInBlob < 20) { + tc->q1 /= 2; + } + + // The quality checks are all performed on the radius of the blob + // its old radius and size. + tc->q1 = pixelInBlob / pixelInResult; + + // always check pixel-ratio and minimal size + sphere_found = tc->q1 > tracker->tracker_t1 && tc->q3 > tracker->tracker_t3; + + // use the mass center if the quality is very good + // TODO: make 0.85 as a CONST + if (tc->q1 > 0.85) { + tc->x = tc->mx; + tc->y = tc->my; + } + // only perform check if we already found the sphere once + if (oldRadius > 0 && tc->search_tile==0) { + tc->q2 = abs(oldRadius - tc->r) / (oldRadius + FLT_EPSILON); + + // additionally check for to big changes + sphere_found = sphere_found && tc->q2 < tracker->tracker_t2; + } + + // only if the quality is okay update the future ROI + if (sphere_found) { + // use adaptive color detection + // only if 1) the sphere has been found + // AND 2) the UPDATE_RATE has passed + // AND 3) the tracking-quality is high; + int do_color_adaption = 0; + long now = psmove_util_get_ticks(); + if (tracker->color_update_rate > 0 && (now - tc->last_color_update) > tracker->color_update_rate*1000){ + do_color_adaption = 1; + } + + if (do_color_adaption && tc->q1 > tracker->color_t1 && tc->q2 < tracker->color_t2 && tc->q3 > tracker->color_t3) { + // calculate the new estimated color (adaptive color estimation) + CvScalar newColor = cvAvg(tracker->frame, roi_m); + + tc->eColor = th_scalar_mul(th_scalar_add(tc->eColor, newColor), 0.5); + + tc->eColorHSV = th_brg2hsv(tc->eColor); + tc->last_color_update = now; + // CHECK if the current estimate is too far away from its original estimation + if (psmove_tracker_hsvcolor_diff(tc) > tracker->adapt_t1) { + tc->eColor = tc->eFColor; + tc->eColorHSV = tc->eFColorHSV; + sphere_found = 0; + } + } + + // update the future roi box + br.width = MAX(br.width, br.height) * 3; + br.height = br.width; + // find a suitable ROI level + for (i = 0; i < ROIS; i++) { + if (br.width > tracker->roiI[i]->width && br.height > tracker->roiI[i]->height){ + break; + } + + if (tc->roi_level_fixed) { + tc->roi_level = 0; + } else { + tc->roi_level = i; + } + // update easy accessors + roi_i = tracker->roiI[tc->roi_level]; + roi_m = tracker->roiM[tc->roi_level]; + } + + // assure that the roi is within the target image + psmove_tracker_set_roi(tracker, tc, tc->x - roi_i->width / 2, tc->y - roi_i->height / 2, roi_i->width, roi_i->height); + } + } + cvClearMemStorage(tracker->storage); + cvResetImageROI(tracker->frame); + + if (sphere_found) { + //tc->search_tile = 0; + // the sphere was found + break; + }else if(tc->roi_level>0){ + // the sphere was not found, increase the ROI and search again! + tc->roi_x += roi_i->width / 2; + tc->roi_y += roi_i->height / 2; + + if (tc->roi_level_fixed) { + tc->roi_level = 0; + } else { + tc->roi_level = tc->roi_level - 1; + } + // update easy accessors + roi_i = tracker->roiI[tc->roi_level]; + roi_m = tracker->roiM[tc->roi_level]; + + // assure that the roi is within the target image + psmove_tracker_set_roi(tracker, tc, tc->roi_x -roi_i->width / 2, tc->roi_y - roi_i->height / 2, roi_i->width, roi_i->height); + }else { + int rx; + int ry; + // the sphere could not be found til a reasonable roi-level + + rx = tracker->search_tile_width * (tc->search_tile % + tracker->search_tiles_horizontal); + ry = tracker->search_tile_height * (int)(tc->search_tile / + tracker->search_tiles_horizontal); + tc->search_tile = ((tc->search_tile + 2) % + tracker->search_tiles_count); + + tc->roi_level=0; + psmove_tracker_set_roi(tracker, tc, rx, ry, tracker->roiI[tc->roi_level]->width, tracker->roiI[tc->roi_level]->height); + break; } + } - // calculate upper & lower bounds for the color filter - CvScalar min = th_scalar_sub(tc->eColorHSV, tracker->rHSV); - CvScalar max = th_scalar_add(tc->eColorHSV, tracker->rHSV); - - // this is the tracking algorithm - while (1) { - // get pointers to data structures for the given ROI-Level - IplImage *roi_i = tracker->roiI[tc->roi_level]; - IplImage *roi_m = tracker->roiM[tc->roi_level]; - - // adjust the ROI, so that the blob is fully visible, but only if we have a reasonable FPS - if (tracker->debug_fps > ROI_ADJUST_FPS_T) { - // TODO: check for validity differently - CvPoint nRoiCenter; - if (psmove_tracker_center_roi_on_controller(tc, tracker, &nRoiCenter)) { - psmove_tracker_set_roi(tracker, tc, nRoiCenter.x, nRoiCenter.y, roi_i->width, roi_i->height); - } - } - - // apply the ROI - cvSetImageROI(tracker->frame, cvRect(tc->roi_x, tc->roi_y, roi_i->width, roi_i->height)); - cvCvtColor(tracker->frame, roi_i, CV_BGR2HSV); - - // apply color filter - cvInRangeS(roi_i, min, max, roi_m); - - #ifdef DEBUG_WINDOWS - cvShowImage("binary:0", roi_m); - cvShowImage("hsv:0", roi_i); - #endif - - // find the biggest contour in the image - float sizeBest = 0; - CvSeq* contourBest = NULL; - psmove_tracker_biggest_contour(roi_m, tracker->storage, &contourBest, &sizeBest); - - if (contourBest) { - CvMoments mu; // ImageMoments are use to calculate the center of mass of the blob - CvRect br = cvBoundingRect(contourBest, 0); - - // restore the biggest contour - cvSet(roi_m, TH_COLOR_BLACK, NULL); - cvDrawContours(roi_m, contourBest, TH_COLOR_WHITE, TH_COLOR_WHITE, -1, CV_FILLED, 8, cvPoint(0, 0)); - // calucalte image-moments - cvMoments(roi_m, &mu, 0); - // calucalte the mass center - CvPoint p = cvPoint(mu.m10 / mu.m00, mu.m01 / mu.m00); - CvPoint oldMCenter = cvPoint(tc->mx, tc->my); - tc->mx = p.x + tc->roi_x; - tc->my = p.y + tc->roi_y; - CvPoint newMCenter = cvPoint(tc->mx, tc->my); - - // remember the old radius and calcutlate the new x/y position and radius of the found contour - float oldRadius = tc->r; - // estimate x/y position and radius of the sphere - psmove_tracker_estimate_circle_from_contour(contourBest, &x, &y, &tc->r); - - // apply radius-smoothing if enabled - if (tracker->tracker_adaptive_z) { - // calculate the difference between calculated radius and the smoothed radius of the past - float rDiff = abs(tc->rs - tc->r); - // calcualte a adaptive smoothing factor - // a big distance leads to no smoothing, a small one to strong smoothing - float rf = MIN(rDiff/4+0.15,1); - - // apply adaptive smoothing of the radius - tc->rs = tc->rs * (1 - rf) + tc->r * rf; - tc->r = tc->rs; - } - - // apply x/y coordinate smoothing if enabled - if (tracker->tracker_adaptive_z) { - // a big distance between the old and new center of mass results in no smoothing - // a little one to strong smoothing - float diff = sqrt(th_dist_squared(oldMCenter, newMCenter)); - float f = MIN(diff / 7 + 0.15, 1); - // apply adaptive smoothing - tc->x = tc->x * (1 - f) + (x + tc->roi_x) * f; - tc->y = tc->y * (1 - f) + (y + tc->roi_y) * f; - } else { - // do NOT apply adaptive smoothing - tc->x = x + tc->roi_x; - tc->y = y + tc->roi_y; - } - - // calculate the quality of the tracking - int pixelInBlob = cvCountNonZero(roi_m); - float pixelInResult = tc->r * tc->r * M_PI; - tc->q1 = 0; - tc->q2 = FLT_MAX; - tc->q3 = tc->r; - - // decrease TQ1 by half if below 20px (gives better results if controller is far away) - if (pixelInBlob < 20) { - tc->q1 /= 2; - } - - // The quality checks are all performed on the radius of the blob - // its old radius and size. - tc->q1 = pixelInBlob / pixelInResult; - - // always check pixel-ratio and minimal size - sphere_found = tc->q1 > tracker->tracker_t1 && tc->q3 > tracker->tracker_t3; - - // use the mass center if the quality is very good - // TODO: make 0.85 as a CONST - if (tc->q1 > 0.85) { - tc->x = tc->mx; - tc->y = tc->my; - } - // only perform check if we already found the sphere once - if (oldRadius > 0 && tc->search_tile==0) { - tc->q2 = abs(oldRadius - tc->r) / (oldRadius + FLT_EPSILON); - - // additionally check for to big changes - sphere_found = sphere_found && tc->q2 < tracker->tracker_t2; - } - - // only if the quality is okay update the future ROI - if (sphere_found) { - // use adaptive color detection - // only if 1) the sphere has been found - // AND 2) the UPDATE_RATE has passed - // AND 3) the tracking-quality is high; - int do_color_adaption = 0; - long now = psmove_util_get_ticks(); - if (tracker->color_update_rate > 0 && (now - tc->last_color_update) > tracker->color_update_rate*1000) - do_color_adaption = 1; - - if (do_color_adaption && tc->q1 > tracker->color_t1 && tc->q2 < tracker->color_t2 && tc->q3 > tracker->color_t3) { - // calculate the new estimated color (adaptive color estimation) - CvScalar newColor = cvAvg(tracker->frame, roi_m); - - tc->eColor = th_scalar_mul(th_scalar_add(tc->eColor, newColor), 0.5); - - tc->eColorHSV = th_brg2hsv(tc->eColor); - tc->last_color_update = now; - // CHECK if the current estimate is too far away from its original estimation - if (psmove_tracker_hsvcolor_diff(tc) > tracker->adapt_t1) { - tc->eColor = tc->eFColor; - tc->eColorHSV = tc->eFColorHSV; - sphere_found = 0; - } - } - - // update the future roi box - br.width = MAX(br.width, br.height) * 3; - br.height = br.width; - // find a suitable ROI level - for (i = 0; i < ROIS; i++) { - if (br.width > tracker->roiI[i]->width && br.height > tracker->roiI[i]->height) - break; - - if (tc->roi_level_fixed) { - tc->roi_level = 0; - } else { - tc->roi_level = i; - } - // update easy accessors - roi_i = tracker->roiI[tc->roi_level]; - roi_m = tracker->roiM[tc->roi_level]; - } - - // assure that the roi is within the target image - psmove_tracker_set_roi(tracker, tc, tc->x - roi_i->width / 2, tc->y - roi_i->height / 2, roi_i->width, roi_i->height); - } - } - cvClearMemStorage(tracker->storage); - cvResetImageROI(tracker->frame); - - if (sphere_found) { - //tc->search_tile = 0; - // the sphere was found - break; - }else if(tc->roi_level>0){ - // the sphere was not found, increase the ROI and search again! - tc->roi_x += roi_i->width / 2; - tc->roi_y += roi_i->height / 2; - - if (tc->roi_level_fixed) { - tc->roi_level = 0; - } else { - tc->roi_level = tc->roi_level - 1; - } - // update easy accessors - roi_i = tracker->roiI[tc->roi_level]; - roi_m = tracker->roiM[tc->roi_level]; - - // assure that the roi is within the target image - psmove_tracker_set_roi(tracker, tc, tc->roi_x -roi_i->width / 2, tc->roi_y - roi_i->height / 2, roi_i->width, roi_i->height); - }else { - int rx; - int ry; - // the sphere could not be found til a reasonable roi-level - - rx = tracker->search_tile_width * (tc->search_tile % - tracker->search_tiles_horizontal); - ry = tracker->search_tile_height * (int)(tc->search_tile / - tracker->search_tiles_horizontal); - tc->search_tile = ((tc->search_tile + 2) % - tracker->search_tiles_count); - - tc->roi_level=0; - psmove_tracker_set_roi(tracker, tc, rx, ry, tracker->roiI[tc->roi_level]->width, tracker->roiI[tc->roi_level]->height); - break; - } - } - - // remember if the sphere was found - tc->is_tracked = sphere_found; - return sphere_found; + // remember if the sphere was found + tc->is_tracked = sphere_found; + return sphere_found; } int @@ -1506,7 +1508,7 @@ psmove_tracker_get_position(PSMoveTracker *tracker, PSMove *move, void psmove_tracker_get_size(PSMoveTracker *tracker, - int *width, int *height) + int *width, int *height) { psmove_return_if_fail(tracker != NULL); psmove_return_if_fail(tracker->frame != NULL); @@ -1565,7 +1567,8 @@ psmove_tracker_adapt_to_light(PSMoveTracker *tracker, float target_luminance) int i; for (i=0; i<7; i++) { camera_control_set_parameters(tracker->cc, 0, 0, 0, - (int)current_exposure, 0, 0xffff, 0xffff, 0xffff, -1, -1); + (int)current_exposure, 0, + 0xffff, 0xffff, 0xffff, -1, -1); IplImage* frame; psmove_tracker_wait_for_frame(tracker, &frame, 50); @@ -1627,147 +1630,154 @@ psmove_tracker_wait_for_frame(PSMoveTracker *tracker, IplImage **frame, int dela } void psmove_tracker_get_diff(PSMoveTracker* tracker, PSMove* move, - struct PSMove_RGBValue rgb, IplImage* on, IplImage* diff, int delay, - float dimming_factor) + struct PSMove_RGBValue rgb, IplImage* on, + IplImage* diff, int delay, float dimming_factor) { - // the time to wait for the controller to set the color up - IplImage* frame; - // switch the LEDs ON and wait for the sphere to be fully lit - rgb.r *= dimming_factor; - rgb.g *= dimming_factor; - rgb.b *= dimming_factor; - psmove_set_leds(move, rgb.r, rgb.g, rgb.b); - psmove_update_leds(move); - - // take the first frame (sphere lit) - psmove_tracker_wait_for_frame(tracker, &frame, delay); - cvCopy(frame, on, NULL); - - // switch the LEDs OFF and wait for the sphere to be off - psmove_set_leds(move, 0, 0, 0); - psmove_update_leds(move); - - // take the second frame (sphere iff) - psmove_tracker_wait_for_frame(tracker, &frame, delay); - - // convert both to grayscale images - IplImage* grey1 = cvCloneImage(diff); - IplImage* grey2 = cvCloneImage(diff); - cvCvtColor(frame, grey1, CV_BGR2GRAY); - cvCvtColor(on, grey2, CV_BGR2GRAY); - - // calculate the diff of to images and save it in "diff" - cvAbsDiff(grey1, grey2, diff); - - // clean up - cvReleaseImage(&grey1); - cvReleaseImage(&grey2); + // the time to wait for the controller to set the color up + IplImage* frame; + // switch the LEDs ON and wait for the sphere to be fully lit + rgb.r *= dimming_factor; + rgb.g *= dimming_factor; + rgb.b *= dimming_factor; + psmove_set_leds(move, rgb.r, rgb.g, rgb.b); + psmove_update_leds(move); + + // take the first frame (sphere lit) + psmove_tracker_wait_for_frame(tracker, &frame, delay); + cvCopy(frame, on, NULL); + + // switch the LEDs OFF and wait for the sphere to be off + psmove_set_leds(move, 0, 0, 0); + psmove_update_leds(move); + + // take the second frame (sphere iff) + psmove_tracker_wait_for_frame(tracker, &frame, delay); + + // convert both to grayscale images + IplImage* grey1 = cvCloneImage(diff); + IplImage* grey2 = cvCloneImage(diff); + cvCvtColor(frame, grey1, CV_BGR2GRAY); + cvCvtColor(on, grey2, CV_BGR2GRAY); + + // calculate the diff of to images and save it in "diff" + cvAbsDiff(grey1, grey2, diff); + + // clean up + cvReleaseImage(&grey1); + cvReleaseImage(&grey2); } -void psmove_tracker_set_roi(PSMoveTracker* tracker, TrackedController* tc, int roi_x, int roi_y, int roi_width, int roi_height) { - tc->roi_x = roi_x; - tc->roi_y = roi_y; - - if (tc->roi_x < 0) - tc->roi_x = 0; - if (tc->roi_y < 0) - tc->roi_y = 0; - - if (tc->roi_x + roi_width > tracker->frame->width) - tc->roi_x = tracker->frame->width - roi_width; - if (tc->roi_y + roi_height > tracker->frame->height) - tc->roi_y = tracker->frame->height - roi_height; +void psmove_tracker_set_roi(PSMoveTracker* tracker, TrackedController* tc, + int roi_x, int roi_y, int roi_width, int roi_height) { + tc->roi_x = roi_x; + tc->roi_y = roi_y; + + if (tc->roi_x < 0) { + tc->roi_x = 0; + } + if (tc->roi_y < 0) { + tc->roi_y = 0; + } + + if (tc->roi_x + roi_width > tracker->frame->width) { + tc->roi_x = tracker->frame->width - roi_width; + } + if (tc->roi_y + roi_height > tracker->frame->height) { + tc->roi_y = tracker->frame->height - roi_height; + } } void psmove_tracker_annotate(PSMoveTracker* tracker) { - CvPoint p; - IplImage* frame = tracker->frame; - - CvFont fontSmall = cvFont(0.8, 1); - CvFont fontNormal = cvFont(1, 1); - - char text[256]; - CvScalar c; - CvScalar avgC; - float avgLum = 0; - int roi_w = 0; - int roi_h = 0; - - // general statistics - avgC = cvAvg(frame, 0x0); - avgLum = th_color_avg(avgC); - cvRectangle(frame, cvPoint(0, 0), cvPoint(frame->width, 25), TH_COLOR_BLACK, CV_FILLED, 8, 0); - sprintf(text, "fps:%.0f", tracker->debug_fps); - cvPutText(frame, text, cvPoint(10, 20), &fontNormal, TH_COLOR_WHITE); - if (tracker->duration) { - tracker->debug_fps = (0.85 * tracker->debug_fps + 0.15 * - (1000. / (double)tracker->duration)); + CvPoint p; + IplImage* frame = tracker->frame; + + CvFont fontSmall = cvFont(0.8, 1); + CvFont fontNormal = cvFont(1, 1); + + char text[256]; + CvScalar c; + CvScalar avgC; + float avgLum = 0; + int roi_w = 0; + int roi_h = 0; + + // general statistics + avgC = cvAvg(frame, 0x0); + avgLum = th_color_avg(avgC); + cvRectangle(frame, cvPoint(0, 0), cvPoint(frame->width, 25), TH_COLOR_BLACK, CV_FILLED, 8, 0); + sprintf(text, "fps:%.0f", tracker->debug_fps); + cvPutText(frame, text, cvPoint(10, 20), &fontNormal, TH_COLOR_WHITE); + if (tracker->duration) { + tracker->debug_fps = (0.85 * tracker->debug_fps + 0.15 * + (1000. / (double)tracker->duration)); + } + sprintf(text, "avg(lum):%.0f", avgLum); + cvPutText(frame, text, cvPoint(255, 20), &fontNormal, TH_COLOR_WHITE); + + + // draw all/one controller information to camera image + TrackedController *tc; + for_each_controller(tracker, tc) { + if (tc->is_tracked) { + // controller specific statistics + p.x = tc->x; + p.y = tc->y; + roi_w = tracker->roiI[tc->roi_level]->width; + roi_h = tracker->roiI[tc->roi_level]->height; + c = tc->eColor; + + cvRectangle(frame, cvPoint(tc->roi_x, tc->roi_y), cvPoint(tc->roi_x + roi_w, tc->roi_y + roi_h), TH_COLOR_WHITE, 3, 8, 0); + cvRectangle(frame, cvPoint(tc->roi_x, tc->roi_y), cvPoint(tc->roi_x + roi_w, tc->roi_y + roi_h), TH_COLOR_RED, 1, 8, 0); + cvRectangle(frame, cvPoint(tc->roi_x, tc->roi_y - 45), cvPoint(tc->roi_x + roi_w, tc->roi_y - 5), TH_COLOR_BLACK, CV_FILLED, 8, 0); + + int vOff = 0; + if (roi_h == frame->height) { + vOff = roi_h; + } + sprintf(text, "RGB:%x,%x,%x", (int) c.val[2], (int) c.val[1], (int) c.val[0]); + cvPutText(frame, text, cvPoint(tc->roi_x, tc->roi_y + vOff - 5), &fontSmall, c); + + sprintf(text, "ROI:%dx%d", roi_w, roi_h); + cvPutText(frame, text, cvPoint(tc->roi_x, tc->roi_y + vOff - 15), &fontSmall, c); + + double distance = psmove_tracker_distance_from_radius(tracker, tc->r); + sprintf(text, "radius: %.2f", tc->r); + cvPutText(frame, text, cvPoint(tc->roi_x, tc->roi_y + vOff - 35), &fontSmall, c); + sprintf(text, "dist: %.2f cm", distance); + cvPutText(frame, text, cvPoint(tc->roi_x, tc->roi_y + vOff - 25), &fontSmall, c); + + cvCircle(frame, p, tc->r, TH_COLOR_WHITE, 1, 8, 0); + } else { + roi_w = tracker->roiI[tc->roi_level]->width; + roi_h = tracker->roiI[tc->roi_level]->height; + cvRectangle(frame, cvPoint(tc->roi_x, tc->roi_y), cvPoint(tc->roi_x + roi_w, tc->roi_y + roi_h), tc->eColor, 3, 8, 0); } - sprintf(text, "avg(lum):%.0f", avgLum); - cvPutText(frame, text, cvPoint(255, 20), &fontNormal, TH_COLOR_WHITE); - - - // draw all/one controller information to camera image - TrackedController *tc; - for_each_controller(tracker, tc) { - if (tc->is_tracked) { - // controller specific statistics - p.x = tc->x; - p.y = tc->y; - roi_w = tracker->roiI[tc->roi_level]->width; - roi_h = tracker->roiI[tc->roi_level]->height; - c = tc->eColor; - - cvRectangle(frame, cvPoint(tc->roi_x, tc->roi_y), cvPoint(tc->roi_x + roi_w, tc->roi_y + roi_h), TH_COLOR_WHITE, 3, 8, 0); - cvRectangle(frame, cvPoint(tc->roi_x, tc->roi_y), cvPoint(tc->roi_x + roi_w, tc->roi_y + roi_h), TH_COLOR_RED, 1, 8, 0); - cvRectangle(frame, cvPoint(tc->roi_x, tc->roi_y - 45), cvPoint(tc->roi_x + roi_w, tc->roi_y - 5), TH_COLOR_BLACK, CV_FILLED, 8, 0); - - int vOff = 0; - if (roi_h == frame->height) - vOff = roi_h; - sprintf(text, "RGB:%x,%x,%x", (int) c.val[2], (int) c.val[1], (int) c.val[0]); - cvPutText(frame, text, cvPoint(tc->roi_x, tc->roi_y + vOff - 5), &fontSmall, c); - - sprintf(text, "ROI:%dx%d", roi_w, roi_h); - cvPutText(frame, text, cvPoint(tc->roi_x, tc->roi_y + vOff - 15), &fontSmall, c); - - double distance = psmove_tracker_distance_from_radius(tracker, tc->r); - - sprintf(text, "radius: %.2f", tc->r); - cvPutText(frame, text, cvPoint(tc->roi_x, tc->roi_y + vOff - 35), &fontSmall, c); - sprintf(text, "dist: %.2f cm", distance); - cvPutText(frame, text, cvPoint(tc->roi_x, tc->roi_y + vOff - 25), &fontSmall, c); - - cvCircle(frame, p, tc->r, TH_COLOR_WHITE, 1, 8, 0); - } else { - roi_w = tracker->roiI[tc->roi_level]->width; - roi_h = tracker->roiI[tc->roi_level]->height; - cvRectangle(frame, cvPoint(tc->roi_x, tc->roi_y), cvPoint(tc->roi_x + roi_w, tc->roi_y + roi_h), tc->eColor, 3, 8, 0); - } - } + } } float psmove_tracker_hsvcolor_diff(TrackedController* tc) { - float diff = 0; - diff += abs(tc->eFColorHSV.val[0] - tc->eColorHSV.val[0]) * 1; // diff of HUE is very important - diff += abs(tc->eFColorHSV.val[1] - tc->eColorHSV.val[1]) * 0.5; // saturation and value not so much - diff += abs(tc->eFColorHSV.val[2] - tc->eColorHSV.val[2]) * 0.5; - return diff; + float diff = 0; + diff += abs(tc->eFColorHSV.val[0] - tc->eColorHSV.val[0]) * 1; // diff of HUE is very important + diff += abs(tc->eFColorHSV.val[1] - tc->eColorHSV.val[1]) * 0.5; // saturation and value not so much + diff += abs(tc->eFColorHSV.val[2] - tc->eColorHSV.val[2]) * 0.5; + return diff; } -void psmove_tracker_biggest_contour(IplImage* img, CvMemStorage* stor, CvSeq** resContour, float* resSize) { - CvSeq* contour; - *resSize = 0; - *resContour = 0; - cvFindContours(img, stor, &contour, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0, 0)); - - for (; contour; contour = contour->h_next) { - float f = cvContourArea(contour, CV_WHOLE_SEQ, 0); - if (f > *resSize) { - *resSize = f; - *resContour = contour; - } - } +void psmove_tracker_biggest_contour(IplImage* img, CvMemStorage* stor, + CvSeq** resContour, float* resSize) { + CvSeq* contour; + *resSize = 0; + *resContour = 0; + cvFindContours(img, stor, &contour, sizeof(CvContour), + CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0, 0)); + + for (; contour; contour = contour->h_next) { + float f = cvContourArea(contour, CV_WHOLE_SEQ, 0); + if (f > *resSize) { + *resSize = f; + *resContour = contour; + } + } } void @@ -1776,39 +1786,39 @@ psmove_tracker_estimate_circle_from_contour(CvSeq* cont, float *x, float *y, flo psmove_return_if_fail(cont != NULL); psmove_return_if_fail(x != NULL && y != NULL && radius != NULL); - int i, j; - float d = 0; - float cd = 0; - CvPoint m1 = cvPoint( 0, 0 ); - CvPoint m2 = cvPoint( 0, 0 ); - CvPoint * p1; - CvPoint * p2; - int found = 0; - - int step = MAX(1,cont->total/20); - - // compare every two points of the contour (but not more than 20) - // to find the most distant pair - for (i = 0; i < cont->total; i += step) { - p1 = (CvPoint*) cvGetSeqElem(cont, i); - for (j = i + 1; j < cont->total; j += step) { - p2 = (CvPoint*) cvGetSeqElem(cont, j); - cd = th_dist_squared(*p1,*p2); - if (cd > d) { - d = cd; - m1 = *p1; - m2 = *p2; - found = 1; - } - } - } - // calculate center of that pair - if (found) { - *x = 0.5 * (m1.x + m2.x); - *y = 0.5 * (m1.y + m2.y); + int i, j; + float d = 0; + float cd = 0; + CvPoint m1 = cvPoint( 0, 0 ); + CvPoint m2 = cvPoint( 0, 0 ); + CvPoint * p1; + CvPoint * p2; + int found = 0; + + int step = MAX(1,cont->total/20); + + // compare every two points of the contour (but not more than 20) + // to find the most distant pair + for (i = 0; i < cont->total; i += step) { + p1 = (CvPoint*) cvGetSeqElem(cont, i); + for (j = i + 1; j < cont->total; j += step) { + p2 = (CvPoint*) cvGetSeqElem(cont, j); + cd = th_dist_squared(*p1,*p2); + if (cd > d) { + d = cd; + m1 = *p1; + m2 = *p2; + found = 1; + } } - // calcualte the radius - *radius = sqrt(d) / 2; + } + // calculate center of that pair + if (found) { + *x = 0.5 * (m1.x + m2.x); + *y = 0.5 * (m1.y + m2.y); + } + // calcualte the radius + *radius = sqrt(d) / 2; } int @@ -1818,37 +1828,37 @@ psmove_tracker_center_roi_on_controller(TrackedController* tc, PSMoveTracker* tr psmove_return_val_if_fail(tracker != NULL, 0); psmove_return_val_if_fail(center != NULL, 0); - CvScalar min = th_scalar_sub(tc->eColorHSV, tracker->rHSV); - CvScalar max = th_scalar_add(tc->eColorHSV, tracker->rHSV); - - IplImage *roi_i = tracker->roiI[tc->roi_level]; - IplImage *roi_m = tracker->roiM[tc->roi_level]; - - // cut out the roi! - cvSetImageROI(tracker->frame, cvRect(tc->roi_x, tc->roi_y, roi_i->width, roi_i->height)); - cvCvtColor(tracker->frame, roi_i, CV_BGR2HSV); - - // apply color filter - cvInRangeS(roi_i, min, max, roi_m); - - float sizeBest = 0; - CvSeq* contourBest = NULL; - psmove_tracker_biggest_contour(roi_m, tracker->storage, &contourBest, &sizeBest); - if (contourBest) { - cvSet(roi_m, TH_COLOR_BLACK, NULL); - cvDrawContours(roi_m, contourBest, TH_COLOR_WHITE, TH_COLOR_WHITE, -1, CV_FILLED, 8, cvPoint(0, 0)); - // calucalte image-moments to estimate the better ROI center - CvMoments mu; - cvMoments(roi_m, &mu, 0); - - *center = cvPoint(mu.m10 / mu.m00, mu.m01 / mu.m00); - center->x += tc->roi_x - roi_m->width / 2; - center->y += tc->roi_y - roi_m->height / 2; - } - cvClearMemStorage(tracker->storage); - cvResetImageROI(tracker->frame); - - return (contourBest != NULL); + CvScalar min = th_scalar_sub(tc->eColorHSV, tracker->rHSV); + CvScalar max = th_scalar_add(tc->eColorHSV, tracker->rHSV); + + IplImage *roi_i = tracker->roiI[tc->roi_level]; + IplImage *roi_m = tracker->roiM[tc->roi_level]; + + // cut out the roi! + cvSetImageROI(tracker->frame, cvRect(tc->roi_x, tc->roi_y, roi_i->width, roi_i->height)); + cvCvtColor(tracker->frame, roi_i, CV_BGR2HSV); + + // apply color filter + cvInRangeS(roi_i, min, max, roi_m); + + float sizeBest = 0; + CvSeq* contourBest = NULL; + psmove_tracker_biggest_contour(roi_m, tracker->storage, &contourBest, &sizeBest); + if (contourBest) { + cvSet(roi_m, TH_COLOR_BLACK, NULL); + cvDrawContours(roi_m, contourBest, TH_COLOR_WHITE, TH_COLOR_WHITE, -1, CV_FILLED, 8, cvPoint(0, 0)); + // calucalte image-moments to estimate the better ROI center + CvMoments mu; + cvMoments(roi_m, &mu, 0); + + *center = cvPoint(mu.m10 / mu.m00, mu.m01 / mu.m00); + center->x += tc->roi_x - roi_m->width / 2; + center->y += tc->roi_y - roi_m->height / 2; + } + cvClearMemStorage(tracker->storage); + cvResetImageROI(tracker->frame); + + return (contourBest != NULL); } float @@ -1925,4 +1935,3 @@ _psmove_tracker_fix_roi_size(PSMoveTracker *tracker) tc->roi_level_fixed = PSMove_True; } } - From 3bde3fc889f214b260d93f2c564c850e126f5539 Mon Sep 17 00:00:00 2001 From: Chadwick Boulay Date: Sun, 15 Feb 2015 22:32:28 -0500 Subject: [PATCH 5/7] Added preprocessing directives so PS3EYEDriver-specific parameters only used when compiling for PS3EYEDriver. --- src/tracker/psmove_tracker.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/tracker/psmove_tracker.c b/src/tracker/psmove_tracker.c index 48dbce53..8a04611a 100644 --- a/src/tracker/psmove_tracker.c +++ b/src/tracker/psmove_tracker.c @@ -473,7 +473,12 @@ psmove_tracker_set_exposure(PSMoveTracker *tracker, float target_luminance = 0; switch (tracker->exposure_mode) { case Exposure_LOW: +#if defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER) + /* Bypasses autoreturn in psmove_tracker_adapt_to_light */ target_luminance = 1; +#else + target_luminance = 0; +#endif break; case Exposure_MEDIUM: target_luminance = 25; @@ -1547,7 +1552,12 @@ psmove_tracker_free(PSMoveTracker *tracker) int psmove_tracker_adapt_to_light(PSMoveTracker *tracker, float target_luminance) { - float minimum_exposure = 1; // Was 2051 +#if defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER) + /* PS3EYE_DRIVER seems to require different parameter ranges. */ + float minimum_exposure = 1; +#else + float minimum_exposure = 2051; +#endif float maximum_exposure = 65535; float current_exposure = (maximum_exposure + minimum_exposure) / 2.; From 99013912ac1e8d427217c863855bbefc8bb90cac Mon Sep 17 00:00:00 2001 From: Chadwick Boulay Date: Sun, 15 Feb 2015 23:46:13 -0500 Subject: [PATCH 6/7] Uses PS3EYEDriver submodule update for new set_parameter interface. --- external/PS3EYEDriver | 2 +- src/tracker/platform/camera_control_macosx.c | 24 ++++++++++++++++---- src/tracker/platform/camera_control_win32.c | 23 ++++++++++++++++--- 3 files changed, 41 insertions(+), 8 deletions(-) diff --git a/external/PS3EYEDriver b/external/PS3EYEDriver index 71176678..7d0cf32e 160000 --- a/external/PS3EYEDriver +++ b/external/PS3EYEDriver @@ -1 +1 @@ -Subproject commit 71176678562ade98b2d87a25518e8c7a1d5d0631 +Subproject commit 7d0cf32e7032499e865cadee5a4a3dd03aef76e4 diff --git a/src/tracker/platform/camera_control_macosx.c b/src/tracker/platform/camera_control_macosx.c index 9314e751..65cdf80f 100644 --- a/src/tracker/platform/camera_control_macosx.c +++ b/src/tracker/platform/camera_control_macosx.c @@ -44,10 +44,26 @@ camera_control_set_parameters(CameraControl* cc, int contrast, int brightness) { #if defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER) - // TODO: Implement setting those parameters on cc->eye - ps3eye_set_parameters(cc->eye, autoG > 0, autoWB > 0, gain, exposure, contrast, brightness); - //cc->eye->eye->setExposure(exposure); - psmove_WARNING("Unimplemented: Only some PS3EYEDriver parameters set.\n"); + ps3eye_set_parameter(cc->eye, PS3EYE_EXPOSURE, exposure); + + /** Many of the values passed to this function do not work with PS3EYEDriver. + * comment out these functions until a sane value scheme is implemented. + * ps3eye_set_parameter(cc->eye, PS3EYE_AUTO_GAIN, autoG); + * ps3eye_set_parameter(cc->eye, PS3EYE_GAIN, gain); + * ps3eye_set_parameter(cc->eye, PS3EYE_AUTO_WHITEBALANCE, autoWB); + * ps3eye_set_parameter(cc->eye, PS3EYE_CONTRAST, contrast); + * ps3eye_set_parameter(cc->eye, PS3EYE_BRIGHTNESS, brightness); + * ps3eye_set_parameter(cc->eye, PS3EYE_REDBALANCE, wbRed); + * ps3eye_set_parameter(cc->eye, PS3EYE_BLUEBALANCE, wbGreen); + **/ + psmove_WARNING("Only PS3EYEDriver parameter 'exposure' set by ps3eye_set_parameter.\n"); + /** Some PS3EYEDriver parameters that could be set are not set by this function + * ps3eye_set_parameter(cc->eye, PS3EYE_SHARPNESS, autoG); + * ps3eye_set_parameter(cc->eye, PS3EYE_HUE, ???); + * ps3eye_set_parameter(cc->eye, PS3EYE_HFLIP, ??); + * ps3eye_set_parameter(cc->eye, PS3EYE_VFLIP, ??); + **/ + #else macosx_camera_set_exposure_lock(1); #endif diff --git a/src/tracker/platform/camera_control_win32.c b/src/tracker/platform/camera_control_win32.c index aa984e22..4ce58008 100644 --- a/src/tracker/platform/camera_control_win32.c +++ b/src/tracker/platform/camera_control_win32.c @@ -196,9 +196,26 @@ void camera_control_set_parameters(CameraControl* cc, int autoE, int autoG, int if (cc->capture) { cvReleaseCapture(&cc->capture); } - - ps3eye_set_parameters(cc->eye, autoG > 0, autoWB > 0, gain, exposure, contrast, brightness); - + + ps3eye_set_parameter(cc->eye, PS3EYE_EXPOSURE, exposure); + /** Many of the values passed to this function do not work with PS3EYEDriver. + * comment out these functions until a sane value scheme is implemented. + * ps3eye_set_parameter(cc->eye, PS3EYE_AUTO_GAIN, autoG); + * ps3eye_set_parameter(cc->eye, PS3EYE_GAIN, gain); + * ps3eye_set_parameter(cc->eye, PS3EYE_AUTO_WHITEBALANCE, autoWB); + * ps3eye_set_parameter(cc->eye, PS3EYE_CONTRAST, contrast); + * ps3eye_set_parameter(cc->eye, PS3EYE_BRIGHTNESS, brightness); + * ps3eye_set_parameter(cc->eye, PS3EYE_REDBALANCE, wbRed); + * ps3eye_set_parameter(cc->eye, PS3EYE_BLUEBALANCE, wbGreen); + **/ + psmove_WARNING("Only PS3EYEDriver parameter 'exposure' set by ps3eye_set_parameter.\n"); + /** Some PS3EYEDriver parameters that could be set are not set by this function + * ps3eye_set_parameter(cc->eye, PS3EYE_SHARPNESS, autoG); + * ps3eye_set_parameter(cc->eye, PS3EYE_HUE, ???); + * ps3eye_set_parameter(cc->eye, PS3EYE_HFLIP, ??); + * ps3eye_set_parameter(cc->eye, PS3EYE_VFLIP, ??); + **/ + int width, height; get_metrics(&width, &height); From 455c91be9cbe27c0d69de93e29f268ee6abce8a7 Mon Sep 17 00:00:00 2001 From: Chadwick Boulay Date: Mon, 23 Feb 2015 21:23:14 -0500 Subject: [PATCH 7/7] Fail gracefully when PS3EYEDriver expected but not present. --- examples/c/test_tracker.c | 5 +++++ src/tracker/camera_control.c | 1 - src/tracker/psmove_tracker.c | 9 +++++++-- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/examples/c/test_tracker.c b/examples/c/test_tracker.c index 9c115c20..0afe77f2 100644 --- a/examples/c/test_tracker.c +++ b/examples/c/test_tracker.c @@ -56,6 +56,11 @@ int main(int arg, char** args) { fprintf(stderr, "Trying to init PSMoveTracker..."); PSMoveTracker* tracker = psmove_tracker_new(); + if (!tracker) + { + fprintf(stderr, "Could not init PSMoveTracker.\n"); + return 1; + } psmove_tracker_set_mirror(tracker, PSMove_True); fprintf(stderr, "OK\n"); diff --git a/src/tracker/camera_control.c b/src/tracker/camera_control.c index f838f3bc..f39b1094 100644 --- a/src/tracker/camera_control.c +++ b/src/tracker/camera_control.c @@ -142,7 +142,6 @@ camera_control_new(int cameraID) #elif defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER) ps3eye_init(); int cams = ps3eye_count_connected(); - if (cams <= cameraID) { free(cc); return NULL; diff --git a/src/tracker/psmove_tracker.c b/src/tracker/psmove_tracker.c index 8a04611a..bc500317 100644 --- a/src/tracker/psmove_tracker.c +++ b/src/tracker/psmove_tracker.c @@ -568,11 +568,16 @@ psmove_tracker_new_with_camera(int camera) { #endif // start the video capture device for tracking - tracker->cc = camera_control_new(camera); + tracker->cc = camera_control_new(camera); /* Returns NULL if no control found. Happens when PS3EYEDriver is used at compile but not plugged in. */ + if (!tracker->cc) + { + free(tracker); + return NULL; + } char *intrinsics_xml = psmove_util_get_file_path(INTRINSICS_XML); char *distortion_xml = psmove_util_get_file_path(DISTORTION_XML); - camera_control_read_calibration(tracker->cc, intrinsics_xml, distortion_xml); + camera_control_read_calibration(tracker->cc, intrinsics_xml, distortion_xml); /* Crashes if cc is NULL */ free(intrinsics_xml); free(distortion_xml);