bool KITECH_MonteCarloLocalizationComp::Update(std::vector<double> sensorValue) { double *laserData = new double [sensorValue.size()+5]; double sum_prob = 0.; memcpy (laserData, &sensorValue[0], sizeof (double) * sensorValue.size()); for (int i = 0, n = particles.size(); i < n; ++i) { VirtualRobot &vr = particles[i]; double prob = (epsilon < vr.prob) ? GetProbability (vr, laserData) : epsilon; vr.prob *= prob; sum_prob += vr.prob; } if (epsilon < sum_prob) { for (int i = 0, n = particles.size(); i < n; ++i) { particles[i].prob /= sum_prob; } } else { for (int i = 0, n = particles.size(); i < n; ++i) { particles[i].prob = 1./n; } } Resampling (); if (particles.size () <= 0) { RandomizeAt (currentPosition.x, currentPosition.y, 2.5); } delete [] laserData; return false; }
int amain(void) { Mat* image_array; Mat* Contour_out_image_array = new Mat[number_of_image]; Mat* Cutting_image_array = new Mat[number_of_image]; Mat* Resampled_image_array = new Mat[number_of_image]; int i; int height=0; int width=0; int period=0; double Bounding_box_ratio[number_of_image]; vector<Point> contour_point_array; vector<Point> refer_point; vector<vector<Point> > Segment; // Image_open; image_array = Image_open(number_of_image); // Loop for(i=1; i<number_of_image; i++) { // Cutting the silhouette area and Calculating the width&height ratio Cutting_image_array[i-1] = Cutting_silhouette_area(&image_array[i-1], &height, &width); Bounding_box_ratio[i-1] = (double)height/(double)width; // Contour extraction Contour_out_image_array[i-1] = Contour(&Cutting_image_array[i-1],&contour_point_array); // Resampling refer_point = Find_refer_point(contour_point_array); Segment = Resampling(&contour_point_array,&refer_point); Resampled_image_array[i-1] = Draw_Resampling(Segment,Contour_out_image_array[i-1]); imshow("Original", image_array[i-1]); waitKey(0); imshow("Cutting_image",Cutting_image_array[i-1]); waitKey(0); imshow("Contour_image",Contour_out_image_array[i-1]); waitKey(0); imshow("Resampling_image",Resampled_image_array[i-1]); waitKey(0); } Image_save(Resampled_image_array); //period = Gait_period_cal(number_of_image, Bounding_box_ratio); return 0; }
void Patch::resampling(const DoublyConnectedEdgeList &dcel) { Resampling(dcel).transform(*this->mesh); this->updateCentroid(); }