---------------------------------------------------------------- by Markala
opencv2.3以后,condensation算法放在legacy中了,也就是说要引入下面文件,
#include
condensation算法的函数原型在” …\OpenCV\sources\modules\legacy\src”中,可以直接搜索condens.cpp。
整个Condensation算法只涉及5个变量和函数:CvConDensation、cvCreateConDensation、cvConDensInitSampleSet 、cvConDensUpdateByTime、cvReleaseConDensation。下面一一介绍
CvConDensation结构体
typedef struct CvConDensation
{
int MP; /* 测量向量维数 */
int DP; /* 状态向量维数 */
float* DynamMatr; /* 转移矩阵 */
float* State; /* Vector of State */
int SamplesNum; /* 粒子数 */
float** flSamples; /* 表示粒子的向量 */
float** flNewSamples; /* temporary array of the Sample Vectors */
float* flConfidence; /* 每个粒子的权重 */
float* flCumulative; /* Cumulative confidence */
float* Temp; /* Temporary vector */
float* RandomSample; /* RandomVector to update sample set */
struct CvRandState* RandS; /* Array of structures to generate random vectors */
} CvConDensation;
这个是粒子结构体,最重要的参数是MP、DP、SamplesNum、flConfidence、flSamples、DynamMatr
ConDens->flConfidence[i] 表示第i个粒子的权重
ConDens->flSamples[i][k] 表示第i个粒子的状态向量的第k维的值
cvCreateConDensation( int DP, int MP, int SamplesNum );
这个函数是创建粒子结构体,只需要定义DP、MP和SameplesNum。
cvConDensInitSampleSet(CvConDensation* condens,
CvMat* lower_bound, //粒子初始化时,取值下界
CvMat* upper_bound ); //粒子初始化时,取值上界
用这个函数对粒子结构体中的其他参数进行初始化,根据粒子滤波的相关知识知道,初始化时将产生一个(lower_bound,upper_bound)范围内均匀分布的点集。
假设MP=DP=2,也就是说用2×1的向量来表示粒子的状态,那么我们可以这么初始化
float minRange[] = { xmin, ymin };
float maxRange[] = { xmax, ymax };
CvMat LB, UB;
cvInitMatHeader(&LB, 2, 1, CV_32FC1, minRange);
cvInitMatHeader(&UB, 2, 1, CV_32FC1, maxRange);
cvConDensInitSampleSet(condens, &LB, &UB);
或者这样
CvMat* lowerBound; // 下界
CvMat* upperBound; // 上界
lowerBound //创建2行1列的矩阵lowerBound和upperBound
upperBound = cvCreateMat(2, 1, CV_32F);
cvmSet( lowerBound, 0, 0, xmin ); cvmSet( upperBound, 0, 0, xmax );
cvmSet( lowerBound, 1, 0, ymin ); cvmSet( upperBound, 1, 0, ymax );
cvConDensInitSampleSet(ConDens, lowerBound, upperBound);
上面只是初始化了粒子结构体的相关参数,我们还需要对转移矩阵进行初始化,一般这么做
condens->DynamMatr[0] = 1.0; condens->DynamMatr[1] = 0.0;
condens->DynamMatr[2] = 0.0; condens->DynamMatr[3] = 1.0;
这是DP=MP=2的情形,一般会设置成一个对角阵,可根据实际情况调整。
cvConDensUpdateByTime(ConDens);
本函数用于更新粒子的状态,主要是权值。在进行更新前,需要自己定义权值的计算方式,也就是给ConDens->flConfidence[i]赋值。
cvReleaseConDensation( CvConDensation** condens );
本函数用于释放内存。
http://blog.csdn.net/pp5576155/article/details/6972824
1)定义DP、MP和SameplesNum,创建CvConDensation结构体
cvCreateConDensation( int DP, int MP, int SamplesNum );
2)初始化,主要是定义LB、UB和DynamMatr[]
float minRange[] = { xmin, ymin };
float maxRange[] = { xmax, ymax };
CvMat LB, UB;
cvInitMatHeader(&LB, 2, 1, CV_32FC1, minRange);
cvInitMatHeader(&UB, 2, 1, CV_32FC1, maxRange);
cvConDensInitSampleSet(condens, &LB, &UB);
condens->DynamMatr[0] = 1.0; condens->DynamMatr[1] = 0.0;
condens->DynamMatr[2] = 0.0; condens->DynamMatr[3] = 1.0;
3)定义权值计算方式,进行更新
cvConDensUpdateByTime(ConDens);
例1实现了在窗口上对鼠标进行检测与跟踪
http://answers.opencv.org/question/6985/syntax-for-particle-filter-in-opencv-243/
// Example of how to use the OpenCV Particle Filter.
//
// Stolen largely from morethantechnical.com's nice mouse_kalman project.
//
#include
#include
#include
#include
#include
#include
using namespace cv;
using namespace std;
#define drawCross( center, color, d ) \
line( img, Point( center.x - d, center.y - d ), \
Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
line( img, Point( center.x + d, center.y - d ), \
Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )
struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;
vector< Point> mouseV, particleV;
int counter = -1;
// Define this to proceed one click at a time.
//#define CLICK 1
#define PLOT_PARTICLES 1
void on_mouse(int event, int x, int y, int flags, void* param) {
#ifdef CLICK
if (event == CV_EVENT_LBUTTONUP)
#endif
{
last_mouse = mouse_info;
mouse_info.x = x;
mouse_info.y = y;
counter = 0;
}
}
int main (int argc, char * const argv[]) {
Mat img(650, 650, CV_8UC3);
char code = (char)-1;
namedWindow("mouse particle");
setMouseCallback("mouse particle", on_mouse, 0);
Mat_ measurement(2,1);
measurement.setTo( Scalar(0));
int dim = 2;
int nParticles = 25;
float xRange = 650.0;
float yRange = 650.0;
float minRange[] = { 0, 0 };
float maxRange[] = { xRange, yRange };
CvMat LB, UB;
cvInitMatHeader(&LB, 2, 1, CV_32FC1, minRange);
cvInitMatHeader(&UB, 2, 1, CV_32FC1, maxRange);
CvConDensation* condens = cvCreateConDensation(dim, dim, nParticles);
cvConDensInitSampleSet(condens, &LB, &UB);
// The OpenCV documentation doesn't tell you to initialize this
// transition matrix, but you have to do it. For this 2D example,
// we're just using a 2x2 identity matrix. I'm sure there's a slicker
// way to do this, left as an exercise for the reader.
condens->DynamMatr[0] = 1.0;
condens->DynamMatr[1] = 0.0;
condens->DynamMatr[2] = 0.0;
condens->DynamMatr[3] = 1.0;
for(;;)
{
if (mouse_info.x < 0 || mouse_info.y < 0)
{
imshow("mouse particle", img);
waitKey(30);
continue;
}
mouseV.clear();
particleV.clear();
for(;;)
{
code = (char) waitKey(100);
if( code > 0 )
break;
#ifdef CLICK
if (counter++ > 0) {
continue;
}
#endif
measurement(0) = mouse_info.x;
measurement(1) = mouse_info.y;
Point measPt(measurement(0),measurement(1));
mouseV.push_back(measPt);
// Clear screen
img = Scalar::all(100);
for (int i = 0; i < condens->SamplesNum; i++) {
float diffX = (measurement(0) - condens->flSamples[i][0])/xRange;
float diffY = (measurement(1) - condens->flSamples[i][1])/yRange;
condens->flConfidence[i] = 1.0 / (sqrt(diffX * diffX + diffY * diffY));
// plot particles
#ifdef PLOT_PARTICLES
Point partPt(condens->flSamples[i][0], condens->flSamples[i][1]);
drawCross(partPt , Scalar(255,0,255), 2);
#endif
}
cvConDensUpdateByTime(condens);
Point statePt(condens->State[0], condens->State[1]);
particleV.push_back(statePt);
// plot points
drawCross( statePt, Scalar(255,255,255), 5 );
drawCross( measPt, Scalar(0,0,255), 5 );
for (int i = 0; i < mouseV.size() - 1; i++) {
line(img, mouseV[i], mouseV[i+1], Scalar(255,255,0), 1);
}
for (int i = 0; i < particleV.size() - 1; i++) {
line(img, particleV[i], particleV[i+1], Scalar(0,255,0), 1);
}
imshow( "mouse particle", img );
}
if( code == 27 || code == 'q' || code == 'Q' )
break;
}
return 0;
}
效果图
http://blog.chinaunix.net/uid-25906157-id-3246199.html
// Condensation_demo.cpp : 定义控制台应用程序的入口点。
#ifdef _CH_
#pragma package
#endif
#ifndef _EiC
#include "cv.h"
#include "cvAux.h"
#include "highgui.h"
#include "cxcore.h"
#include
#include
#endif
// 从图片的x、y坐标处返回相应的色调、饱和度和亮度
int getpixel(IplImage *image, int x, int y, int *h, int *s, int *v){
*h =(uchar) image->imageData[y *image->widthStep+x * image->nChannels];
*s =(uchar) image->imageData[y *image->widthStep+ x * image->nChannels + 1];
*v =(uchar) image->imageData[y *image->widthStep+ x * image->nChannels + 2];
return 0;
}
//--------------------------------------------------------------------------------
int main( int argc, char** argv ){
CvCapture* capture = 0;
IplImage* image = 0;
IplImage* HSV = 0;
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] );
}
if( !capture ){
fprintf(stderr,"Could not initialize capturing...\n");
return -1;
}
printf( "Hot keys: \n"
"\tESC - quit the program\n");
//创建Normal窗口
cvNamedWindow("Normal", CV_WINDOW_AUTOSIZE );
//Condensation结构体初始化-------------------------------------------------
int DP=2; // 状态向量的维数
int MP=2; // 观测向量的维数
int SamplesNum=300; // 样本粒子的数量
CvConDensation* ConDens=cvCreateConDensation( DP, MP, SamplesNum );
//-----------------------------------------------------------------------
//Condensation结构体中一些参数的初始化-----------------------------------
CvMat* lowerBound; // 下界
CvMat* upperBound; // 上界
lowerBound = cvCreateMat(2, 1, CV_32F);
upperBound = cvCreateMat(2, 1, CV_32F);
//设置粒子坐标的上下界为窗口大小640*480
cvmSet( lowerBound, 0, 0, 0.0 ); cvmSet( upperBound, 0, 0, 640.0 );
cvmSet( lowerBound, 1, 0, 0.0 ); cvmSet( upperBound, 1, 0, 480.0 );
cvConDensInitSampleSet(ConDens, lowerBound, upperBound);
//-----------------------------------------------------------------------
//设置窗口的中心为追踪的初始点------------------------------
for(int i=0; i < SamplesNum; i++){
ConDens->flSamples[i][0]+=320.0;
ConDens->flSamples[i][1]+=240.0;
}
//-----------------------------------------------------------------------
//迁移矩阵的初始化----------------------------
ConDens->DynamMatr[0]=1.0;ConDens->DynamMatr[1]=0.0;
ConDens->DynamMatr[2]=0.0;ConDens->DynamMatr[3]=1.0;
//-----------------------------------------------------------------------
for(;;){
IplImage* frame = 0;
int c;
int X,Y,XX,YY;
int H,S,V;
frame = cvQueryFrame( capture );
if( !frame ){
break;
}
if( !image ){
image = cvCreateImage( cvGetSize(frame), 8, 3 );
image->origin = frame->origin;
HSV = cvCreateImage( cvGetSize(frame), 8, 3 );
HSV->origin = frame->origin;
}
cvCopy( frame, image, 0 );
cvCvtColor(image ,HSV , CV_BGR2HSV);
//粒子的置信度计算,置信度需要自己建模---------------------------------------------------
for(int i=0; i < SamplesNum; i++){
X=(int)ConDens->flSamples[i][0];
Y=(int)ConDens->flSamples[i][1];
if(X>=0 && X<=640 && Y>=0 && Y<=480){ //粒子的坐标在窗口范围之内
getpixel(HSV, X, Y, &H, &S, &V);
if(H<=19 && S>=48){ // 肤色的判定 //H<=19 S>=48
cvCircle(image, cvPoint(X,Y), 4, CV_RGB(255,0,0), 1);
ConDens->flConfidence[i]=1.0;
}
else{
ConDens->flConfidence[i]=0.0;
}
}
else{
ConDens->flConfidence[i]=0.0;
}
}
//--------------------------------------------------------------------------
//更新滤波器状态
cvConDensUpdateByTime(ConDens);
cvShowImage( "Normal", image );
c = cvWaitKey(20);
if( c == 27 ){
break;
}
}
//释放内存------------------------------------
cvReleaseImage(&image);
cvReleaseImage(&HSV);
cvReleaseConDensation(&ConDens);
cvReleaseMat( &lowerBound );
cvReleaseMat( &upperBound );
cvReleaseCapture( &capture );
cvDestroyWindow("Normal");
//---------------------------------------------
return 0;
}
#ifdef _EiC
main(1,"condensation.cpp");
#endif
http://blog.csdn.net/onezeros/article/details/6318944
#include
#include
#include
#include
#include
#include
#include
using namespace std;
const int winHeight=600;
const int winWidth=800;
CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);
//mouse event callback
void mouseEvent(int event,int x,int y,int flags,void *param )
{
if (event==CV_EVENT_MOUSEMOVE) {
mousePosition=cvPoint(x,y);
}
}
int main (void)
{
//1.condensation setup
const int stateNum=4;
const int measureNum=2;
const int sampleNum=2000;
CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);
CvMat* lowerBound;
CvMat* upperBound;
lowerBound = cvCreateMat(stateNum, 1, CV_32F);
upperBound = cvCreateMat(stateNum, 1, CV_32F);
cvmSet(lowerBound,0,0,0.0 );
cvmSet(upperBound,0,0,winWidth );
cvmSet(lowerBound,1,0,0.0 );
cvmSet(upperBound,1,0,winHeight );
cvmSet(lowerBound,2,0,0.0 );
cvmSet(upperBound,2,0,0.0 );
cvmSet(lowerBound,3,0,0.0 );
cvmSet(upperBound,3,0,0.0 );
float A[stateNum][stateNum] ={
1,0,1,0,
0,1,0,1,
0,0,1,0,
0,0,0,1
};
memcpy(condens->DynamMatr,A,sizeof(A));
cvConDensInitSampleSet(condens, lowerBound, upperBound);
CvRNG rng_state = cvRNG(0xffffffff);
for(int i=0; i < sampleNum; i++){
condens->flSamples[i][0] = float(cvRandInt( &rng_state ) % winWidth); //width
condens->flSamples[i][1] = float(cvRandInt( &rng_state ) % winHeight);//height
}
CvFont font;
cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1); //set the font_face height and weight
char* winName="condensation";
cvNamedWindow(winName);
cvSetMouseCallback(winName,mouseEvent);
IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);
bool isPredictOnly=false;//trigger for prediction only,press SPACEBAR
while (1){
//2.condensation prediction
CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);
float variance[measureNum]={0,0};
//get variance/standard deviation of each state
for (int i=0;iSamplesNum;j++) {
sumState+=condens->flSamples[i][j];
}
//average
sumState/=sampleNum;
//variance
for (int j=0;jSamplesNum;j++) {
variance[i]+=(condens->flSamples[i][j]-sumState)*
(condens->flSamples[i][j]-sumState);
}
variance[i]/=sampleNum-1;
}
//3.update particals confidence
CvPoint pt;
if (isPredictOnly) {
pt=predict_pt;
}else{
pt=mousePosition;
}
for (int i=0;iSamplesNum;i++) {
float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])
*(pt.x-condens->flSamples[i][0])/(2*variance[0]));
float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])
*(pt.y-condens->flSamples[i][1])/(2*variance[1]));
condens->flConfidence[i]=probX*probY;
}
//4.update condensation
cvConDensUpdateByTime(condens);
//draw
cvSet(img,cvScalar(255,255,255,0));
cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green
char buf[256];
sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);
cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));
if (!isPredictOnly) {
cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red
sprintf_s(buf,256,"real position :(%3d,%3d)",mousePosition.x,mousePosition.y);
cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));
}
cvShowImage(winName, img);
int key=cvWaitKey(30);
if (key==27){//esc
break;
}else if (key==' ') {//trigger for prediction
//isPredict=!isPredict;
if (isPredictOnly) {
isPredictOnly=false;
}else{
isPredictOnly=true;
}
}
}
cvReleaseImage(&img);
cvReleaseConDensation(&condens);
return 0;
}
效果图