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();
}
