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;
//１フレームキャプチャし，キャプチャサイズを取得する．
//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);
}
