grasp
•All Data Structures Namespaces Functions Modules
objectReconstr.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 <objectReconstr.h>
18 
19 #include <opencv2/opencv.hpp>
20 
21 using namespace std;
22 using namespace yarp::os;
23 using namespace iCub::data3D;
24 
25 /************************************************************************/
26 ObjectReconstr::ObjectReconstr()
27 {
28  currentState=STATE_WAIT;
29  write=false;
30  visualizationOn=false;
31  closing=false;
32  number=0;
33 }
34 
35 /************************************************************************/
36 bool ObjectReconstr::configure(ResourceFinder &rf)
37 {
38  string robot=rf.check("robot",Value("icub")).asString().c_str();
39  string name=rf.check("name",Value("object-reconstruction")).asString().c_str();
40  setName(name.c_str());
41  outputDir=rf.getHomeContextPath().c_str();
42  computeBB=rf.check("computeBB",Value(false)).asBool();
43 
44  middlex=-1;
45  middley=-1;
46 
47  fileName = "3Dobject";
48 
49  string slash="/";
50 
51  string imL=slash + getName().c_str() + "/left:i";
52  string imR=slash + getName().c_str() + "/right:i";
53  imagePortInLeft.open(imL.c_str());
54  imagePortInRight.open(imR.c_str());
55 
56  bool ok=true;
57  if (robot=="icub")
58  {
59  ok&=Network::connect((slash+robot+"/camcalib/left/out").c_str(),imL.c_str());
60  ok&=Network::connect((slash+robot+"/camcalib/right/out").c_str(),imR.c_str());
61  }
62  else
63  {
64  ok&=Network::connect((slash+robot+"/cam/left").c_str(),imL.c_str());
65  ok&=Network::connect((slash+robot+"/cam/right").c_str(),imR.c_str());
66  }
67 
68  if (!ok)
69  {
70  printf("Cameras not available, closing\n");
71  imagePortInLeft.close();
72  imagePortInRight.close();
73  return false;
74  }
75  segmentationPort.open((slash+getName().c_str()+"/segmentation").c_str());
76 
77  pointCloudPort.open((slash + getName().c_str() + "/mesh:o").c_str());
78  rpc.open((slash + getName().c_str() + "/rpc").c_str());
79  attach(rpc);
80 
81  Property recRoutOptions;
82  recRoutOptions.put("ConfigDisparity",rf.check("ConfigDisparity",Value("icubEyes.ini")).asString().c_str());
83  recRoutOptions.put("CameraContext",rf.check("CameraContext",Value("cameraCalibration")).asString().c_str());
84  recRoutOptions.put("name",getName().c_str());
85 
86  if (!recRoutine.open(recRoutOptions))
87  {
88  fprintf(stdout, "Problem with thread, the module will be closed\n");
89  close();
90  return false;
91  }
92 
93  // Visualizer Thread
94  visThrd = new VisThread(50, "Cloud");
95  if (!visThrd->start())
96  {
97  delete visThrd;
98  visThrd = 0;
99  cout << "\nERROR!!! visThread wasn't instantiated!!\n";
100  return false;
101  }
102  cout << "PCL visualizer Thread istantiated...\n";
103 
104  return true;
105 }
106 
107 /************************************************************************/
108 bool ObjectReconstr::close()
109 {
110  imagePortInLeft.close();
111  imagePortInRight.close();
112 
113  rpc.close();
114  pointCloudPort.close();
115  segmentationPort.close();
116 
117  recRoutine.close();
118  if (visThrd) //Close visualization thread clean.
119  {
120  visThrd->stop();
121  delete visThrd;
122  visThrd = 0;
123  }
124 
125  return true;
126 }
127 
128 /************************************************************************/
129 Bottle ObjectReconstr::getPixelList()
130 {
131  Bottle toSend, reply;
132  toSend.addString("get_component_around");
133  toSend.addInt((int)middlex);
134  toSend.addInt((int)middley);
135 
136  segmentationPort.write(toSend,reply);
137  return reply;
138 }
139 
140 /************************************************************************/
141 void ObjectReconstr::savePointsPly(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
142  const string& name)
143 {
144  stringstream s;
145  s.str("");
146  s<<outputDir + "/" + name.c_str() <<number;
147  string filename=s.str();
148  string filenameNumb=filename+".ply";
149  ofstream plyfile;
150  plyfile.open(filenameNumb.c_str());
151  plyfile << "ply\n";
152  plyfile << "format ascii 1.0\n";
153  plyfile << "element vertex " << cloud->width <<"\n";
154  plyfile << "property float x\n";
155  plyfile << "property float y\n";
156  plyfile << "property float z\n";
157  plyfile << "property uchar diffuse_red\n";
158  plyfile << "property uchar diffuse_green\n";
159  plyfile << "property uchar diffuse_blue\n";
160  plyfile << "end_header\n";
161 
162  for (unsigned int i=0; i<cloud->width; i++)
163  plyfile << cloud->at(i).x << " " << cloud->at(i).y << " " << cloud->at(i).z << " " << (int)cloud->at(i).r << " " << (int)cloud->at(i).g << " " << (int)cloud->at(i).b << "\n";
164 
165  plyfile.close();
166 
167  number++;
168  fprintf(stdout, "Writing finished\n");
169 }
170 
171 /************************************************************************/
172 bool ObjectReconstr::updateCloud()
173 {
174  ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(true);
175  ImageOf<PixelRgb> *tmpR = imagePortInRight.read(true);
176 
177  IplImage* imgL;
178  IplImage* imgR;
179  if(tmpL!=NULL && tmpR!=NULL)
180  {
181  imgL= (IplImage*) tmpL->getIplImage();
182  imgR= (IplImage*) tmpR->getIplImage();
183  }
184  else
185  {
186  fprintf(stdout, "Problem with image ports occurred\n");
187  return false;
188  }
189 
190  if (currentState==STATE_RECONSTRUCT)
191  {
192  Bottle pixelList=getPixelList();
193  return recRoutine.reconstruct(imgL,imgR,pixelList);
194  }
195  else
196  return recRoutine.updateDisparity(imgL,imgR);
197 }
198 
199 /************************************************************************/
200 bool ObjectReconstr::updateModule()
201 {
202  if (!updateCloud())
203  return false;
204  switch(currentState)
205  {
206 
207  case STATE_WAIT:
208  return true;
209 
210  case STATE_RECONSTRUCT:
211  {
212  recRoutine.resetClouds();
213 
214  if (!updateCloud())
215  return false;
216 
217  printf("Cloud reconstructed\n");
218 
219  pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp=recRoutine.getPointCloudComplete();
220  if (tmp->size()==0)
221  {
222  printf("Empty cloud\n");
223  middlex=-1;
224  middley=-1;
225  currentState=STATE_WAIT;
226  SurfaceMeshWithBoundingBox &pointCloudOnPort=pointCloudPort.prepare();
227  pointCloudOnPort.mesh.points.clear();
228  pointCloudOnPort.mesh.rgbColour.clear();
229  pointCloudPort.write();
230  return true;
231  }
232 
233  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
234  filter(tmp,cloud);
235 
236  SurfaceMeshWithBoundingBox &pointCloudOnPort=pointCloudPort.prepare();
237  pointCloudOnPort.mesh.points.clear();
238  pointCloudOnPort.mesh.rgbColour.clear();
239  for (unsigned int i=0; i<cloud->width; i++)
240  {
241  pointCloudOnPort.mesh.points.push_back(PointXYZ(cloud->at(i).x,cloud->at(i).y, cloud->at(i).z));
242  pointCloudOnPort.mesh.rgbColour.push_back(RGBA(cloud->at(i).rgba));
243  }
244  if (computeBB)
245  {
246  cout << " computing BB " << endl;
247  boundingBox=MinimumBoundingBox::getMinimumBoundingBox(cloud);
248  pointCloudOnPort.boundingBox=boundingBox.getBoundingBox();
249  }
250  pointCloudPort.write();
251 
252  if (write)
253  savePointsPly(tmp, fileName);
254 
255  if (visualizationOn)
256  currentState=STATE_VISUALIZE;
257  else
258  currentState=STATE_WAIT;
259 
260  middlex=-1;
261  middley=-1;
262 
263  return true;
264  }
265 
266  case STATE_VISUALIZE:
267  {
268  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud=recRoutine.getPointCloud();
269  if (visualizationOn)
270  {
271  /*
272  boost::shared_ptr<pcl::visualization::PCLVisualizer> tmpViewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));
273  tmpViewer->setBackgroundColor (0, 0, 0);
274  if (computeBB)
275  {
276  boundingBox.drawBoundingBox(tmpViewer);
277  }
278  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudComplete;
279  cloudComplete=recRoutine.getPointCloudComplete();
280  visualize(tmpViewer, cloudComplete);
281  */
282  visThrd->updateCloud(cloud);
283  if (computeBB)
284  {
285  cout << "Plotting BB " << endl;
286  visThrd->addBoundingBox(true);
287  }
288  }
289  currentState=STATE_WAIT;
290  }
291  }
292 
293  return true;
294 }
295 
296 /************************************************************************/
297 void ObjectReconstr::filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in_filtered, bool second)
298 {
299  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_tmp (new pcl::PointCloud<pcl::PointXYZRGB>);
300  pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
301  sor.setInputCloud (cloud_in);
302  sor.setMeanK (cloud_in->size()/2);
303  sor.setStddevMulThresh (1.0);
304  sor.filter (*cloud_in_filtered);
305 }
306 
307 /************************************************************************/
308 bool ObjectReconstr::interruptModule()
309 {
310  closing=true;
311  rpc.interrupt();
312 
313  imagePortInLeft.interrupt();
314  imagePortInRight.interrupt();
315 
316  pointCloudPort.interrupt();
317  segmentationPort.interrupt();
318 
319  return true;
320 }
321 
322 /************************************************************************/
323 double ObjectReconstr::getPeriod()
324 {
325  return 0.1;
326 }
327 
328 /************************************************************************/
329 bool ObjectReconstr::respond(const Bottle& command, Bottle& reply)
330 {
331  if (command.get(0).asString()=="help")
332  {
333  reply.addVocab(ACK);
334  reply.addString("To obtain a reconstruction, the module needs a segmentation algorithm that returns the set of pixels belonging to the object");
335  reply.addString("It typically uses the graphBasedSegmentation module");
336  reply.addString("Provide a point of the object first, in the format x y, for instance clicking on the segmentation image.");
337  reply.addString("Then, when you send the command 3Drec, you will be provided with the reconstructed object along with the minimum enclosing bounding box on the output port of the module -- typically /object-reconstruction/mesh:o.");
338  reply.addString("If you also want to visualize the result, the command is 3Drec visualize.");
339  reply.addString("If you want to reconstruct a single pixel, the command is get point (x y).");
340  return true;
341  }
342 
343  if (command.get(0).asString()=="set")
344  {
345  if (command.get(1).asString()=="write")
346  {
347  if (command.size()>2)
348  {
349  if (command.get(2).asString()=="on")
350  {
351  reply.addVocab(ACK);
352  write=true;
353  }
354  else if (command.get(2).asString()=="off")
355  {
356  write=false;
357  reply.addVocab(ACK);
358  }
359  else
360  reply.addVocab(NACK);
361  return true;
362  }
363  }
364  else
365  {
366  reply.addVocab(NACK);
367  return true;
368  }
369  }
370  if (command.get(0).asString()=="3Drec")
371  {
372  if (middlex==-1 || middley==-1)
373  {
374  reply.addVocab(NACK);
375  reply.addString("I need a pixel of the object");
376  return true;
377  }
378 
379  currentState=STATE_RECONSTRUCT;
380 
381  visualizationOn=false;
382 
383  if (command.size()==2)
384  if (command.get(1).asString()=="visualize")
385  visualizationOn=true;
386 
387  reply.addVocab(ACK);
388  return true;
389  }
390 
391  if (command.get(0).asString()=="name")
392  {
393  if (command.size()>=2)
394  {
395  fileName=command.get(1).asString().c_str();
396  reply.addVocab(ACK);
397  }
398  else
399  {
400  reply.addVocab(NACK);
401  reply.addString("No name was provided");
402  }
403 
404  return true;
405  }
406 
407  if (command.get(0).asString()=="get")
408  {
409  if (command.get(1).asString()=="point" && command.size()==3)
410  {
411  if (currentState!=STATE_RECONSTRUCT)
412  {
413  IplImage* imgL;
414  IplImage* imgR;
415 
416  ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(true);
417  ImageOf<PixelRgb> *tmpR = imagePortInRight.read(true);
418 
419  if(tmpL!=NULL && tmpR!=NULL)
420  {
421  imgL= (IplImage*) tmpL->getIplImage();
422  imgR= (IplImage*) tmpR->getIplImage();
423  }
424  else
425  {
426  reply.addVocab(NACK);
427  return true;
428  }
429 
430  yarp::sig::Vector point2D(2);
431  Bottle *pixel=command.get(2).asList();
432  point2D[0]=pixel->get(0).asDouble();
433  point2D[1]=pixel->get(1).asDouble();
434 
435  yarp::sig::Vector point3D(3);
436 
437  bool done=recRoutine.triangulateSinglePoint(imgL,imgR,point2D,point3D);
438 
439  if (done)
440  {
441  Bottle &result=reply.addList();
442  result.addDouble(point3D[0]);
443  result.addDouble(point3D[1]);
444  result.addDouble(point3D[2]);
445  }
446  else
447  reply.addVocab(NACK);
448  }
449  else
450  {
451  reply.addVocab(NACK);
452  reply.addString("Still processing");
453  }
454  return true;
455  }
456  else
457  {
458  reply.addVocab(NACK);
459  return true;
460  }
461  }
462 
463  if (command.size()==2)
464  {
465  if (command.get(0).asInt()!=0 && command.get(1).asInt()!=0)
466  {
467  middlex=(double)command.get(0).asInt();
468  middley=(double)command.get(1).asInt();
469  reply.addVocab(ACK);
470  return true;
471  }
472  else
473  {
474  reply.addVocab(NACK);
475  return true;
476  }
477  }
478 
479  reply.addVocab(NACK);
480  return true;
481 }
482 
483 /************************************************************************/
484 /*
485 void ObjectReconstr::visualize(boost::shared_ptr<pcl::visualization::PCLVisualizer> tmpViewer, pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
486 {
487  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
488 
489  string id="Object Cloud";
490  tmpViewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, id);
491  tmpViewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, id);
492  tmpViewer->initCameraParameters();
493  while (!tmpViewer->wasStopped())
494  {
495  if (closing)
496  {
497  tmpViewer->close();
498  break;
499  }
500  tmpViewer->spinOnce (100);
501  boost::this_thread::sleep (boost::posix_time::microseconds (100000));
502  }
503  tmpViewer->removePointCloud(id);
504 }
505 
506 */
Generated by   doxygen 1.8.12