推薦大家看論文《An adaptive color-based particle filter》
這次我直接截圖我的碩士畢業論文的第二章的一部分，應該講得比較詳細了。最後給出我當時在pudn找到的最適合學習的實現代碼
代碼實現：
運行方式：按P停止，在前景窗口鼠標點擊目標，會自動生成外接矩形，再次按P，對該選定目標進行跟蹤。
// TwoLevel.cpp : 定義控制檯應用程序的入口點。
//
/************************************************************************/
/*參考文獻real-time Multiple Objects Tracking with Occlusion Handling in Dynamic Scenes */
/************************************************************************/
#include "stdafx.h"
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <math.h>
# include <time.h>
#include <iostream>
using namespace std;
#define B(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3] //B
#define G(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3+1] //G
#define R(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3+2] //R
#define S(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)]
#define Num 10 //幀差的間隔
#define T 40 //Tf
#define Re 30 //
#define ai 0.08 //學習率
#define CONTOUR_MAX_AREA 10000
#define CONTOUR_MIN_AREA 50
# define R_BIN 8 /* 紅色分量的直方圖條數 */
# define G_BIN 8 /* 綠色分量的直方圖條數 */
# define B_BIN 8 /* 蘭色分量的直方圖條數 */
# define R_SHIFT 5 /* 與上述直方圖條數對應 */
# define G_SHIFT 5 /* 的R、G、B分量左移位數 */
# define B_SHIFT 5 /* log2( 256/8 )爲移動位數 */
/*
採用Park and Miller方法產生[0,1]之間均勻分佈的僞隨機數
算法詳細描述見：
[1] NUMERICAL RECIPES IN C: THE ART OF SCIENTIFIC COMPUTING.
Cambridge University Press. 1992. pp.278-279.
[2] Park, S.K., and Miller, K.W. 1988, Communications of the ACM,
vol. 31, pp. 1192–1201.
*/
#define IA 16807
#define IM 2147483647
#define AM (1.0/IM)
#define IQ 127773
#define IR 2836
#define MASK 123459876
typedef struct __SpaceState { /* 狀態空間變量 */
int xt; /* x座標位置 */
int yt; /* x座標位置 */
float v_xt; /* x方向運動速度 */
float v_yt; /* y方向運動速度 */
int Hxt; /* x方向半窗寬 */
int Hyt; /* y方向半窗寬 */
float at_dot; /* 尺度變換速度 */
} SPACESTATE;
bool pause=false;//是否暫停
bool track = false;//是否跟蹤
IplImage *curframe=NULL;
IplImage *pBackImg=NULL;
IplImage *pFrontImg=NULL;
IplImage *pTrackImg =NULL;
unsigned char * img;//把iplimg改到char* 便於計算
int xin,yin;//跟蹤時輸入的中心點
int xout,yout;//跟蹤時得到的輸出中心點
int Wid,Hei;//圖像的大小
int WidIn,HeiIn;//輸入的半寬與半高
int WidOut,HeiOut;//輸出的半寬與半高
long ran_seed = 802163120; /* 隨機數種子，爲全局變量，設置缺省值 */
float DELTA_T = (float)0.05; /* 幀頻，可以爲30，25，15，10等 */
int POSITION_DISTURB = 15; /* 位置擾動幅度 */
float VELOCITY_DISTURB = 40.0; /* 速度擾動幅值 */
float SCALE_DISTURB = 0.0; /* 窗寬高擾動幅度 */
float SCALE_CHANGE_D = (float)0.001; /* 尺度變換速度擾動幅度 */
int NParticle = 75; /* 粒子個數 */
float * ModelHist = NULL; /* 模型直方圖 */
SPACESTATE * states = NULL; /* 狀態數組 */
float * weights = NULL; /* 每個粒子的權重 */
int nbin; /* 直方圖條數 */
float Pi_Thres = (float)0.90; /* 權重閾值 */
float Weight_Thres = (float)0.0001; /* 最大權重閾值，用來判斷是否目標丟失 */
/*
設置種子數
一般利用系統時間來進行設置，也可以直接傳入一個long型整數
*/
long set_seed( long setvalue )
{
if ( setvalue != 0 ) /* 如果傳入的參數setvalue!=0，設置該數爲種子 */
ran_seed = setvalue;
else /* 否則，利用系統時間爲種子數 */
{
ran_seed = time(NULL);
}
return( ran_seed );
}
/*
計算一幅圖像中某個區域的彩色直方圖分佈
輸入參數：
int x0, y0： 指定圖像區域的中心點
int Wx, Hy： 指定圖像區域的半寬和半高
unsigned char * image：圖像數據，按從左至右，從上至下的順序掃描，
顏色排列次序：RGB, RGB, ...
(或者：YUV, YUV, ...)
int W, H： 圖像的寬和高
輸出參數：
float * ColorHist： 彩色直方圖，顏色索引按：
i = r * G_BIN * B_BIN + g * B_BIN + b排列
int bins： 彩色直方圖的條數R_BIN*G_BIN*B_BIN（這裏取8x8x8=512）
*/
void CalcuColorHistogram( int x0, int y0, int Wx, int Hy,
unsigned char * image, int W, int H,
float * ColorHist, int bins )
{
int x_begin, y_begin; /* 指定圖像區域的左上角座標 */
int y_end, x_end;
int x, y, i, index;
int r, g, b;
float k, r2, f;
int a2;
for ( i = 0; i < bins; i++ ) /* 直方圖各個值賦0 */
ColorHist[i] = 0.0;
/* 考慮特殊情況：x0, y0在圖像外面，或者，Wx<=0, Hy<=0 */
/* 此時強制令彩色直方圖爲0 */
if ( ( x0 < 0 ) || (x0 >= W) || ( y0 < 0 ) || ( y0 >= H )
|| ( Wx <= 0 ) || ( Hy <= 0 ) ) return;
x_begin = x0 - Wx; /* 計算實際高寬和區域起始點 */
y_begin = y0 - Hy;
if ( x_begin < 0 ) x_begin = 0;
if ( y_begin < 0 ) y_begin = 0;
x_end = x0 + Wx;
y_end = y0 + Hy;
if ( x_end >= W ) x_end = W-1;
if ( y_end >= H ) y_end = H-1;
a2 = Wx*Wx+Hy*Hy; /* 計算核函數半徑平方a^2 */
f = 0.0; /* 歸一化係數 */
for ( y = y_begin; y <= y_end; y++ )
for ( x = x_begin; x <= x_end; x++ )
{
r = image[(y*W+x)*3] >> R_SHIFT; /* 計算直方圖 */
g = image[(y*W+x)*3+1] >> G_SHIFT; /*移位位數根據R、G、B條數 */
b = image[(y*W+x)*3+2] >> B_SHIFT;
index = r * G_BIN * B_BIN + g * B_BIN + b;
r2 = (float)(((y-y0)*(y-y0)+(x-x0)*(x-x0))*1.0/a2); /* 計算半徑平方r^2 */
k = 1 - r2; /* 核函數k(r) = 1-r^2, |r| < 1; 其他值 k(r) = 0 */
f = f + k;
ColorHist[index] = ColorHist[index] + k; /* 計算核密度加權彩色直方圖 */
}
for ( i = 0; i < bins; i++ ) /* 歸一化直方圖 */
ColorHist[i] = ColorHist[i]/f;
return;
}
/*
計算Bhattacharyya係數
輸入參數：
float * p, * q： 兩個彩色直方圖密度估計
int bins： 直方圖條數
返回值：
Bhattacharyya係數
*/
float CalcuBhattacharyya( float * p, float * q, int bins )
{
int i;
float rho;
rho = 0.0;
for ( i = 0; i < bins; i++ )
rho = (float)(rho + sqrt( p[i]*q[i] ));
return( rho );
}
/*# define RECIP_SIGMA 3.98942280401 / * 1/(sqrt(2*pi)*sigma), 這裏sigma = 0.1 * /*/
# define SIGMA2 0.02 /* 2*sigma^2, 這裏sigma = 0.1 */
float CalcuWeightedPi( float rho )
{
float pi_n, d2;
d2 = 1 - rho;
//pi_n = (float)(RECIP_SIGMA * exp( - d2/SIGMA2 ));
pi_n = (float)(exp( - d2/SIGMA2 ));
return( pi_n );
}
/*
採用Park and Miller方法產生[0,1]之間均勻分佈的僞隨機數
算法詳細描述見：
[1] NUMERICAL RECIPES IN C: THE ART OF SCIENTIFIC COMPUTING.
Cambridge University Press. 1992. pp.278-279.
[2] Park, S.K., and Miller, K.W. 1988, Communications of the ACM,
vol. 31, pp. 1192–1201.
*/
float ran0(long *idum)
{
long k;
float ans;
/* *idum ^= MASK;*/ /* XORing with MASK allows use of zero and other */
k=(*idum)/IQ; /* simple bit patterns for idum. */
*idum=IA*(*idum-k*IQ)-IR*k; /* Compute idum=(IA*idum) % IM without over- */
if (*idum < 0) *idum += IM; /* flows by Schrage’s method. */
ans=AM*(*idum); /* Convert idum to a floating result. */
/* *idum ^= MASK;*/ /* Unmask before return. */
return ans;
}
/*
獲得一個[0,1]之間均勻分佈的隨機數
*/
float rand0_1()
{
return( ran0( &ran_seed ) );
}
/*
獲得一個x - N(u,sigma)Gaussian分佈的隨機數
*/
float randGaussian( float u, float sigma )
{
float x1, x2, v1, v2;
float s = 100.0;
float y;
/*
使用篩選法產生正態分佈N(0,1)的隨機數(Box-Mulles方法)
1. 產生[0,1]上均勻隨機變量X1,X2
2. 計算V1=2*X1-1,V2=2*X2-1,s=V1^2+V2^2
3. 若s<=1,轉向步驟4，否則轉1
4. 計算A=(-2ln(s)/s)^(1/2),y1=V1*A, y2=V2*A
y1,y2爲N(0,1)隨機變量
*/
while ( s > 1.0 )
{
x1 = rand0_1();
x2 = rand0_1();
v1 = 2 * x1 - 1;
v2 = 2 * x2 - 1;
s = v1*v1 + v2*v2;
}
y = (float)(sqrt( -2.0 * log(s)/s ) * v1);
/*
根據公式
z = sigma * y + u
將y變量轉換成N(u,sigma)分佈
*/
return( sigma * y + u );
}
/*
初始化系統
int x0, y0： 初始給定的圖像目標區域座標
int Wx, Hy： 目標的半寬高
unsigned char * img：圖像數據，RGB形式
int W, H： 圖像寬高
*/
int Initialize( int x0, int y0, int Wx, int Hy,
unsigned char * img, int W, int H )
{
int i, j;
float rn[7];
set_seed( 0 ); /* 使用系統時鐘作爲種子，這個函數在 */
/* 系統初始化時候要調用一次,且僅調用1次 */
//NParticle = 75; /* 採樣粒子個數 */
//Pi_Thres = (float)0.90; /* 設置權重閾值 */
states = new SPACESTATE [NParticle]; /* 申請狀態數組的空間 */
if ( states == NULL ) return( -2 );
weights = new float [NParticle]; /* 申請粒子權重數組的空間 */
if ( weights == NULL ) return( -3 );
nbin = R_BIN * G_BIN * B_BIN; /* 確定直方圖條數 */
ModelHist = new float [nbin]; /* 申請直方圖內存 */
if ( ModelHist == NULL ) return( -1 );
/* 計算目標模板直方圖 */
CalcuColorHistogram( x0, y0, Wx, Hy, img, W, H, ModelHist, nbin );
/* 初始化粒子狀態(以(x0,y0,1,1,Wx,Hy,0.1)爲中心呈N(0,0.4)正態分佈) */
states[0].xt = x0;
states[0].yt = y0;
states[0].v_xt = (float)0.0; // 1.0
states[0].v_yt = (float)0.0; // 1.0
states[0].Hxt = Wx;
states[0].Hyt = Hy;
states[0].at_dot = (float)0.0; // 0.1
weights[0] = (float)(1.0/NParticle); /* 0.9; */
for ( i = 1; i < NParticle; i++ )
{
for ( j = 0; j < 7; j++ ) rn[j] = randGaussian( 0, (float)0.6 ); /* 產生7個隨機高斯分佈的數 */
states[i].xt = (int)( states[0].xt + rn[0] * Wx );
states[i].yt = (int)( states[0].yt + rn[1] * Hy );
states[i].v_xt = (float)( states[0].v_xt + rn[2] * VELOCITY_DISTURB );
states[i].v_yt = (float)( states[0].v_yt + rn[3] * VELOCITY_DISTURB );
states[i].Hxt = (int)( states[0].Hxt + rn[4] * SCALE_DISTURB );
states[i].Hyt = (int)( states[0].Hyt + rn[5] * SCALE_DISTURB );
states[i].at_dot = (float)( states[0].at_dot + rn[6] * SCALE_CHANGE_D );
/* 權重統一爲1/N，讓每個粒子有相等的機會 */
weights[i] = (float)(1.0/NParticle);
}
return( 1 );
}
/*
計算歸一化累計概率c'_i
輸入參數：
float * weight： 爲一個有N個權重（概率）的數組
int N： 數組元素個數
輸出參數：
float * cumulateWeight： 爲一個有N+1個累計權重的數組，
cumulateWeight[0] = 0;
*/
void NormalizeCumulatedWeight( float * weight, float * cumulateWeight, int N )
{
int i;
for ( i = 0; i < N+1; i++ )
cumulateWeight[i] = 0;
for ( i = 0; i < N; i++ )
cumulateWeight[i+1] = cumulateWeight[i] + weight[i];
for ( i = 0; i < N+1; i++ )
cumulateWeight[i] = cumulateWeight[i]/ cumulateWeight[N];
return;
}
/*
折半查找，在數組NCumuWeight[N]中尋找一個最小的j，使得
NCumuWeight[j] <=v
float v： 一個給定的隨機數
float * NCumuWeight： 權重數組
int N： 數組維數
返回值：
數組下標序號
*/
int BinearySearch( float v, float * NCumuWeight, int N )
{
int l, r, m;
l = 0; r = N-1; /* extreme left and extreme right components' indexes */
while ( r >= l)
{
m = (l+r)/2;
if ( v >= NCumuWeight[m] && v < NCumuWeight[m+1] ) return( m );
if ( v < NCumuWeight[m] ) r = m - 1;
else l = m + 1;
}
return( 0 );
}
/*
重新進行重要性採樣
輸入參數：
float * c： 對應樣本權重數組pi(n)
int N： 權重數組、重採樣索引數組元素個數
輸出參數：
int * ResampleIndex：重採樣索引數組
*/
void ImportanceSampling( float * c, int * ResampleIndex, int N )
{
float rnum, * cumulateWeight;
int i, j;
cumulateWeight = new float [N+1]; /* 申請累計權重數組內存，大小爲N+1 */
NormalizeCumulatedWeight( c, cumulateWeight, N ); /* 計算累計權重 */
for ( i = 0; i < N; i++ )
{
rnum = rand0_1(); /* 隨機產生一個[0,1]間均勻分佈的數 */
j = BinearySearch( rnum, cumulateWeight, N+1 ); /* 搜索<=rnum的最小索引j */
if ( j == N ) j--;
ResampleIndex[i] = j; /* 放入重採樣索引數組 */
}
delete cumulateWeight;
return;
}
/*
樣本選擇，從N個輸入樣本中根據權重重新挑選出N個
輸入參數：
SPACESTATE * state： 原始樣本集合（共N個）
float * weight： N個原始樣本對應的權重
int N： 樣本個數
輸出參數：
SPACESTATE * state： 更新過的樣本集
*/
void ReSelect( SPACESTATE * state, float * weight, int N )
{
SPACESTATE * tmpState;
int i, * rsIdx;
tmpState = new SPACESTATE[N];
rsIdx = new int[N];
ImportanceSampling( weight, rsIdx, N ); /* 根據權重重新採樣 */
for ( i = 0; i < N; i++ )
tmpState[i] = state[rsIdx[i]];//temState爲臨時變量,其中state[i]用state[rsIdx[i]]來代替
for ( i = 0; i < N; i++ )
state[i] = tmpState[i];
delete[] tmpState;
delete[] rsIdx;
return;
}
/*
傳播：根據系統狀態方程求取狀態預測量
狀態方程爲： S(t) = A S(t-1) + W(t-1)
W(t-1)爲高斯噪聲
輸入參數：
SPACESTATE * state： 待求的狀態量數組
int N： 待求狀態個數
輸出參數：
SPACESTATE * state： 更新後的預測狀態量數組
*/
void Propagate( SPACESTATE * state, int N)
{
int i;
int j;
float rn[7];
/* 對每一個狀態向量state[i](共N個)進行更新 */
for ( i = 0; i < N; i++ ) /* 加入均值爲0的隨機高斯噪聲 */
{
for ( j = 0; j < 7; j++ ) rn[j] = randGaussian( 0, (float)0.6 ); /* 產生7個隨機高斯分佈的數 */
state[i].xt = (int)(state[i].xt + state[i].v_xt * DELTA_T + rn[0] * state[i].Hxt + 0.5);
state[i].yt = (int)(state[i].yt + state[i].v_yt * DELTA_T + rn[1] * state[i].Hyt + 0.5);
state[i].v_xt = (float)(state[i].v_xt + rn[2] * VELOCITY_DISTURB);
state[i].v_yt = (float)(state[i].v_yt + rn[3] * VELOCITY_DISTURB);
state[i].Hxt = (int)(state[i].Hxt+state[i].Hxt*state[i].at_dot + rn[4] * SCALE_DISTURB + 0.5);
state[i].Hyt = (int)(state[i].Hyt+state[i].Hyt*state[i].at_dot + rn[5] * SCALE_DISTURB + 0.5);
state[i].at_dot = (float)(state[i].at_dot + rn[6] * SCALE_CHANGE_D);
cvCircle(pTrackImg,cvPoint(state[i].xt,state[i].yt),3, CV_RGB(0,255,0),-1);
}
return;
}
/*
觀測，根據狀態集合St中的每一個採樣，觀測直方圖，然後
更新估計量，獲得新的權重概率
輸入參數：
SPACESTATE * state： 狀態量數組
int N： 狀態量數組維數
unsigned char * image： 圖像數據，按從左至右，從上至下的順序掃描，
顏色排列次序：RGB, RGB, ...
int W, H： 圖像的寬和高
float * ObjectHist： 目標直方圖
int hbins： 目標直方圖條數
輸出參數：
float * weight： 更新後的權重
*/
void Observe( SPACESTATE * state, float * weight, int N,
unsigned char * image, int W, int H,
float * ObjectHist, int hbins )
{
int i;
float * ColorHist;
float rho;
ColorHist = new float[hbins];
for ( i = 0; i < N; i++ )
{
/* (1) 計算彩色直方圖分佈 */
CalcuColorHistogram( state[i].xt, state[i].yt,state[i].Hxt, state[i].Hyt,
image, W, H, ColorHist, hbins );
/* (2) Bhattacharyya係數 */
rho = CalcuBhattacharyya( ColorHist, ObjectHist, hbins );
/* (3) 根據計算得的Bhattacharyya係數計算各個權重值 */
weight[i] = CalcuWeightedPi( rho );
}
delete ColorHist;
return;
}
/*
估計，根據權重，估計一個狀態量作爲跟蹤輸出
輸入參數：
SPACESTATE * state： 狀態量數組
float * weight： 對應權重
int N： 狀態量數組維數
輸出參數：
SPACESTATE * EstState： 估計出的狀態量
*/
void Estimation( SPACESTATE * state, float * weight, int N,
SPACESTATE & EstState )
{
int i;
float at_dot, Hxt, Hyt, v_xt, v_yt, xt, yt;
float weight_sum;
at_dot = 0;
Hxt = 0; Hyt = 0;
v_xt = 0; v_yt = 0;
xt = 0; yt = 0;
weight_sum = 0;
for ( i = 0; i < N; i++ ) /* 求和 */
{
at_dot += state[i].at_dot * weight[i];
Hxt += state[i].Hxt * weight[i];
Hyt += state[i].Hyt * weight[i];
v_xt += state[i].v_xt * weight[i];
v_yt += state[i].v_yt * weight[i];
xt += state[i].xt * weight[i];
yt += state[i].yt * weight[i];
weight_sum += weight[i];
}
/* 求平均 */
if ( weight_sum <= 0 ) weight_sum = 1; /* 防止被0除，一般不會發生 */
EstState.at_dot = at_dot/weight_sum;
EstState.Hxt = (int)(Hxt/weight_sum + 0.5 );
EstState.Hyt = (int)(Hyt/weight_sum + 0.5 );
EstState.v_xt = v_xt/weight_sum;
EstState.v_yt = v_yt/weight_sum;
EstState.xt = (int)(xt/weight_sum + 0.5 );
EstState.yt = (int)(yt/weight_sum + 0.5 );
return;
}
/************************************************************
模型更新
輸入參數：
SPACESTATE EstState： 狀態量的估計值
float * TargetHist： 目標直方圖
int bins： 直方圖條數
float PiT： 閾值（權重閾值）
unsigned char * img： 圖像數據，RGB形式
int W, H： 圖像寬高
輸出：
float * TargetHist： 更新的目標直方圖
************************************************************/
# define ALPHA_COEFFICIENT 0.2 /* 目標模型更新權重取0.1-0.3 */
int ModelUpdate( SPACESTATE EstState, float * TargetHist, int bins, float PiT,
unsigned char * img, int W, int H )
{
float * EstHist, Bha, Pi_E;
int i, rvalue = -1;
EstHist = new float [bins];
/* (1)在估計值處計算目標直方圖 */
CalcuColorHistogram( EstState.xt, EstState.yt, EstState.Hxt,
EstState.Hyt, img, W, H, EstHist, bins );
/* (2)計算Bhattacharyya係數 */
Bha = CalcuBhattacharyya( EstHist, TargetHist, bins );
/* (3)計算概率權重 */
Pi_E = CalcuWeightedPi( Bha );
if ( Pi_E > PiT )
{
for ( i = 0; i < bins; i++ )
{
TargetHist[i] = (float)((1.0 - ALPHA_COEFFICIENT) * TargetHist[i]
+ ALPHA_COEFFICIENT * EstHist[i]);
}
rvalue = 1;
}
delete EstHist;
return( rvalue );
}
/*
系統清除
*/
void ClearAll()
{
if ( ModelHist != NULL ) delete [] ModelHist;
if ( states != NULL ) delete [] states;
if ( weights != NULL ) delete [] weights;
return;
}
/**********************************************************************
基於彩色直方圖的粒子濾波算法總流程
輸入參數：
unsigned char * img： 圖像數據，RGB形式
int W, H： 圖像寬高
輸出參數：
int &xc, &yc： 找到的圖像目標區域中心座標
int &Wx_h, &Hy_h： 找到的目標的半寬高
float &max_weight： 最大權重值
返回值：
成功1，否則-1
基於彩色直方圖的粒子濾波跟蹤算法的完整使用方法爲：
（1）讀取彩色視頻中的1幀，並確定初始區域，以此獲得該區域的中心點、
目標的半高、寬，和圖像數組（RGB形式）、圖像高寬參數。
採用初始化函數進行初始化
int Initialize( int x0, int y0, int Wx, int Hy,
unsigned char * img, int W, int H )
（2）循環調用下面函數，直到N幀圖像結束
int ColorParticleTracking( unsigned char * image, int W, int H,
int & xc, int & yc, int & Wx_h, int & Hy_h )
每次調用的輸出爲：目標中心座標和目標的半高寬
如果函數返回值<0，則表明目標丟失。
（3）清除系統各個變量，結束跟蹤
void ClearAll()
**********************************************************************/
int ColorParticleTracking( unsigned char * image, int W, int H,
int & xc, int & yc, int & Wx_h, int & Hy_h,
float & max_weight)
{
SPACESTATE EState;
int i;
/* 選擇：選擇樣本，並進行重採樣 */
ReSelect( states, weights, NParticle );
/* 傳播：採樣狀態方程，對狀態變量進行預測 */
Propagate( states, NParticle);
/* 觀測：對狀態量進行更新 */
Observe( states, weights, NParticle, image, W, H,
ModelHist, nbin );
/* 估計：對狀態量進行估計，提取位置量 */
Estimation( states, weights, NParticle, EState );
xc = EState.xt;
yc = EState.yt;
Wx_h = EState.Hxt;
Hy_h = EState.Hyt;
/* 模型更新 */
ModelUpdate( EState, ModelHist, nbin, Pi_Thres, image, W, H );
/* 計算最大權重值 */
max_weight = weights[0];
for ( i = 1; i < NParticle; i++ )
max_weight = max_weight < weights[i] ? weights[i] : max_weight;
/* 進行合法性檢驗，不合法返回-1 */
if ( xc < 0 || yc < 0 || xc >= W || yc >= H ||
Wx_h <= 0 || Hy_h <= 0 ) return( -1 );
else
return( 1 );
}
//把iplimage 轉到img 數組中,BGR->RGB
void IplToImge(IplImage* src, int w,int h)
{
int i,j;
for ( j = 0; j < h; j++ ) // 轉成正向圖像
for ( i = 0; i < w; i++ )
{
img[ ( j*w+i )*3 ] = R(src,i,j);
img[ ( j*w+i )*3+1 ] = G(src,i,j);
img[ ( j*w+i )*3+2 ] = B(src,i,j);
}
}
void mouseHandler(int event, int x, int y, int flags, void* param)//在這裏要注意到要再次調用cvShowImage，才能顯示方框
{
CvMemStorage* storage = cvCreateMemStorage(0);
CvSeq * contours;
IplImage* pFrontImg1 = 0;
int centerX,centerY;
int delt = 10;
pFrontImg1=cvCloneImage(pFrontImg);//這裏也要注意到如果在 cvShowImage("foreground",pFrontImg1)中用pFrontImg產效果，得重新定義並複製
switch(event){
case CV_EVENT_LBUTTONDOWN:
//printf("laskjfkoasfl\n");
//尋找輪廓
if(pause)
{
cvFindContours(pFrontImg,storage,&contours,sizeof(CvContour),CV_RETR_EXTERNAL,
CV_CHAIN_APPROX_SIMPLE);
//在原場景中繪製目標輪廓的外接矩形
for (;contours;contours = contours->h_next)
{
CvRect r = ((CvContour*)contours)->rect;
if(x>r.x&&x<(r.x+r.width)&&y>r.y&&r.y<(r.y+r.height))
{
if (r.height*r.width>CONTOUR_MIN_AREA && r.height*r.width<CONTOUR_MAX_AREA)
{
centerX = r.x+r.width/2;//得到目標中心點
centerY = r.y+r.height/2;
WidIn = r.width/2;//得到目標半寬與半高
HeiIn = r.height/2;
xin = centerX;
yin = centerY;
cvRectangle(pFrontImg1,cvPoint(r.x,r.y),cvPoint(r.x+r.width,r.y+r.height),cvScalar(255,255,255),2,8,0);
//Initial_MeanShift_tracker(centerX,centerY,WidIn,HeiIn,img,Wid,Hei,1./delt); //初始化跟蹤變量
/* 初始化跟蹤器 */
Initialize( centerX, centerY, WidIn, HeiIn, img, Wid, Hei );
track = true;//進行跟蹤
cvShowImage("foreground",pFrontImg1);
return;
}
}
}
}
break;
case CV_EVENT_LBUTTONUP:
printf("Left button up\n");
break;
}
}
//void on_mouse(int event, int x, int y, int flags, void *param)
//{
// if(!image)
// return ;
// if(image->origin)
// {
// image->origin = 0;
// y = image->height - y;
// }
// if(selecting) //正在選擇物體
// {
// selection.x = MIN(x,origin.x);
// selection.y = MIN(y,origin.y);
// selection.width = selection.x + CV_IABS(x - origin.x);
// selection.height = selection.y + CV_IABS(y - origin.y);
//
// selection.x = MAX(selection.x ,0);
// selection.y = MAX(selection.y,0);
// selection.width = MIN(selection.width,image->width);
// selection.height = MIN(selection.height,image->height);
// selection.width -= selection.x;
// selection.height -= selection.y;
// }
// switch(event)
// {
// case CV_EVENT_LBUTTONDOWN:
// origin = cvPoint(x,y);
// selection = cvRect(x,y,0,0);
// selecting = 1;
// break;
// case CV_EVENT_LBUTTONUP:
// selecting = 0;
// if(selection.width >0 && selection.height >0)
// selected = 1;
// break;
// }
//}
void main()
{
int FrameNum=0; //幀號
int k=0;
CvCapture *capture = cvCreateFileCapture("test.avi");
char res1[20],res2[20];
//CvCapture *capture = cvCreateFileCapture("test1.avi");
//CvCapture *capture = cvCreateFileCapture("camera1_mov.avi");
IplImage* frame[Num]; //用來存放圖像
int i,j;
uchar key = false; //用來設置暫停
float rho_v;//表示相似度
float max_weight;
int sum=0; //用來存放兩圖像幀差後的值
for (i=0;i<Num;i++)
{
frame[i]=NULL;
}
IplImage *curFrameGray=NULL;
IplImage *frameGray=NULL;
CvMat *Mat_D,*Mat_F; //動態矩陣與幀差後矩陣
int row ,col;
cvNamedWindow("video",1);
cvNamedWindow("background",1);
cvNamedWindow("foreground",1);
cvNamedWindow("tracking",1);
cvSetMouseCallback("tracking",mouseHandler,0);//響應鼠標
while (capture)
{
curframe=cvQueryFrame(capture); //抓取一幀
if(FrameNum<Num)
{
if(FrameNum==0)//第一幀時初始化過程
{
curFrameGray=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);
frameGray=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);
pBackImg=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);
pFrontImg=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);
pTrackImg = cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,3);
cvSetZero(pFrontImg);
cvCvtColor(curframe,pBackImg,CV_RGB2GRAY);
row=curframe->height;
col=curframe->width;
Mat_D=cvCreateMat(row,col,CV_32FC1);
cvSetZero(Mat_D);
Mat_F=cvCreateMat(row,col,CV_32FC1);
cvSetZero(Mat_F);
Wid = curframe->width;
Hei = curframe->height;
img = new unsigned char [Wid * Hei * 3];
}
frame[k]=cvCloneImage(curframe); //把前num幀存入到圖像數組
pTrackImg = cvCloneImage(curframe);
}
else
{
k=FrameNum%Num;
pTrackImg = cvCloneImage(curframe);
IplToImge(curframe,Wid,Hei);
cvCvtColor(curframe,curFrameGray,CV_RGB2GRAY);
cvCvtColor(frame[k],frameGray,CV_RGB2GRAY);
for(i=0;i<curframe->height;i++)
for(j=0;j<curframe->width;j++)
{
sum=S(curFrameGray,j,i)-S(frameGray,j,i);
sum=sum<0 ? -sum : sum;
if(sum>T) //文獻中公式（1）
{
CV_MAT_ELEM(*Mat_F,float,i,j)=1;
}
else
{
CV_MAT_ELEM(*Mat_F,float,i,j)=0;
}
if(CV_MAT_ELEM(*Mat_F,float,i,j)!=0)//文獻中公式（2）
CV_MAT_ELEM(*Mat_D,float,i,j)=Re;
else{
if(CV_MAT_ELEM(*Mat_D,float,i,j)!=0)
CV_MAT_ELEM(*Mat_D,float,i,j)=CV_MAT_ELEM(*Mat_D,float,i,j)-1;
}
if(CV_MAT_ELEM(*Mat_D,float,i,j)==0.0)
{
//文獻中公式（3）
S(pBackImg,j,i)=(uchar)((1-ai)*S(pBackImg,j,i)+ai*S(curFrameGray,j,i));
}
sum=S(curFrameGray,j,i)-S(pBackImg,j,i);//背景差分法
sum=sum<0 ? -sum : sum;
if(sum>40)
{
S(pFrontImg,j,i)=255;
}
else
S(pFrontImg,j,i)=0;
}
frame[k]=cvCloneImage(curframe);
}
FrameNum++;
k++;
cout<<FrameNum<<endl;
//進行形態學濾波，去噪
cvDilate(pFrontImg, pFrontImg, 0, 2);
cvErode(pFrontImg, pFrontImg, 0, 3);
cvDilate(pFrontImg, pFrontImg, 0, 1);
if(track)
{
/* 跟蹤一幀 */
rho_v = ColorParticleTracking( img, Wid, Hei, xout, yout, WidOut, HeiOut, max_weight);
/* 畫框: 新位置爲藍框 */
if ( rho_v > 0 && max_weight > 0.0001 ) /* 判斷是否目標丟失 */
{
cvRectangle(pFrontImg,cvPoint(xout - WidOut,yout - HeiOut),cvPoint(xout+WidOut,yout+HeiOut),cvScalar(255,255,255),2,8,0);
cvRectangle(pTrackImg,cvPoint(xout - WidOut,yout - HeiOut),cvPoint(xout+WidOut,yout+HeiOut),cvScalar(255,255,255),2,8,0);
xin = xout; yin = yout;
WidIn = WidOut; HeiIn = HeiOut;
/*draw_rectangle( pBuffer, Width, Height, xo, Height-yo-1, wo, ho, 0x00ff0000, 2 );
xb = xo; yb = yo;
wb = wo; hb = ho;*/
}
}
cvShowImage("video",curframe);
cvShowImage("foreground",pFrontImg);
cvShowImage("background",pBackImg);
cvShowImage("tracking",pTrackImg);
/*sprintf(res1,"fore%d.jpg",FrameNum);
cvSaveImage(res1,pFrontImg);
sprintf(res2,"ground%d.jpg",FrameNum);
cvSaveImage(res2,pBackImg);*/
cvSetMouseCallback("foreground",mouseHandler,0);//響應鼠標
key = cvWaitKey(1);
if(key == 'p') pause = true;
while(pause)
if(cvWaitKey(0)=='p')
pause = false;
}
cvReleaseImage(&curFrameGray);
cvReleaseImage(&frameGray);
cvReleaseImage(&pBackImg);
cvReleaseImage(&pFrontImg);
cvDestroyAllWindows();
// Clear_MeanShift_tracker();
ClearAll();
}實驗結果：
自此，畢業論文涉及的經典算法已經全部給出，我自己提出的破算法就不獻醜了。
馬上去華爲上班咯，可能搞通信去了，破企業網部門，唉
如果週末有空的話，我還是會繼續搞圖像處理的，這次下了不少人臉美化、超分辨率修正的論文，得好好讀讀。
另外打個廣告，我畢業前自己弄得android app《色盲相機》，下載地址：
木螞蟻：http://www.mumayi.com/android-631836.html
360： http://zhushou.360.cn/detail/index/soft_id/1780912
網易： http://m.163.com/android/software/32jkam.html
核心思想來自斯坦福大學的課程設計及一個日本老頭公開的matlab代碼
有空大家給我點點廣告哈~~