粒子滤波演示与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

#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <cvaux.h>
#include <cmath>
#include <vector>
#include <iostream>
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);
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};
//get variance/standard deviation of each state
for (int i=0;i<measureNum;i++) {
//sum
float sumState=0;
for (int j=0;j<condens->SamplesNum;j++) {
sumState+=condens->flSamples[i][j];
}
//average
sumState/=sampleNum;
//variance
for (int j=0;j<condens->SamplesNum;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;i<condens->SamplesNum;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;
}

kalman filter 视频演示：

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

请仔细观测效果

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

demo snapshot：

粒子滤波演示与opencv代码相关推荐

kalman 滤波演示与opencv代码
在机器视觉中追踪时常会用到预测算法,kalman是你一定知道的.它可以用来预测各种状态,比如说位置,速度等.关于它的理论有很多很好的文献可以参考.opencv给出了kalman filter的一个实现 ...
OpenCV学习笔记（二十一）——绘图函数core OpenCV学习笔记（二十二）——粒子滤波跟踪方法 OpenCV学习笔记（二十三）——OpenCV的GUI之凤凰涅槃Qt OpenCV学习笔记（二十
OpenCV学习笔记(二十一)--绘图函数core 在图像中,我们经常想要在图像中做一些标识记号,这就需要绘图函数.OpenCV虽然没有太优秀的GUI,但在绘图方面还是做得很完整的.这里就介绍一下相关 ...
粒子滤波算法（Matlab代码实现）
非线性非高斯模型的改进粒子滤波算法（Matlab代码实现）
粒子滤波和蒙特卡洛定位
算法主要过程如下: 1.根据观测更新粒子权重 2.根据权重resample,也就是根据权重更新所有粒子位置,并使得所有粒子权重恢复到一样. 3.利用一个模型让所有粒子随着robot的移动而移动. 无公 ...
学习OpenCV——粒子滤波（网上两篇文章总结）
粒子滤波的理论实在是太美妙了,用一组不同权重的随机状态来逼近复杂的概率密度函数.其再非线性.非高斯系统中具有优良的特性.opencv给出了一个实现,但是没有给出范例,学习过程中发现网络上也找不到.le ...
目标跟踪之粒子滤波---Opencv实现粒子滤波算法
目标跟踪学习笔记_2(particle filter初探1) 目标跟踪学习笔记_3(particle filter初探2) 前面2篇博客已经提到当粒子数增加时会内存报错,后面又仔细查了下程序,是代码方 ...
OpenCV3学习（12.5） opencv实现粒子滤波目标跟踪
OpenCV高版本已经把粒子滤波的CV方面的condensation算法给去掉了,以前学的condensation算法不能用C++开发还是只能用C版本,(OpenCV3学习(12.4) 粒子滤波Con ...
【回归预测】基于粒子滤波实现锂离子电池寿命预测附matlab代码
1 内容介绍随着现代生产生活对系统设备可靠性.安全性要求的提高,从成本.可靠性的角度考虑,电子系统正逐步由原来的定期维修变成视情维修(CBM, Condition Based Maintenanc ...

粒子滤波演示与opencv代码

粒子滤波演示与opencv代码相关推荐

最新文章

热门文章

粒子滤波 演示与opencv代码

粒子滤波 演示与opencv代码相关推荐

最新文章

热门文章

粒子滤波演示与opencv代码

粒子滤波演示与opencv代码相关推荐