meanshiftc.cpp

example mex file in matlab

/*
 *      Coffeeright (c) 2002-2006 by J. Luis
 *
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License as published by
 *      the Free Software Foundation; version 2 of the License.
 *
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *
 *      Contact info: w3.ualg.pt/~jluis
 *--------------------------------------------------------------------*/

#include <math.h>
#include <limits.h>
#include <stdlib.h>
#include "mex.h"

#define sub2ind(x,y,stepsize)     (((x)*(stepsize))+(y))
#define sub2ind_ptr(ptr,x,y,stepsize)     ( (ptr)+ ind2sub(x,y,stepsize) )
#define mxMax(a,b)  ( ((a)>(b))? (a) : (b))
#define mxMin(a,b)	( ((a)<(b))? (a) : (b))


void meanshift(double *outimg, const double *img, const int width, const int height, 
			   const double sig_spt, const double sig_rng, const double mindelta, 
			   const int radius, const int maxiter)
{
	const int stepSize = height; 
	double *sigx = new double[width*height];
	double *sigy = new double[width*height];
	double *data = new double [width*height];
	for(int y=0; y < height; y++) {
		for(int x=0; x < width; x++) {
			int index = sub2ind(x,y,stepSize);
			sigx[index] = x/(double)sig_spt;
			sigy[index] = y/(double)sig_spt;
			data[index] = img[index]/sig_rng;
		}
	}

	for(int y=0; y < height; y++) {
		for(int x=0; x < width; x++) {
			double oldx, oldy, oldv;
			int index = sub2ind(x,y,stepSize);
			oldx = sigx[index];	
			oldy = sigy[index];
			oldv = data[index];
			
			int itercount = 0;
			double diff = 1.0;
			while( itercount < maxiter && diff > mindelta ) {
				int mx = (int)(oldx*sig_spt);
				int my = (int)(oldy*sig_spt);

				int boudsx_min = mxMax(0,mx-radius);
				int boudsx_max = mxMin(width,mx+radius);
				int boudsy_min = mxMax(0,my-radius);
				int boudsy_max = mxMin(height,my+radius);

				double count = 0;
				double sumx = 0.0, sumy = 0.0, sumv = 0.0;
				for(int by=boudsy_min; by < boudsy_max; by++) {
					for(int bx=boudsx_min; bx < boudsx_max; bx++) {
						int windex = sub2ind(bx,by,stepSize);
						register double dist,v;
						v = data[windex];
						register double diffx = sigx[windex]-oldx;
						register double diffy = sigy[windex]-oldy;
						register double diffv = v-oldv;

						diffx *= diffx;	diffy *= diffy;	diffv *= diffv;
						dist = diffx + diffy + diffv;
						if( dist < 1.0 ) {
							sumx += sigx[windex];
							sumy += sigy[windex];
							sumv += v;
							count++;
						}
					}
				}

				if( count == 0 ) {
					break;					
				}
				else {
					sumx /= count;
					sumy /= count;
					sumv /= count;
				}

				// just only check the coordinate changed
				register double dx, dy;
				dx = oldx - sumx;
				dy = oldy - sumy;
				diff = dx*dx + dy*dy;


				oldx = sumx;
				oldy = sumy;
				oldv = sumv;

				itercount++;
			}

			outimg[index] = oldv*sig_rng;
		}
	}

	delete [] data;
	delete [] sigx;
	delete [] sigy;
}

/* --------------------------------------------------------------------------- */
/* Matlab Gateway routine */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) 
{
	int width = 0, height = 0;
	
	int maxiter = 100;	
	double sig_spt = 5;
	double sig_rng = 0.2;
	double mindelta = 0.001;
	int radius = sig_spt;

    double *img;
    
	/* ---- Check for errors in user's call to function.  -------------------- */
	if (nrhs == 0) {
		mexPrintf("Usage: B = locamaxima(A);\n");
		mexPrintf("       where A is a double MxN \n");
		return;
	}

    if (nrhs != 3)
		mexErrMsgTxt("locamaxima: requires only one input arguments!");

    if (!mxIsNumeric(prhs[0]) || mxIsComplex(prhs[0]) || mxIsSparse(prhs[0]))
		mexErrMsgTxt("locamaxima: Input image must be a real matrix");
	else if (nlhs != 1)
		mexErrMsgTxt("locamaxima returns only 2 output argument!");

	/* Find out in which data type was given the input array */
	if (!mxIsDouble(prhs[0])) {
		mexPrintf("locamaxima error: Invalid input data type.\n");
		mexErrMsgTxt("Valid types are: double.\n");
	}
	/* ------- input error out --------*/

	sig_spt = mxGetScalar(prhs[1]);
	sig_rng = mxGetScalar(prhs[2]);
	radius = sig_spt;
	
	height = mxGetM(prhs[0]);	width = mxGetN(prhs[0]);
    img = mxGetPr(prhs[0]);

	plhs[0] = mxCreateDoubleMatrix(height,width,mxREAL);
	double *outimg = mxGetPr(plhs[0]);


	meanshift(outimg, img, width, height, sig_spt, sig_rng, mindelta, radius, maxiter);
}