grasp
•All Data Structures Namespaces Functions Modules
powerGrasp.cpp
1 /* Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
2  * Authors: Ilaria Gori
3  * email: ilaria.gori@iit.it
4  * Permission is granted to copy, distribute, and/or modify this program
5  * under the terms of the GNU General Public License, version 2 or any
6  * later version published by the Free Software Foundation.
7  *
8  * A copy of the license can be found in the file LICENSE located in the
9  * root directory.
10  *
11  * This program is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
14  * Public License for more details
15 */
16 
17 #include <cmath>
18 
19 #include "powerGrasp.h"
20 
21 using namespace std;
22 using namespace pcl;
23 using namespace pcl::io;
24 using namespace yarp::os;
25 using namespace yarp::sig;
26 using namespace yarp::math;
27 using namespace yarp::dev;
28 using namespace iCub::iKin;
29 using namespace iCub::ctrl;
30 using namespace iCub::learningmachine;
31 using namespace iCub::data3D;
32 
33 
34 /************************************************************************/
35 PowerGrasp::PowerGrasp() : cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
36  cloudxyz(new pcl::PointCloud<pcl::PointXYZ>),
37  normals(new pcl::PointCloud <pcl::Normal>)
38 {
39  modality=MODALITY_AUTO;
40  path="";
41 
42  chosenPoint.resize(3,0.0);
43  chosenNormal.resize(3,0.0);
44  chosenPixel.resize(2,0.0);
45  chosenPoint[0]=-0.3;
46  chosenPoint[2]=0.3;
47 
48  offsetR.resize(3,0.0);
49  offsetL.resize(3,0.0);
50 
51  resetBools();
52  train=false;
53  visualize=true;
54  straight=true;
55  rightBlocked=false;
56  leftBlocked=false;
57  rightDisabled=false;
58  leftDisabled=false;
59  fromFileFinished=false;
60  filterCloud=true;
61  graspSpecificPoint=false;
62  writeCloud=false;
63  nFile=0;
64 
65  data.cloud=cloud;
66  data.normals=normals;
67  data.boundingBox=&boundingBox;
68  data.goodPointsIndexes=&rankIndices;
69 
70  currentState=STATE_WAIT;
71  visualizationThread=new VisualizationThread(data);
72 }
73 
74 /************************************************************************/
75 void PowerGrasp::configureGeneralInfo(ResourceFinder &rf)
76 {
77  radiusSearch=rf.check("radiusSearch",Value(0.045)).asDouble();
78  numberOfBestPoints=rf.check("numberOfBestPoints",Value(10)).asInt();
79  bestCurvature=(float)rf.check("curvature",Value(0.005)).asDouble();
80  handSize=rf.check("handSize",Value(0.08)).asDouble();
81  fingerSize=rf.check("fingerSize",Value(0.08)).asDouble();
82  string useFile=rf.check("fromFile",Value("false")).asString().c_str();
83  string useLearning=rf.check("useLearning",Value("false")).asString().c_str();
84  fromFile=(useFile=="true");
85  testWithLearning=(useLearning=="true");
86  posx=rf.check("x",Value(0)).asInt();
87  posy=rf.check("y",Value(0)).asInt();
88  visualizationThread->setPosition(posx, posy);
89  sizex=rf.check("w",Value(320)).asInt();
90  sizey=rf.check("h",Value(240)).asInt();
91  visualizationThread->setSize(sizex, sizey);
92 
93  if (!rf.check("disableRight"))
94  orientationThreadRight=new OrientationThread();
95  else
96  {
97  rightDisabled=true;
98  printf("**********RIGHT ARM DISABLED**************\n");
99  }
100  if (!rf.check("disableLeft"))
101  orientationThreadLeft=new OrientationThread();
102  else
103  {
104  leftDisabled=true;
105  printf("**********LEFT ARM DISABLED**************\n");
106  }
107 }
108 
109 /************************************************************************/
110 void PowerGrasp::configureSVM(Bottle &bottleSVM)
111 {
112  // lssvm: default values
113  scalerIn.setLowerBoundIn(0.0);
114  scalerIn.setUpperBoundIn(0.03);
115  scalerIn.setLowerBoundOut(0.0);
116  scalerIn.setUpperBoundOut(1.0);
117 
118  scalerOut.setLowerBoundIn(0.0);
119  scalerOut.setUpperBoundIn(0.6);
120  scalerOut.setLowerBoundOut(0.0);
121  scalerOut.setUpperBoundOut(1.0);
122 
123  machine.setDomainSize(1);
124  machine.setCoDomainSize(1);
125  machine.setC(20.0);
126  machine.getKernel()->setGamma(100.0);
127 
128  if (!bottleSVM.isNull())
129  {
130  if (bottleSVM.check("c"))
131  machine.setC(bottleSVM.find("c").asDouble());
132 
133  if (bottleSVM.check("gamma"))
134  machine.getKernel()->setGamma(bottleSVM.find("gamma").asDouble());
135 
136  if (Bottle *v=bottleSVM.find("in_bounds").asList())
137  {
138  if (v->size()>=2)
139  {
140  scalerIn.setLowerBoundIn(v->get(0).asDouble());
141  scalerIn.setUpperBoundIn(v->get(1).asDouble());
142  }
143  }
144 
145  if (Bottle *v=bottleSVM.find("out_bounds").asList())
146  {
147  if (v->size()>=2)
148  {
149  scalerOut.setLowerBoundIn(v->get(0).asDouble());
150  scalerOut.setUpperBoundIn(v->get(1).asDouble());
151  }
152  }
153 
154  if (Bottle *v=bottleSVM.find("machine").asList())
155  testWithLearningEnabled=machine.fromString(v->toString().c_str());
156  else
157  testWithLearningEnabled=false;
158  }
159  else
160  testWithLearningEnabled=false;
161 }
162 
163 /************************************************************************/
164 bool PowerGrasp::configure(ResourceFinder &rf)
165 {
166  configureGeneralInfo(rf);
167 
168  string name=rf.check("name",Value("power-grasp")).asString().c_str();
169 
170  int nAngles=rf.check("nAngles",Value(50)).asInt();
171  string robot=rf.check("robot",Value("icub")).asString().c_str();
172  string rightArm="right_arm";
173  string leftArm="left_arm";
174 
175  if (rightDisabled && leftDisabled)
176  {
177  printf("Both arms disabled\n");
178  return false;
179  }
180 
181  bool rightOK=true;
182  if (!rightDisabled)
183  rightOK=orientationThreadRight->open(name,rightArm,robot,nAngles);
184  bool leftOK=true;
185  if (!leftDisabled)
186  leftOK=orientationThreadLeft->open(name,leftArm,robot,nAngles);
187  if (!rightOK || !leftOK)
188  {
189  printf("Orientation threads did not open\n");
190  return false;
191  }
192 
193  outputDir=rf.find("outputDir").asString().c_str();
194 
195  if (fromFile)
196  {
197  path=rf.find("path").asString().c_str();
198  if (path=="")
199  {
200  printf("Please specify a folder to find the .ply file\n");
201  return false;
202  }
203  }
204 
205  Bottle *pR=rf.find("offsetR").asList();
206  if (pR->size()>0)
207  {
208  for (int i=0; i<pR->size(); i++)
209  offsetR[i]=pR->get(i).asDouble();
210  }
211 
212  Bottle *pL=rf.find("offsetL").asList();
213  if (pL->size()>0)
214  {
215  for (int i=0; i<pL->size(); i++)
216  offsetR[i]=pL->get(i).asDouble();
217  }
218 
219  rpc.open(("/"+name+"/rpc").c_str());
220  attach(rpc);
221 
222  if (!meshPort.open(("/"+name+"/mesh:i").c_str()))
223  return false;
224 
225  if (!areCmdPort.open(("/"+name+"/are/cmd:o").c_str()))
226  return false;
227 
228  if (!reconstructionPort.open(("/"+name+"/reconstruction").c_str()))
229  return false;
230 
231  if (!depth2kin.open(("/"+name+"/depth2kin:o").c_str()))
232  return false;
233 
234  Bottle &bMachine=rf.findGroup("lssvm");
235  configureSVM(bMachine);
236 
237  if (!rightDisabled)
238  orientationThreadRight->start();
239  if (!leftDisabled)
240  orientationThreadLeft->start();
241 
242  return true;
243 }
244 
245 /************************************************************************/
246 bool PowerGrasp::interruptModule()
247 {
248  eventRpc.signal();
249  reconstructionPort.interrupt();
250  areCmdPort.interrupt();
251  depth2kin.interrupt();
252  meshPort.interrupt();
253  rpc.interrupt();
254 
255  return true;
256 }
257 
258 /************************************************************************/
259 bool PowerGrasp::close()
260 {
261  reconstructionPort.close();
262  areCmdPort.close();
263  depth2kin.close();
264  meshPort.close();
265  rpc.close();
266 
267  if (visualizationThread->isRunning())
268  visualizationThread->stop();
269  delete visualizationThread;
270 
271  if (!rightDisabled)
272  {
273  orientationThreadRight->stop();
274  delete orientationThreadRight;
275  }
276  if (!leftDisabled)
277  {
278  orientationThreadLeft->stop();
279  delete orientationThreadLeft;
280  }
281 
282  return true;
283 }
284 
285 /************************************************************************/
286 void PowerGrasp::filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in_filtered)
287 {
288  pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
289  sor.setInputCloud (cloud_in);
290  sor.setMeanK (cloud_in->size()/2);
291  sor.setStddevMulThresh (1.0);
292  sor.filter (*cloud_in_filtered);
293 }
294 
295 /************************************************************************/
296 void PowerGrasp::write(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in, const int nFile)
297 {
298  stringstream ss;
299  ss << nFile;
300  string str = ss.str();
301  string filename=outputDir+"/cloud"+str+".ply";
302  ofstream cloudFile;
303  cloudFile.open (filename.c_str());
304  cloudFile << "ply\n";
305  cloudFile << "format ascii 1.0\n";
306  cloudFile << "element vertex " << cloud_in->size() << "\n";
307  cloudFile << "property float x\n";
308  cloudFile << "property float y\n";
309  cloudFile << "property float z\n";
310  cloudFile << "property uchar diffuse_red\n";
311  cloudFile << "property uchar diffuse_green\n";
312  cloudFile << "property uchar diffuse_blue\n";
313  cloudFile << "end_header\n";
314  for (int i=0; i<cloud_in->size(); i++)
315  {
316  cloudFile << cloud_in->at(i).x << " " << cloud_in->at(i).y << " " << cloud_in->at(i).z << " " << (int)cloud_in->at(i).r << " " << (int)cloud_in->at(i).g << " " << (int)cloud_in->at(i).b << "\n";
317  }
318  cloudFile.close();
319 }
320 
321 /************************************************************************/
322 void PowerGrasp::fromSurfaceMesh (const SurfaceMeshWithBoundingBox& msg)
323 {
324  cloud->clear();
325  cloudxyz->clear();
326 
327  for (size_t i = 0; i<msg.mesh.points.size(); ++i)
328  {
329  PointXYZRGB pointrgb;
330  pointrgb.x=msg.mesh.points.at(i).x;
331  pointrgb.y=msg.mesh.points.at(i).y;
332  pointrgb.z=msg.mesh.points.at(i).z;
333  if (i<msg.mesh.rgbColour.size())
334  {
335  int32_t rgb= msg.mesh.rgbColour.at(i).rgba;
336  pointrgb.rgba=rgb;
337  pointrgb.r = (rgb >> 16) & 0x0000ff;
338  pointrgb.g = (rgb >> 8) & 0x0000ff;
339  pointrgb.b = (rgb) & 0x0000ff;
340  }
341  else
342  pointrgb.rgb=0;
343 
344  pcl::PointXYZ point;
345  point.x=pointrgb.x;
346  point.y=pointrgb.y;
347  point.z=pointrgb.z;
348  cloudxyz->push_back(point);
349  cloud->push_back(pointrgb);
350  }
351 
352  boundingBox.setBoundingBox(msg.boundingBox);
353 }
354 
355 /************************************************************************/
356 bool PowerGrasp::fillCloudFromFile()
357 {
358  struct dirent *entry;
359  DIR *dp;
360 
361  dp = opendir(path.c_str());
362  if (dp == NULL)
363  {
364  perror("opendir");
365  return false;
366  }
367 
368  while((entry = readdir(dp)))
369  {
370  if (strcmp(entry->d_name, ".") == 0 || strcmp(entry->d_name, "..") == 0)
371  continue;
372  else
373  break;
374  }
375 
376  if (entry->d_name!=NULL)
377  {
378  pcl::PointCloud<PointXYZRGB>::Ptr cloud_in_rgb(new pcl::PointCloud<PointXYZRGB>);
379 
380  string root=path+"/";
381  string name=entry->d_name;
382  string file=root+name;
383 
384  if (loadPLYFile(file.c_str(), *cloud_in_rgb) == -1)
385  return false;
386 
387  if (filterCloud)
388  {
389  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
390  filter(cloud_in_rgb,cloud_in_filtered);
391  addPointCloud(cloud_in_filtered);
392  }
393  else
394  addPointCloud(cloud_in_rgb);
395  }
396  closedir(dp);
397  return true;
398 }
399 
400 /************************************************************************/
401 int PowerGrasp::findIndexFromCloud(const yarp::sig::Vector &point)
402 {
403  double minNorm=1e10;
404  int index=-1;
405  for (int i=0; i<cloud->size(); i++)
406  {
407  yarp::sig::Vector currentPoint=vectorFromCloud(i);
408 
409  if (norm(point-currentPoint)<minNorm)
410  {
411  index=i;
412  minNorm=norm(point-currentPoint);
413  }
414  }
415  return index;
416 }
417 
418 /************************************************************************/
419 void PowerGrasp::normalEstimation()
420 {
421  pcl::search::Search<pcl::PointXYZ>::Ptr tree=boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
422  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
423  normal_estimator.setSearchMethod (tree);
424  normal_estimator.setRadiusSearch (radiusSearch);
425  normal_estimator.setInputCloud (cloudxyz);
426  normal_estimator.compute (*normals);
427 }
428 
429 /************************************************************************/
430 void PowerGrasp::rankPoints()
431 {
432  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
433  kdtree.setInputCloud (cloudxyz);
434 
435  yarp::sig::Vector center=boundingBox.getCenter();
436 
437  std::vector<int> pointIdxRadiusSearch;
438  std::vector<float> pointRadiusSquaredDistance;
439 
440  int nGoodPoints=0;
441 
442  bool done=false;
443  //Choose best points
444  int minNeighbohrs=300;
445  while(!done)
446  {
447  rankIndices.clear();
448  rankScores.clear();
449  for (int i=0; i<cloudxyz->size(); i++)
450  {
451  int index=i;
452  int n_neighbohrs=kdtree.radiusSearch(cloudxyz->at(index),radiusSearch,pointIdxRadiusSearch,pointRadiusSquaredDistance);
453  if (n_neighbohrs>minNeighbohrs)
454  {
455  if (normalPointingOut(index,center))
456  {
457  nGoodPoints++;
458  double score=scoreFunction(index);
459  insertElement(score,index);
460  }
461  }
462  }
463  if (nGoodPoints<numberOfBestPoints && minNeighbohrs>0)
464  {
465  minNeighbohrs-=10;
466  nGoodPoints=0;
467  }
468  else
469  {
470  done=true;
471  break;
472  }
473  }
474 }
475 
476 /************************************************************************/
477 yarp::sig::Vector PowerGrasp::vectorFromNormal(const int index)
478 {
479  yarp::sig::Vector currNormal(3);
480  currNormal[0]=normals->at(index).normal_x;
481  currNormal[1]=normals->at(index).normal_y;
482  currNormal[2]=normals->at(index).normal_z;
483 
484  return currNormal;
485 }
486 
487 /************************************************************************/
488 yarp::sig::Vector PowerGrasp::vectorFromCloud(const int index)
489 {
490  yarp::sig::Vector currPoint(3);
491  currPoint[0]=cloudxyz->at(index).x;
492  currPoint[1]=cloudxyz->at(index).y;
493  currPoint[2]=cloudxyz->at(index).z;
494 
495  return currPoint;
496 }
497 
498 /************************************************************************/
499 void PowerGrasp::chooseCandidatePoints()
500 {
501  if (train)
502  {
503  double tmp=Random::uniform();
504  currentCurvature=(float)fmod(tmp,maxCurvature);
505  printf("Chosen curvature %g\n", currentCurvature);
506  printf("Max curvature %g\n", maxCurvature);
507  }
508  else
509  currentCurvature=bestCurvature;
510 
511  currentModality=modality;
512 
513  if (currentModality==MODALITY_AUTO)
514  manageModality();
515 
516  rankPoints();
517 
518  string smodality;
519  if (currentModality==MODALITY_CENTER)
520  smodality="center";
521  else if (currentModality==MODALITY_LEFT)
522  smodality="left";
523  else if (currentModality==MODALITY_RIGHT)
524  smodality="right";
525  else
526  smodality="top";
527 
528  printf("modality chosen %s\n", smodality.c_str());
529 }
530 
531 /************************************************************************/
532 void PowerGrasp::startVisualization()
533 {
534  data.chosenPoint=chosenPoint;
535  data.chosenOrientation=chosenOrientation;
536  data.hand=chosenHand;
537 
538  visualizationThread->start();
539 }
540 
541 /************************************************************************/
542 bool PowerGrasp::updateModule()
543 {
544  LockGuard lg(mutex);
545 
546  if ((fromFile && !fromFileFinished) || (currentState==STATE_ESTIMATE))
547  {
548  double totTime=Time::now();
549  if (fromFile)
550  {
551  if (!fillCloudFromFile())
552  {
553  printf("Error in reading the .ply file\n");
554  return false;
555  }
556  boundingBox=MinimumBoundingBox::getMinimumBoundingBox(cloud);
557  }
558  else
559  {
560  if (currentState==STATE_ESTIMATE)
561  {
562  if (SurfaceMeshWithBoundingBox *receivedMesh=meshPort.read())
563  fromSurfaceMesh(*receivedMesh);
564  else
565  return true;
566  }
567  }
568 
569  if (writeCloud && outputDir!="")
570  {
571  write(cloud,nFile);
572  nFile++;
573  }
574 
575  computeDim();
576 
577  normalEstimation();
578 
579  rankIndices.clear();
580  rankScores.clear();
581 
582  if (graspSpecificPoint)
583  {
584  int pointIndex;
585  pointIndex=findIndexFromCloud(chosenPoint);
586  chosenNormal=vectorFromNormal(pointIndex);
587 
588  double score=1.0;
589  insertElement(score,pointIndex);
590  }
591  else
592  chooseCandidatePoints();
593 
594  Matrix designedOrientation=eye(4,4);
595  winnerIndex=-1;
596 
597  string hand=chooseBestPointAndOrientation(winnerIndex,designedOrientation);
598 
599  yarp::sig::Vector tmp=dcm2axis(designedOrientation);
600 
601  if (hand==NO_HAND || tmp.size()==0)
602  {
603  currentState=STATE_WAIT;
604  eventRpc.signal();
605  if (fromFile)
606  fromFileFinished=true;
607  return true;
608  }
609 
610  chosenHand=hand;
611  chosenOrientation=designedOrientation;
612 
613  if (!graspSpecificPoint)
614  {
615  chosenPoint=vectorFromCloud(winnerIndex);
616  chosenNormal=vectorFromNormal(winnerIndex);
617  }
618 
619  if (visualize)
620  startVisualization();
621 
622  printf("Chosen point: %s\n", chosenPoint.toString().c_str());
623  printf("Chosen orientation: %s\n", tmp.toString().c_str());
624  printf("Chosen hand: %s\n", chosenHand.c_str());
625 
626  if (!fromFile)
627  {
628  readyToGrasp=true;
629  currentState=straight?STATE_GRASP:STATE_WAIT;
630  }
631  else
632  fromFileFinished=true;
633 
634  graspSpecificPoint=false;
635  printf("Tot time %g\n", Time::now()-totTime);
636  return true;
637  }
638 
639  if (currentState==STATE_GRASP)
640  {
641  askToGrasp();
642  currentState=STATE_WAIT;
643  eventRpc.signal();
644  return true;
645  }
646 
647  return true;
648 }
649 
650 /************************************************************************/
651 yarp::sig::Vector PowerGrasp::assignIndexToAxes(double &anglez)
652 {
653  yarp::sig::Vector indices(3); indices=-1;
654  yarp::sig::Vector refz(3); refz=0.0; refz[2]=1.0;
655  yarp::sig::Vector refy(3); refy=0.0; refy[1]=1.0;
656  yarp::sig::Vector x,y,z;
657  boundingBox.getAxis(x,y,z);
658  x=x/norm(x);
659  y=y/norm(y);
660  z=z/norm(z);
661  double dotzx=fabs(dot(x,refz));
662  double dotzy=fabs(dot(y,refz));
663  double dotzz=fabs(dot(z,refz));
664  double dotyx=fabs(dot(x,refy));
665  double dotyy=fabs(dot(y,refy));
666  double dotyz=fabs(dot(z,refy));
667 
668  if (dotzx>dotzy && dotzx>dotzz)
669  {
670  indices[2]=0;
671  anglez=acos(dotzx);
672  if (dotyy>dotyz)
673  {
674  indices[0]=2;
675  indices[1]=1;
676  }
677  else
678  {
679  indices[0]=1;
680  indices[1]=2;
681  }
682  }
683  else if (dotzy>dotzx && dotzy>dotzz)
684  {
685  indices[2]=1;
686  anglez=acos(dotzy);
687  if (dotyx>dotyz)
688  {
689  indices[0]=2;
690  indices[1]=0;
691  }
692  else
693  {
694  indices[0]=0;
695  indices[1]=2;
696  }
697  }
698  else
699  {
700  indices[2]=2;
701  anglez=acos(dotzz);
702  if (dotyx>dotyy)
703  {
704  indices[0]=1;
705  indices[1]=0;
706  }
707  else
708  {
709  indices[0]=0;
710  indices[1]=1;
711  }
712  }
713 
714  return indices;
715 }
716 
717 /************************************************************************/
718 void PowerGrasp::manageModality()
719 {
720  yarp::sig::Vector center=boundingBox.getCenter();
721  yarp::sig::Vector dim=boundingBox.getDim();
722 
723  double anglez;
724  yarp::sig::Vector indices=assignIndexToAxes(anglez);
725 
726  yarp::sig::Vector right(3); right=0.0;
727  right[0]=-0.3; right[1]=0.3;
728  yarp::sig::Vector left(3); left=0.0;
729  left[0]=-0.3; left[1]=-0.3;
730 
731  double distRight=norm(right-center);
732  double distLeft=norm(left-center);
733 
734  if (distRight/distLeft>1.8)
735  {
736  printf("left closer\n");
737  blockRightTmp=true;
738  }
739  else if (distLeft/distRight>1.8)
740  {
741  printf("right closer\n");
742  blockLeftTmp=true;
743  }
744 
745  if (dimz<handSize/2 || dimz<fingerSize)
746  {
747  printf("object too small to be taken from side %g\n", dimz);
748  currentModality=MODALITY_TOP;
749  }
750  else if ((dim[indices[2]]/dim[indices[1]]>1.8) && (dim[indices[2]]/dim[indices[0]]>1.8) && (anglez<60.0*M_PI/180.0))
751  {
752  printf("object very tall, going from side\n");
753  if (distRight/distLeft>1.3 || rightBlocked || rightDisabled || blockRightTmp)
754  currentModality=MODALITY_LEFT;
755  else if (distLeft/distRight>1.3 || leftBlocked || leftDisabled || blockLeftTmp)
756  currentModality=MODALITY_RIGHT;
757  else
758  currentModality=(int)(Random::uniform(0,1)+0.5);
759  }
760  else if ((dim[indices[0]]/dim[indices[1]]>1.5 && dim[indices[0]]/dim[indices[2]]>1.5) || (dim[indices[1]]/dim[indices[0]]>1.5 && dim[indices[1]]/dim[indices[2]]>1.5))
761  {
762  printf("center\n");
763  currentModality=MODALITY_CENTER;
764  }
765  else
766  {
767  printf("random\n");
768  Random::seed((int)Time::now()*1000);
769  if (rightDisabled || rightBlocked)
770  {
771  int tmp=(int)(Random::uniform(0,1)+0.5);
772  if (tmp==1)
773  currentModality=MODALITY_TOP;
774  else
775  currentModality=MODALITY_LEFT;
776  }
777  else if (leftDisabled || leftBlocked)
778  {
779  int tmp=(int)(Random::uniform(0,1)+0.5);
780  if (tmp==1)
781  currentModality=MODALITY_TOP;
782  else
783  currentModality=MODALITY_RIGHT;
784  }
785  else
786  currentModality=(int)(Random::uniform(0,2)+0.5);
787  }
788  if (currentModality==MODALITY_LEFT)
789  blockRightTmp=true;
790  if (currentModality==MODALITY_RIGHT)
791  blockLeftTmp=true;
792 }
793 
794 /************************************************************************/
795 yarp::sig::Vector PowerGrasp::computeApproachVector(const yarp::sig::Vector &chosenPoint)
796 {
797  yarp::sig::Vector approachVector(4); approachVector=0.0;
798  yarp::sig::Vector center=boundingBox.getCenter();
799 
800  if (currentModality==MODALITY_LEFT || currentModality==MODALITY_RIGHT)
801  {
802  if (chosenHand==LEFT_HAND)
803  {
804  approachVector[2]=0.08;
805  approachVector[3]=40.0;
806  }
807  else
808  {
809  approachVector[2]=-0.08;
810  approachVector[3]=-40.0;
811  }
812  }
813  else if (currentModality==MODALITY_TOP)
814  {
815  //if the point is much further than the center of the x, add a displacement back along x
816  if (chosenPoint[0]-center[0]<-handSize/2)
817  approachVector[0]=-0.08;
818  if (chosenHand==LEFT_HAND)
819  {
820  approachVector[2]=0.08;
821  approachVector[3]=15.0;
822  }
823  else
824  {
825  approachVector[2]=-0.08;
826  approachVector[3]=-15.0;
827  }
828  }
829 
830  return approachVector;
831 }
832 
833 /************************************************************************/
834 void PowerGrasp::askToGrasp()
835 {
836  yarp::sig::Vector approachVector=computeApproachVector(chosenPoint);
837  Bottle cmd,reply;
838  if (depth2kin.getOutputCount()>0)
839  {
840  cmd.addString("getPoint");
841  cmd.addString(chosenHand.c_str());
842  cmd.addDouble(chosenPoint[0]);
843  cmd.addDouble(chosenPoint[1]);
844  cmd.addDouble(chosenPoint[2]);
845 
846  depth2kin.write(cmd,reply);
847  if (reply.get(0).asString()=="ok")
848  chosenPoint=pointFromBottle(reply,1);
849  }
850 
851  cmd.clear(); reply.clear();
852  printf("Chosen point depth2kin %g %g %g\n", chosenPoint[0],chosenPoint[1],chosenPoint[2]);
853 
854  yarp::sig::Vector tmp=dcm2axis(chosenOrientation);
855 
856  printf("dimz %g fingerSize %g\n", dimz, fingerSize);
857 
858  if (currentModality=MODALITY_TOP && dimz<fingerSize)
859  {
860  double diff=fingerSize-dimz;
861  chosenPoint+=(chosenNormal*diff);
862  printf("dbg: chosen point %s\n", chosenPoint.toString().c_str());
863  }
864 
865  if (chosenHand=="left")
866  chosenPoint+=offsetL;
867  if (chosenHand=="right")
868  chosenPoint+=offsetR;
869 
870  cmd.addString("grasp");
871  Bottle &point=cmd.addList();
872  point.addDouble(chosenPoint[0]);
873  point.addDouble(chosenPoint[1]);
874  point.addDouble(chosenPoint[2]);
875  point.addDouble(tmp[0]);
876  point.addDouble(tmp[1]);
877  point.addDouble(tmp[2]);
878  point.addDouble(tmp[3]);
879  cmd.addString(chosenHand.c_str());
880  Bottle &approach=cmd.addList();
881  approach.addString("approach");
882  Bottle &vector=approach.addList();
883  vector.addDouble(approachVector[0]);
884  vector.addDouble(approachVector[1]);
885  vector.addDouble(approachVector[2]);
886  vector.addDouble(approachVector[3]);
887 
888  if (areCmdPort.getOutputCount()>0)
889  {
890  areCmdPort.write(cmd,reply);
891  if (reply.get(0).asString()==ACK)
892  grasped=true;
893  }
894  readyToGrasp=false;
895 }
896 
897 /************************************************************************/
898 void PowerGrasp::resetBools()
899 {
900  grasped=false;
901  readyToGrasp=false;
902  blockRightTmp=false;
903  blockLeftTmp=false;
904  noResult=false;
905  tooFar=false;
906 }
907 
908 /************************************************************************/
909 yarp::sig::Vector PowerGrasp::pointFromBottle(const Bottle &bot, const int index)
910 {
911  yarp::sig::Vector vector(3);
912  vector[0]=bot.get(index).asDouble();
913  vector[1]=bot.get(index+1).asDouble();
914  vector[2]=bot.get(index+2).asDouble();
915 
916  return vector;
917 }
918 
919 /************************************************************************/
920 bool PowerGrasp::get3DPoint(const yarp::sig::Vector &point2D, yarp::sig::Vector &point3D)
921 {
922  Bottle cmd; Bottle reply;
923  cmd.clear(); reply.clear();
924  cmd.addString("get");
925  cmd.addString("point");
926  cmd.addDouble(point2D[0]);
927  cmd.addDouble(point2D[1]);
928 
929  if (reconstructionPort.getOutputCount()>0)
930  reconstructionPort.write(cmd,reply);
931 
932  if (reply.size()>0 && reply.get(0).asString()!=NACK)
933  {
934  point3D=pointFromBottle(reply,0);
935  return true;
936  }
937  else
938  return false;
939 }
940 
941 /************************************************************************/
942 bool PowerGrasp::respond(const Bottle& command, Bottle& reply)
943 {
944  LockGuard lg(mutex);
945 
946  string tag_0=command.get(0).asString().c_str();
947  if (tag_0=="set")
948  {
949  if (command.size()<3)
950  reply.addString("command not recognized");
951  else
952  {
953  string tag_1=command.get(1).asString().c_str();
954  if (tag_1=="visualization")
955  {
956  visualize=(command.get(2).asString()=="on");
957  reply.addString(ACK);
958  }
959  else if (tag_1=="x")
960  {
961  if (command.size()>1)
962  {
963  posx=command.get(1).asInt();
964  visualizationThread->setPosition(posx,posy);
965  reply.addString(ACK);
966  }
967  }
968  else if (tag_1=="y")
969  {
970  if (command.size()>1)
971  {
972  posy=command.get(1).asInt();
973  visualizationThread->setPosition(posx,posy);
974  reply.addString(ACK);
975  }
976  }
977  else if (tag_1=="w")
978  {
979  if (command.size()>1)
980  {
981  sizex=command.get(1).asInt();
982  visualizationThread->setSize(sizex,sizey);
983  reply.addString(ACK);
984  }
985  }
986  else if (tag_1=="h")
987  {
988  if (command.size()>1)
989  {
990  sizey=command.get(1).asInt();
991  visualizationThread->setSize(sizex,sizey);
992  reply.addString(ACK);
993  }
994  }
995  else if (tag_1=="train")
996  {
997  if (command.get(2).asString()=="on")
998  {
999  train=true;
1000  testWithLearning=false;
1001  }
1002  else
1003  train=false;
1004  reply.addString(ACK);
1005  }
1006  else if (tag_1=="testWithSVM")
1007  {
1008  if (command.get(2).asString()=="on")
1009  {
1010  if (testWithLearningEnabled)
1011  {
1012  testWithLearning=true;
1013  train=false;
1014  }
1015  else
1016  printf("SVM machine not set\n");
1017  }
1018  else
1019  testWithLearning=false;
1020  reply.addString(ACK);
1021  }
1022  else if (tag_1=="offsetR")
1023  {
1024  if (command.size()<5)
1025  reply.addString("nack, check offset size");
1026  else
1027  {
1028  offsetR=pointFromBottle(command,2);
1029  reply.addString(ACK);
1030  }
1031  }
1032  else if (tag_1=="offsetL")
1033  {
1034  if (command.size()<5)
1035  reply.addString("nack, check offset size");
1036  else
1037  {
1038  offsetL=pointFromBottle(command,2);
1039  reply.addString(ACK);
1040  }
1041  }
1042  else if (tag_1=="modality")
1043  {
1044  if (command.get(2).asString()=="right")
1045  modality=MODALITY_RIGHT;
1046  else if (command.get(2).asString()=="left")
1047  modality=MODALITY_LEFT;
1048  else if (command.get(2).asString()=="center")
1049  modality=MODALITY_CENTER;
1050  else if (command.get(2).asString()=="top")
1051  modality=MODALITY_TOP;
1052  else
1053  modality=MODALITY_AUTO;
1054  reply.addString(ACK);
1055  }
1056  else if (tag_1=="filter")
1057  {
1058  filterCloud=(command.get(2).asString()=="on");
1059  reply.addString(ACK);
1060  }
1061  else if (tag_1=="write")
1062  {
1063  writeCloud=(command.get(2).asString()=="on");
1064  reply.addString(ACK);
1065  }
1066  }
1067  }
1068  else if (tag_0=="help")
1069  {
1070  reply.addString("set visualization on/off");
1071  reply.addString("set x");
1072  reply.addString("set y");
1073  reply.addString("set w");
1074  reply.addString("set h");
1075  reply.addString("set train on/off");
1076  reply.addString("set testWithSVM on/off");
1077  reply.addString("set offsetL x y z");
1078  reply.addString("set offsetR x y z");
1079  reply.addString("set modality right/left/top/center");
1080  reply.addString("set filter on/off");
1081  reply.addString("set write on/off");
1082  reply.addString("block right/left");
1083  reply.addString("unblock right/left");
1084  reply.addString("grasp (x y) [wait]");
1085  reply.addString("go");
1086  reply.addString("dont");
1087  reply.addString("isGrasped");
1088  }
1089  else if (tag_0=="go")
1090  {
1091  if (readyToGrasp)
1092  {
1093  currentState=STATE_GRASP;
1094  eventRpc.reset();
1095  eventRpc.wait();
1096  reply.addString(ACK);
1097  }
1098  }
1099  else if (tag_0=="block")
1100  {
1101  if (command.size()>=2)
1102  {
1103  if (command.get(1).asString()=="right")
1104  rightBlocked=true;
1105  else
1106  leftBlocked=true;
1107  reply.addString(ACK);
1108  }
1109  }
1110  else if (tag_0=="unblock")
1111  {
1112  if (command.size()>=2)
1113  {
1114  if (command.get(1).asString()=="right")
1115  rightBlocked=false;
1116  else
1117  leftBlocked=false;
1118  reply.addString(ACK);
1119  }
1120  }
1121  else if (tag_0=="isGrasped")
1122  reply.addString(grasped?ACK:NACK);
1123  else if (tag_0=="dont")
1124  {
1125  resetBools();
1126  currentState=STATE_WAIT;
1127  reply.addString(ACK);
1128  }
1129  else if (tag_0=="grasp")
1130  {
1131  if ((currentState!=STATE_ESTIMATE) && (currentState!=STATE_GRASP))
1132  {
1133  if (Bottle *pos=command.get(1).asList())
1134  {
1135  if (pos->size()>=2)
1136  {
1137  chosenPixel[0]=pos->get(0).asInt();
1138  chosenPixel[1]=pos->get(1).asInt();
1139 
1140  Bottle cmd1,rep1;
1141  cmd1.addInt(chosenPixel[0]);
1142  cmd1.addInt(chosenPixel[1]);
1143 
1144  if (reconstructionPort.getOutputCount()>0)
1145  reconstructionPort.write(cmd1,rep1);
1146 
1147  if (visualizationThread->isRunning())
1148  visualizationThread->stop();
1149 
1150  resetBools();
1151 
1152  if (train && (command.size()>1))
1153  currentCurvature=(float)command.get(1).asDouble();
1154 
1155  if (pos->size()>2)
1156  {
1157  if (pos->get(2).asString()=="point")
1158  {
1159  if (get3DPoint(chosenPixel,chosenPoint))
1160  graspSpecificPoint=true;
1161  else
1162  {
1163  reply.addString(NACK);
1164  return true;
1165  }
1166  }
1167  }
1168 
1169  Bottle cmd2,rep2;
1170  cmd2.addString("3Drec");
1171  if (visualize)
1172  cmd2.addString("visualize");
1173 
1174  if (reconstructionPort.getOutputCount()>0)
1175  reconstructionPort.write(cmd2,rep2);
1176 
1177  if ((rep2.size()>0) && (rep2.get(0).asString()==ACK))
1178  {
1179  currentState=STATE_ESTIMATE;
1180  straight=true;
1181  if (command.size()>2)
1182  straight=(command.get(2).asString()!="wait");
1183  if (straight)
1184  {
1185  eventRpc.reset();
1186  eventRpc.wait();
1187  }
1188 
1189  if (noResult)
1190  {
1191  reply.addString(NACK);
1192  reply.addString("no_result");
1193  noResult=false;
1194  }
1195  else if (tooFar)
1196  {
1197  reply.addString(NACK);
1198  reply.addString("too_far");
1199  tooFar=false;
1200  }
1201  else
1202  reply.addString(ACK);
1203  }
1204  }
1205  }
1206  }
1207  }
1208 
1209  if (reply.size()==0)
1210  reply.addString(NACK);
1211 
1212  return true;
1213 }
1214 
1215 /************************************************************************/
1216 yarp::sig::Vector PowerGrasp::findBiggestAxis(int &ind)
1217 {
1218  yarp::sig::Vector biggestAxis;
1219  yarp::sig::Vector vx,vy,vz;
1220  boundingBox.getAxis(vx,vy,vz);
1221 
1222  double normx=norm(vx);
1223  double normy=norm(vy);
1224  double normz=norm(vz);
1225 
1226  int indB;
1227  if (normx>normy && normx>normz)
1228  {
1229  biggestAxis=vx;
1230  indB=0;
1231  }
1232  else if (normy>normx && normy>normz)
1233  {
1234  biggestAxis=vy;
1235  indB=1;
1236  }
1237  else
1238  {
1239  biggestAxis=vz;
1240  indB=2;
1241  }
1242 
1243  biggestAxis/=norm(biggestAxis);
1244  return biggestAxis;
1245 }
1246 
1247 /************************************************************************/
1248 string PowerGrasp::chooseBestPointAndOrientation(int &winnerIndex, yarp::sig::Matrix &designedOrientation)
1249 {
1250  yarp::sig::Vector center(3), eePos(3);
1251  yarp::sig::Vector pyRight(3), pxRight(3), pyLeft(3), pxLeft(3);
1252  yarp::sig::Vector pointNormal(3), normalRight(3), normalLeft(3);
1253  yarp::sig::Vector tmpRight(3), tmpLeft(3);
1254  yarp::sig::Vector xhandaxisRight(3), xhandaxisLeft(3);
1255  Matrix orientationRight, orientationLeft;
1256  Matrix tmpOrientationRight, tmpOrientationLeft;
1257  double rightMan,leftMan;
1258  double bestRightMan=0.0;
1259  double bestLeftMan=0.0;
1260  int rightIndex=-1;
1261  int leftIndex=-1;
1262 
1263  yarp::sig::Vector x(3);
1264  x[0]=-1.0;
1265  x[1]=0.0;
1266  x[2]=0.0;
1267 
1268  string hand=NO_HAND;
1269 
1270  int indB;
1271  yarp::sig::Vector biggestAxis=findBiggestAxis(indB);
1272 
1273  if (!rightDisabled)
1274  {
1275  orientationThreadRight->reset();
1276  //if (areCmdPort.getOutputCount()==0)
1277  orientationThreadRight->preAskForPose();
1278  }
1279  if (!leftDisabled)
1280  {
1281  orientationThreadLeft->reset();
1282  //if (areCmdPort.getOutputCount()==0)
1283  orientationThreadLeft->preAskForPose();
1284  }
1285  for (int i=0; i<rankScores.size(); i++)
1286  {
1287  pxRight=0.0;
1288 
1289  pointNormal=vectorFromNormal(rankIndices[i]);
1290  normalRight=-1.0*pointNormal;
1291  normalLeft=pointNormal;
1292 
1293  tmpRight=0.0;
1294  tmpRight=cross(x,normalRight);
1295  pxRight=cross(normalRight,tmpRight);
1296  pxRight=pxRight/norm(pxRight);
1297 
1298  tmpLeft=0.0;
1299  tmpLeft=cross(x,normalLeft);
1300  pxLeft=cross(normalLeft,tmpLeft);
1301  pxLeft=pxLeft/norm(pxLeft);
1302 
1303  pyRight=cross(normalRight,pxRight);
1304  pyRight=pyRight/norm(pyRight);
1305 
1306  pyLeft=cross(normalLeft,pxLeft);
1307  pyLeft=pyLeft/norm(pyLeft);
1308 
1309  center=vectorFromCloud(rankIndices[i])+pointNormal;
1310  eePos=vectorFromCloud(rankIndices[i]);
1311 
1312  yarp::sig::Vector tmpBiggest=biggestAxis;
1313 
1314  if (!rightBlocked && !blockRightTmp && !rightDisabled)
1315  {
1316  orientationThreadRight->setInfo(eePos,pxRight,pyRight,normalRight,center,tmpBiggest);
1317  orientationThreadRight->resume();
1318  }
1319  if (!leftBlocked && !blockLeftTmp && !leftDisabled)
1320  {
1321  orientationThreadLeft->setInfo(eePos,pxLeft,pyLeft,normalLeft,center,tmpBiggest);
1322  orientationThreadLeft->resume();
1323  }
1324 
1325  if (!rightDisabled)
1326  {
1327  while (!orientationThreadRight->checkDone())
1328  {
1329  Time::delay(0.01);
1330  }
1331  }
1332 
1333  if (!leftDisabled)
1334  {
1335  while (!orientationThreadLeft->checkDone())
1336  {
1337  Time::delay(0.01);
1338  }
1339  }
1340 
1341  if (!rightBlocked && !blockRightTmp && !rightDisabled)
1342  {
1343  orientationThreadRight->getBestManip(rightMan,tmpOrientationRight);
1344  if (rightMan>bestRightMan)
1345  {
1346  bestRightMan=rightMan;
1347  orientationRight=tmpOrientationRight;
1348  rightIndex=i;
1349  }
1350  }
1351 
1352  if (!leftBlocked && !blockLeftTmp && !leftDisabled)
1353  {
1354  orientationThreadLeft->getBestManip(leftMan,tmpOrientationLeft);
1355  if (leftMan>bestLeftMan)
1356  {
1357  bestLeftMan=leftMan;
1358  orientationLeft=tmpOrientationLeft;
1359  leftIndex=i;
1360  }
1361  }
1362  }
1363 
1364  bool noResultR=true;
1365  bool noResultL=true;
1366  if (!rightBlocked && !blockRightTmp && !rightDisabled)
1367  {
1368  //if (areCmdPort.getOutputCount()==0)
1369  orientationThreadRight->postAskForPose();
1370  noResultR=orientationThreadRight->getResult();
1371  }
1372  if (!leftBlocked && !blockLeftTmp && !leftDisabled)
1373  {
1374  //if (areCmdPort.getOutputCount()==0)
1375  orientationThreadLeft->postAskForPose();
1376  noResultL=orientationThreadLeft->getResult();
1377  }
1378 
1379  if (noResultR && noResultL)
1380  {
1381  noResult=true;
1382  return hand;
1383  }
1384  if (bestLeftMan==0.0 && bestRightMan==0.0)
1385  {
1386  tooFar=true;
1387  return hand;
1388  }
1389 
1390  if (rightBlocked || blockRightTmp || bestLeftMan>bestRightMan)
1391  {
1392  winnerIndex=rankIndices[leftIndex];
1393  designedOrientation=orientationLeft;
1394  hand=LEFT_HAND;
1395  }
1396 
1397  if (leftBlocked || blockLeftTmp || bestRightMan>bestLeftMan)
1398  {
1399  winnerIndex=rankIndices[rightIndex];
1400  designedOrientation=orientationRight;
1401  hand=RIGHT_HAND;
1402  }
1403 
1404  return hand;
1405 }
1406 
1407 /************************************************************************/
1408 void PowerGrasp::addPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in)
1409 {
1410  cloud->clear();
1411  cloudxyz->clear();
1412 
1413  for (size_t j = 0; j < cloud_in->points.size (); ++j)
1414  {
1415  PointXYZRGB pointrgb=cloud_in->points[j];
1416  pcl::PointXYZ point;
1417  point.x=pointrgb.x;
1418  point.y=pointrgb.y;
1419  point.z=pointrgb.z;
1420  cloudxyz->push_back(point);
1421  cloud->push_back(pointrgb);
1422  }
1423 }
1424 
1425 /************************************************************************/
1426 double PowerGrasp::getPeriod()
1427 {
1428  return 0.1;
1429 }
1430 
1431 /************************************************************************/
1432 double PowerGrasp::scoreFunction(const int index)
1433 {
1434  double score=0.0;
1435 
1436  yarp::sig::Vector point=vectorFromCloud(index);
1437  yarp::sig::Vector center=boundingBox.getCenter();
1438 
1439  if (point[2]<center[2])
1440  return 0.0;
1441 
1442  yarp::sig::Vector normal=vectorFromNormal(index);
1443  float curvature=normals->at(index).curvature;
1444 
1445  yarp::sig::Vector x,y,z;
1446  boundingBox.getAxis(x,y,z);
1447  x/=norm(x); y/=norm(y); z/=norm(z);
1448 
1449  yarp::sig::Vector dim=boundingBox.getDim();
1450  double minDim;
1451  if (dim[0]<dim[1])
1452  minDim=dim[0];
1453  else
1454  minDim=dim[1];
1455  if (dim[2]<minDim)
1456  minDim=dim[2];
1457 
1458  int count=0;
1459  for (int i=0; i<boundingBox.getCorners().size(); i++)
1460  {
1461  iCub::data3D::PointXYZ p=boundingBox.getCorners().at(i);
1462  yarp::sig::Vector tmp(3);
1463  tmp[0]=p.x;
1464  tmp[1]=p.y;
1465  tmp[2]=p.z;
1466  if (norm(tmp-point)>minDim/2)
1467  count++;
1468  }
1469 
1470  if (count==8)
1471  score+=1.0;
1472 
1473  if (train)
1474  return exp(-(fabs(curvature-currentCurvature)/maxCurvature))/2;
1475 
1476  if (testWithLearning)
1477  {
1478  Vector in(1,scalerIn.transform(curvature));
1479  Prediction prediction=machine.predict(in);
1480  Vector out=prediction.getPrediction();
1481  score+=scalerOut.unTransform(out[0]);
1482  }
1483  else
1484  score+=exp(-(fabs(curvature-currentCurvature)/maxCurvature))/2;
1485 
1486  if (currentModality==MODALITY_TOP)
1487  {
1488  if (point[2]-center[2]>0.0)
1489  return score+fabs(point[2]-center[2])/dimz;
1490  else
1491  return score;
1492  }
1493  else if (currentModality==MODALITY_RIGHT)
1494  {
1495  if (point[1]-center[1]>0.0)
1496  {
1497  yarp::sig::Vector tmp=center;
1498  tmp[1]=center[1]+(dimy/2);
1499  return score+exp(-(norm(point-tmp)/dimy))/2;
1500  }
1501  else
1502  return score;
1503  }
1504  else if (currentModality==MODALITY_LEFT)
1505  {
1506  if (point[1]-center[1]<0.0)
1507  {
1508  yarp::sig::Vector tmp=center;
1509  tmp[1]=center[1]-(dimy/2);
1510  return score+exp(-(norm(point-tmp)/dimy))/2;
1511  }
1512  else
1513  return score;
1514  }
1515  else
1516  {
1517  yarp::sig::Vector tmp(3);
1518  tmp=center;
1519  tmp[2]=center[2]+(dimz/2);
1520 
1521  return score+=exp(-(norm(point-tmp)/dimz))/2;
1522  }
1523 }
1524 
1525 /************************************************************************/
1526 void PowerGrasp::insertElement(const double score, const int index)
1527 {
1528  if (rankScores.size()==0)
1529  {
1530  rankScores.push_back(score);
1531  rankIndices.push_back(index);
1532  }
1533  else if (rankScores.size()<numberOfBestPoints)
1534  {
1535  bool assigned=false;
1536  std::vector<int>::iterator itind=rankIndices.begin();
1537  for (std::vector<double>::iterator itsc = rankScores.begin(); itsc!=rankScores.end(); itsc++)
1538  {
1539  if (*itsc<score)
1540  {
1541  rankScores.insert(itsc,score);
1542  rankIndices.insert(itind,index);
1543  assigned=true;
1544  break;
1545  }
1546  itind++;
1547  }
1548  if (!assigned)
1549  {
1550  rankScores.push_back(score);
1551  rankIndices.push_back(index);
1552  }
1553  }
1554  else
1555  {
1556  if (rankScores[rankScores.size()-1]>score)
1557  {
1558  return;
1559  }
1560  else if (rankScores[0]<score)
1561  {
1562  std::vector<double>::iterator itsc=rankScores.begin();
1563  std::vector<int>::iterator itind=rankIndices.begin();
1564  rankScores.insert(itsc,score);
1565  rankIndices.insert(itind,index);
1566  rankScores.pop_back();
1567  rankIndices.pop_back();
1568  }
1569  else
1570  {
1571  std::vector<int>::iterator itind=rankIndices.begin();
1572  for (std::vector<double>::iterator itsc = rankScores.begin(); itsc!=rankScores.end(); itsc++)
1573  {
1574  if (*itsc<score)
1575  {
1576  rankScores.insert(itsc,score);
1577  rankIndices.insert(itind,index);
1578  rankScores.pop_back();
1579  rankIndices.pop_back();
1580  break;
1581  }
1582  itind++;
1583  }
1584  }
1585  }
1586 }
1587 
1588 /************************************************************************/
1589 bool PowerGrasp::normalPointingOut(const int index, const yarp::sig::Vector &center)
1590 {
1591  yarp::sig::Vector p=vectorFromCloud(index);
1592  yarp::sig::Vector n=vectorFromNormal(index);
1593 
1594  yarp::sig::Vector fromCenter=(center-p)/norm(center-p);
1595  n=n/norm(n);
1596 
1597  return (dot(n,fromCenter)<0);
1598 }
1599 
1600 /************************************************************************/
1601 void PowerGrasp::computeDim()
1602 {
1603  maxy=-1.0;
1604  maxz=-1.0;
1605  double miny=1.0;
1606  double minz=1.0;
1607  for (int i=0; i<cloudxyz->size(); i++)
1608  {
1609  pcl::PointXYZ point=cloudxyz->at(i);
1610 
1611  if (point.y>maxy)
1612  maxy=point.y;
1613  if (point.z>maxz)
1614  maxz=point.z;
1615  if (point.y<miny)
1616  miny=point.y;
1617  if (point.z<minz)
1618  minz=point.z;
1619  }
1620 
1621  printf("maxz %g minz %g max-min %g\n", maxz, minz, maxz-minz);
1622  dimz=maxz-minz;
1623  dimy=maxy-miny;
1624 }
1625 
1626 
iCub::data3D::MinimumBoundingBox::getMinimumBoundingBox
iCub::data3D::BoundingBox getMinimumBoundingBox(pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud)
Given a point cloud (defined in the Point Cloud Library), it computes the minimum enclosing bounding ...
Definition: minBoundBox.cpp:212
Generated by   doxygen 1.8.12