Permalink
Browse files

Tracker: Proper radius-to-distance mapping function

  • Loading branch information...
1 parent 8eeba15 commit 1b09093860dead9130ec8133d1070b3f51d11742 @thp committed Dec 1, 2012
Showing with 165 additions and 44 deletions.
  1. +11 −0 bindings/swig/psmove.i
  2. +56 −0 examples/python/test_distance.py
  3. +30 −0 include/psmove_tracker.h
  4. +68 −44 src/tracker/psmove_tracker.c
View
@@ -304,6 +304,17 @@ void reinit();
void get_size(int *OUTPUT, int *OUTPUT);
+ float distance_from_radius(float radius) {
+ return psmove_tracker_distance_from_radius($self, radius);
+ }
+
+ void set_distance_parameters(float height, float center,
+ float hwhm, float shape)
+ {
+ psmove_tracker_set_distance_parameters($self, height,
+ center, hwhm, shape);
+ }
+
~PSMoveTracker() {
psmove_tracker_free($self);
}
@@ -0,0 +1,56 @@
+
+#
+# PS Move API - An interface for the PS Move Motion Controller
+# Copyright (c) 2012 Thomas Perl <m@thp.io>
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in the
+# documentation and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+
+import sys
+import os
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..', 'build'))
+
+import psmove
+
+move = psmove.PSMove()
+tracker = psmove.PSMoveTracker()
+
+while tracker.enable(move) != psmove.Tracker_CALIBRATED:
+ pass
+
+# Loop and update the controller
+while True:
+ tracker.update_image()
+ tracker.update()
+
+ # Check the tracking status
+ status = tracker.get_status(move)
+ if status == psmove.Tracker_TRACKING:
+ x, y, radius = tracker.get_position(move)
+ distance = tracker.distance_from_radius(radius)
+ print 'Position: (%5.2f, %5.2f), Distance: %3.2fcm' % (x, y, distance)
+ else:
+ print 'Not currently tracking.'
+
View
@@ -286,6 +286,36 @@ ADDCALL psmove_tracker_get_size(PSMoveTracker *tracker,
int *width, int *height);
/**
+ * Calculate the physical distance (in cm) of the controller
+ *
+ * Given the radius of the controller in the image (in pixels), this function
+ * calculates the physical distance of the controller from the camera (in cm).
+ *
+ * By default, this function's parameters are set up for the PS Eye camera in
+ * wide angle view. You can set different parameters using the function
+ * psmove_tracker_set_distance_parameters().
+ **/
+ADDAPI float
+ADDCALL psmove_tracker_distance_from_radius(PSMoveTracker *tracker,
+ float radius);
+
+/**
+ * Set the parameters for the distance mapping function
+ *
+ * This function sets the parameters for the Pearson VII distribution
+ * function that's used to map radius values to distance values in
+ * psmove_tracker_distance_from_radius(). By default, the parameters are
+ * set up so that they work well for a PS Eye camera in wide angle mode.
+ *
+ * The function is defined as in: http://fityk.nieto.pl/model.html
+ *
+ * distance = height / ((1+((radius-center)/hwhm)^2 * (2^(1/shape)-1)) ^ shape)
+ **/
+ADDAPI void
+ADDCALL psmove_tracker_set_distance_parameters(PSMoveTracker *tracker,
+ float height, float center, float hwhm, float shape);
+
+/**
* Destroy an existing tracker instance and free allocated resources
*
* tracker - A valid PSMoveTracker * instance
@@ -60,9 +60,7 @@
#define COLOR_FILTER_RANGE_H 12 // +- 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 CAMERA_FOCAL_LENGTH 28.3 // focal lenght constant of the ps-eye camera in (degrees)
-#define CAMERA_PIXEL_HEIGHT 5 // pixel height constant of the ps-eye camera in (µm)
-#define PS_MOVE_DIAMETER 47 // orb diameter constant of the ps-move controller in (mm)
+
/* 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
@@ -159,6 +157,30 @@ struct ColorMappingRingBuffer {
unsigned char next_slot;
};
+/**
+ * Parameters of the Pearson type VII distribution
+ * Source: http://fityk.nieto.pl/model.html
+ * Used for calculating the distance from the radius
+ **/
+struct PSMoveTracker_DistanceParameters {
+ float height;
+ float center;
+ float hwhm;
+ float shape;
+};
+
+/**
+ * Experimentally-determined parameters for a PS Eye camera
+ * in wide angle mode with a PS Move, color = (255, 0, 255)
+ **/
+static struct PSMoveTracker_DistanceParameters
+pseye_distance_parameters = {
+ /* height = */ 517.281,
+ /* center = */ 1.297338,
+ /* hwhm = */ 3.752844,
+ /* shape = */ 0.4762335,
+};
+
struct _PSMoveTracker {
CameraControl* cc;
IplImage* frame; // the current frame of the camera
@@ -169,6 +191,9 @@ struct _PSMoveTracker {
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;
+
enum PSMoveTracker_Exposure exposure_mode; // exposure mode
float dimming_factor; // dimming factor used on LED RGB values
@@ -186,11 +211,6 @@ struct _PSMoveTracker {
int search_tiles_count; // number of search tiles
// internal variables
- float cam_focal_length; // in (mm)
- float cam_pixel_height; // in (µm)
- float ps_move_diameter; // in (mm)
- float user_factor_dist; // user defined factor used in distance calulation
-
int tracker_adaptive_xy; // should adaptive x/y-smoothing be used
int tracker_adaptive_z; // should adaptive z-smoothing be used
@@ -307,16 +327,6 @@ void psmove_tracker_draw_tracking_stats(PSMoveTracker* tracker);
void psmove_tracker_biggest_contour(IplImage* img, CvMemStorage* stor, CvSeq** resContour, float* resSize);
/*
- * This calculates the distance of the orb of the controller.
- *
- * tracker - (in) the PSMoveTracker to use (used to read variables)
- * blob_diameter - (in) the diameter size of the orb in pixels
- *
- * Returns: The distance between the orb and the camera in (mm).
- */
-float psmove_tracker_calculate_distance(PSMoveTracker* tracker, float blob_diameter);
-
-/*
* This returns a subjective distance between the first estimated (during calibration process) color and the currently estimated color.
* Subjective, because it takes the different color components not equally into account.
* Result calculates like: abs(c1.h-c2.h) + abs(c1.s-c2.s)*0.5 + abs(c1.v-c2.v)*0.5
@@ -518,10 +528,6 @@ psmove_tracker_new_with_camera(int camera) {
tracker->storage = cvCreateMemStorage(0);
tracker->dimming_factor = 0.;
- tracker->cam_focal_length = CAMERA_FOCAL_LENGTH;
- tracker->cam_pixel_height = CAMERA_PIXEL_HEIGHT;
- tracker->ps_move_diameter = PS_MOVE_DIAMETER;
- tracker->user_factor_dist = 1.05;
tracker->calibration_t = CALIBRATION_DIFF_T;
tracker->tracker_t1 = TRACKER_QUALITY_T1;
@@ -580,6 +586,9 @@ psmove_tracker_new_with_camera(int camera) {
free(filename);
#endif
+ // 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);
@@ -1672,7 +1681,7 @@ void psmove_tracker_draw_tracking_stats(PSMoveTracker* tracker) {
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_calculate_distance(tracker, tc->r * 2);
+ 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);
@@ -1696,27 +1705,6 @@ float psmove_tracker_hsvcolor_diff(TrackedController* tc) {
return diff;
}
-float psmove_tracker_calculate_distance(PSMoveTracker* tracker, float blob_diameter) {
-
- // PS Eye uses OV7725 Chip --> http://image-sensors-world.blogspot.co.at/2010/10/omnivision-vga-sensor-inside-sony-eye.html
- // http://www.ovt.com/download_document.php?type=sensor&sensorid=80
- // http://photo.stackexchange.com/questions/12434/how-do-i-calculate-the-distance-of-an-object-in-a-photo
- /*
- distance to object (mm) = focal length (mm) * real height of the object (mm) * image height (pixels)
- ---------------------------------------------------------------------------
- object height (pixels) * sensor height (mm)
- */
-
- // TODO: use measured distance only if the psmoveeye is used
- //int n = 32;
- // distance in mm
- //int x[]={100,150,200,300,400,500,600,700,800,900,1000,1100,1200,1300,1400,1500,1600,1700,1800,1900,2000,2100,2200,2300,2400,2500,2600,2700,2800,2900,3000};
- // radius in pixel
- //float y[]={122.27,79.8,60.02,40.34,30.16,24.26,20.18,17.41,15.37,13.65,12.32,11.28,10.35,9.58,8.93,8.35,7.83,7.37,7.03,6.62,6.38,6.13,5.82,5.58,5.33,5.39,5.07,5.02,4.94,4.5,4.45};
-
- return (tracker->cam_focal_length * tracker->ps_move_diameter * tracker->user_factor_dist) / (blob_diameter * tracker->cam_pixel_height / 100.0 + FLT_EPSILON);
-}
-
void psmove_tracker_biggest_contour(IplImage* img, CvMemStorage* stor, CvSeq** resContour, float* resSize) {
CvSeq* contour;
*resSize = 0;
@@ -1813,6 +1801,42 @@ psmove_tracker_center_roi_on_controller(TrackedController* tc, PSMoveTracker* tr
return (contourBest != NULL);
}
+float
+psmove_tracker_distance_from_radius(PSMoveTracker *tracker, float radius)
+{
+ psmove_return_val_if_fail(tracker != NULL, 0.);
+
+ double height = (double)tracker->distance_parameters.height;
+ double center = (double)tracker->distance_parameters.center;
+ double hwhm = (double)tracker->distance_parameters.hwhm;
+ double shape = (double)tracker->distance_parameters.shape;
+ double x = (double)radius;
+
+ /**
+ * Pearson type VII distribution
+ * http://fityk.nieto.pl/model.html
+ **/
+ double a = pow((x - center) / hwhm, 2.);
+ double b = pow(2., 1. / shape) - 1.;
+ double c = 1. + a * b;
+ double distance = height / pow(c, shape);
+
+ return (float)distance;
+}
+
+void
+psmove_tracker_set_distance_parameters(PSMoveTracker *tracker,
+ float height, float center, float hwhm, float shape)
+{
+ psmove_return_if_fail(tracker != NULL);
+
+ tracker->distance_parameters.height = height;
+ tracker->distance_parameters.center = center;
+ tracker->distance_parameters.hwhm = hwhm;
+ tracker->distance_parameters.shape = shape;
+}
+
+
int
psmove_tracker_color_is_used(PSMoveTracker *tracker, struct PSMove_RGBValue color)
{

0 comments on commit 1b09093

Please sign in to comment.