Learning opencv-particle filtering (two articles on the Internet)

The theory of particle filter is really amazing. It uses a set of random states with different weights to approach complex probability density functions. It has excellent characteristics in non-linear and non-Gaussian systems. Opencv provides an implementation, but does not provide an example. During the learning process, the network cannot be found. Learning opencv is introduced in the book, but there is still some distance from direct use. After some ups and downs, I can finally use it. I hope it will help you.

The example given in this article is the same as my other blog post, which uses smoothing and Prediction of two-dimensional coordinates.

Usage:

1. Create and initialize

Const int statenum = 4; // number of States
Const int measurenum = 2; // Number of measurement variables
Const int samplenum = 2000; // number of particles

Cvcondensation * condens = cvcreatecondensation (statenum, measurenum, samplenum );

The larger the number of particles, the more stable the system performance without affecting performance

For other initialization content, see learning opencv

2. Prediction
3. Update the credibility of the example, that is, the weight. The update method in this example is different from that in learning opencv.
4. Update cvcondensation

 

Code:

# Include <cv. h> <br/> # include <cxcore. h> <br/> # include <pighgui. 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 * pa Ram) <br/>{< br/> If (event = cv_event_mousemove) {<br/> mouseposition = cvpoint (x, y ); <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 = cvcreat EMAT (statenum, 1, cv_32f); <br/> upperbound = cvcreatemat (statenum, 1, cv_32f); <br/> cvmset (lowerbound, 0.0 ); <br/> cvmset (upperbound, 0.0, winwidth); <br/> cvmset (lowerbound,); <br/> cvmset (upperbound, winheight ); <br/> cvmset (lowerbound, 0.0, 0.0); <br/> cvmset (upperbound, 0.0,); <br/> cvmset (lowerbound ); <br/> cvmset (upperbound, 3,0, 0.0); <br/> float a [statenum] [statenum] ={ <br />, <Br/> }; <br/> memcpy (condens-> dynammatr, A, sizeof (a); <br/> cvcondensinitsampleset (condens, lowerbound, upperbound ); </P> <p> cvrng rng_state = cvrng (0 xffffffff); <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) % win Height); // 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 predictio N <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 participant 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) E XP (-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 (), & font, cv_rgb (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 )); <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/>}</P> <p> cvreleaseimage (& IMG); <br/> cvreleasecondensation (& condens ); <br/> return 0; <br/>}< br/>

 

Kalman Filter video demonstration:

In the demonstration, the number of particles is 100,200,200 0, respectively.

Observe the effect carefully

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

Demo snapshot:

 

//The above is a demonstration point tracking, the originalHttp://blog.csdn.net/onezeros/article/details/6319180

// This is the window tracking for a buddy demonstration handed in! (With code) http://www.cnblogs.com/yangyangcv/archive/2010/05/23/1742263.html

The theory of particle filter is really amazing. It uses a set of random states with different weights to approach complex probability density functions. It has excellent characteristics in non-linear and non-Gaussian systems. Opencv provides an implementation, but does not provide an example. During the learning process, the network cannot be found. Learning opencv is introduced in the book, but there is still some distance from direct use. After some ups and downs, I can finally use it. I hope it will help you.

The example given in this article is the same as my other blog post, which uses smoothing and Prediction of two-dimensional coordinates.

Usage:

1. Create and initialize

Const int statenum = 4; // number of States
Const int measurenum = 2; // Number of measurement variables
Const int samplenum = 2000; // number of particles

Cvcondensation * condens = cvcreatecondensation (statenum, measurenum, samplenum );

The larger the number of particles, the more stable the system performance without affecting performance

For other initialization content, see learning opencv

2. Prediction
3. Update the credibility of the example, that is, the weight. The update method in this example is different from that in learning opencv.
4. Update cvcondensation

 

Code:

# Include <cv. h> <br/> # include <cxcore. h> <br/> # include <pighgui. 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 * pa Ram) <br/>{< br/> If (event = cv_event_mousemove) {<br/> mouseposition = cvpoint (x, y ); <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 = cvcreat EMAT (statenum, 1, cv_32f); <br/> upperbound = cvcreatemat (statenum, 1, cv_32f); <br/> cvmset (lowerbound, 0.0 ); <br/> cvmset (upperbound, 0.0, winwidth); <br/> cvmset (lowerbound,); <br/> cvmset (upperbound, winheight ); <br/> cvmset (lowerbound, 0.0, 0.0); <br/> cvmset (upperbound, 0.0,); <br/> cvmset (lowerbound ); <br/> cvmset (upperbound, 3,0, 0.0); <br/> float a [statenum] [statenum] ={ <br />, <Br/> }; <br/> memcpy (condens-> dynammatr, A, sizeof (a); <br/> cvcondensinitsampleset (condens, lowerbound, upperbound ); </P> <p> cvrng rng_state = cvrng (0 xffffffff); <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) % win Height); // 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 predictio N <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 participant 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) E XP (-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 (), & font, cv_rgb (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 )); <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/>}</P> <p> cvreleaseimage (& IMG); <br/> cvreleasecondensation (& condens ); <br/> return 0; <br/>}< br/>

 

Kalman Filter video demonstration:

In the demonstration, the number of particles is 100,200,200 0, respectively.

Observe the effect carefully

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

Demo snapshot: