void condensPose::init(vpHomogeneousMatrix& cMo, float rotPerturb, float transPerturb) { vpPoseVector pv; pv.buildFrom(cMo); float minRange[] = { pv[0] - rotPerturb, pv[1] - rotPerturb, pv[2] - rotPerturb, pv[3] - transPerturb, pv[4] - transPerturb, pv[5] - transPerturb}; float maxRange[] = { pv[0] + rotPerturb, pv[1] + rotPerturb, pv[2] + rotPerturb, pv[3] + transPerturb, pv[4] + transPerturb, pv[5] + transPerturb}; CvMat LB, UB; cvInitMatHeader(&LB, this->dim, 1, CV_32FC1, minRange); cvInitMatHeader(&UB, this->dim, 1, CV_32FC1, maxRange); cvConDensInitSampleSet(condens, &LB, &UB); }
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 } }
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; }
void OpticalFlow::UpdateCondensation(IplImage* /*rgbImage*/, int prev_indx, int curr_indx) { //VERBOSE5(3, "m_condens_state x %f, y %f, vx %f, vy %f, a %f", // m_condens_state.x, m_condens_state.y, m_condens_state.vx, m_condens_state.vy, m_condens_state.angle); // for each condensation sample, predict the feature locations, // compare to the observed KLT tracking, and check the probmask // at each predicted location. The combination of these yields the // confidence in this sample's estimate. int num_ft = (int) m_features[prev_indx].size(); CPointVector predicted; predicted.resize(num_ft); CDoubleVector probs_locations; CDoubleVector probs_colors; probs_locations.reserve(m_pConDens->SamplesNum); probs_colors.reserve(m_pConDens->SamplesNum); double sum_probs_locations = 0.0; double sum_probs_colors = 0.0; CDoubleVector old_lens; CDoubleVector old_d_angles; // prepare data structures so that prediction based on centroid // is fast PreparePredictFeatureLocations(m_condens_state, m_features[prev_indx], old_lens, old_d_angles); CvPoint2D32f avg_obs, avg_prev; GetAverage(m_features[curr_indx], avg_prev); // GetAverage(m_features[prev_indx], avg_prev); GetAverage(m_features[curr_indx]/*_observation*/, avg_obs); double dvx = avg_obs.x - avg_prev.x; double dvy = avg_obs.y - avg_prev.y; // for each sample for (int scnt=0; scnt<m_pConDens->SamplesNum; scnt++) { // hack - todo if (scnt==m_pConDens->SamplesNum-1) { m_pConDens->flSamples[scnt][0] = avg_obs.x; m_pConDens->flSamples[scnt][2] = avg_obs.y; m_pConDens->flSamples[scnt][1] = (float) dvx; m_pConDens->flSamples[scnt][3] = (float) dvy; } // the Condensation sample's guess: CondensState sample_state; sample_state.x = m_pConDens->flSamples[scnt][0]; sample_state.y = m_pConDens->flSamples[scnt][2]; sample_state.vx = m_pConDens->flSamples[scnt][1]; sample_state.vy = m_pConDens->flSamples[scnt][3]; sample_state.angle = 0;//m_pConDens->flSamples[scnt][4]; ASSERT(!isnan(sample_state.x) && !isnan(sample_state.y) && !isnan(sample_state.angle)); double fac = (m_condens_init_rect.right-m_condens_init_rect.left)/3.0; double dx = avg_obs.x - sample_state.x; double dy = avg_obs.y - sample_state.y; double probloc = dx*dx+dy*dy; probloc = fac/(fac+probloc); probs_locations.push_back(probloc); sum_probs_locations += probloc; #if 0 PredictFeatureLocations(old_lens, old_d_angles, sample_state, predicted); // probability of predicted feature locations given the KLT observation int discard_num_distances = (int)(0.15*(double)num_ft); double probloc = EstimateProbability(predicted, m_features[curr_indx]/*_observation*/, discard_num_distances); probs_locations.push_back(probloc); sum_probs_locations += probloc; // probability of predicted feature locations given the outside probability map (color) double probcol = EstimateProbability(predicted, rgbImage); probs_colors.push_back(probcol); sum_probs_colors += probcol; #endif } // end for each sample // ASSERT(!isnan(sum_probs_locations) && sum_probs_locations>0); // // normalize the individual probabilities and set sample confidence // int best_sample_indx = -1; double best_confidence = 0; for (int scnt=0; scnt<m_pConDens->SamplesNum; scnt++) { double norm_prob_locations = probs_locations[scnt]/sum_probs_locations; // double norm_prob_colors = probs_colors[scnt]/sum_probs_colors; double confidence; if (sum_probs_colors>0) { // confidence = norm_prob_locations*norm_prob_colors; confidence = norm_prob_locations; } else { confidence = norm_prob_locations; } m_pConDens->flConfidence[scnt] = (float) confidence; m_sample_confidences[scnt] = confidence; if (confidence>best_confidence) { best_confidence = confidence; best_sample_indx = scnt; } } // for (int scnt=0; scnt<m_pConDens->SamplesNum; scnt++) { // VERBOSE2(3, "%d: %f ", scnt, m_sample_confidences[scnt]); // } ASSERT(best_sample_indx!=-1); ASSERT(best_sample_indx==m_pConDens->SamplesNum-1); CondensState best_sample_state; best_sample_state.x = m_pConDens->flSamples[best_sample_indx][0]; best_sample_state.y = m_pConDens->flSamples[best_sample_indx][2]; best_sample_state.vx = m_pConDens->flSamples[best_sample_indx][1]; best_sample_state.vy = m_pConDens->flSamples[best_sample_indx][3]; best_sample_state.angle = m_pConDens->flSamples[best_sample_indx][4]; //VERBOSE3(3, "sample_state %f, %f, %f", // sample_state.x, sample_state.y, sample_state.angle); // VERBOSE4(3, "sample_state %f, %f, %f, %f"), // sample_state.x, sample_state.y, sample_state.vx, sample_state.vy); ASSERT(!isnan(best_sample_state.x) && !isnan(best_sample_state.y) && !isnan(best_sample_state.angle)); // probability of predicted feature locations given the KLT observation m_tmp_predicted.resize(m_features[0].size()); PredictFeatureLocations(old_lens, old_d_angles, best_sample_state, m_tmp_predicted); // // do one condensation step, then get the state prediction from Condensation; // cvConDensUpdateByTime(m_pConDens); #if 0 if (false) { // todo m_condens_state.x = max(0, min(rgbImage->width-1, m_pConDens->State[0])); m_condens_state.y = max(0, min(rgbImage->height-1, m_pConDens->State[2])); m_condens_state.vx = m_pConDens->State[1]; m_condens_state.vy = m_pConDens->State[3]; m_condens_state.angle = m_pConDens->State[4]; } else #endif { m_condens_state.x = best_sample_state.x; m_condens_state.y = best_sample_state.y; m_condens_state.vx = best_sample_state.vx; m_condens_state.vy = best_sample_state.vy ; m_condens_state.angle = best_sample_state.angle; } ASSERT(!isnan(m_condens_state.x) && !isnan(m_condens_state.y) && !isnan(m_condens_state.angle)); ASSERT(!isnan(m_condens_state.vx) && !isnan(m_condens_state.vy)); // now move the current features to where Condensation thinks they should be; // the observation is no longer needed #if 0 if (false) { // todo PredictFeatureLocations(old_lens, old_d_angles, m_condens_state, m_tmp_predicted); FollowObservationForSmallDiffs(m_tmp_predicted, m_features[curr_indx]/*observation*/, m_features[curr_indx]/*output*/, 2.0); } else #endif { PredictFeatureLocations(old_lens, old_d_angles, m_condens_state, m_features[curr_indx]); } { // 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; CvPoint2D32f avg; GetAverage(m_features[curr_indx]/*_observation*/, avg); double cx = avg.x; double cy = avg.y; 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); } }
void CCondens::ApplyCamShift( CvImage* image, bool initialize ) { CvSize size; int bins = 20; m_cCamShift.set_hist_dims( 1, &bins ); m_cCamShift.set_thresh( 0, 1, 180 ); m_cCamShift.set_min_ch_val( 1, m_params.Smin ); m_cCamShift.set_max_ch_val( 1, 255 ); m_cCamShift.set_min_ch_val( 2, m_params.Vmin ); m_cCamShift.set_max_ch_val( 2, 255 ); cvGetImageRawData( image, 0, 0, &size ); if( m_object.x > size.width - m_object.width - 1 ) m_object.x = size.width - m_object.width - 1; if( m_object.x < 0 ) m_object.x = 0; if( m_object.y > size.height - m_object.height - 1 ) m_object.y = size.height - m_object.height - 1; if( m_object.y < 0 ) m_object.y = 0; m_cCamShift.set_window(m_object); if( initialize ) { m_cCamShift.reset_histogram(); m_cCamShift.update_histogram( image ); } m_cCamShift.track_object( image ); m_object = m_cCamShift.get_window(); LBound[0] = (float)m_object.x; LBound[1] = (float)-m_object.width*0.5f; LBound[2] = (float)m_object.y; LBound[3] = (float)- m_object.height*0.5f; UBound[0] = (float)m_object.x + m_object.width; UBound[1] = (float)m_object.width*0.5f; UBound[2] = (float)m_object.y + m_object.height; UBound[3] = (float)m_object.height*0.5f; Measurement[0] = (float)m_object.x+m_object.width*0.5f; Measurement[1] = initialize ? 0 : (float)(Measurement[0] - m_Old.x); Measurement[2] = (float)m_object.y+m_object.height*0.5f; Measurement[3] = initialize ? 0 : (float)(Measurement[2] - m_Old.y); m_Old.x = cvRound( Measurement[0] ); m_Old.y = cvRound( Measurement[2] ); if( initialize ) { CvMat LB = cvMat(4,1,CV_MAT4x1_32F,LBound); CvMat UB = cvMat(4,1,CV_MAT4x1_32F,UBound); cvConDensInitSampleSet(ConDens,&LB,&UB); } XCor = 1.5f/m_object.width; VXCor = 3.0f/m_object.width; YCor = 1.5f/m_object.height; VYCor = 3.0f/m_object.height; CondProbDens(ConDens,Measurement); m_Old.x = cvRound( Measurement[0] ); m_Old.y = cvRound( Measurement[2] ); }
//パーティクルフィルタ 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; //1フレームキャプチャし,キャプチャサイズを取得する. //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); }