anonymizer/Sources/Tracking/Algorithm1/observetemplate.h

//------------------------------------------------------------------------------
//
//  Project:   Anonymizer
//
//             Brno University of Technology
//             Faculty of Information Technology
//
//------------------------------------------------------------------------------
//
//      This project was financially supported by project VG20102015006 funds
//           provided by Ministry of the Interior of the Czech republic.
//
//------------------------------------------------------------------------------
/*!

@file      observetemplate.h
@brief     Header file
@details   Particle evaluation functions
@authors   Filip Orsag (orsag@fit.vutbr.cz)
@date      2014-2015
@note      This project was supported by MV CR project VG20102015006.
@copyright BUT OPEN SOURCE LICENCE (see License.txt)

*/

#ifndef CV_PARTICLE_OBSERVE_TEMPLATE_H
#define CV_PARTICLE_OBSERVE_TEMPLATE_H

#include "opencvx/cvparticle.h"
#include "opencvx/cvrect32f.h"
#include "opencvx/cvcropimageroi.h"
#include "state.h"

#define HIST_SIZE 128
#define DIVIDER 4

using namespace std;


/******************** Function Prototypes **********************/
#ifndef NO_DOXYGEN

/*!
 ** Initialization function for the particle evaluation by color histogram.
 ** Histograms of the R, G and B color channels of the reference image are created
 ** (the reference image is not being modified) and stored at the given addresses in memory.
 ** 
 ** @param reference Reference image.
 ** @param histReferenceRed Reference histogram of the red channel.
 ** @param histReferenceGreen Reference histogram of the green channel.
 ** @param histReferenceBlue Reference histogram of the blue channel.
 **/
void initializeRGBHist(IplImage *reference, CvHistogram** histReferenceRed, 
					   CvHistogram** histReferenceGreen, CvHistogram** histReferenceBlue);

/*!
 ** Initialization function for the particle evaluation by gray scale histogram.
 ** Histograms of intensities of the reference image is created
 ** (the reference image is not being modified) and stored at the given address in memory.
 **
 ** @param reference Reference image.
 ** @param histReferenceGray Reference histogram of the grayscale image.
 **/
void initializeGrayHist(IplImage *reference, CvHistogram** histReferenceGray);

/*!
 ** Initialization function for the particle evaluation by a hybrid way.
 ** The reference image is divided to nx*ny parts. Each of the parts is evaluated by a color
 ** histogram implemented by an array of matrices. Each part of the reference image is related
 ** to one matrix, which consists of 3 rows for each of the R, G and B color channels and
 ** as many columns as there are parts ("bins") of the image.
 **
 ** @param reference Reference image.
 ** @param matRef Array of matrices with histograms.
 ** @param nx Count of the horizontal parts.
 ** @param ny Count of the vertical parts.
 **/
void initializeHyb(IplImage *reference, CvMat **matRef, int nx, int ny);

/*!
 ** Particle evaluation function using color histogram. Each particle contains histograms of the
 ** individual color channels. These histograms are compared to the reference values.
 **
 ** @param p Pointer to the particles.
 ** @param frame Current image in the video stream.
 ** @param histReferenceRed Referencne histogram of the red channel.
 ** @param histReferenceGreen Referencne histogram of the green channel.
 ** @param histReferenceBlue Referencne histogram of the blue channel.
 ** @param featSize Size of each particle (particles are resized to this size).
 ** @param numParticlesDyn Current count of the particels.
 **/
void particleEvalRGBHist( CvParticle* p, IplImage* frame, CvHistogram* histReferenceRed, CvHistogram* histReferenceGreen, 
						 CvHistogram* histReferenceBlue, CvSize featSize, int numParticlesDyn);

/*!
 ** Particle evaluation function using grayscale histogram. Each particle contains histograms of the
 ** intensities. These histograms are compared to the reference values.
 **
 ** @param p Pointer to the particles.
 ** @param frame Current image of the video.
 ** @param histReferenceGray Referencne grayscales histogram.
 ** @param featSize Size of each particle (particles are resized to this size).
 ** @param numParticlesDyn Current count of the particels.
 **/
void particleEvalGrayHist( CvParticle* p, IplImage* frame, CvHistogram* histReferenceGray, CvSize featSize, int numParticlesDyn);

/*!
 ** Particle evaluation function using elementary method "pixel-by-pixel". Each particle
 ** is compared to the reference image using function cvNorm(...).
 **
 ** @param p Pointer to the particles.
 ** @param frame Current image of the video.
 ** @param reference Referencne image.
 ** @param featSize Size of each particle (particles are resized to this size).
 ** @param numParticlesDyn Current count of the particels.
 **/
void particleEvalDefault( CvParticle* p, IplImage* frame, IplImage *reference,CvSize featSize, int numParticlesDyn );

/*!
 ** Particle evaluation function using a hybrid method. For each particle an array of histograms (matrices, see function @ref initializeHyb)
 ** is created and these are then compared to the reference histograms.
 **
 ** @param p Pointer to the particles.
 ** @param frame Current image of the video.
 ** @param reference Referencne image.
 ** @param matRef Array of matrices with histograms.
 ** @param nx Count of the horizontal parts.
 ** @param ny Count of the vertical parts.
 ** @param featSize Size of each particle (particles are resized to this size).
 ** @param numParticlesDyn Current count of the particels.
 **/
void particleEvalHybrid( CvParticle* p, IplImage* frame, IplImage *reference, CvMat **matRef, int nx, int ny,CvSize featSize, int numParticlesDyn );

#endif
/***************************************************************/


void initializeRGBHist(IplImage *reference, CvHistogram** histReferenceRed, 
					   CvHistogram** histReferenceGreen, CvHistogram** histReferenceBlue)
{
	int hist_size = HIST_SIZE;
	IplImage* referenceRed = cvCreateImage(cvSize(reference->width,reference->height), IPL_DEPTH_8U, 1);
    IplImage* referenceGreen = cvCreateImage(cvSize(reference->width,reference->height), IPL_DEPTH_8U, 1);
    IplImage* referenceBlue = cvCreateImage(cvSize(reference->width,reference->height), IPL_DEPTH_8U, 1);

	cvSplit(reference, referenceRed, referenceGreen, referenceBlue, NULL);	

    *histReferenceRed = cvCreateHist(1, &hist_size, CV_HIST_ARRAY);
    *histReferenceGreen = cvCreateHist(1, &hist_size, CV_HIST_ARRAY);
    *histReferenceBlue = cvCreateHist(1, &hist_size, CV_HIST_ARRAY);

	cvCalcHist( &referenceRed, *histReferenceRed, 0, NULL );
	cvCalcHist( &referenceGreen, *histReferenceGreen, 0, NULL );
	cvCalcHist( &referenceBlue, *histReferenceBlue, 0, NULL );


}

void initializeGrayHist(IplImage *reference, CvHistogram** histReferenceGray)
{	
	int hist_size = HIST_SIZE;
	IplImage* referenceGray = cvCreateImage(cvSize(reference->width,reference->height), IPL_DEPTH_8U, 1);
	cvCvtColor(reference, referenceGray, CV_BGR2GRAY);

	*histReferenceGray = cvCreateHist(1, &hist_size, CV_HIST_ARRAY);
	cvCalcHist(&referenceGray, *histReferenceGray, 0, NULL);	
}



void initializeHyb(IplImage *reference, CvMat **matRef, int nx, int ny)
{
	int a,b,x,y;
	int i = 0;			
	int stepX = reference->width/nx; 
	int stepY = reference->height/ny;	

    uchar *ptrRef = NULL;
	
		for(a = 0; a < ny; a++)
		{
			for(b = 0; b < nx; b++)
			{
				for(y = a * stepY; y < ((a + 1) * stepY); y++)
				{
					if(y < reference->height)
					{
						ptrRef = (uchar*) (reference->imageData + y * reference->widthStep);
						
						for(x = b * stepX; x < ((b + 1) * stepX); x++)
						{
							if(x < reference->width)
							{
                                (*(matRef[i]->data.ptr + (*(ptrRef+3*x))/DIVIDER))++;
                                (*(matRef[i]->data.ptr + matRef[i]->cols + (*(ptrRef + 3*x+1))/DIVIDER))++;
                                (*(matRef[i]->data.ptr + matRef[i]->cols * 2 + (*(ptrRef + 3*x+2))/DIVIDER))++;
								
							}
						}
					}
				}
				i++;
			}
		}
}

void initializeRGB2(IplImage *reference, CvMat *matRef, int nx, int ny)
{
	int y,x;
	int i = 0;					

    uchar *ptrRef = NULL;

	for(y = 0; y < 24; y++)
	{	
		ptrRef = (uchar*) (reference->imageData + y * reference->widthStep);				
		for(x = 0; x < 24; x++)
		{			
            (*(matRef->data.ptr + (*(ptrRef+3*x))/DIVIDER))++;
            (*(matRef->data.ptr + matRef->cols + (*(ptrRef + 3*x+1))/DIVIDER))++;
            (*(matRef->data.ptr + matRef->cols * 2 + (*(ptrRef + 3*x+2))/DIVIDER))++;
		}
	}	
}


void particleEvalRGBHist( CvParticle* p, IplImage* frame, CvHistogram* histReferenceRed,CvHistogram* histReferenceGreen,
						 CvHistogram* histReferenceBlue, CvSize featSize, int numParticlesDyn )
{
    int i;
    double likeli;
	double likeliRed;
	double likeliGreen;
	double likeliBlue;
    IplImage *patch;
    IplImage *resize;
    resize = cvCreateImage( featSize, frame->depth, frame->nChannels );
    int hist_size = HIST_SIZE;
	IplImage* frameRed;
	IplImage* frameGreen;
	IplImage* frameBlue;
	CvHistogram* histFrameRed;
	CvHistogram* histFrameGreen;
	CvHistogram* histFrameBlue;


    for( i = 0; i < numParticlesDyn; i++ ) 
    {
        CvParticleState s = cvParticleStateGet( p, i );		
        CvBox32f box32f = cvBox32f( s.x, s.y, s.width, s.height, s.angle );
        CvRect32f rect32f = cvRect32fFromBox32f( box32f );
        CvRect rect = cvRectFromRect32f( rect32f );
        
        patch = cvCreateImage( cvSize(rect.width,rect.height), frame->depth, frame->nChannels );
        cvCropImageROI( frame, patch, rect32f );
        cvResize( patch, resize );

		frameRed = cvCreateImage(cvSize(resize->width,resize->height), IPL_DEPTH_8U, 1);
		frameGreen = cvCreateImage(cvSize(resize->width,resize->height), IPL_DEPTH_8U, 1);
		frameBlue = cvCreateImage(cvSize(resize->width,resize->height), IPL_DEPTH_8U, 1);
		
		cvSplit(resize, frameRed, frameGreen,frameBlue, NULL);

		histFrameRed = cvCreateHist(1, &hist_size, CV_HIST_ARRAY);
		histFrameGreen = cvCreateHist(1, &hist_size, CV_HIST_ARRAY);
		histFrameBlue = cvCreateHist(1, &hist_size, CV_HIST_ARRAY);

		cvCalcHist( &frameRed, histFrameRed, 0, NULL );
		cvCalcHist( &frameGreen, histFrameGreen, 0, NULL );
		cvCalcHist( &frameBlue, histFrameBlue, 0, NULL );

		likeliRed = cvCompareHist(histFrameRed, histReferenceRed, CV_COMP_INTERSECT);
		likeliGreen = cvCompareHist(histFrameGreen, histReferenceGreen, CV_COMP_INTERSECT);
		likeliBlue = cvCompareHist(histFrameBlue, histReferenceBlue, CV_COMP_INTERSECT);		

		likeli = likeliRed + likeliBlue + likeliGreen;		

       cvmSet( p->weights, 0, i, likeli );
        
        cvReleaseImage( &patch );
		cvReleaseImage( &frameRed );
		cvReleaseImage( &frameGreen );
		cvReleaseImage( &frameBlue );
		cvReleaseHist(&histFrameRed);
		cvReleaseHist(&histFrameGreen);
		cvReleaseHist(&histFrameBlue);
		
    }
    cvReleaseImage( &resize );

	for(i = numParticlesDyn; i < p->num_particles; i++)
		cvmSet( p->weights, 0, i, -99999.0 );
}

void particleEvalGrayHist( CvParticle* p, IplImage* frame, CvHistogram* histReferenceGray, CvSize featSize, int numParticlesDyn )
{
    int i;
    double likeli;
    IplImage *patch;
    IplImage *resize;
	int hist_size = HIST_SIZE;
    resize = cvCreateImage( featSize, frame->depth, frame->nChannels );

    for( i = 0; i < numParticlesDyn; i++ ) 
    {
        CvParticleState s = cvParticleStateGet( p, i );
        CvBox32f box32f = cvBox32f( s.x, s.y, s.width, s.height, s.angle );
        CvRect32f rect32f = cvRect32fFromBox32f( box32f );
        CvRect rect = cvRectFromRect32f( rect32f );
        
        patch = cvCreateImage( cvSize(rect.width,rect.height), frame->depth, frame->nChannels );
        cvCropImageROI( frame, patch, rect32f );
        cvResize( patch, resize );

		IplImage* grayResize = cvCreateImage(cvSize(resize->width,resize->height), IPL_DEPTH_8U, 1);
		cvCvtColor(resize, grayResize, CV_BGR2GRAY);
	
	    CvHistogram* histResize = cvCreateHist(1, &hist_size, CV_HIST_ARRAY);
    
		cvCalcHist( &grayResize, histResize, 0, NULL );


		likeli = cvCompareHist(histResize, histReferenceGray, CV_COMP_INTERSECT);
	
		cvmSet( p->weights, 0, i, likeli );       
        cvReleaseImage( &patch );
    }
    cvReleaseImage( &resize );

	for(i = numParticlesDyn; i < p->num_particles; i++)
		cvmSet( p->weights, 0, i, -99999.0 );
}

void particleEvalDefault( CvParticle* p, IplImage* frame, IplImage *reference, CvSize featSize, int numParticlesDyn )
{
    int i;
    double likeli;
    IplImage *patch;
    IplImage *resize;
    resize = cvCreateImage( featSize, frame->depth, frame->nChannels );

    for( i = 0; i < numParticlesDyn; i++ ) 
    {
        CvParticleState s = cvParticleStateGet( p, i );		
		CvBox32f box32f = cvBox32f( s.x, s.y, s.width, s.height, s.angle );
        CvRect32f rect32f = cvRect32fFromBox32f( box32f );
    	CvRect rect = cvRectFromRect32f( rect32f );        
		patch = cvCreateImage( cvSize(rect.width,rect.height), frame->depth, frame->nChannels );
        cvCropImageROI( frame, patch, rect32f );
        cvResize( patch, resize );

        likeli = -cvNorm( resize, reference, CV_L2 ); 

        cvmSet( p->weights, 0, i, likeli );
        
        cvReleaseImage( &patch );
    }
    cvReleaseImage( &resize );

	for(i = numParticlesDyn; i < p->num_particles; i++)
		cvmSet( p->weights, 0, i, -99999.0 );
}


void particleEvalHybrid( CvParticle* p, IplImage* frame, IplImage *reference, CvMat **matRef, int nx, int ny, CvSize featSize, int numParticlesDyn )
{
	
    int i,a,b,x,y;
	int j = 0;
		
    double likeli;
    IplImage *patch;
    IplImage *resize;
    resize = cvCreateImage( featSize, frame->depth, frame->nChannels );

	int stepX = featSize.width/nx;
	int stepY = featSize.height/ny;
	
	CvMat *matRes = cvCreateMat(3, (256/DIVIDER) + 1, CV_8UC1);

	cvZero(matRes);

	uchar *ptrRes = NULL;

    for( i = 0; i < numParticlesDyn; i++ ) 
    {
        CvParticleState s = cvParticleStateGet( p, i );
		//CvBox32f = The Constructor of Center Coordinate Floating Rectangle Structure.
        CvBox32f box32f = cvBox32f( s.x, s.y, s.width, s.height, s.angle );
        CvRect32f rect32f = cvRect32fFromBox32f( box32f );
        CvRect rect = cvRectFromRect32f( rect32f );
        
        patch = cvCreateImage( cvSize(rect.width,rect.height), frame->depth, frame->nChannels );
        cvCropImageROI( frame, patch, rect32f );
        cvResize( patch, resize );		

		likeli = 0;
		j = 0;

		for(a = 0; a < ny; a++)
		{
			for(b = 0; b < nx; b++)
			{
				for(y = a * stepY; y < ((a + 1) * stepY); y++)
				{
					if(y < featSize.height)
					{					
						ptrRes = (uchar*) (resize->imageData + y * resize->widthStep);
						for(x = b * stepX; x < ((b + 1) * stepX); x++)
						{
							if(x < featSize.width)
							{
                                (*(matRes->data.ptr + (*(ptrRes+3*x))/DIVIDER))++;
                                (*(matRes->data.ptr + matRes->cols + (*(ptrRes + 3*x+1))/DIVIDER))++;
                                (*(matRes->data.ptr + matRes->cols * 2 + (*(ptrRes + 3*x+2))/DIVIDER))++;
							}
						}
					}
				}				
				likeli += cvNorm( matRef[j], matRes, CV_L2 );								
				j++;

				cvZero(matRes);

			}
		}

		likeli *= -1;				

        cvmSet( p->weights, 0, i, likeli );

		cvZero(matRes);
		
        
        cvReleaseImage( &patch );
    }
    cvReleaseImage( &resize );

	for(i = numParticlesDyn; i < p->num_particles; i++)
		cvmSet( p->weights, 0, i, -99999.0 );
}

#endif