void Filter::Particle_Filter::init() {
condens = cvCreateConDensation(stateNum,measureNum,sampleNum);
lowerBound = cvCreateMat(stateNum, 1, CV_32F);
upperBound = cvCreateMat(stateNum, 1, CV_32F);
lowerBound = cvCreateMat(stateNum, 1, CV_32F);
upperBound = cvCreateMat(stateNum, 1, CV_32F);
cvmSet(lowerBound,0,0,0.0);
cvmSet(upperBound,0,0,winWidth/8);
cvmSet(lowerBound,1,0,0.0);
cvmSet(upperBound,1,0,winHeight/8);
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
}
}
//
// Constructor
//
CCondens::CCondens(TCHAR *tszName,LPUNKNOWN punk,HRESULT *phr) :
CTransInPlaceFilter(tszName, punk, CLSID_CCondens,phr)
{
m_params.nSamples = 64;
m_params.x = 0.4f;
m_params.y = 0.3f;
m_params.width = 0.2f;
m_params.height = 0.3f;
m_params.Smin = 20;
m_params.Vmin = 40;
m_params.view = 0;
m_Indicat1.x = 0;
m_Indicat1.y = 0;
m_Indicat1.width = 100;
m_Indicat1.height = 60;
IsTracking = false;
IsInit =false;
ConDens=cvCreateConDensation(4,4,m_params.nSamples);
CvMat Dyn = cvMat(4,4,CV_MAT4x4_32F,ConDens->DynamMatr);
cvmSetIdentity(&Dyn);
cvmSet(&Dyn,0,1,0.9);
cvmSet(&Dyn,2,3,0.9);
ASSERT(tszName);
ASSERT(phr);
} // CCondens
void ParticleFilter::init()
{
cout << "Initialisierung des ParticleFilters" << endl;
// Das condensation-Objekt
// Diese Struktur hält alle Daten aus der Messung und ist wie folgt aufgebaut
/**
CvConDensation
{
int MP; // Dimension of measurement vector
int DP; // Dimension of state vector
float* DynamMatr; // Matrix of the linear Dynamics system
float* State; // Vector of State
int SamplesNum; // Number of Samples
float** flSamples; // array of the Sample Vectors
float** flNewSamples; // temporary array of the Sample Vectors
float* flConfidence; // Confidence for each Sample
float* flCumulative; // Cumulative confidence
float* Temp; // Temporary vector
float* RandomSample; // RandomVector to update sample set
CvRandState* RandS; // Array of structures to generate random vectors
} CvConDensation;
*/
condensation = cvCreateConDensation(dynamical_vector,messurement_vector,number_of_samples);
// Zuallererst muss sie initialisiert werden. Das geht mittels einer Funktion, kann aber auch per Hand gelöst werden
CvMat* lowerBound = cvCreateMat((int)vector_size,1,CV_32F); //lowerBound = lowerBound;
CvMat* upperBound = cvCreateMat((int)vector_size,1,CV_32F); //upperBound = upperBound;
// die Boundaries müssen nun noch initialisiert werden (das mache ich mal so wie im OpenCV-Buch)
for (size_t i = 0; i < vector_size; ++i) {
lowerBound->data.fl[i] = -1.0f;
upperBound->data.fl[i] = 1.0f;
}
cvConDensInitSampleSet(condensation,lowerBound,upperBound);
cout << "DONE!";
}
void particle::genParticles(glm::vec3 particleV)
{
particleCenterM.setTo(cv::Scalar(0));
//Bereich der Partikelstreuung
setRanges(particleV.x, particleV.y, particleV.z, 0.5);
CvMat LB, UB;
cvInitMatHeader(&LB, 3, 1, CV_32FC1, minRange);
cvInitMatHeader(&UB, 3, 1, CV_32FC1, maxRange);
CvConDensation* condens = cvCreateConDensation(dim, dim, nParticles);
cvConDensInitSampleSet(condens, &LB, &UB);
//Einheitsmatrix
condens->DynamMatr[0] = 1.0;
condens->DynamMatr[1] = 0.0;
condens->DynamMatr[2] = 0.0;
condens->DynamMatr[3] = 0.0;
condens->DynamMatr[4] = 1.0;
condens->DynamMatr[5] = 0.0;
condens->DynamMatr[6] = 0.0;
condens->DynamMatr[7] = 0.0;
condens->DynamMatr[8] = 1.0;
cameraV.clear();
newCameraV.clear();
for (int i = 0; i < condens->SamplesNum; i++) {
//Berechnung der Abweichung
// float diffX = (particleV.x - condens->flSamples[i][0])/xRange;
// float diffY = (particleV.y - condens->flSamples[i][1])/yRange;
// float diffZ = (particleV.z - condens->flSamples[i][2])/zRange;
// condens->flConfidence[i] = 1.0 / (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ));
// Partikelstreuung werde ich benötigen
//cv::Point3f partPt(condens->flSamples[i][0], condens->flSamples[i][1], condens->flSamples[i][2]);
glm::vec3 partCenter(condens->flSamples[i][0], condens->flSamples[i][1], condens->flSamples[i][2]);
particleCenterM(i,0) = partCenter.x;
particleCenterM(i,1) = partCenter.y;
particleCenterM(i,2) = partCenter.z;
genParticles(lookAtCamera, partCenter, i);
//cout << "PartikelPos: X-Achse: " << condens->flSamples[i][0] << "/" << lastCam(0) << " Y-Achse: " << condens->flSamples[i][1] << "/" << lastCam(1)<< " Z-Achse: " << condens->flSamples[i][2] << "/" << lastCam(2)<< endl;
//writeFile(condens->flSamples[i][0], condens->flSamples[i][1], condens->flSamples[i][2], "particlePos.txt");
}
//cvConDensUpdateByTime(condens);
//Bester Partikel, ist aber keine der Partikelpositionen
//cv::Point3f statePt(condens->State[0], condens->State[1], condens->State[2]);
//newCameraV.push_back(statePt);
//cout << "NeuePose: X-Achse: " << condens->State[0] << "/" << lastCam(0) << " Y-Achse: " << condens->State[1] << "/" << lastCam(1)<< " Z-Achse: " << condens->State[2] << "/" << lastCam(2)<< endl;
}
/** Initialize the Condensation data structure and state dynamics
*/
void OpticalFlow::InitCondensation(int condens_num_samples)
{
// initialize Condensation data structure and set the
// system dynamics
m_sample_confidences.resize(condens_num_samples);
if (m_pConDens) {
cvReleaseConDensation(&m_pConDens);
}
m_pConDens = cvCreateConDensation(OF_CONDENS_DIMS, OF_CONDENS_DIMS, condens_num_samples);
CvMat dyn = cvMat(OF_CONDENS_DIMS, OF_CONDENS_DIMS, CV_32FC1, m_pConDens->DynamMatr);
// CvMat dyn = cvMat(OF_CONDENS_DIMS, OF_CONDENS_DIMS, CV_MAT3x3_32F, m_pConDens->DynamMatr);
cvmSetIdentity(&dyn);
cvmSet(&dyn, 0, 1, 0.0);
cvmSet(&dyn, 2, 3, 0.0);
// initialize bounds for state
float lower_bound[OF_CONDENS_DIMS];
float upper_bound[OF_CONDENS_DIMS];
// velocity bounds highly depend on the frame rate that we will achieve,
// increase the factor for lower frame rates;
// it states how much the center can move in either direction in a single
// frame, measured in terms of the width or height of the initial match size
double velocity_factor = .25;
double cx = (m_condens_init_rect.left+m_condens_init_rect.right)/2.0;
double cy = (m_condens_init_rect.top+m_condens_init_rect.bottom)/2.0;
double width = (m_condens_init_rect.right-m_condens_init_rect.left)*velocity_factor;
double height = (m_condens_init_rect.bottom-m_condens_init_rect.top)*velocity_factor;
lower_bound[0] = (float) (cx-width);
upper_bound[0] = (float) (cx+width);
lower_bound[1] = (float) (-width);
upper_bound[1] = (float) (+width);
lower_bound[2] = (float) (cy-height);
upper_bound[2] = (float) (cy+height);
lower_bound[3] = (float) (-height);
upper_bound[3] = (float) (+height);
lower_bound[4] = (float) (-10.0*velocity_factor*M_PI/180.0);
upper_bound[4] = (float) (+10.0*velocity_factor*M_PI/180.0);
CvMat lb = cvMat(OF_CONDENS_DIMS, 1, CV_MAT3x1_32F, lower_bound);
CvMat ub = cvMat(OF_CONDENS_DIMS, 1, CV_MAT3x1_32F, upper_bound);
cvConDensInitSampleSet(m_pConDens, &lb, &ub);
// set the state that will later be computed by condensation to
// the currently observed state
m_condens_state.x = cx;
m_condens_state.y = cy;
m_condens_state.vx = 0;
m_condens_state.vy = 0;
m_condens_state.angle = 0;
// debugging:
// DbgSetModuleLevel(LOG_CUSTOM1, 3);
}
CvConDensation* initCondensation ( CvMat** indexMat, int nSample, int maxWidth, int maxHeight ){
int DP = indexMat[0]->cols; //! number of state vector dimensions */
int MP = indexMat[2]->rows; //! number of measurement vector dimensions */
CvConDensation* ConDens = cvCreateConDensation( DP, MP, nSample );
CvMat* lowerBound;
CvMat* upperBound;
lowerBound = cvCreateMat(DP, 1, CV_32F);
upperBound = cvCreateMat(DP, 1, CV_32F);
cvmSet( lowerBound, 0, 0, 0.0 );
cvmSet( upperBound, 0, 0, maxWidth );
cvmSet( lowerBound, 1, 0, 0.0 );
cvmSet( upperBound, 1, 0, maxHeight );
cvmSet( lowerBound, 2, 0, 0.0 );
cvmSet( upperBound, 2, 0, 0.0 );
cvmSet( lowerBound, 3, 0, 0.0 );
cvmSet( upperBound, 3, 0, 0.0 );
//ConDens->DynamMatr = &indexMat[0]; fa il set della matrice del sistema
for (int i=0;i<DP*DP;i++){
ConDens->DynamMatr[i]= indexMat[0]->data.fl[i];
}
cvConDensInitSampleSet(ConDens, lowerBound, upperBound);
CvRNG rng_state = cvRNG(0xffffffff);
for(int i=0; i < nSample; i++){
ConDens->flSamples[i][0] = cvRandInt( &rng_state ) % maxWidth; //0 represent the widht (x coord)
ConDens->flSamples[i][1] = cvRandInt( &rng_state ) % maxHeight; //1 represent the height (1 coord)
}
//ConDens->DynamMatr=(float*)indexMat[0];
//ConDens->State[0]=maxWidth/2;ConDens->State[1]=maxHeight/2;ConDens->State[2]=0;ConDens->State[3]=0;
return ConDens;
}
condensPose::condensPose(int np)
:nParticles(np)
{
// the dimension of pose
dim = 6;
condens = cvCreateConDensation(dim, dim, np);
// TODO: additional init steps here
//
// init the DynamMatr, since the prediction is not given by the DynamMatr, here we just simply set DynamMatr as identity matrix
for (int i = 0; i < dim; i++)
for (int j = 0; j < dim; j++)
{
if (i == j)
condens->DynamMatr[i * dim + j] = 1.0;
else
condens->DynamMatr[i * dim + j] = 0.0;
}
}
int main( int argc, char *argv[] ) {
int webcamRun = 0;
CvCapture *capture = 0;
CvCapture *stereoCapture = 0;
CvCapture **cptPtr = &capture;
CvCapture **scptPtr = &stereoCapture;
initCaptureFiles( argc, argv, cptPtr, scptPtr, &webcamRun, &enable3D );
char locationMean[300];
char locationCov[300];
strncpy( locationMean, argv[1], 300 );
strncpy( locationCov, argv[2], 300 );
/* check for stereo file */
if( !enable3D ) {
displayStereoFrame = 0;
displayPositionFrame = 0;
depthScaling = 0;
}
/* Fast-forwards through the video to the action */
int kl;
for( kl = 0; kl < 100 && webcamRun == 0; kl++ ) {
cvQueryFrame( capture );
if( enable3D ) cvQueryFrame( stereoCapture );
}
/*
Housekeeping
*/
int maxBox[10];
// this should be in the relevant file
//! Create matrix files to hold intermediate calculations for likelihood
/* this should have its own matrix setup function and not be hard-coded */
//! Create N-dimensional matrices to hold mean and cov data
int backMeanSizes[] = {_sizeY,_sizeX,4,_numResponses};
int backCovSizes[] = {_sizeY,_sizeX,4,_numResponses};
CvMatND *backMean = cvCreateMatND( 4, backMeanSizes, CV_32F );
CvMatND *backCov = cvCreateMatND( 4, backCovSizes, CV_32F );
/* Fix this */
if( webcamRun ) {
backMean = (CvMatND*) cvLoad( "D:/Will/Dropbox/My Dropbox/Project/Matlab/backMean.xml", NULL, NULL, NULL );
backCov = (CvMatND*) cvLoad( "D:/Will/Dropbox/My Dropbox/Project/Matlab/backCov.xml", NULL, NULL, NULL );
} else {
backMean = (CvMatND*) cvLoad( locationMean, NULL, NULL, NULL );
backCov = (CvMatND*) cvLoad( locationCov, NULL, NULL, NULL );
}
/* end here */
CvMat *imgLikelihood = cvCreateMat( 48, 64, CV_32F );
img = cvLoadImage( "foreground8/image (1).jpg", CV_LOAD_IMAGE_COLOR );
yImg = cvLoadImage( "foreground8/image (1).jpg", CV_LOAD_IMAGE_COLOR );
cvNamedWindow( "Tracker", CV_WINDOW_AUTOSIZE );
/* Condensation stuff */
ConDens = cvCreateConDensation( DP, MP, nSamples );
bx = 320; by = 240;
//hm = 0; //vm = 0;
/* Initialize the random number generator */
rng_state = cvRNG(0xffffffff);
initializeCondensation();
/* This allows us to change the probabiity with which Condensation alters each variable */
cvRandInit( &(ConDens->RandS[0]), -75, 75, 0, CV_RAND_UNI);
cvRandInit( &(ConDens->RandS[1]), -5, 5, 1, CV_RAND_UNI);
cvRandInit( &(ConDens->RandS[2]), -5, 5, 2, CV_RAND_UNI);
cvRandInit( &(ConDens->RandS[3]), -2, 2, 3, CV_RAND_UNI);
cvRandInit( &(ConDens->RandS[4]), -75, 75, 4, CV_RAND_UNI);
cvRandInit( &(ConDens->RandS[5]), -5, 5, 5, CV_RAND_UNI);
cvRandInit( &(ConDens->RandS[6]), -5, 5, 6, CV_RAND_UNI);
cvRandInit( &(ConDens->RandS[7]), -2, 2, 7, CV_RAND_UNI);
/*
If we have depth scaling, the depth controls the width & height of the box
So we don't want any randomness
*/
if( depthScaling ) {
cvRandInit( &(ConDens->RandS[3]), 0, 0, 3, CV_RAND_UNI);
cvRandInit( &(ConDens->RandS[7]), 0, 0, 7, CV_RAND_UNI);
}
IplImage* heatFrame = NULL;
heatFrame = cvQueryFrame( capture );
if(enable3D) cvQueryFrame( stereoCapture ); // making sure they stay in sync (and init positionFrame)
int frameNumb = 0;
// int mjk;
// for( mjk = 0; mjk < 320; mjk++ ) {
// cvQueryFrame( capture );
// cvQueryFrame( stereoCapture );
// frameNumb++;
// }
positionFrame = cvCreateImage(cvSize(640, 510), 8, 3);
int trailLength = 20;
int planX1Pos[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int planX2Pos[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int planZ1Pos[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int planZ2Pos[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int key = 0;
double totalTime = 0;
int totalFrames = 0;
while( key != 'q' ) {
// stopCount++;
// strcpy(frameNameTemp, frameName);
// itoa(stopCount, frameCount, 10);
// strcat( frameNameTemp, frameCount);
// strcat(frameNameTemp, ".jpg");
// cvSaveImage(frameNameTemp, positionFrame, 0);
/* Start timing */
clock_t start = clock();
frameNumb++;
printf("Frame %d\n", frameNumb);
if( frameNumb == 210 ) {
for( ; frameNumb < 290; frameNumb++ ) {
cvQueryFrame( capture );
cvQueryFrame( stereoCapture );
}
}
if( frameNumb == 350 ) {
return(0);
}
/* Get the first video frame, and maybe stereo frame */
frame = cvQueryFrame( capture );
if(enable3D) stereoFrame = cvQueryFrame( stereoCapture );
/* Double-check that we haven't reached the end */
if( !frame ) break;
if( enable3D ) if( !stereoFrame ) break;
/* Compute likelihoods for new frame, using mean and cov */
likelihood( frame, backMean, backCov, imgLikelihood );
/* Update the Condensation model using new likelihoods */
updateCondensation( 0, 0, imgLikelihood, maxBox );
/* Maybe display the Condensation particles */
if( displayParticles )
drawParticles();
/* Draw tracking boxes onto the video and stereo video frame */
drawTrackingBoxes( 0, maxBox, frame, stereoFrame );
drawTrackingBoxes( 1, maxBox, frame, stereoFrame );
/* Show the latest video frame (with boxes and particles) */
cvShowImage( "Tracker", frame );
/* Maybe show latest stereo depth frame */
if( displayStereoFrame )
cvShowImage( "Stereo", stereoFrame );
/* Maybe show latest position map */
if( displayPositionFrame )
drawPositionTrail( maxBox, trailLength,
depth,
planX1Pos, planX2Pos, planZ1Pos,
planZ2Pos, positionFrame );
/* Maybe show the heat map of the input image (the likelihood for each patch) */
if( displayHeatFrame ) {
drawHeatmap( maxBox, imgLikelihood, heatFrame );
cvShowImage( "Heat", heatFrame );
}
/* Update previous x, y positions */
prevX[0] = maxBox[0]; prevY[0] = maxBox[1];
prevX[1] = maxBox[4]; prevY[1] = maxBox[5];
/* Calculate fps and average fps */
printf("fps: %3.1f, ", (double)1/(((double)clock() - start) / CLOCKS_PER_SEC));
totalTime = totalTime + (double)1/(((double)clock() - start) / CLOCKS_PER_SEC);
totalFrames++;
printf(" average fps %3.1f", totalTime/totalFrames);
/* Check for key presses */
key = cvWaitKey( 50 );
/* Enable debugging mode */
if( key == 'd' ) {
debug = 1;
printf("\n*********\n\nDebug: ON\n\n*********\n");
}
/* Enable debugging mode */
if( key == 'p' ) {
if( stateDraw )
stateDraw = 0;
else
stateDraw = 1;
printf("\n*********\n\nParticle Drawing Toggled\n\n*********\n");
}
}
printf("\n\nquitting");
//cvReleaseVideoWriter( &writerTracker );
cvReleaseImage( &img );
cvReleaseImage( &frame );
cvDestroyWindow( "Tracker" );
cvDestroyWindow( "Heat" );
cvDestroyWindow( "Stereo" );
cvDestroyWindow( "Position" );
return(0);
}
//パーティクルフィルタ
void particleFilter()
{
int i, c;
double w = 0.0, h = 0.0;
cv::VideoCapture capture(0);
//CvCapture *capture = 0;
//capture = cvCreateCameraCapture (0);
int n_stat = 4;
int n_particle = 4000;
CvConDensation *cond = 0;
CvMat *lowerBound = 0;
CvMat *upperBound = 0;
int xx, yy;
capture >> capframe;
//１フレームキャプチャし，キャプチャサイズを取得する．
//frame = cvQueryFrame (capture);
//redimage=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
//greenimage=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
//blueimage=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
w = capframe.cols;
h = capframe.rows;
//w = frame->width;
//h = frame->height;
cv::namedWindow("Condensation", CV_WINDOW_AUTOSIZE);
cv::setMouseCallback("Condensation", on_mouse, 0);
//cvNamedWindow ("Condensation", CV_WINDOW_AUTOSIZE);
//cvSetMouseCallback("Condensation",on_mouse,0);
//フォントの設定
//CvFont dfont;
//float hscale = 0.7f;
//float vscale = 0.7f;
//float italicscale = 0.0f;
//int thickness = 1;
//char text[255] = "";
//cvInitFont (&dfont, CV_FONT_HERSHEY_SIMPLEX , hscale, vscale, italicscale, thickness, CV_AA);
//Condensation構造体を作成する．
cond = cvCreateConDensation (n_stat, 0, n_particle);
//状態ベクトル各次元の取りうる最小値・最大値を指定する
//今回は位置(x,y)と速度(xpixcel/frame,ypixcel/frame)の4次元
lowerBound = cvCreateMat (4, 1, CV_32FC1);
upperBound = cvCreateMat (4, 1, CV_32FC1);
cvmSet (lowerBound, 0, 0, 0.0);
cvmSet (lowerBound, 1, 0, 0.0);
cvmSet (lowerBound, 2, 0, -20.0);
cvmSet (lowerBound, 3, 0, -20.0);
cvmSet (upperBound, 0, 0, w);
cvmSet (upperBound, 1, 0, h);
cvmSet (upperBound, 2, 0, 20.0);
cvmSet (upperBound, 3, 0, 20.0);
//Condensation構造体を初期化する
cvConDensInitSampleSet (cond, lowerBound, upperBound);
//ConDensationアルゴリズムにおける状態ベクトルのダイナミクスを指定する
cond->DynamMatr[0] = 1.0;
cond->DynamMatr[1] = 0.0;
cond->DynamMatr[2] = 1.0;
cond->DynamMatr[3] = 0.0;
cond->DynamMatr[4] = 0.0;
cond->DynamMatr[5] = 1.0;
cond->DynamMatr[6] = 0.0;
cond->DynamMatr[7] = 1.0;
cond->DynamMatr[8] = 0.0;
cond->DynamMatr[9] = 0.0;
cond->DynamMatr[10] = 1.0;
cond->DynamMatr[11] = 0.0;
cond->DynamMatr[12] = 0.0;
cond->DynamMatr[13] = 0.0;
cond->DynamMatr[14] = 0.0;
cond->DynamMatr[15] = 1.0;
//ノイズパラメータを再設定する．
cvRandInit (&(cond->RandS[0]), -25, 25, (int) cvGetTickCount ());
cvRandInit (&(cond->RandS[1]), -25, 25, (int) cvGetTickCount ());
cvRandInit (&(cond->RandS[2]), -5, 5, (int) cvGetTickCount ());
cvRandInit (&(cond->RandS[3]), -5, 5, (int) cvGetTickCount ());
while (1)
{
capture >> capframe;
//frame = cvQueryFrame (capture);
//各パーティクルについて尤度を計算する．
for (i = 0; i < n_particle; i++)
{
xx = (int) (cond->flSamples[i][0]);
yy = (int) (cond->flSamples[i][1]);
if (xx < 0 || xx >= w || yy < 0 || yy >= h)
{
cond->flConfidence[i] = 0.0;
}
else
{
cond->flConfidence[i] = calc_likelihood (capframe, xx, yy);
//cond->flConfidence[i] = calc_likelihood (frame, xx, yy);
cv::circle(capframe, cv::Point(xx, yy), 1, CV_RGB(0, 255, 200));
//cvCircle (frame, cvPoint (xx, yy), 1, CV_RGB (0, 255, 200), -1);
}
}
//重みの総和&重心を求める
double wx = 0, wy = 0;
double sumWeight = 0;
for (i = 0; i < n_particle; i++)
{
sumWeight += cond->flConfidence[i];
}
for (i = 0; i < n_particle; i++)
{
wx += (int) (cond->flSamples[i][0]) * (cond->flConfidence[i] / sumWeight);
wy += (int) (cond->flSamples[i][1]) * (cond->flConfidence[i] / sumWeight);
}
//重心表示
cv::circle(capframe, cv::Point((int)wx, (int)wy), 10, cv::Scalar(0, 0, 255));
cv::circle(capframe, cv::Point(20, 20), 10, CV_RGB(red, green, blue), 6);
cv::putText(capframe, "target", cv::Point(0, 50), cv::FONT_HERSHEY_SIMPLEX, 0.7, CV_RGB(red, green, blue));
cv::imshow("Condensation", capframe);
//cvCircle(frame,cvPoint(20,20),10,CV_RGB(red,green,blue),-1);
//cvPutText(frame,"target",cvPoint(0,50),&dfont,CV_RGB(red,green,blue));
//cvShowImage ("Condensation", frame);
c = cv::waitKey(30);
//c = cvWaitKey (30);
if (c == 27) break;
//次のモデルの状態を推定する
cvConDensUpdateByTime (cond);
}
cv::destroyWindow("Condensation");
//cvDestroyWindow ("Condensation");
//cvReleaseCapture (&capture);
//cvReleaseImage(&redimage);
//cvReleaseImage(&greenimage);
//cvReleaseImage(&blueimage);
cvReleaseConDensation (&cond);
cvReleaseMat (&lowerBound);
cvReleaseMat (&upperBound);
}
