utilities.cpp

国外一个大牛人写的MEAN-SHIFT目标跟踪算法

/***************************************************************************
                          utilities.cpp  -  description
                             -------------------
    begin                : Sun Jul 29 2001
    copyright            : (C) 2001 by Alper Yilmaz
    email                : [e-mail]
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   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; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/

#include "utilities.h"
#include <iostream.h>
#include <fstream.h>
#include <stdio.h>
#include "newmatio.h"
/***************************************************************************
 *                                                                         *
 *   Create special filters                                                *
 *                                                                         *
 ***************************************************************************/
ReturnMatrix filter	(FILTER_TYPE type,int radius,float sigma,
					 int orientation,float scale)
{
   	Matrix 	flt(2*radius+1,2*radius+1);
    int 	i,j;
    Real 	a,b;
	switch ( type )
    {	case R1:
   	    	for (i=-radius; i <= radius; i++)
    		for (j=-radius; j <= radius; j++)
         		flt(i+radius+1,j+radius+1) = exp(- (fabs(i)+fabs(j)) / sigma);
			flt=flt/flt.Sum();
            break;
    	case GAUSS:
    		a=1/(sqrt(2*22/7)*sigma);
    		b=2*pow(sigma,2);
   	    	for (i=-radius; i <= radius; i++)
    		for (j=-radius; j <= radius; j++)
         		flt(i+radius+1,j+radius+1) = a*exp(- (i*i+j*j) / b);
     		flt=flt/flt.Sum();
     		break;
    	case ALPER:
   	    	for (i=-radius; i <= radius; i++)
    		for (j=-radius; j <= radius; j++)
         		flt(i+radius+1,j+radius+1) = exp(- sqrt(i*i+j*j) / sigma);
     		flt=flt/flt.Sum();
     		break;
     	case GABOR:
     		Matrix gauss(2*radius+1,2*radius+1);
     		gauss=filter(GAUSS,radius,sigma);
     		switch (orientation)
     		{	case   0: a=1;b= 0;break;
     			case  45: a=1;b=-1;break;
     			case  90: a=0;b= 1;break;
     			case 135: a=b=1   ;break;
     		}
   	    	for (i=-radius; i <= radius; i++)
    		for (j=-radius; j <= radius; j++)
         		flt(i+radius+1,j+radius+1) = sin(scale*((j+radius+1)*a+(i+radius+1)*b))*gauss(i+radius+1,j+radius+1);         	
			if (fabs(flt.Sum())>1) flt=flt/flt.Sum();
			break;     		
    }
   	flt.Release();return flt;
}

/***************************************************************************
 *                                                                         *
 *   Convolve 2 matrices                                                   *
 *                                                                         *
 ***************************************************************************/
ReturnMatrix conv (Matrix a,Matrix b)
{
	int i,j,k,l;
	int rows=a.Nrows();
	int cols=a.Ncols();
	int mrow=b.Nrows()/2;
	int mcol=b.Ncols()/2;
	
	Matrix result;
	result.ReSize(rows,cols);
	result=0;
	
	for (i=mrow+1;i<=rows-mrow;i++)
	for (j=mcol+1;j<=cols-mcol;j++)
	{	
		for (k=1;k<=b.Nrows();k++)
		for (l=1;l<=b.Ncols();l++)
			result(i,j)+=a(i+k-mrow-1,j+l-mcol-1)*b(k,l);
	}
	result.Release();return result;
}

/***************************************************************************
 *                                                                         *
 *   Convolve 2 matrices                                                   *
 *                                                                         *
 ***************************************************************************/
ReturnMatrix pinv (Matrix A)
{
	Matrix U;
	DiagonalMatrix D;
	Matrix V;
	Matrix result;
	int i;

	SVD(A, D, U, V, true, true);
	
	for (i=1;i<=D.Nrows();i++)
		if (fabs(D(i,i))<0.01) D(i,i)=0.0;
		else D(i,i)=1/D(i,i);
	
		
	result=V*D*U.t();
	result.Release();return result;
}

/***************************************************************************
 *                                                                         *
 *   Write matrix as image                                                 *
 *                                                                         *
 ***************************************************************************/
void write_as_image(char*filename,Matrix a,bool normalize)
{
	int  i,j;
/*	
	ofstream outfile;
	outfile.open(filename,ios::out);

	outfile <<"P5"<<endl
		    <<a.Nrows()<<" "<<a.Ncols()<<endl
		    <<255<<endl;
	
    //write the intensity data
	for (i=1;i<=a.Nrows();i++) for (j=1;j<=a.Ncols();j++)
    	outfile << (unsigned char)fabs(a(i,j));
	outfile.close();
*/
	if (normalize)
	{
		Real max=a.Maximum();
		Real min=a.Minimum();
		FILE *fp=fopen(filename,"w");
		unsigned char temp;
		fprintf(fp,"P5\n# alper\n%d %d\n%d\n",a.Nrows(),a.Ncols(),255);
		for (i=1;i<=a.Nrows();i++) for (j=1;j<=a.Ncols();j++)
		{	temp=(unsigned char)((a(i,j)-min)/(max-min)*255.0);
			fprintf(fp,"%c",temp);
		}
		fclose(fp);
	}
	else
	{
		FILE *fp=fopen(filename,"w");
		unsigned char temp;
		fprintf(fp,"P5\n# alper\n%d %d\n%d\n",a.Nrows(),a.Ncols(),255);
		for (i=1;i<=a.Nrows();i++) for (j=1;j<=a.Ncols();j++)
		{	temp=(unsigned char)a(i,j);
			fprintf(fp,"%c",temp);
		}
		fclose(fp);
	}
}

/***************************************************************************
 *                                                                         *
 *   return absolute value of a matrix                                     *
 *                                                                         *
 ***************************************************************************/
ReturnMatrix abs (Matrix A)
{
	Matrix result(A.Nrows(),A.Ncols());
	int i,j;
	for (i=1;i<=A.Nrows();i++)
	for (j=1;j<=A.Ncols();j++)
		result(i,j)=fabs(A(i,j));
	result.Release();return result;
}

/***************************************************************************
 *                                                                         *
 *   histogram                                                             *
 *                                                                         *
 ***************************************************************************/
ReturnMatrix hist(Matrix A)
{
	int i,j;
	RowVector h(256);
	h=0;
	for (i=1;i<=A.Nrows();i++)
	for (j=1;j<=A.Ncols();j++)
		h((int)A(i,j)+1)++;
	h.Release();return h;
}