strade/anonymizer
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Initial commit.

Browse Source 
Final release of the project Anonymizer (2015).
Project settings for the Qt Creator (ver. 3.6).
This commit is contained in:
Filip Orság
2016-01-25 18:17:34 +01:00
commit 22dbc25cce
479 changed files with 141991 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
Sources/Tracking/Algorithm1/observe1.h Normal file
View File

@ -0,0 +1,88 @@
/** @file
*
* Template matching observation model for particle filter
* CvParticleState s must have s.x, s.y, s.width, s.height, s.angle
*/
/* The MIT License
*
* Copyright (c) 2008, Naotoshi Seo <sonots(at)sonots.com>
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#ifndef CV_PARTICLE_OBSERVE_TEMPLATE_H
#define CV_PARTICLE_OBSERVE_TEMPLATE_H
#include "<opencv\cvparticle.h>"
#include "<opencv\cvrect32f.h>"
#include "<opencv\cvcropimageroi.h>"
using namespace std;
/********************* Globals **********************************/
int num_observes = 1;
CvSize feature_size = cvSize(24, 24);
/******************** Function Prototypes **********************/
#ifndef NO_DOXYGEN
void cvParticleObserveMeasure( CvParticle* p, IplImage* cur_frame, IplImage *pre_frame );
#endif
/**
* Measure and weight particles.
*
* The proposal function q is set p(xt|xt-1) in SIR/Condensation, and it results
* that "weights" are set to be proportional to the likelihood probability
* (Normalize later).
* Rewrite here if you want to use a different proposal function q.
*
* CvParticleState s must have s.x, s.y, s.width, s.height, s.angle
*
* @param particle
* @param frame
* @param reference
*/
void cvParticleObserveMeasure( CvParticle* p, IplImage* frame, IplImage *reference )
{
int i;
double likeli;
IplImage *patch;
IplImage *resize;
resize = cvCreateImage( feature_size, frame->depth, frame->nChannels );
for( i = 0; i < p->num_particles; 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 );
// log likeli. kinds of Gaussian model
// exp( -d^2 / sigma^2 )
// sigma can be omitted because common param does not affect ML estimate
likeli = -cvNorm( resize, reference, CV_L2 );
cvmSet( p->weights, 0, i, likeli );
cvReleaseImage( &patch );
}
cvReleaseImage( &resize );
}
#endif

185
Sources/Tracking/Algorithm1/observe2.h Normal file
View File

@ -0,0 +1,185 @@
/** @file
*
* Moghaddam's PCA DIFS + DFFS (distance-in-feature-space + distance-from-feature-space)
* observation model for particle filter
* CvParticleState must have x, y, width, height, and angle
*/
/* The MIT License
*
* Copyright (c) 2008, Naotoshi Seo <sonots(at)sonots.com>
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#ifndef CV_PARTICLE_OBSERVE_PCADIFFS_H
#define CV_PARTICLE_OBSERVE_PCADIFFS_H
#include "cvparticle.h"
#include "cvrect32f.h"
#include "cvcropimageroi.h"
#include "cvpcadiffs.h"
#include "cvgaussnorm.h"
#include <iostream>
using namespace std;
/********************************* Globals ******************************************/
int num_observes = 1;
CvSize feature_size = cvSize(24, 24);
string data_dir = "";
string data_pcaval = "pcaval.xml";
string data_pcavec = "pcavec.xml";
string data_pcaavg = "pcaavg.xml";
/******************************* Globals in this file ******************************/
CvMat *eigenvalues;
CvMat *eigenvectors;
CvMat *eigenavg;
/****************************** Function Prototypes ********************************/
#ifndef NO_DOXYGEN
void cvParticleObserveInitialize();
void cvParticleObserveFinalize();
void icvPreprocess( const IplImage* patch, CvMat *mat );
void icvGetFeatures( const CvParticle* p, const IplImage* frame, CvMat* features );
void cvParticleObserveMeasure( CvParticle* p, IplImage* cur_frame, IplImage *pre_frame );
#endif
/****************************** Functions ******************************************/
/**
* Initialization
*/
void cvParticleObserveInitialize()
{
string filename;
filename = data_dir + data_pcaval;
if( (eigenvalues = (CvMat*)cvLoad( filename.c_str() )) == NULL ) {
cerr << filename << " is not loadable." << endl << flush;
exit( 1 );
}
filename = data_dir + data_pcavec;
if( (eigenvectors = (CvMat*)cvLoad( filename.c_str() )) == NULL ) {
cerr << filename << " is not loadable." << endl << flush;
exit( 1 );
}
filename = data_dir + data_pcaavg;
if( (eigenavg = (CvMat*)cvLoad( filename.c_str() )) == NULL ) {
cerr << filename << " is not loadable." << endl << flush;
exit( 1 );
}
}
/**
* Finalization
*/
void cvParticleObserveFinalize()
{
cvReleaseMat( &eigenvalues );
cvReleaseMat( &eigenvectors );
cvReleaseMat( &eigenavg );
}
/**
* Preprocess as did in training PCA subspace
*/
void icvPreprocess( const IplImage* patch, CvMat *mat )
{
IplImage *gry;
if( patch->nChannels != 1 ) {
gry = cvCreateImage( cvGetSize(patch), patch->depth, 1 );
cvCvtColor( patch, gry, CV_BGR2GRAY );
} else {
gry = (IplImage*)patch;
}
IplImage *resize = cvCreateImage( cvSize(mat->rows, mat->cols), patch->depth, 1 );
cvResize( gry, resize );
cvConvert( resize, mat );
cvImgGaussNorm( mat, mat );
cvReleaseImage( &resize );
if( gry != patch )
cvReleaseImage( &gry );
}
/**
* Get observation features
*
* CvParticleState must have x, y, width, height, angle
*/
void icvGetFeatures( const CvParticle* p, const IplImage* frame, CvMat* features )
{
int feature_height = feature_size.height;
int feature_width = feature_size.width;
//cvNamedWindow( "patch" );
CvMat* normed = cvCreateMat( feature_height, feature_width, CV_64FC1 );
CvMat* normedT = cvCreateMat( feature_width, feature_height, CV_64FC1 );
CvMat* feature, featurehdr;
IplImage *patch;
for( int n = 0; n < p->num_particles; n++ ) {
CvParticleState s = cvParticleStateGet( p, n );
CvBox32f box32f = cvBox32f( s.x, s.y, s.width, s.height, s.angle );
CvRect32f rect32f = cvRect32fFromBox32f( box32f );
// get image patch and preprocess
patch = cvCreateImage( cvSize( cvRound( s.width ), cvRound( s.height ) ),
frame->depth, frame->nChannels );
cvCropImageROI( (IplImage*)frame, patch, rect32f );
//cvShowImage( "patch", patch );
//cvWaitKey( 10 );
icvPreprocess( patch, normed );
cvReleaseImage( &patch );
// vectorize
cvT( normed, normedT ); // transpose to make the same with matlab's reshape
feature = cvReshape( normedT, &featurehdr, 1, feature_height * feature_width );
cvSetCol( feature, features, n );
}
cvReleaseMat( &normedT );
cvReleaseMat( &normed );
}
/**
* Measure and weight particles.
*
* The proposal function q is set p(xt|xt-1) in SIR/Condensation, and it results
* that "weights" are set to be proportional to the likelihood probability
* (Normalize later).
* Rewrite here if you want to use a different proposal function q.
*
* CvParticleState s must have s.x, s.y, s.width, s.height, s.angle
*
* @param particle
* @param frame
* @param reference
*/
void cvParticleObserveMeasure( CvParticle* p, IplImage* frame )
{
int feature_height = feature_size.height;
int feature_width = feature_size.width;
// extract features from particle states
CvMat* features = cvCreateMat( feature_height*feature_width, p->num_particles, CV_64FC1 );
icvGetFeatures( p, frame, features );
// Likelihood measurments
cvMatPcaDiffs( features, eigenavg, eigenvalues, eigenvectors, p->weights, 0, TRUE);
}
#endif

443
Sources/Tracking/Algorithm1/observetemplate.h Normal file
View File

@ -0,0 +1,443 @@
//------------------------------------------------------------------------------
//
// 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

255
Sources/Tracking/Algorithm1/state.h Normal file
View File

@ -0,0 +1,255 @@
/*
*
* The MIT License
*
* Copyright (c) 2008, Naotoshi Seo <sonots(at)sonots.com>
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*/
#ifndef CV_PARTICLE_ROTRECT_H
#define CV_PARTICLE_ROTRECT_H
#include "opencvx/cvparticle.h"
#include "opencvx/cvdrawrectangle.h"
#include "opencvx/cvcropimageroi.h"
#include "opencvx/cvrect32f.h"
#include <float.h>
//using namespace std;
/********************** Definition of a particle *****************************/
int num_states = 5;
/*! Definition of meanings of 5 states.
This kinds of structures is not necessary to be defined,
but I recommend to define them because it makes clear meanings of states
*/
typedef struct CvParticleState {
double x; //!< Center coord of a rectangle
double y; //!< Center coord of a rectangle
double width; //!< Width of a rectangle
double height; //!< Height of a rectangle
double angle; //!< Rotation around center. degree
} CvParticleState;
/*! Definition of dynamics model:
@code
new_particle = cvMatMul( dynamics, particle ) + noise
@endcode
@code
curr_x =: curr_x + noise
@endcode
*/
double dynamics[] = {
1, 0, 0, 0, 0,
0, 1, 0, 0, 0,
0, 0, 1, 0, 0,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1,
};
/********************** Function Prototypes *********************************/
// Functions for CvParticleState structure ( constructor, getter, setter )
inline CvParticleState cvParticleState( double x,
double y,
double width,
double height,
double angle = 0 );
CvParticleState cvParticleStateFromMat( const CvMat* state );
void cvParticleStateToMat( const CvParticleState &state, CvMat* state_mat );
CvParticleState cvParticleStateGet( const CvParticle* p, int p_id );
void cvParticleStateSet( CvParticle* p, int p_id, const CvParticleState &state );
// Particle Filter configuration
void cvParticleStateConfig( CvParticle* p, CvSize imsize, CvParticleState& std );
void cvParticleStateAdditionalBound( CvParticle* p, CvSize imsize );
// Utility Functions
void cvParticleStateDisplay( const CvParticleState& state, IplImage* frame, CvScalar color );
void cvParticleStatePrint( const CvParticleState& state );
/****************** Functions for CvParticleState structure ******************/
// This kinds of state definitions are not necessary,
// but helps readability of codes for sure.
/*!
* Constructor
*/
inline CvParticleState cvParticleState( double x,
double y,
double width,
double height,
double angle )
{
CvParticleState state = { x, y, width, height, angle };
return state;
}
/*!
* Convert a matrix state representation to a state structure
*
* @param state num_states x 1 matrix
*/
CvParticleState cvParticleStateFromMat( const CvMat* state )
{
CvParticleState s;
s.x = cvmGet( state, 0, 0 );
s.y = cvmGet( state, 1, 0 );
s.width = cvmGet( state, 2, 0 );
s.height = cvmGet( state, 3, 0 );
s.angle = cvmGet( state, 4, 0 );
return s;
}
/*!
* Convert a state structure to CvMat
*
* @param state A CvParticleState structure
* @param state_mat num_states x 1 matrix
* @return void
*/
void cvParticleStateToMat( const CvParticleState& state, CvMat* state_mat )
{
cvmSet( state_mat, 0, 0, state.x );
cvmSet( state_mat, 1, 0, state.y );
cvmSet( state_mat, 2, 0, state.width );
cvmSet( state_mat, 3, 0, state.height );
cvmSet( state_mat, 4, 0, state.angle );
}
/*!
* Get a state from a particle filter structure
*
* @param p particle filter struct
* @param p_id particle id
*/
CvParticleState cvParticleStateGet( const CvParticle* p, int p_id )
{
CvMat* state, hdr;
state = cvGetCol( p->particles, &hdr, p_id );
return cvParticleStateFromMat( state );
}
/*!
* Set a state to a particle filter structure
*
* @param state A CvParticleState structure
* @param p particle filter struct
* @param p_id particle id
* @return void
*/
void cvParticleStateSet( CvParticle* p, int p_id, const CvParticleState& state )
{
CvMat* state_mat, hdr;
state_mat = cvGetCol( p->particles, &hdr, p_id );
cvParticleStateToMat( state, state_mat );
}
/*************************** Particle Filter Configuration *********************************/
/*!
* Configuration of Particle filter
*/
void cvParticleStateConfig( CvParticle* p, CvSize imsize, CvParticleState& std )
{
// config dynamics model
CvMat dynamicsmat = cvMat( p->num_states, p->num_states, CV_64FC1, dynamics );
// config random noise standard deviation
CvRNG rng = cvRNG( time( NULL ) );
double stdarr[] = {
std.x,
std.y,
std.width,
std.height,
std.angle
};
CvMat stdmat = cvMat( p->num_states, 1, CV_64FC1, stdarr );
// config minimum and maximum values of states
// lowerbound, upperbound, circular flag (useful for degree)
// lowerbound == upperbound to express no bounding
double boundarr[] = {
0, imsize.width - 1, false,
0, imsize.height - 1, false,
1, imsize.width, false,
1, imsize.height, false,
0, 360, true
};
CvMat boundmat = cvMat( p->num_states, 3, CV_64FC1, boundarr );
cvParticleSetDynamics( p, &dynamicsmat );
cvParticleSetNoise( p, rng, &stdmat );
cvParticleSetBound( p, &boundmat );
}
/*!
* @todo
* CvParticle does not support this type of bounding currently
* Call after transition
*/
void cvParticleStateAdditionalBound( CvParticle* p, CvSize imsize )
{
for( int np = 0; np < p->num_particles; np++ )
{
double x = cvmGet( p->particles, 0, np );
double y = cvmGet( p->particles, 1, np );
double width = cvmGet( p->particles, 2, np );
double height = cvmGet( p->particles, 3, np );
width = MIN( width, imsize.width - (x) ); // another state x is used
height = MIN( height, imsize.height - (y) ); // another state y is used
cvmSet( p->particles, 2, np, width );
cvmSet( p->particles, 3, np, height );
}
}
void cvParticleRestoreSize(CvParticle *p, CvSize size)
{
int i = 0;
if((cvmGet(p->std, 0, 2) == 0.0) && (cvmGet(p->std, 0, 3) == 0.0))
{
printf("aaa");
for(i = 0; i < p->num_particles; i++)
{
cvmSet(p->particles, 2, i, size.width);
cvmSet(p->particles, 3, i, size.height);
}
}
}
/***************************** Utility Functions ****************************************/
void cvParticleStateDisplay( const CvParticleState& state, IplImage* img, CvScalar color, int thickness )
{
CvBox32f box32f = cvBox32f( state.x, state.y, state.width, state.height, state.angle );
CvRect32f rect32f = cvRect32fFromBox32f( box32f );
cvDrawRectangle( img, rect32f, cvPoint2D32f(0,0), color, thickness,8,0);
//cvDrawRectangle( img, rect32f, cvPoint2D32f(0,0), color);
}
void cvParticleStatePrint( const CvParticleState& state )
{
printf( "x :%.2f ", state.x );
printf( "y :%.2f ", state.y );
printf( "width :%.2f ", state.width );
printf( "height :%.2f ", state.height );
printf( "angle :%.2f\n", state.angle );
fflush( stdout );
}
#endif

248
Sources/Tracking/Algorithm1/state1.h Normal file
View File

@ -0,0 +1,248 @@
/** @file
* Rotated rectangle state particle filter +
* 1nd order AR dynamics model ( in fact, next = current + noise )
*
* Use this file as a template of definitions of states and
* state transition model for particle filter
*
* Currently cvparticle.h supports only linear combination of states transition
* model only. you may create another cvParticleTransition to support more complex
* non-linear state transition model. Most of other functions still should be
* available modifications
*/
/* The MIT License
*
* Copyright (c) 2008, Naotoshi Seo <sonots(at)sonots.com>
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#ifndef CV_PARTICLE_ROTRECT_H
#define CV_PARTICLE_ROTRECT_H
#include "cvparticle.h"
#include "cvdrawrectangle.h"
#include "cvcropimageroi.h"
#include "cvrect32f.h"
#include <float.h>
using namespace std;
/********************** Definition of a particle *****************************/
int num_states = 5;
// Definition of meanings of 5 states.
// This kinds of structures is not necessary to be defined,
// but I recommend to define them because it makes clear meanings of states
typedef struct CvParticleState {
double x; // center coord of a rectangle
double y; // center coord of a rectangle
double width; // width of a rectangle
double height; // height of a rectangle
double angle; // rotation around center. degree
} CvParticleState;
// Definition of dynamics model
// new_particle = cvMatMul( dynamics, particle ) + noise
// curr_x =: curr_x + noise
double dynamics[] = {
1, 0, 0, 0, 0,
0, 1, 0, 0, 0,
0, 0, 1, 0, 0,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1,
};
/********************** Function Prototypes *********************************/
#ifndef NO_DOXYGEN
// Functions for CvParticleState structure ( constructor, getter, setter )
inline CvParticleState cvParticleState( double x,
double y,
double width,
double height,
double angle = 0 );
CvParticleState cvParticleStateFromMat( const CvMat* state );
void cvParticleStateToMat( const CvParticleState &state, CvMat* state_mat );
CvParticleState cvParticleStateGet( const CvParticle* p, int p_id );
void cvParticleStateSet( CvParticle* p, int p_id, const CvParticleState &state );
// Particle Filter configuration
void cvParticleStateConfig( CvParticle* p, CvSize imsize, CvParticleState& std );
void cvParticleStateAdditionalBound( CvParticle* p, CvSize imsize );
// Utility Functions
void cvParticleStateDisplay( const CvParticleState& state, IplImage* frame, CvScalar color );
void cvParticleStatePrint( const CvParticleState& state );
#endif
/****************** Functions for CvParticleState structure ******************/
// This kinds of state definitions are not necessary,
// but helps readability of codes for sure.
/**
* Constructor
*/
inline CvParticleState cvParticleState( double x,
double y,
double width,
double height,
double angle )
{
CvParticleState state = { x, y, width, height, angle };
return state;
}
/**
* Convert a matrix state representation to a state structure
*
* @param state num_states x 1 matrix
*/
CvParticleState cvParticleStateFromMat( const CvMat* state )
{
CvParticleState s;
s.x = cvmGet( state, 0, 0 );
s.y = cvmGet( state, 1, 0 );
s.width = cvmGet( state, 2, 0 );
s.height = cvmGet( state, 3, 0 );
s.angle = cvmGet( state, 4, 0 );
return s;
}
/**
* Convert a state structure to CvMat
*
* @param state A CvParticleState structure
* @param state_mat num_states x 1 matrix
* @return void
*/
void cvParticleStateToMat( const CvParticleState& state, CvMat* state_mat )
{
cvmSet( state_mat, 0, 0, state.x );
cvmSet( state_mat, 1, 0, state.y );
cvmSet( state_mat, 2, 0, state.width );
cvmSet( state_mat, 3, 0, state.height );
cvmSet( state_mat, 4, 0, state.angle );
}
/**
* Get a state from a particle filter structure
*
* @param p particle filter struct
* @param p_id particle id
*/
CvParticleState cvParticleStateGet( const CvParticle* p, int p_id )
{
CvMat* state, hdr;
state = cvGetCol( p->particles, &hdr, p_id );
return cvParticleStateFromMat( state );
}
/**
* Set a state to a particle filter structure
*
* @param state A CvParticleState structure
* @param p particle filter struct
* @param p_id particle id
* @return void
*/
void cvParticleStateSet( CvParticle* p, int p_id, const CvParticleState& state )
{
CvMat* state_mat, hdr;
state_mat = cvGetCol( p->particles, &hdr, p_id );
cvParticleStateToMat( state, state_mat );
}
/*************************** Particle Filter Configuration *********************************/
/**
* Configuration of Particle filter
*/
void cvParticleStateConfig( CvParticle* p, CvSize imsize, CvParticleState& std )
{
// config dynamics model
CvMat dynamicsmat = cvMat( p->num_states, p->num_states, CV_64FC1, dynamics );
// config random noise standard deviation
CvRNG rng = cvRNG( time( NULL ) );
double stdarr[] = {
std.x,
std.y,
std.width,
std.height,
std.angle
};
CvMat stdmat = cvMat( p->num_states, 1, CV_64FC1, stdarr );
// config minimum and maximum values of states
// lowerbound, upperbound, circular flag (useful for degree)
// lowerbound == upperbound to express no bounding
double boundarr[] = {
0, imsize.width - 1, false,
0, imsize.height - 1, false,
1, imsize.width, false,
1, imsize.height, false,
0, 360, true
};
CvMat boundmat = cvMat( p->num_states, 3, CV_64FC1, boundarr );
cvParticleSetDynamics( p, &dynamicsmat );
cvParticleSetNoise( p, rng, &stdmat );
cvParticleSetBound( p, &boundmat );
}
/**
* @todo
* CvParticle does not support this type of bounding currently
* Call after transition
*/
void cvParticleStateAdditionalBound( CvParticle* p, CvSize imsize )
{
for( int np = 0; np < p->num_particles; np++ )
{
double x = cvmGet( p->particles, 0, np );
double y = cvmGet( p->particles, 1, np );
double width = cvmGet( p->particles, 2, np );
double height = cvmGet( p->particles, 3, np );
width = MIN( width, imsize.width - x ); // another state x is used
height = MIN( height, imsize.height - y ); // another state y is used
cvmSet( p->particles, 2, np, width );
cvmSet( p->particles, 3, np, height );
}
}
/***************************** Utility Functions ****************************************/
void cvParticleStateDisplay( const CvParticleState& state, IplImage* img, CvScalar color )
{
CvBox32f box32f = cvBox32f( state.x, state.y, state.width, state.height, state.angle );
CvRect32f rect32f = cvRect32fFromBox32f( box32f );
cvDrawRectangle( img, rect32f, cvPoint2D32f(0,0), color );
}
void cvParticleStatePrint( const CvParticleState& state )
{
printf( "x :%.2f ", state.x );
printf( "y :%.2f ", state.y );
printf( "width :%.2f ", state.width );
printf( "height :%.2f ", state.height );
printf( "angle :%.2f\n", state.angle );
fflush( stdout );
}
#endif

301
Sources/Tracking/Algorithm1/state2.h Normal file
View File

@ -0,0 +1,301 @@
/** @file
* Rotated rectangle state particle filter +
* 2nd order AR dynamics model ( in fact, next = current + speed + noise )
*
* Use this file as a template of definitions of states and
* state transition model for particle filter
*
* Currently cvparticle.h supports only linear combination of states transition
* model only. you may create another cvParticleTransition to support more complex
* non-linear state transition model. Most of other functions still should be
* available modifications
*/
/* The MIT License
*
* Copyright (c) 2008, Naotoshi Seo <sonots(at)sonots.com>
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#ifndef CV_PARTICLE_ROTRECT2_H
#define CV_PARTICLE_ROTRECT2_H
#include "cvparticle.h"
#include "cvdrawrectangle.h"
#include "cvcropimageroi.h"
#include "cvrect32f.h"
#include <float.h>
using namespace std;
/********************** Definition of a particle *****************************/
int num_states = 10;
// Definition of meanings of 10 states.
// This kinds of structures is not necessary to be defined,
// but I recommend to define them because it makes clear meanings of states
typedef struct CvParticleState {
double x; // center coord of a rectangle
double y; // center coord of a rectangle
double width; // width of a rectangle
double height; // height of a rectangle
double angle; // rotation around center. degree
double xp; // previous center coord of a rectangle
double yp; // previous center coord of a rectangle
double widthp; // previous width of a rectangle
double heightp; // previous height of a rectangle
double anglep; // previous rotation around center. degree
} CvParticleState;
// Definition of dynamics model
// new_particle = cvMatMul( dynamics, particle ) + noise
// curr_x =: curr_x + dx + noise = curr_x + (curr_x - prev_x) + noise
// prev_x =: curr_x
double dynamics[] = {
2, 0, 0, 0, 0, -1, 0, 0, 0, 0,
0, 2, 0, 0, 0, 0, -1, 0, 0, 0,
0, 0, 2, 0, 0, 0, 0, -1, 0, 0,
0, 0, 0, 2, 0, 0, 0, 0, -1, 0,
0, 0, 0, 0, 2, 0, 0, 0, 0, -1,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
};
/********************** Function Prototypes *********************************/
#ifndef NO_DOXYGEN
// Functions for CvParticleState structure ( constructor, getter, setter )
inline CvParticleState cvParticleState( double x,
double y,
double width,
double height,
double angle = 0,
double xp = 0,
double yp = 0,
double widthp = 0,
double heightp = 0,
double anglep =0 );
CvParticleState cvParticleStateFromMat( const CvMat* state );
void cvParticleStateToMat( const CvParticleState &state, CvMat* state_mat );
CvParticleState cvParticleStateGet( const CvParticle* p, int p_id );
void cvParticleStateSet( CvParticle* p, int p_id, const CvParticleState &state );
// Particle Filter configuration
void cvParticleStateConfig( CvParticle* p, CvSize imsize, CvParticleState& std );
void cvParticleStateAdditionalBound( CvParticle* p, CvSize imsize );
// Utility Functions
void cvParticleStateDisplay( const CvParticleState& state, IplImage* frame, CvScalar color );
void cvParticleStatePrint( const CvParticleState& state );
#endif
/****************** Functions for CvParticleState structure ******************/
// This kinds of state definitions are not necessary,
// but helps readability of codes for sure.
/**
* Constructor
*/
inline CvParticleState cvParticleState( double x,
double y,
double width,
double height,
double angle,
double xp,
double yp,
double widthp,
double heightp,
double anglep )
{
CvParticleState state = { x, y, width, height, angle,
xp, yp, widthp, heightp, anglep };
return state;
}
/**
* Convert a matrix state representation to a state structure
*
* @param state num_states x 1 matrix
*/
CvParticleState cvParticleStateFromMat( const CvMat* state )
{
CvParticleState s;
s.x = cvmGet( state, 0, 0 );
s.y = cvmGet( state, 1, 0 );
s.width = cvmGet( state, 2, 0 );
s.height = cvmGet( state, 3, 0 );
s.angle = cvmGet( state, 4, 0 );
s.xp = cvmGet( state, 5, 0 );
s.yp = cvmGet( state, 6, 0 );
s.widthp = cvmGet( state, 7, 0 );
s.heightp = cvmGet( state, 8, 0 );
s.anglep = cvmGet( state, 9, 0 );
return s;
}
/**
* Convert a state structure to CvMat
*
* @param state A CvParticleState structure
* @param state_mat num_states x 1 matrix
* @return void
*/
void cvParticleStateToMat( const CvParticleState& state, CvMat* state_mat )
{
cvmSet( state_mat, 0, 0, state.x );
cvmSet( state_mat, 1, 0, state.y );
cvmSet( state_mat, 2, 0, state.width );
cvmSet( state_mat, 3, 0, state.height );
cvmSet( state_mat, 4, 0, state.angle );
cvmSet( state_mat, 5, 0, state.xp );
cvmSet( state_mat, 6, 0, state.yp );
cvmSet( state_mat, 7, 0, state.widthp );
cvmSet( state_mat, 8, 0, state.heightp );
cvmSet( state_mat, 9, 0, state.anglep );
}
/**
* Get a state from a particle filter structure
*
* @param p particle filter struct
* @param p_id particle id
*/
CvParticleState cvParticleStateGet( const CvParticle* p, int p_id )
{
CvMat* state, hdr;
state = cvGetCol( p->particles, &hdr, p_id );
return cvParticleStateFromMat( state );
}
/**
* Set a state to a particle filter structure
*
* @param state A CvParticleState structure
* @param p particle filter struct
* @param p_id particle id
* @return void
*/
void cvParticleStateSet( CvParticle* p, int p_id, const CvParticleState& state )
{
CvMat* state_mat, hdr;
state_mat = cvGetCol( p->particles, &hdr, p_id );
cvParticleStateToMat( state, state_mat );
}
/*************************** Particle Filter Configuration *********************************/
/**
* Configuration of Particle filter
*/
void cvParticleStateConfig( CvParticle* p, CvSize imsize, CvParticleState& std )
{
// config dynamics model
CvMat dynamicsmat = cvMat( p->num_states, p->num_states, CV_64FC1, dynamics );
// config random noise standard deviation
CvRNG rng = cvRNG( time( NULL ) );
double stdarr[] = {
std.x,
std.y,
std.width,
std.height,
std.angle,
0,
0,
0,
0,
0
};
CvMat stdmat = cvMat( p->num_states, 1, CV_64FC1, stdarr );
// config minimum and maximum values of states
// lowerbound, upperbound, circular flag (useful for degree)
// lowerbound == upperbound to express no bounding
double boundarr[] = {
0, imsize.width - 1, false,
0, imsize.height - 1, false,
1, imsize.width, false,
1, imsize.height, false,
0, 360, true,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0
};
CvMat boundmat = cvMat( p->num_states, 3, CV_64FC1, boundarr );
cvParticleSetDynamics( p, &dynamicsmat );
cvParticleSetNoise( p, rng, &stdmat );
cvParticleSetBound( p, &boundmat );
}
/**
* @todo
* CvParticle does not support this type of bounding currently
* Call after transition
*/
void cvParticleStateAdditionalBound( CvParticle* p, CvSize imsize )
{
for( int np = 0; np < p->num_particles; np++ )
{
double x = cvmGet( p->particles, 0, np );
double y = cvmGet( p->particles, 1, np );
double width = cvmGet( p->particles, 2, np );
double height = cvmGet( p->particles, 3, np );
width = MIN( width, imsize.width - x ); // another state x is used
height = MIN( height, imsize.height - y ); // another state y is used
cvmSet( p->particles, 2, np, width );
cvmSet( p->particles, 3, np, height );
}
}
/***************************** Utility Functions ****************************************/
/**
* Draw tracking state on an image
*/
void cvParticleStateDisplay( const CvParticleState& state, IplImage* img, CvScalar color )
{
CvBox32f box32f = cvBox32f( state.x, state.y, state.width, state.height, state.angle );
CvRect32f rect32f = cvRect32fFromBox32f( box32f );
cvDrawRectangle( img, rect32f, cvPoint2D32f(0,0), color );
}
/**
* Print the tracking state
*/
void cvParticleStatePrint( const CvParticleState& state )
{
printf( "x :%.2f ", state.x );
printf( "y :%.2f ", state.y );
printf( "width :%.2f ", state.width );
printf( "height :%.2f ", state.height );
printf( "angle :%.2f\n", state.angle );
printf( "xp:%.2f ", state.xp );
printf( "yp:%.2f ", state.yp );
printf( "widthp:%.2f ", state.widthp );
printf( "heightp:%.2f ", state.heightp );
printf( "anglep:%.2f\n", state.anglep );
fflush( stdout );
}
#endif