學習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

 

代碼：

#include <cv.h><br />#include <cxcore.h><br />#include <highgui.h><br />#include <cvaux.h></p><p>#include <cmath><br />#include <vector><br />#include <iostream><br />using namespace std;</p><p>const int winHeight=600;<br />const int winWidth=800;</p><p>CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);</p><p>//mouse event callback<br />void mouseEvent(int event,int x,int y,int flags,void *param )<br />{<br />if (event==CV_EVENT_MOUSEMOVE) {<br />mousePosition=cvPoint(x,y);<br />}<br />}</p><p>int main (void)<br />{<br />//1.condensation setup<br />const int stateNum=4;<br />const int measureNum=2;<br />const int sampleNum=2000;</p><p>CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);<br />CvMat* lowerBound;<br />CvMat* upperBound;<br />lowerBound = cvCreateMat(stateNum, 1, CV_32F);<br />upperBound = cvCreateMat(stateNum, 1, CV_32F);<br />cvmSet(lowerBound,0,0,0.0 );<br />cvmSet(upperBound,0,0,winWidth );<br />cvmSet(lowerBound,1,0,0.0 );<br />cvmSet(upperBound,1,0,winHeight );<br />cvmSet(lowerBound,2,0,0.0 );<br />cvmSet(upperBound,2,0,0.0 );<br />cvmSet(lowerBound,3,0,0.0 );<br />cvmSet(upperBound,3,0,0.0 );<br />float A[stateNum][stateNum] ={<br />1,0,1,0,<br />0,1,0,1,<br />0,0,1,0,<br />0,0,0,1<br />};<br />memcpy(condens->DynamMatr,A,sizeof(A));<br />cvConDensInitSampleSet(condens, lowerBound, upperBound);</p><p>CvRNG rng_state = cvRNG(0xffffffff);<br />for(int i=0; i < sampleNum; i++){<br />condens->flSamples[i][0] = float(cvRandInt( &rng_state ) % winWidth); //width<br />condens->flSamples[i][1] = float(cvRandInt( &rng_state ) % winHeight);//height<br />}</p><p>CvFont font;<br />cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);</p><p>char* winName="condensation";<br />cvNamedWindow(winName);<br />cvSetMouseCallback(winName,mouseEvent);<br />IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);<br />bool isPredictOnly=false;//trigger for prediction only,press SPACEBAR<br />while (1){<br />//2.condensation prediction<br />CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);</p><p>float variance[measureNum]={0};<br />//get variance/standard deviation of each state<br />for (int i=0;i<measureNum;i++) {<br />//sum<br />float sumState=0;<br />for (int j=0;j<condens->SamplesNum;j++) {<br />sumState+=condens->flSamples[i][j];<br />}<br />//average<br />sumState/=sampleNum;<br />//variance<br />for (int j=0;j<condens->SamplesNum;j++) {<br />variance[i]+=(condens->flSamples[i][j]-sumState)*<br />(condens->flSamples[i][j]-sumState);<br />}<br />variance[i]/=sampleNum-1;<br />}<br />//3.update particals confidence<br />CvPoint pt;<br />if (isPredictOnly) {<br />pt=predict_pt;<br />}else{<br />pt=mousePosition;<br />}<br />for (int i=0;i<condens->SamplesNum;i++) {<br />float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])<br />*(pt.x-condens->flSamples[i][0])/(2*variance[0]));<br />float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])<br />*(pt.y-condens->flSamples[i][1])/(2*variance[1]));<br />condens->flConfidence[i]=probX*probY;<br />}<br />//4.update condensation<br />cvConDensUpdateByTime(condens);</p><p>//draw<br />cvSet(img,cvScalar(255,255,255,0));<br />cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green<br />char buf[256];<br />sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);<br />cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));<br />if (!isPredictOnly) {<br />cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red<br />sprintf_s(buf,256,"real position :(%3d,%3d)",mousePosition.x,mousePosition.y);<br />cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));<br />}</p><p>cvShowImage(winName, img);<br />int key=cvWaitKey(30);<br />if (key==27){//esc<br />break;<br />}else if (key==' ') {//trigger for prediction<br />//isPredict=!isPredict;<br />if (isPredictOnly) {<br />isPredictOnly=false;<br />}else{<br />isPredictOnly=true;<br />}<br />}<br />} </p><p>cvReleaseImage(&img);<br />cvReleaseConDensation(&condens);<br />return 0;<br />}<br />

 

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

粒子濾波的理論實在是太美妙了，用一組不同權重的隨機狀態來逼近複雜的機率密度函數。其再非線性、非高斯系統中具有優良的特性。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

 

代碼：

#include <cv.h><br />#include <cxcore.h><br />#include <highgui.h><br />#include <cvaux.h></p><p>#include <cmath><br />#include <vector><br />#include <iostream><br />using namespace std;</p><p>const int winHeight=600;<br />const int winWidth=800;</p><p>CvPoint mousePosition=cvPoint(winWidth>>1,winHeight>>1);</p><p>//mouse event callback<br />void mouseEvent(int event,int x,int y,int flags,void *param )<br />{<br />if (event==CV_EVENT_MOUSEMOVE) {<br />mousePosition=cvPoint(x,y);<br />}<br />}</p><p>int main (void)<br />{<br />//1.condensation setup<br />const int stateNum=4;<br />const int measureNum=2;<br />const int sampleNum=2000;</p><p>CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);<br />CvMat* lowerBound;<br />CvMat* upperBound;<br />lowerBound = cvCreateMat(stateNum, 1, CV_32F);<br />upperBound = cvCreateMat(stateNum, 1, CV_32F);<br />cvmSet(lowerBound,0,0,0.0 );<br />cvmSet(upperBound,0,0,winWidth );<br />cvmSet(lowerBound,1,0,0.0 );<br />cvmSet(upperBound,1,0,winHeight );<br />cvmSet(lowerBound,2,0,0.0 );<br />cvmSet(upperBound,2,0,0.0 );<br />cvmSet(lowerBound,3,0,0.0 );<br />cvmSet(upperBound,3,0,0.0 );<br />float A[stateNum][stateNum] ={<br />1,0,1,0,<br />0,1,0,1,<br />0,0,1,0,<br />0,0,0,1<br />};<br />memcpy(condens->DynamMatr,A,sizeof(A));<br />cvConDensInitSampleSet(condens, lowerBound, upperBound);</p><p>CvRNG rng_state = cvRNG(0xffffffff);<br />for(int i=0; i < sampleNum; i++){<br />condens->flSamples[i][0] = float(cvRandInt( &rng_state ) % winWidth); //width<br />condens->flSamples[i][1] = float(cvRandInt( &rng_state ) % winHeight);//height<br />}</p><p>CvFont font;<br />cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);</p><p>char* winName="condensation";<br />cvNamedWindow(winName);<br />cvSetMouseCallback(winName,mouseEvent);<br />IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);<br />bool isPredictOnly=false;//trigger for prediction only,press SPACEBAR<br />while (1){<br />//2.condensation prediction<br />CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);</p><p>float variance[measureNum]={0};<br />//get variance/standard deviation of each state<br />for (int i=0;i<measureNum;i++) {<br />//sum<br />float sumState=0;<br />for (int j=0;j<condens->SamplesNum;j++) {<br />sumState+=condens->flSamples[i][j];<br />}<br />//average<br />sumState/=sampleNum;<br />//variance<br />for (int j=0;j<condens->SamplesNum;j++) {<br />variance[i]+=(condens->flSamples[i][j]-sumState)*<br />(condens->flSamples[i][j]-sumState);<br />}<br />variance[i]/=sampleNum-1;<br />}<br />//3.update particals confidence<br />CvPoint pt;<br />if (isPredictOnly) {<br />pt=predict_pt;<br />}else{<br />pt=mousePosition;<br />}<br />for (int i=0;i<condens->SamplesNum;i++) {<br />float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])<br />*(pt.x-condens->flSamples[i][0])/(2*variance[0]));<br />float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])<br />*(pt.y-condens->flSamples[i][1])/(2*variance[1]));<br />condens->flConfidence[i]=probX*probY;<br />}<br />//4.update condensation<br />cvConDensUpdateByTime(condens);</p><p>//draw<br />cvSet(img,cvScalar(255,255,255,0));<br />cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green<br />char buf[256];<br />sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);<br />cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));<br />if (!isPredictOnly) {<br />cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with red<br />sprintf_s(buf,256,"real position :(%3d,%3d)",mousePosition.x,mousePosition.y);<br />cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));<br />}</p><p>cvShowImage(winName, img);<br />int key=cvWaitKey(30);<br />if (key==27){//esc<br />break;<br />}else if (key==' ') {//trigger for prediction<br />//isPredict=!isPredict;<br />if (isPredictOnly) {<br />isPredictOnly=false;<br />}else{<br />isPredictOnly=true;<br />}<br />}<br />} </p><p>cvReleaseImage(&img);<br />cvReleaseConDensation(&condens);<br />return 0;<br />}<br />

 

kalman filter 視頻示範：

示範中粒子數分別為100,200,2000

請仔細觀測效果

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

demo snapshot：