void imageProcessing(IplImage* img)
{
// Create temporary images
CvSize sz = cvGetSize(img);
IplImage* hsv_image = cvCreateImage(sz,8,3);
IplImage* hsv_mask = cvCreateImage(sz,8,1);
// HSV Conversion and Thresholding
cvCvtColor(img,hsv_image,CV_BGR2HSV);
cvInRangeS(hsv_image,hsv_min,hsv_max, hsv_mask);
// Init
if((height != sz.height) | (width != sz.width))
{
height = sz.height;
width = sz.width;
initParticles();
}
// Filter
particleFilter(hsv_mask);
// Compute variance
std::pair<double,double> V = particlesVariance();
//ROS_INFO("Variances: Vx = %f, Vy = %f",V.first,V.second);
if((V.first < Var_min) & (V.second < Var_min))
{
robotDetected = true;
}
// Draw particles
showParticles(hsv_mask);
// Show result
//cvNamedWindow("Search",1); cvShowImage("Search",hsv_mask);
cvWaitKey(10);
}
int main(int argc, char * argv[]){
char* usage = "openmp.out -x <dimX> -y <dimY> -z <Nfr> -np <Nparticles>";
//check number of arguments
if(argc != 9)
{
printf("%s\n", usage);
return 0;
}
//check args deliminators
if( strcmp( argv[1], "-x" ) || strcmp( argv[3], "-y" ) || strcmp( argv[5], "-z" ) || strcmp( argv[7], "-np" ) ) {
printf( "%s\n",usage );
return 0;
}
int IszX, IszY, Nfr, Nparticles;
//converting a string to a integer
if( sscanf( argv[2], "%d", &IszX ) == EOF ) {
printf("ERROR: dimX input is incorrect");
return 0;
}
if( IszX <= 0 ) {
printf("dimX must be > 0\n");
return 0;
}
//converting a string to a integer
if( sscanf( argv[4], "%d", &IszY ) == EOF ) {
printf("ERROR: dimY input is incorrect");
return 0;
}
if( IszY <= 0 ) {
printf("dimY must be > 0\n");
return 0;
}
//converting a string to a integer
if( sscanf( argv[6], "%d", &Nfr ) == EOF ) {
printf("ERROR: Number of frames input is incorrect");
return 0;
}
if( Nfr <= 0 ) {
printf("number of frames must be > 0\n");
return 0;
}
//converting a string to a integer
if( sscanf( argv[8], "%d", &Nparticles ) == EOF ) {
printf("ERROR: Number of particles input is incorrect");
return 0;
}
if( Nparticles <= 0 ) {
printf("Number of particles must be > 0\n");
return 0;
}
//establish seed
int * seed = (int *)malloc(sizeof(int)*Nparticles);
int i;
for(i = 0; i < Nparticles; i++)
seed[i] = time(0)*i;
//malloc matrix
int * I = (int *)malloc(sizeof(int)*IszX*IszY*Nfr);
long long start = get_time();
//call video sequence
videoSequence(I, IszX, IszY, Nfr, seed);
long long endVideoSequence = get_time();
printf("VIDEO SEQUENCE TOOK %f\n", elapsed_time(start, endVideoSequence));
//call particle filter
particleFilter(I, IszX, IszY, Nfr, seed, Nparticles);
long long endParticleFilter = get_time();
printf("PARTICLE FILTER TOOK %f\n", elapsed_time(endVideoSequence, endParticleFilter));
printf("ENTIRE PROGRAM TOOK %f\n", elapsed_time(start, endParticleFilter));
free(seed);
free(I);
return 0;
}
/**
* Function that allows the 2 particle filter implementations to be run
* @details The number of arguments provided to this function determines which function will be called. 7 args will call the video processing version. 6 (leaving out the number of frames) will call the image processing version.
* @param nlhs (Number on the Left Hand Side) The number of items to return (2 will be in this case; the x and y arrays)
* @param plhs (Parameters on the Left Hand Side) A pointer to the arrays containing the x and y arrays
* @param nrhs (Number on the Right Hand Side) The number of arguments to take in (7 are needed for video processing (The image as an unsigned char, the x dimension, the y dimension, the number of frames, the number of particles, the x starting position, the y starting position)
* 6 are needed for the image processing (same as before but leave out the number of frames)
* @param prhs (Parameters on the Right Hand Side) A pointer to the arrays containing the parameters
*/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
int * I;
int IszX, IszY, Nfr, Nparticles;
if(nrhs < 6)
{
printf("ERROR: TOO FEW ARGS HAVE BEEN ENTERED\n");
printf("EXITING\n");
exit(0);
}
else if(nrhs == 7)
{
IszX = (int)(mxGetScalar(prhs[1]));
IszY = (int)(mxGetScalar(prhs[2]));
Nfr = (int)(mxGetScalar(prhs[3]));
Nparticles = (int)(mxGetScalar(prhs[4]));
unsigned char * cI = (unsigned char *)mxGetData(prhs[0]);
I = (int *)mxCalloc(IszX*IszY*Nfr, sizeof(int));
int x, y, z;
for(x = 0; x < IszX; x++){
for(y = 0; y < IszY; y++){
for(z = 0; z < Nfr; z++){
I[x*IszY*Nfr + y*Nfr + z] = (int)cI[x*IszY*Nfr + y*Nfr + z];
}
}
}
double xe = (double)mxGetScalar(prhs[5]);
double ye = (double)mxGetScalar(prhs[6]);
int * seed = (int *)mxCalloc(Nparticles, sizeof(int));
int i;
for(i = 0; i < Nparticles; i++)
seed[i] = time(0)*i;
double * posX = (double *)mxCalloc(Nfr, sizeof(double));
double * posY = (double *)mxCalloc(Nfr, sizeof(double));
particleFilter(I, IszX, IszY, Nfr, seed, Nparticles, posX, posY, xe, ye);
mxFree(I);
mxFree(seed);
plhs[0] = mxCreateDoubleMatrix(Nfr, 1, mxREAL);
plhs[1] = mxCreateDoubleMatrix(Nfr, 1, mxREAL);
double * bufferX = mxGetPr(plhs[0]);
double * bufferY = mxGetPr(plhs[1]);
for(i = 0; i < Nfr; i++)
{
bufferX[i] = posX[i];
bufferY[i] = posY[i];
}
mxFree(posX);
mxFree(posY);
}
else if(nrhs == 6)
{
IszX = (int)(mxGetScalar(prhs[1]));
IszY = (int)(mxGetScalar(prhs[2]));
Nparticles = (int)(mxGetScalar(prhs[3]));
double startX = (double)mxGetScalar(prhs[4]);
double startY = (double)mxGetScalar(prhs[5]);
unsigned char * cI = (unsigned char *)mxGetData(prhs[0]);
I = (int *)mxCalloc(IszX*IszY, sizeof(int));
int x, y, z;
for(x = 0; x < IszX; x++){
for(y = 0; y < IszY; y++){
I[x*IszX + y] = (int)cI[x*IszX + y];
}
}
int * seed = (int *)mxCalloc(Nparticles, sizeof(int));
int i;
for(i = 0; i < Nparticles; i++)
seed[i] = time(0)*i;
double posX[1];
double posY[1];
particleFilter1F(I, IszX, IszY, seed, Nparticles, posX, posY, startX, startY);
mxFree(I);
mxFree(seed);
plhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
double * bufferX = mxGetPr(plhs[0]);
double * bufferY = mxGetPr(plhs[1]);
bufferX[0] = posX[0];
bufferY[0] = posY[0];
}
else
{
printf("ERROR: TOO MANY ARGS\n");
printf("EXITING\n");
exit(0);
}
}
int main(int argc, char * argv[]){
printf("Starting ...\n");
init_timer(timer_ctrl, timer_counter_l, timer_counter_h);
start_timer(timer_ctrl);
int IszX, IszY, Nfr, Nparticles;
/*
char* usage = "naive.out -x <dimX> -y <dimY> -z <Nfr> -np <Nparticles>";
//check number of arguments
if(argc != 9)
{
printf("%s\n", usage);
return 0;
}
//check args deliminators
if( strcmp( argv[1], "-x" ) || strcmp( argv[3], "-y" ) || strcmp( argv[5], "-z" ) || strcmp( argv[7], "-np" ) ) {
printf( "%s\n",usage );
return 0;
}
//converting a string to a integer
if( sscanf( argv[2], "%d", &IszX ) == EOF ) {
printf("ERROR: dimX input is incorrect");
return 0;
}
if( IszX <= 0 ) {
printf("dimX must be > 0\n");
return 0;
}
//converting a string to a integer
if( sscanf( argv[4], "%d", &IszY ) == EOF ) {
printf("ERROR: dimY input is incorrect");
return 0;
}
if( IszY <= 0 ) {
printf("dimY must be > 0\n");
return 0;
}
//converting a string to a integer
if( sscanf( argv[6], "%d", &Nfr ) == EOF ) {
printf("ERROR: Number of frames input is incorrect");
return 0;
}
if( Nfr <= 0 ) {
printf("number of frames must be > 0\n");
return 0;
}
//converting a string to a integer
if( sscanf( argv[8], "%d", &Nparticles ) == EOF ) {
printf("ERROR: Number of particles input is incorrect");
return 0;
}
if( Nparticles <= 0 ) {
printf("Number of particles must be > 0\n");
return 0;
}
*/
IszX = 128;
IszY = 128;
Nfr = 10;
Nparticles = 1700;
//establish seed
int * seed = (int *)malloc(sizeof(int)*Nparticles);
int i;
for(i = 0; i < Nparticles; i++)
seed[i] = i;//time(0)*i;
//malloc matrix
int * I = (int *)malloc(sizeof(int)*IszX*IszY*Nfr);
memset(I, 0, sizeof(int) * IszX * IszY * Nfr);
//long long start = get_time();
//call video sequence
videoSequence(I, IszX, IszY, Nfr, seed);
//long long endVideoSequence = get_time();
//printf("VIDEO SEQUENCE TOOK %f\n", elapsed_time(start, endVideoSequence));
//call particle filter
int passed = particleFilter(I, IszX, IszY, Nfr, seed, Nparticles);
//long long endParticleFilter = get_time();
//printf("PARTICLE FILTER TOOK %f\n", elapsed_time(endVideoSequence, endParticleFilter));
//printf("ENTIRE PROGRAM TOOK %f\n", elapsed_time(start, endParticleFilter));
free(seed);
free(I);
#ifdef VERIFY
if (passed == 1) {
printf("PASSED.\n");
} else {
printf("FAILED.\n");
}
#endif
stop_timer(timer_ctrl);
printf("Execution time %lld us\n\r", elapsed_time());
}
