学习OpenCV——粒子滤波（网上两篇文章总结）

粒子滤波的理论实在是太美妙了，用一组不同权重的随机状态来逼近复杂的概率密度函数。其再非线性、非高斯系统中具有优良的特性。opencv给出了一个实现，但是没有给出范例，学习过程中发现网络上也找不到。learning opencv一书中有介绍，但距离直接使用还是有些距离。在经过一番坎坷后，终于可以用了，希望对你有帮助。

本文中给出的例子跟 我的另一篇博文是同一个应用例子，都是对二维坐标进行平滑、预测

使用方法：

1.创建并初始化

const int stateNum=4;//状态数
 const int measureNum=2;//测量变量数
 const int sampleNum=2000;//粒子数

CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);

在不影响性能的情况下，粒子数量越大，系统表现的越稳定

其他初始化内容请参考learning opencv

2.预测
3.更新例子可信度，也就是权重。本例中更新方法与learning opencv中有所不同，想看代码 
4.更新CvConDensation

代码：

[cpp] view plain copy

print?

1. #include <cv.h>
2. #include <cxcore.h>
3. #include <highgui.h>
4. #include <cvaux.h>
5. #include <cmath>
6. #include <vector>
7. #include <iostream>
8. using namespace std;
9. const int winHeight=600;
10. const int winWidth=800;
11. CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);
12. //mouse event callback
13. void mouseEvent(int event,int x,int y,int flags,void *param )
14. {
15. if (event==CV_EVENT_MOUSEMOVE) {
16. mousePosition=cvPoint(x,y);
17. }
18. }
19. int main (void)
20. {
21. //1.condensation setup
22. const int stateNum=4;
23. const int measureNum=2;
24. const int sampleNum=2000;
25. CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);
26. CvMat* lowerBound;
27. CvMat* upperBound;
28. lowerBound = cvCreateMat(stateNum, 1, CV_32F);
29. upperBound = cvCreateMat(stateNum, 1, CV_32F);
30. cvmSet(lowerBound,0,0,0.0 );
31. cvmSet(upperBound,0,0,winWidth );
32. cvmSet(lowerBound,1,0,0.0 );
33. cvmSet(upperBound,1,0,winHeight );
34. cvmSet(lowerBound,2,0,0.0 );
35. cvmSet(upperBound,2,0,0.0 );
36. cvmSet(lowerBound,3,0,0.0 );
37. cvmSet(upperBound,3,0,0.0 );
38. float A[stateNum][stateNum] ={
39. 1,0,1,0,
40. 0,1,0,1,
41. 0,0,1,0,
42. 0,0,0,1
43. };
44. memcpy(condens->DynamMatr,A,sizeof(A));
45. cvConDensInitSampleSet(condens, lowerBound, upperBound);
46. CvRNG rng_state = cvRNG(0xffffffff);
47. for(int i=0; i < sampleNum; i++){
48. condens->flSamples[i][0] = float(cvRandInt( &rng_state ) % winWidth); //width
49. condens->flSamples[i][1] = float(cvRandInt( &rng_state ) % winHeight);//height
50. }
51. CvFont font;
52. cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);
53. char* winName="condensation";
54. cvNamedWindow(winName);
55. cvSetMouseCallback(winName,mouseEvent);
56. IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);
57. bool isPredictOnly=false;//trigger for prediction only,press SPACEBAR
58. while (1){
59. //2.condensation prediction
60. CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);
61. float variance[measureNum]={0};
62. //get variance/standard deviation of each state
63. for (int i=0;i<measureNum;i++) {
64. //sum
65. float sumState=0;
66. for (int j=0;j<condens->SamplesNum;j++) {
67. sumState+=condens->flSamples[i][j];
68. }
69. //average
70. sumState/=sampleNum;
71. //variance
72. for (int j=0;j<condens->SamplesNum;j++) {
73. variance[i]+=(condens->flSamples[i][j]-sumState)*
74. (condens->flSamples[i][j]-sumState);
75. }
76. variance[i]/=sampleNum-1;
77. }
78. //3.update particals confidence
79. CvPoint pt;
80. if (isPredictOnly) {
81. pt=predict_pt;
82. }else{
83. pt=mousePosition;
84. }
85. for (int i=0;i<condens->SamplesNum;i++) {
86. float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])
87. *(pt.x-condens->flSamples[i][0])/(2*variance[0]));
88. float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])
89. *(pt.y-condens->flSamples[i][1])/(2*variance[1]));
90. condens->flConfidence[i]=probX*probY;
91. }
92. //4.update condensation
93. cvConDensUpdateByTime(condens);
94. //draw
95. cvSet(img,cvScalar(255,255,255,0));
96. cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green
97. char buf[256];
98. sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);
99. cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));
100. if (!isPredictOnly) {
101. cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red
102. sprintf_s(buf,256,"real position :(%3d,%3d)",mousePosition.x,mousePosition.y);
103. cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));
104. }
105. cvShowImage(winName, img);
106. int key=cvWaitKey(30);
107. if (key==27){//esc
108. break;
109. }else if (key==‘ ‘) {//trigger for prediction
110. //isPredict=!isPredict;
111. if (isPredictOnly) {
112. isPredictOnly=false;
113. }else{
114. isPredictOnly=true;
115. }
116. }
117. }
118. cvReleaseImage(&img);
119. cvReleaseConDensation(&condens);
120. return 0;
121. }

kalman filter 视频演示：

演示中粒子数分别为100,200,2000

请仔细观测效果

http://v.youku.com/v_show/id_XMjU4MzE0ODgw.html

demo snapshot：

//上面这一篇是演示点跟踪，原文http://blog.csdn.net/onezeros/article/details/6319180

//这一篇是上交的一哥们演示的窗口跟踪！（有code）

http://www.cnblogs.com/yangyangcv/archive/2010/05/23/1742263.html

from: http://blog.csdn.net/yangtrees/article/details/7616483