#include "gauss.h"
#include "linalg.h"
#include "pointerRecognition.hh"
using namespace boost;
using namespace mimas;
using namespace std;
#define GAUSSERROR (32.0f / 256.0f)
PointerRecognition::PointerRecognition
( CameraProjectorCalibrationPtr _cameraProjectorCalibration,
const image< rgba< unsigned char > > &_refImg ):
cameraProjectorCalibration( _cameraProjectorCalibration ),
x(0), y(0), w(320), h(200), numPixels(0), threshold( 0.025 ),
haveOldPos(false), minX(0), minY(0), maxX(0), maxY(0),
trackingRange(0)
{
setReferenceImage( _refImg );
setSigma( 3.0 );
}
void PointerRecognition::setReferenceImage
( const image< rgba< unsigned char > > &_refImg )
{
// Resize to zero (otherwise old content is preserved).
histogram.resize( extents[ 0 ][ 0 ][ 0 ] );
histogram.resize( extents[ 8 ][ 8 ][ 8 ] );
numPixels = _refImg.getWidth() * _refImg.getHeight();
for ( const rgba< unsigned char > *p = _refImg.rawData();
p != _refImg.rawData() + numPixels; p++ )
histogram[ (int)p->getRed() / 32 ]
[ (int)p->getGreen() / 32 ]
[ (int)p->getBlue() / 32 ]++;
}
bool PointerRecognition::findPointer
( const image< rgba< unsigned char > > &_frame,
Vector &camPos, Vector &pos )
throw (mimasexception)
{
MMERROR( histogram.num_elements() > 0, mimasexception, ,
"Reference image was not initialised." );
int minLevel = (int)( numPixels * threshold );
minX = 0;
minY = 0;
maxX = _frame.getWidth() - 1;
maxY = _frame.getHeight() - 1;
if ( haveOldPos ) {
if ( minX < oldX + deltaX - trackingRange )
minX = oldX + deltaX - trackingRange;
if ( maxX > oldX + deltaX + trackingRange )
maxX = oldX + deltaX + trackingRange;
if ( minY < oldY + deltaY - trackingRange )
minY = oldY + deltaY - trackingRange;
if ( maxY > oldY + deltaY + trackingRange )
maxY = oldY + deltaY + trackingRange;
};
if ( maxX - minX <= minSize ) {
if ( minX == 0 )
maxX = minSize;
else
minX = maxX - minSize;
};
if ( maxY - minY <= minSize ) {
if ( minY == 0 )
maxY = minSize;
else
minY = maxY - minSize;
};
image< float > binary; binary.init( maxX + 1 - minX, maxY + 1 - minY );
{
const rgba< unsigned char > *p = &_frame.pixel( minX, minY );
float *q = binary.rawData();
for ( int j=minY; j<=maxY; j++ ) {
for ( int i=minX; i<=maxX; i++ ) {
if ( histogram[ (int)p->getRed() / 32 ]
[ (int)p->getGreen() / 32 ]
[ (int)p->getBlue() / 32 ] >= minLevel )
*q = 255.0;
else
*q = 0.0;
p++;
q++;
};
p += _frame.getWidth() - ( maxX + 1 - minX );
};
};
segmentedImage = gaussBlur< float >( binary, (float)sigma, GAUSSERROR );
bool retVal = false;
{
unsigned char *p = segmentedImage.rawData();
for ( int j=0; j<segmentedImage.getHeight(); j++ ){
for ( int i=0; i<segmentedImage.getWidth(); i++ ){
if ( *p++ >= 64 ) {
camPos[0] = minX + i;
camPos[1] = minY + j;
camPos[2] = 1.0;
pos = prod( inv( cameraProjectorCalibration->getHomography() ),
camPos );
pos[0] /= pos[2];
pos[1] /= pos[2];
pos[2] = 1.0;
if ( pos[0] >= x && pos[1] >= y &&
pos[0] < x + w && pos[1] < y + h ) {
// cerr << camPos[0] << ", " << camPos[1] << " -> " << endl;
// cerr << pos[0] << ", " << pos[1] << endl;
if ( haveOldPos ) {
deltaX = minX + i - oldX;
deltaY = minY + i - oldY;
} else {
deltaX = 0;
deltaY = 0;
};
retVal = true;
oldX = minX + i;
oldY = minY + j;
i=segmentedImage.getWidth(); j=segmentedImage.getHeight();
};
}
}
}
};
haveOldPos = retVal;
return haveOldPos;
}
void PointerRecognition::setSigma( double _sigma )
{
sigma = _sigma;
minSize = gaussBlurFilter< float >( _sigma, GAUSSERROR ).size();
}