Rev Author Line No. Line
178 kaklik 1 #include "gauss.h"
2 #include "linalg.h"
3 #include "pointerRecognition.hh"
4  
5 using namespace boost;
6 using namespace mimas;
7 using namespace std;
8  
9 #define GAUSSERROR (32.0f / 256.0f)
10  
11 PointerRecognition::PointerRecognition
12 ( CameraProjectorCalibrationPtr _cameraProjectorCalibration,
13 const image< rgba< unsigned char > > &_refImg ):
14 cameraProjectorCalibration( _cameraProjectorCalibration ),
15 x(0), y(0), w(320), h(200), numPixels(0), threshold( 0.025 ),
16 haveOldPos(false), minX(0), minY(0), maxX(0), maxY(0),
17 trackingRange(0)
18 {
19 setReferenceImage( _refImg );
20 setSigma( 3.0 );
21 }
22  
23 void PointerRecognition::setReferenceImage
24 ( const image< rgba< unsigned char > > &_refImg )
25 {
26 // Resize to zero (otherwise old content is preserved).
27 histogram.resize( extents[ 0 ][ 0 ][ 0 ] );
28 histogram.resize( extents[ 8 ][ 8 ][ 8 ] );
29 numPixels = _refImg.getWidth() * _refImg.getHeight();
30 for ( const rgba< unsigned char > *p = _refImg.rawData();
31 p != _refImg.rawData() + numPixels; p++ )
32 histogram[ (int)p->getRed() / 32 ]
33 [ (int)p->getGreen() / 32 ]
34 [ (int)p->getBlue() / 32 ]++;
35 }
36  
37 bool PointerRecognition::findPointer
38 ( const image< rgba< unsigned char > > &_frame,
39 Vector &camPos, Vector &pos )
40 throw (mimasexception)
41 {
42 MMERROR( histogram.num_elements() > 0, mimasexception, ,
43 "Reference image was not initialised." );
44  
45 int minLevel = (int)( numPixels * threshold );
46  
47 minX = 0;
48 minY = 0;
49 maxX = _frame.getWidth() - 1;
50 maxY = _frame.getHeight() - 1;
51  
52 if ( haveOldPos ) {
53 if ( minX < oldX + deltaX - trackingRange )
54 minX = oldX + deltaX - trackingRange;
55 if ( maxX > oldX + deltaX + trackingRange )
56 maxX = oldX + deltaX + trackingRange;
57 if ( minY < oldY + deltaY - trackingRange )
58 minY = oldY + deltaY - trackingRange;
59 if ( maxY > oldY + deltaY + trackingRange )
60 maxY = oldY + deltaY + trackingRange;
61 };
62  
63 if ( maxX - minX <= minSize ) {
64 if ( minX == 0 )
65 maxX = minSize;
66 else
67 minX = maxX - minSize;
68 };
69 if ( maxY - minY <= minSize ) {
70 if ( minY == 0 )
71 maxY = minSize;
72 else
73 minY = maxY - minSize;
74 };
75  
76 image< float > binary; binary.init( maxX + 1 - minX, maxY + 1 - minY );
77 {
78 const rgba< unsigned char > *p = &_frame.pixel( minX, minY );
79 float *q = binary.rawData();
80 for ( int j=minY; j<=maxY; j++ ) {
81 for ( int i=minX; i<=maxX; i++ ) {
82 if ( histogram[ (int)p->getRed() / 32 ]
83 [ (int)p->getGreen() / 32 ]
84 [ (int)p->getBlue() / 32 ] >= minLevel )
85 *q = 255.0;
86 else
87 *q = 0.0;
88 p++;
89 q++;
90 };
91 p += _frame.getWidth() - ( maxX + 1 - minX );
92 };
93 };
94  
95 segmentedImage = gaussBlur< float >( binary, (float)sigma, GAUSSERROR );
96 bool retVal = false;
97 {
98 unsigned char *p = segmentedImage.rawData();
99 for ( int j=0; j<segmentedImage.getHeight(); j++ ){
100 for ( int i=0; i<segmentedImage.getWidth(); i++ ){
101 if ( *p++ >= 64 ) {
102 camPos[0] = minX + i;
103 camPos[1] = minY + j;
104 camPos[2] = 1.0;
105 pos = prod( inv( cameraProjectorCalibration->getHomography() ),
106 camPos );
107 pos[0] /= pos[2];
108 pos[1] /= pos[2];
109 pos[2] = 1.0;
110 if ( pos[0] >= x && pos[1] >= y &&
111 pos[0] < x + w && pos[1] < y + h ) {
112 // cerr << camPos[0] << ", " << camPos[1] << " -> " << endl;
113 // cerr << pos[0] << ", " << pos[1] << endl;
114 if ( haveOldPos ) {
115 deltaX = minX + i - oldX;
116 deltaY = minY + i - oldY;
117 } else {
118 deltaX = 0;
119 deltaY = 0;
120 };
121 retVal = true;
122 oldX = minX + i;
123 oldY = minY + j;
124 i=segmentedImage.getWidth(); j=segmentedImage.getHeight();
125 };
126 }
127 }
128 }
129 };
130  
131 haveOldPos = retVal;
132 return haveOldPos;
133 }
134  
135  
136 void PointerRecognition::setSigma( double _sigma )
137 {
138 sigma = _sigma;
139 minSize = gaussBlurFilter< float >( _sigma, GAUSSERROR ).size();
140 }