// particlebase.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <ctype.h>
#include <time.h>
#include "cvAux.h" //粒子滤波所需的头文件
#include <cmath>
#include <iostream>
using namespace std;
IplImage *image = 0, *hsv = 0, *hue = 0, *mask = 0;
CvHistogram *hist = 0;
int select_object = 0; //
int track_object = 0; //
CvPoint origin;
CvRect selection; //the region of the object
CvRect track_window; //the region of tracking place
int left_x = 0;
int left_y = 0;
double xv = 0.0;
double yv = 0.0;
//on_mouse select the region .
//initialize the tracking place.
void on_mouse( int event, int x, int y, int flags, void *Non)
{
if(!image)
return;
if(image->origin)
{
y = image->height - y;
}
if(select_object)
{
selection.x = MIN(x, origin.x); //ensure the start point less than the end point
selection.y = MIN(y, origin.y);
selection.width = selection.x + CV_IABS(x - origin.x); //ensure the width valid
selection.height = selection.y + CV_IABS(y - origin.y);
selection.x = MAX(selection.x, 0); // start point must not be less than 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;
track_window = cvRect(selection.x,selection.y,selection.width,selection.height);
}
switch(event)
{
case CV_EVENT_LBUTTONDOWN:
origin = cvPoint(x, y);
selection = cvRect(x, y, 0, 0);
select_object = 1; //start to select object
break;
case CV_EVENT_LBUTTONUP:
select_object = 0; //select finished
left_x = selection.x;
left_y = selection.y;
if(selection.width > 0 && selection.height > 0)
track_object = -1; // if the region is valid ,then track the object
cout<<image->width<<" "<<image->height<<" "<<x<<" "<<y<<" "<<origin.x<<" "<<origin.y<<endl;
break;
}
}
/****************************************************/
//CalWeight
double CalWeight(int x, int y, IplImage *img, CvHistogram *object)
{
IplImage* hue1;
CvRect P_Region;
CvHistogram *P_Hist;
int histSize=180; //直方图针数
float range0[]={0,180}; //第0维数值变化范围
float * ranges[]={range0}; //第1维数值变化范围
P_Hist = cvCreateHist(1,&histSize,CV_HIST_ARRAY,ranges,1);//创建一个直方图
P_Region = cvRect(x,y, selection.width, selection.height);
cvSetImageROI(img, P_Region);
hue1 = cvCreateImage( cvGetSize( img ), 8, 1);
cvSplit(img, hue1, 0, 0, 0);
cvResetImageROI(img);
//计算粒子的直方图
cvCalcHist(&hue1,P_Hist, 0, NULL);
cvReleaseImage(&hue1);
//直方图的归一化
cvNormalizeHist(P_Hist, 1.0);
cvNormalizeHist(object, 1.0);
double ans = cvCompareHist(P_Hist, object,CV_COMP_BHATTACHARYYA);
// delete P_Hist;
return 1- ans;
// cout<<"ans = "<<ans<<endl;
// ans = (1/sqrt(2*3.1415926535897))*exp(-ans*ans);
// cout<<"weight = "<<ans<<endl;
// return ans;
}
int main(int argc, char** argv)
{
CvCapture* capture = 0;
IplImage* tmp;
IplImage* frame = 0;
//直方图部分
CvHistogram *ObjectHist;
int histSize=180; //直方图针数
float range0[]={0,180}; //第0维数值变化范围
float * ranges[]={range0}; //第1维数值变化范围
ObjectHist=cvCreateHist(1,&histSize,CV_HIST_ARRAY,ranges,1);//创建一个直方图
//构造粒子对象
int DP=2; // 维度是2
int MP=2; //
const int SamplesNum=100; // 100 状态向量(x,y)的次元抽样个数为200
CvConDensation* ConDens=cvCreateConDensation( DP, MP, SamplesNum );
//视频图像采集部分
/* capture = cvCreateFileCapture( "2.avi" );*/
if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );//打开摄像头
else if( argc == 2 )
capture = cvCaptureFromAVI( argv[1] );//打开AVI文件
if( !capture )
{
fprintf(stderr,"Could not initialize capturing...\n");//打开视频流失败处理
return -1;
}
//set a window for track
cvNamedWindow("Particle_tracking_object", 1);
//设置回调函数
cvSetMouseCallback("Particle_tracking_object", on_mouse);
for( ; ; )
{
frame = cvQueryFrame( capture );
if( !frame )
{
break;
}
if(! image)
{
image = cvCreateImage(cvGetSize(frame), 8, 3);
image->origin = frame->origin ;
hsv = cvCreateImage( cvGetSize(frame), 8, 3 );
// add more information here
}
cvCopy(frame, image, 0);
cvCvtColor( image, hsv, CV_BGR2HSV ); // convert RGB to HSV
if(track_object)
{
//初始化粒子部分
if(track_object<0)
{
cvSetImageROI( hsv, selection );//set the region of interest
tmp = cvCreateImage( cvGetSize( hsv ), 8, 3 );
hue = cvCreateImage( cvGetSize(hsv), 8, 1 );
cvCopy( hsv, tmp, NULL );
cvSplit(tmp, hue, 0, 0, 0);
//计算目标区域的直方图
cvCalcHist(&hue,ObjectHist,0,NULL);
cvResetImageROI( hsv );//free the ROI
//开始跟踪
track_object = 1;
//--------ConDens构造体的各抽样的初始化---------------------------
CvMat* lowerBound; //各次元的下限表示向量
CvMat* upperBound; // 各次元的上限表示向量
lowerBound = cvCreateMat(2, 1, CV_32F);
upperBound = cvCreateMat(2, 1, CV_32F);
double Low_x = track_window.x - 60;
double Upper_x = track_window.x + 60;
double Low_y = track_window.y - 60;
double Upper_y = track_window.y + 60;
if (Low_x<0)
{
Low_x = 0.0;
}
if (Low_y < 0)
{
Low_y = 0.0;
}
if (Upper_x+selection.width>=image->width)
{
Upper_x = image->width - selection.width-1;
}
if (Upper_y + selection.height >= image->height)
{
Upper_y = image->height - selection.height -1;
}
cvmSet( lowerBound, 0, 0, Low_x ); cvmSet( upperBound, 0, 0,Upper_x ); //cvmSet(M,i,j,2.0); // Set M(i,j)
cvmSet( lowerBound, 1, 0, Low_y); cvmSet( upperBound, 1, 0, Upper_y );
cvConDensInitSampleSet(ConDens, lowerBound, upperBound);
//-----------------------------------------------------------------------
//状态转移方程---------------------------
ConDens->DynamMatr[0]=1.0;ConDens->DynamMatr[1]=0.0;
ConDens->DynamMatr[2]=0.0;ConDens->DynamMatr[3]=1.0;
}
// double weight[SamplesNum] ;
double total_weight = 0.0;
double leftx = 0.0;
double lefty = 0.0;
int X, Y;
//计算权值
// double big = 0.0;
// int max_id;
for(int i=0; i < SamplesNum; i++)
{
X=(int)ConDens->flSamples[i][0];
Y=(int)ConDens->flSamples[i][1];
if (X<0)
{
X = 0;
}
if (X+selection.width>=image->width)
{
X = image->width -selection.width -1;
}
if (Y<0)
{
Y = 0;
}
if (Y+selection.height>=image->height)
{
Y = image->height -selection.height -1;
}
// weight[i] = CalWeight(X,Y,hsv, ObjectHist);
ConDens->flConfidence[i] = CalWeight(X,Y,hsv, ObjectHist);
/* if (weight[i]>big)
{
big = weight[i];
max_id = i;
}*/
// if (weight[i]>0.4)
// {
// ConDens->flConfidence[i]=1.0;
// }
// else
/* ConDens->flConfidence[i]=0.1;*/
total_weight += ConDens->flConfidence[i];
}
//权值归一化
for (int i=0; i < SamplesNum; i++)
{
// weight[i] = weight[i]/total_weight;
// cout<<weight[i]<<endl;
// leftx += ConDens->flSamples[i][0]*weight[i];
// lefty += ConDens->flSamples[i][1]*weight[i];
leftx += ConDens->flSamples[i][0]* ConDens->flConfidence[i]/total_weight;
lefty += ConDens->flSamples[i][1]* ConDens->flConfidence[i]/total_weight;
}
//估计当前的位置，更新
track_window.x = (int)(leftx);
track_window.y = (int)(lefty);
// track_window.x = (int)ConDens->flSamples[max_id][0];
// track_window.y = (int)ConDens->flSamples[max_id][1];
track_window.width = selection.width;
track_window.height = selection.height;
if(track_window.x >=image->width||track_window.y>=image->height)
{
track_window.x = image->width-1;
track_window.y =image->height-1;
}
if((track_window.x+track_window.width)>image->width ||(track_window.y+track_window.height)>image->height)
{
track_window.height = 0;
track_window.width = 0;
}
CvPoint pt1 = cvPoint(track_window.x, track_window.y);
CvPoint pt2 = cvPoint(track_window.x+track_window.width, track_window.y+track_window.height);
cvRectangle(image,pt1,pt2,cvScalar(0,0,255),3);
//定义均匀分布随机数
/* CvRNG *rng_state =NULL;
CvMat *locations = cvCreateMat(100, 1, CV_64FC1);
cvRandArr(rng_state, locations, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(1));
double particle_x[SamplesNum],particle_y[SamplesNum];
for (int i=0; i < SamplesNum; i++)
{
ConDens->flConfidence[i]=0.0;
cout<<cvGetReal2D(locations, 2, 0)<<endl;
double u = cvGetReal2D(locations, i, 0);
double wsum = 0.0;
for (int j=0; j<SamplesNum;j++)
{
wsum += weight[j];
if (wsum>=u)
{
particle_x[i] = ConDens->flSamples[j][0];
particle_y[i] = ConDens->flSamples[j][1];
// ConDens->flConfidence[j]=0.0;
}
}
}*/
/* for (int i=0; i < SamplesNum; i++)
{
if (weight[i]>0.4)
{
ConDens->flConfidence[i]=1.0;
}
else
ConDens->flConfidence[i]=0.0;
}*/
cvConDensUpdateByTime(ConDens);
/* for (int i=0; i < SamplesNum; i++)
{
ConDens->flSamples[i][0] = particle_x[i];
ConDens->flSamples[i][1] = particle_y[i];
}*/
}
int c;
cvShowImage("Particle_tracking_object",image);
c = cvWaitKey(100);
if(c == 27)
break;
}
delete ObjectHist;
cvReleaseCapture( &capture );
cvDestroyWindow("Particle_tracking_object");
return 0;
}
