17 #include "psoThread.h" 29 PsoThread::PsoThread() : RateThread(20), cloud(new
pcl::PointCloud<
pcl::PointXYZ>),
30 normals (new
pcl::PointCloud <
pcl::Normal>)
43 bool PsoThread::open(
const Property &options)
45 Property &opt=
const_cast<Property&
>(options);
46 particles=opt.check(
"particles",Value(20)).asInt();
47 omega=opt.check(
"omega",Value(1.0)).asDouble();
48 phi_p=opt.check(
"phi_p",Value(0.02)).asDouble();
49 phi_g=opt.check(
"phi_g",Value(0.02)).asDouble();
50 iterations=opt.check(
"iterations",Value(300)).asInt();
51 limit_finger_max=opt.check(
"limit_finger_max",Value(0.08)).asDouble();
52 limit_finger_min=opt.check(
"limit_finger_min",Value(0.02)).asDouble();
53 hand_area=opt.check(
"hand_area",Value(0.00225)).asDouble();
54 dimension=opt.check(
"dimension",Value(9)).asInt();
55 vmax=opt.check(
"vmax",Value(0.3)).asDouble();
56 vmin=opt.check(
"vmin",Value(-0.3)).asDouble();
58 nsg.resize(dimension);
66 void PsoThread::setData(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud <pcl::Normal>::Ptr normals,
double alpha,
int overlapping_cones)
69 this->normals->clear();
70 copyPointCloud(*cloud,*this->cloud);
71 copyPointCloud(*normals,*this->normals);
72 this->overlapping_cones=overlapping_cones;
86 int seed=(int)Time::now()*1000;
88 kdtree.setInputCloud(cloud);
99 double time=Time::now();
102 for (
int i=0; i<cloud->size(); i++)
104 for (
int j=i+1; j<cloud->size(); j++)
106 for (
int k=j+1; k<cloud->size(); k++)
108 yarp::sig::Vector ids(3); ids[0]=i; ids[1]=j; ids[2]=k;
109 yarp::sig::Vector p=assignPoints(ids);
110 yarp::sig::Vector n=assignNormals(ids);
112 double f=fitness(triplet);
123 printf(
"Time exhaustive search %g\n", Time::now()-time);
124 printf(
"best fg %g\n", fg);
126 else if (rand_method==3)
129 int iterationsRandom=6000;
131 while (iter<=iterationsRandom && fg>cost)
133 yarp::sig::Vector ids(3);
135 yarp::sig::Vector p=assignPoints(ids);
136 yarp::sig::Vector n=assignNormals(ids);
138 double f=fitness(triplet);
149 printf(
"iteration %i\n", iter);
150 printf(
"fg %g\n", fg);
156 for (
int i=0; i<cloud->size(); i++)
158 for (
int j=i+1; j<cloud->size(); j++)
160 for (
int k=j+1; k<cloud->size(); k++)
162 yarp::sig::Vector tmp(3);
163 tmp[0]=i; tmp[1]=j; tmp[2]=k;
164 visited.push_back(tmp);
168 printf(
"time for structure %d %g\n", overlapping_cones, Time::now()-time);
171 double tot=iterations;
176 double time1=Time::now();
177 while (iter<=iterations && fg>cost)
179 for (
int i=0; i<particles; i++)
181 yarp::sig::Vector ids(3,0.0);
186 int id=Random::uniform(0,visited.size()-1);
188 visited.erase(visited.begin()+id);
195 for (
int j=0; j<dimension; j++)
197 double rand_p=phi_p*Random::uniform();
198 double rand_g=phi_g*Random::uniform();
199 double velocity=omega*v.at(i)[j]+(rand_p*(p.at(i)[j]-x.at(i)[j]))+(rand_g*(g[j]-x.at(i)[j]));
202 else if (velocity>vmax)
208 ids=findTriplet(x.at(i));
211 x.at(i)=assignPoints(ids);
212 ns.at(i)=assignNormals(ids);
214 double f=fitness(triplet);
228 printf(
"Time inizialization %d %g\n", overlapping_cones, Time::now()-time1);
232 omega=(1-(counter/tot));
239 if ((
int)iter%50==0 &&
set)
242 for (
int j=0; j<x.size(); j++)
243 distances+=norm(x.at(j)-g);
248 tot=iterations-iter+1;
255 printf(
"Time pso %d %g\n", overlapping_cones, Time::now()-time1);
256 printf(
"iteration %g\n", iter);
257 printf(
"fg %g\n", fg);
266 yarp::sig::Vector PsoThread::findRandomId()
268 yarp::sig::Vector ids(3);
272 yarp::sig::Vector tmp(3);
273 tmp[0]=Random::uniform(0,cloud->size()-1);
274 tmp[1]=Random::uniform(0,cloud->size()-2);
275 tmp[2]=Random::uniform(0,cloud->size()-3);
276 ids=findDifferentIds(tmp);
277 if (visited.size()==0)
279 visited.push_back(ids);
285 for (std::vector<yarp::sig::Vector>::iterator it=visited.begin(); it!=visited.end(); it++)
287 if ((*it)[0]==ids[0] && (*it)[1]==ids[1] && (*it)[2]==ids[2])
296 visited.push_back(ids);
304 void PsoThread::initialization()
306 yarp::sig::Vector idx(3);
308 for (
int i=0; i<particles; i++)
312 int id=Random::uniform(0,visited.size()-1);
314 visited.erase(visited.begin()+id);
321 x.push_back(assignPoints(idx));
322 p.push_back(assignPoints(idx));
323 ns.push_back(assignNormals(idx));
324 yarp::sig::Vector tmp(dimension);
325 for (
int j=0; j<tmp.size(); j++)
326 tmp[j]=fmod(Random::uniform(),vmax)*pow(-1.0,Random::uniform(1,2));
331 x.at(i)=assignPoints(idx);
332 ns.at(i)=assignNormals(idx);
333 for (
int j=0; j<dimension; j++)
334 v.at(i)[j]=fmod(Random::uniform(),vmax)*pow(-1.0,Random::uniform(1,2));
341 bool PsoThread::isSuccessful()
350 bool a=isForceClosure(triplet);
351 printf(
"fc %d %s\n", overlapping_cones, a?
"yes":
"no");
354 yarp::sig::Vector angles(3);
355 yarp::sig::Vector idx(3);
356 if (!counterOverlap(triplet.n1,triplet.n2,angles[0]))
358 if (!counterOverlap(triplet.n1,triplet.n3,angles[1]))
360 if (!counterOverlap(triplet.n2,triplet.n3,angles[2]))
362 printf(
"ov cones %d %d\n", overlapping_cones, n_cones);
363 triplet.ov_cones=n_cones;
365 yarp::sig::Vector n1=triplet.n1/norm(triplet.n1)*0.003;
366 yarp::sig::Vector n2=triplet.n2/norm(triplet.n2)*0.003;
367 yarp::sig::Vector n3=triplet.n3/norm(triplet.n3)*0.003;
377 double PsoThread::getCost()
383 ContactPoints PsoThread::fillContactPoints(yarp::sig::Vector &xi, yarp::sig::Vector &nsi)
385 yarp::sig::Vector c1(3),c2(3),c3(3),n1(3),n2(3),n3(3);
413 triplet.alpha1=alpha;
414 triplet.alpha2=alpha;
415 triplet.alpha3=alpha;
424 double dist1=norm(triplet.c1-triplet.c2);
425 double dist2=norm(triplet.c1-triplet.c3);
426 double dist3=norm(triplet.c2-triplet.c3);
428 if (dist1>limit_finger_max || dist2>limit_finger_max || dist3>limit_finger_max)
430 if (dist1<0.01 || dist2<0.01 || dist3<0.01)
432 if (dist1>limit_finger_min && dist2>limit_finger_min && dist3>limit_finger_min)
435 if (!isForceClosure(triplet))
439 yarp::sig::Vector angles(3);
440 yarp::sig::Vector idx(3);
441 if (!counterOverlap(triplet.n1,triplet.n2,angles[0]))
448 if (!counterOverlap(triplet.n1,triplet.n3,angles[1]))
455 if (!counterOverlap(triplet.n2,triplet.n3,angles[2]))
463 if (n_cones!=overlapping_cones)
464 f+=penaltyCones(n_cones,idx,angles);
466 double diff=(double)(abs(overlapping_cones-n_cones));
467 double w=1e-01*(pow(2,diff)+1);
468 double current_area=triangleArea(triplet.c1,triplet.c2,triplet.c3);
471 if (current_area>hand_area)
472 f+=1e03*current_area;
476 double diff_area2=(current_area-hand_area)*(current_area-hand_area);
477 double diff_area4=diff_area2*diff_area2;
478 f+=w*1e11*abs(diff_area4);
485 double PsoThread::penaltyCones(
int n_cones, yarp::sig::Vector &idx, yarp::sig::Vector &angles)
488 int diff=overlapping_cones-n_cones;
497 double w=abs(diff*diff*diff);
498 std::vector<double> missingAngles;
499 for (
int i=0; i<idx.size(); i++)
502 missingAngles.push_back(angles[i]);
504 std::sort(missingAngles.begin(), missingAngles.end());
505 for (
int i=0; i<abs(diff); i++)
506 f+=(w*missingAngles[i]);
511 double PsoThread::triangleArea(yarp::sig::Vector &p1, yarp::sig::Vector &p2, yarp::sig::Vector &p3)
513 return norm(cross(p2-p1,p3-p1))/2;
517 bool PsoThread::counterOverlap(yarp::sig::Vector &n1, yarp::sig::Vector &n2,
double &angle)
522 double theta=abs(M_PI-acos(dot(n1,n2)));
523 if (theta<2*this->alpha)
525 angle=(2*this->alpha)-theta;
530 angle=theta-(2*this->alpha);
536 yarp::sig::Vector PsoThread::assignPoints(
const yarp::sig::Vector &ids)
538 yarp::sig::Vector res(dimension);
539 res[0]=cloud->at(ids[0]).x;
540 res[1]=cloud->at(ids[0]).y;
541 res[2]=cloud->at(ids[0]).z;
542 res[3]=cloud->at(ids[1]).x;
543 res[4]=cloud->at(ids[1]).y;
544 res[5]=cloud->at(ids[1]).z;
545 res[6]=cloud->at(ids[2]).x;
546 res[7]=cloud->at(ids[2]).y;
547 res[8]=cloud->at(ids[2]).z;
552 yarp::sig::Vector PsoThread::assignNormals(
const yarp::sig::Vector &ids)
554 yarp::sig::Vector res(dimension);
555 res[0]=normals->at(ids[0]).normal_x;
556 res[1]=normals->at(ids[0]).normal_y;
557 res[2]=normals->at(ids[0]).normal_z;
558 res[3]=normals->at(ids[1]).normal_x;
559 res[4]=normals->at(ids[1]).normal_y;
560 res[5]=normals->at(ids[1]).normal_z;
561 res[6]=normals->at(ids[2]).normal_x;
562 res[7]=normals->at(ids[2]).normal_y;
563 res[8]=normals->at(ids[2]).normal_z;
568 int PsoThread::findClosestPoint(pcl::PointXYZ &point)
571 std::vector<int> pointIdxNKNSearch;
572 std::vector<float> pointNKNSquaredDistance;
575 if (kdtree.nearestKSearch (point, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
576 id=pointIdxNKNSearch[0];
583 yarp::sig::Vector PsoThread::findTriplet(
const yarp::sig::Vector &vect)
585 pcl::PointXYZ p1(vect[0],vect[1],vect[2]);
586 pcl::PointXYZ p2(vect[3],vect[4],vect[5]);
587 pcl::PointXYZ p3(vect[6],vect[7],vect[8]);
589 yarp::sig::Vector ids(3);
591 ids[0]=findClosestPoint(p1);
592 ids[1]=findClosestPoint(p2);
593 ids[2]=findClosestPoint(p3);
595 if (ids[0]!=ids[1] && ids[0]!=ids[2] && ids[1]!=ids[2])
601 int id=Random::uniform(0,visited.size()-1);
603 visited.erase(visited.begin()+id);
607 yarp::sig::Vector tmp(3);
608 tmp[0]=Random::uniform(0,cloud->size()-1);
609 tmp[1]=Random::uniform(0,cloud->size()-2);
610 tmp[2]=Random::uniform(0,cloud->size()-3);
611 ids=findDifferentIds(tmp);
618 yarp::sig::Vector PsoThread::findDifferentIds(
const yarp::sig::Vector &ids)
620 yarp::sig::Vector idsNew(3);
640 else if (ids[2]>=small)
643 idsNew[2]=idsNew[2]+1;
649 bool PsoThread::checkDone()
655 void PsoThread::close()
661 void PsoThread::threadRelease()