segmentation
•All Data Structures Namespaces Files Functions Variables Modules Pages
main.cpp
1 /*
2  * Copyright (C) 2015 iCub Facility - Istituto Italiano di Tecnologia
3  * Author: Ugo Pattacini, Tanis Mar
4  * email: ugo.pattacini@iit.it, tanis.mar@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
18 #include <vector>
19 #include <algorithm>
20 #include <string>
21 #include <fstream>
22 #include <iomanip>
23 #include <mutex>
24 
25 #include <opencv2/opencv.hpp>
26 #include <opencv2/highgui/highgui.hpp>
27 
28 #include <yarp/os/all.h>
29 #include <yarp/sig/all.h>
30 #include <yarp/math/Math.h>
31 #include <yarp/cv/Cv.h>
32 
33 using namespace std;
34 using namespace yarp::os;
35 using namespace yarp::sig;
36 using namespace yarp::math;
37 using namespace yarp::cv;
38 
39 
40 /*******************************************************************************/
41 class Seg2cloudModule : public RFModule, public PortReader
42 {
43 protected:
44  vector<cv::Point> contour;
45  vector<cv::Point> floodPoints;
46  cv::Point seed;
47  cv::Rect rect;
48 
49  string homeContextPath;
50  string savename;
51  string fileFormat;
52  int fileCount;
53 
54  int downsampling;
55  double spatial_distance;
56  int color_distance;
57  mutex mtx;
58  bool polygon,flood3d,flood,seg;
59  bool seedAuto;
60  bool saving;
61 
62  BufferedPort<ImageOf<PixelMono> > portDispIn;
63  BufferedPort<ImageOf<PixelRgb> > portDispOut;
64  BufferedPort<ImageOf<PixelRgb> > portImgIn;
65  BufferedPort<Bottle> portPointsOut;
66  BufferedPort<Bottle> portPoints2DOut;
67 
68  Port portSeedIn;
69  //Port portContour;
70  RpcClient portSFM;
71  RpcClient portSeg;
72  RpcServer portRpc;
73 
74  std::string name; //name of the module
75 
76  /*******************************************************************************/
77  bool read(ConnectionReader &connection)
78  {
79  Bottle data; data.read(connection);
80  cout << " Read: " << data.toString() << endl;
81  if (data.size()>=2)
82  {
83  lock_guard<mutex> lg(mtx);
84  cv::Point point(data.get(0).asInt32(),data.get(1).asInt32());
85  contour.push_back(point);
86  if (contour.size()>12){
87  contour.pop_back();
88  }
89  seed = point;
90  }
91 
92  return true;
93  }
94 
95 public:
96  /*******************************************************************************/
97  bool configure(ResourceFinder &rf)
98  {
99  name = rf.check("name", Value("seg2cloud"), "Getting module name").asString();
100 
101  homeContextPath=rf.getHomeContextPath();
102  savename = rf.check("savename", Value("cloud3D"), "Default file savename").asString();
103  saving = rf.check("savingClouds", Value(false), "Toggle save clouds as file").asBool();
104  fileFormat = rf.check("format", Value("off"), "Default file format").asString();
105  seedAuto = rf.check("seedAuto", Value(true), "Automatic seed computation").asBool();
106 
107 
108  cout << "Files will be saved in "<< homeContextPath << " folder, as " << savename <<"N." << fileFormat <<", with increasing numeration N" << endl;
109  fileCount = 0;
110 
111  downsampling=std::max(1,rf.check("downsampling",Value(1)).asInt32());
112  spatial_distance=rf.check("spatial_distance",Value(0.005)).asFloat64();
113  color_distance=rf.check("color_distance",Value(6)).asInt32();
114 
115 
116  printf("Opening ports\n" );
117  bool ret= true;
118  ret = ret && portDispIn.open("/"+name+"/disp:i");
119  ret = ret && portImgIn.open("/"+name+"/img:i");
120  ret = ret && portSeedIn.open("/"+name+"/seed:i");
121 
122  ret = ret && portPointsOut.open("/"+name+"/pnt:o");
123  ret = ret && portPoints2DOut.open("/"+name+"/pnt2D:o");
124  ret = ret && portDispOut.open("/"+name+"/disp:o");
125 
126  ret = ret && portSFM.open("/"+name+"/SFM:rpc");
127  ret = ret && portSeg.open("/"+name+"/seg:rpc");
128  ret = ret && portRpc.open("/"+name+"/rpc:i");
129 
130  if (!ret){
131  printf("Problems opening ports\n");
132  return false;
133  }
134  printf("Ports opened\n");
135 
136  portSeedIn.setReader(*this);
137  attach(portRpc);
138 
139  polygon=flood3d=flood=seg=false;
140 
141  seed.x = -1; seed.y = -1; // Negativ values mean undefined seed
142 
143  rect = cv::Rect(1, 1, 0,0);
144 
145  return true;
146  }
147 
148  /*******************************************************************************/
149  bool interruptModule()
150  {
151  portDispIn.interrupt();
152  portDispOut.interrupt();
153  portImgIn.interrupt();
154  portPointsOut.interrupt();
155  portPoints2DOut.interrupt();
156  portSFM.interrupt();
157  portSeg.interrupt();
158  portRpc.interrupt();
159  return true;
160  }
161 
162  /*******************************************************************************/
163  bool close()
164  {
165  portDispIn.close();
166  portDispOut.close();
167  portImgIn.close();
168  portPointsOut.close();
169  portPoints2DOut.close();
170  portSFM.close();
171  portSeg.close();
172  portRpc.close();
173  return true;
174  }
175 
176  /*******************************************************************************/
177  double getPeriod()
178  {
179  return 0.1;
180  }
181 
182  /*******************************************************************************/
183  bool updateModule()
184  {
185  ImageOf<PixelMono> *imgDispIn=portDispIn.read();
186  if (imgDispIn==NULL)
187  return false;
188 
189  ImageOf<PixelRgb> *imgIn=portImgIn.read();
190  if (imgIn==NULL)
191  return false;
192 
193  lock_guard<mutex> lg(mtx);
194 
195  ImageOf<PixelRgb> &imgDispOut=portDispOut.prepare();
196  imgDispOut.resize(imgDispIn->width(),imgDispIn->height());
197 
198  cv::Mat imgInMat=toCvMat(*imgIn);
199  cv::Mat imgDispInMat=toCvMat(*imgDispIn);
200  cv::Mat imgDispOutMat=toCvMat(imgDispOut);
201  cv::cvtColor(imgDispInMat,imgDispOutMat,CV_GRAY2RGB);
202 
203  vector<vector<cv::Point> > contours;
204  contours.push_back(contour);
205  cv::drawContours(imgDispOutMat,contours,0,cv::Scalar(255,255,0));
206  cv::rectangle(imgDispOutMat,rect,cv::Scalar(255,50,0));
207 
208  if ((seed.x>0) && (seed.y>0)){
209  clearRec();
210  cv::circle(imgDispOutMat,seed, 3,cv::Scalar(0,0,255), 2);
211 
212  }
213 
214  if (polygon||flood3d||flood||seg){
215 
216  clearRec();
217 
218  // Create variables to store points in any case
219  Bottle &bpoints=portPointsOut.prepare();
220  vector<Vector> points;
221 
222  if (polygon){
223  if (contour.size()>3)
224  {
225  cout << "3D points from selected contour "<<endl;
226 
227  pointsFromContour(imgIn, contour, rect, points, bpoints);
228 
229  cout << "Retrieved " << points.size() << " 3D points" <<endl;
230 
231  contour.clear();
232 
233  }else{
234  cout << "Please select a polygon on the image before calling 'polygon' " << endl;
235  }
236 
237 
238  } else if (flood3d)
239  {
240  cout << "Getting seed "<<endl;
241 
242 
243  if (seedAuto){ // Autoseed overwrites present values of 'seed'
244  if(!getDepthSeed(imgDispInMat,seed)){
245  portPointsOut.unprepare();
246  portDispOut.write();
247  return true;
248  }
249  }else
250  // Wait for click only if seed is not auto and coords have not been given by command or on a previous click.
251  if ((seed.x<0) && (seed.y<0))
252  {
253  cout << " click for a seed" << endl;
254  portPointsOut.unprepare();
255  portDispOut.write();
256  return true;
257  }
258  // If none of the conditions apply, means seed was either given by command or clicked before.
259 
260  cout << "3D points flood3D "<<endl;
261 
262  // Use the seed point to get points from SFM with the Flood3D command, and spatial_distance given
263  Bottle cmdSFM,replySFM;
264  cmdSFM.addString("Flood3D");
265  cmdSFM.addInt32(seed.x);
266  cmdSFM.addInt32(seed.y);
267  cmdSFM.addFloat64(spatial_distance);
268  bool ok = portSFM.write(cmdSFM,replySFM);
269  if (ok)
270  {
271  for (int i=0; i<replySFM.size(); i+=5)
272  {
273  int x=replySFM.get(i+0).asInt32();
274  int y=replySFM.get(i+1).asInt32();
275  PixelRgb px=imgIn->pixel(x,y);
276 
277  Vector point(6,0.0);
278  point[0]=replySFM.get(i+2).asFloat64();
279  point[1]=replySFM.get(i+3).asFloat64();
280  point[2]=replySFM.get(i+4).asFloat64();
281  point[3]=px.r;
282  point[4]=px.g;
283  point[5]=px.b;
284 
285  points.push_back(point);
286  floodPoints.push_back(cv::Point(x,y));
287 
288  Bottle &bpoint = bpoints.addList();
289  bpoint.addFloat64(point[0]);
290  bpoint.addFloat64(point[1]);
291  bpoint.addFloat64(point[2]);
292  }
293  cout << "Retrieved " << points.size() << " 3D points" <<endl;
294  }else{
295  cout << " SFM didn't reply" << endl;
296  return true;
297  }
298  }
299 
300 
301  else if (flood)
302  {
303  // Get or read seed
304  if ((seed.x<0) && (seed.y<0))
305  {
306  cout << " click for a seed" << endl;
307  portPointsOut.unprepare();
308  portDispOut.write();
309  return true;
310  }
311 
312  cout << "Extracting 3D points from 2D color flood blob"<<endl;
313  // Set flooding parameters
314  //PixelMono c = imgDispIn->pixel(seed.x,seed.y);
315  cv::Scalar delta(color_distance);
316 
317  // flood region and copy flood onto mask
318  cv::Mat mask = cv::Mat::zeros(imgInMat.rows + 2, imgInMat.cols + 2, CV_8U);
319  cv::floodFill(imgDispInMat, mask, seed, cv::Scalar(255), NULL, delta, delta, 4 + (255 << 8) | cv::FLOODFILL_FIXED_RANGE| cv::FLOODFILL_MASK_ONLY);
320 
321  // Get the contours of the color flooded region from mask.
322  vector<vector<cv::Point> > contoursFlood;
323  vector<cv::Vec4i> hierarchy;
324  cv::findContours(mask, contoursFlood, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
325  vector<cv::Point> contourFlood = contoursFlood[0];
326 
327  // Get the 3D points from the 2D points within the contour.
328  pointsFromContour(imgIn, contourFlood, rect, points, bpoints);
329 
330  cout << "Retrieved " << points.size() << " 3D points" <<endl;
331  }
332 
333  else if (seg)
334  {
335  // Wait for click only if seed is not auto and coords have not been given by command or on a previous click.
336  if ((seed.x<0) && (seed.y<0))
337  {
338  cout << " click for a seed" << endl;
339  portPointsOut.unprepare();
340  portDispOut.write();
341  return true;
342  }
343  // If none of the conditions apply, means seed was either given by command or clicked before.
344 
345  cout << "Extracting 3D points from segmented blob "<<endl;
346 
347  // Get segmented region from external segmentation module
348  Bottle cmdSeg, replySeg;
349  cmdSeg.addString("get_component_around");
350  cmdSeg.addInt32(seed.x);
351  cmdSeg.addInt32(seed.y);
352  if (portSeg.write(cmdSeg,replySeg))
353  {
354  Bottle* pixelList=replySeg.get(0).asList();
355 
356  if (pixelList->size()==0)
357  {
358  cout << "Empty bottle received" <<endl;
359  seg=false;
360  points.clear();
361  bpoints.clear();
362  portPointsOut.write(); // Return empty bottle if no data was received.
363  return true;
364  }
365 
366  cout << "Read " << pixelList->size() << " points from segmentation algorithm" <<endl;
367  cv::Mat binImg = cv::Mat(imgDispInMat.rows, imgDispInMat.cols, CV_8U, 0.0);
368  for (int i=0; i<pixelList->size(); i++)
369  {
370  Bottle* point=pixelList->get(i).asList();
371  int x = point->get(0).asFloat64();
372  int y = point->get(1).asFloat64();
373  binImg.at<uchar>(y,x) = 255;
374  }
375 
376  // Get the contours of the segmented region.
377  vector<vector<cv::Point> > contoursSeg;
378  vector<cv::Vec4i> hierarchy;
379  cv::findContours(binImg, contoursSeg, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
380  vector<cv::Point> contourSeg = contoursSeg[0];
381  //cout << "Contours extracted." <<endl;
382 
383  // Get the 3D points from the 2D points within the contour.
384  pointsFromContour(imgIn, contourSeg, rect, points, bpoints);
385 
386  cout << "Retrieved " << points.size() << " 3D points" <<endl;
387  }else{
388 
389  cout << "Segmentation module did not provide reply" << endl;
390  return true;
391  }
392  }
393 
394  //------------------------//
395  // Send and/or save points
396  if (points.size()>0)
397  {
398  if (saving){
399  saveCloud(points);
400  }
401  for (int t = 0; t < 10; t++){
402  portPointsOut.write();
403  cout << " 3D points sent time" << t << endl;
404  Time::delay(0.2);
405  }
406 
407  }else {
408  portPointsOut.unprepare();
409  }
410 
411  seed.x = -1; seed.y = -1; // Reset seed after each rec call
412  polygon=flood=flood3d=seg=false;
413  points.clear();
414  bpoints.clear();
415  }
416 
417  if (floodPoints.size()>0){
418  PixelRgb color(130,255,0);
419  Bottle &bpoints2D = portPoints2DOut.prepare();
420  bpoints2D.clear();
421  for (size_t i=0; i<floodPoints.size(); i++){
422  imgDispOut.pixel(floodPoints[i].x,floodPoints[i].y)=color;
423  Bottle &bpoint2D = bpoints2D.addList();
424  // Remember that cv::Points are (x =column, y = row) and YARP coords are (u = row, v = column)
425  bpoint2D.addInt32(floodPoints[i].x);
426  bpoint2D.addInt32(floodPoints[i].y);
427 
428  }
429  // Send images and 2D points for a second
430  for (int t = 0; t < 10; t++){
431  portDispOut.write();
432  portPoints2DOut.write();
433  cout << " 2D points sent time" << t << endl;
434  Time::delay(0.2);
435  }
436  floodPoints.clear();
437  return true;
438  }else{
439  portPoints2DOut.unprepare();
440  }
441 
442 
443  portDispOut.write();
444  return true;
445  }
446 
447 
448  /*******************************************************************************/
449  bool saveCloud(const vector<Vector> &points)
450  {
451  ofstream fout;
452  stringstream fileName;
453  string fileNameFormat;
454  fileName.str("");
455  fileName << homeContextPath + "/" + savename << fileCount;
456 
457  if (fileFormat == "ply")
458  {
459  fileNameFormat = fileName.str()+".ply";
460  cout << "Saving as " << fileNameFormat << endl;
461  fout.open(fileNameFormat.c_str());
462  if (fout.is_open())
463  {
464  fout << "ply\n";
465  fout << "format ascii 1.0\n";
466  fout << "element vertex " << points.size() <<"\n";
467  fout << "property float x\n";
468  fout << "property float y\n";
469  fout << "property float z\n";
470  fout << "property uchar diffuse_red\n";
471  fout << "property uchar diffuse_green\n";
472  fout << "property uchar diffuse_blue\n";
473  fout << "end_header\n";
474 
475  for (unsigned int i=0; i<points.size(); i++){
476  fout << points[i][0] << " " << points[i][1] << " " << points[i][2] << " " << (int)points[i][3] << " " << (int)points[i][4] << " " << (int)points[i][5] << "\n";
477  //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";
478  }
479 
480  fout.close();
481  cout << "Points saved as " << fileNameFormat << endl;
482  fileCount++;
483  }
484 
485  }else if (fileFormat == "off"){
486  fileNameFormat = fileName.str()+".off";
487  cout << "Saving as " << fileNameFormat << endl;
488  fout.open(fileNameFormat.c_str());
489  if (fout.is_open())
490  {
491 
492  fout<<"COFF"<<endl;
493  fout<<points.size()<<" 0 0"<<endl;
494  fout<<endl;
495  for (size_t i=0; i<points.size(); i++)
496  {
497  fout<<points[i].subVector(0,2).toString(3,4)<<" "<<
498  points[i].subVector(3,5).toString(0,3)<<endl;
499  }
500  fout<<endl;
501  }
502 
503  fout.close();
504  cout << "Points saved as " << fileNameFormat << endl;
505  fileCount++;
506 
507  }else if (fileFormat == "none"){
508  cout << "Points not saved" << endl;
509  return false;
510  }
511  return true;
512  }
513 
514 
515  /*******************************************************************************/
516  bool pointsFromContour(const ImageOf<PixelRgb> *imgIn, const vector<cv::Point> contourIn, cv::Rect &boundBox, vector<Vector> &pointsInContour, Bottle &bpoints)
517  {
518  boundBox = cv::boundingRect(contourIn);
519 
520  Bottle cmd,reply;
521  cmd.addString("Rect");
522  cmd.addInt32(boundBox.x); cmd.addInt32(boundBox.y);
523  cmd.addInt32(boundBox.width); cmd.addInt32(boundBox.height);
524  cmd.addInt32(downsampling);
525  if (portSFM.write(cmd,reply))
526  {
527  int idx=0;
528  for (int x=boundBox.x; x<boundBox.x+boundBox.width; x+=downsampling)
529  {
530  for (int y=boundBox.y; y<boundBox.y+boundBox.height; y+=downsampling)
531  {
532  if (cv::pointPolygonTest(contourIn,cv::Point2f((float)x,(float)y),false)>0.0)
533  {
534  floodPoints.push_back(cv::Point(x,y));
535 
536  Vector point(6,0.0);
537  point[0]=reply.get(idx+0).asFloat64();
538  point[1]=reply.get(idx+1).asFloat64();
539  point[2]=reply.get(idx+2).asFloat64();
540 
541  Bottle &bpoint = bpoints.addList();
542  bpoint.addFloat64(point[0]);
543  bpoint.addFloat64(point[1]);
544  bpoint.addFloat64(point[2]);
545 
546  if (norm(point)>0.0)
547  {
548  PixelRgb px=imgIn->pixel(x,y);
549  point[3]=px.r;
550  point[4]=px.g;
551  point[5]=px.b;
552 
553  bpoint.addFloat64(point[3]);
554  bpoint.addFloat64(point[4]);
555  bpoint.addFloat64(point[5]);
556 
557  pointsInContour.push_back(point);
558  }
559  }
560 
561  idx+=3;
562  }
563  }
564  }
565  return true;
566  }
567 
568 
569 
570  /*******************************************************************************/
571  bool getDepthSeed(cv::Point2i &seedPoint)
572  {
573  ImageOf<PixelMono> *imgDispIn=portDispIn.read();
574  if (imgDispIn==NULL)
575  return false;
576 
577  cv::Mat imgDispInMat=toCvMat(*imgDispIn);
578 
579  return getDepthSeed(imgDispInMat,seedPoint);
580  }
581 
582 
583  bool getDepthSeed(const cv::Mat &disparity,cv::Point2i &seedPoint)
584  {
585  cout << "Finding seed automatically" << endl;
586  cv::Mat depth = disparity.clone();
587 
588  /* Filter */
589  int gaussSize = 5;
590  double sigmaX1 = 1.5;
591  double sigmaY1 = 1.5;
592  cv::GaussianBlur(depth, depth, cv::Size(gaussSize,gaussSize), sigmaX1, sigmaY1);
593 
594  cv::threshold(depth, depth, 30, -1, CV_THRESH_TOZERO);
595 
596  int dilate_niter = 4;
597  int erode_niter = 2;
598  double sigmaX2 = 2;
599  double sigmaY2 = 2;
600 
601  cv::dilate(depth, depth, cv::Mat(), cv::Point(-1,-1), dilate_niter, cv::BORDER_CONSTANT, cv::morphologyDefaultBorderValue());
602 
603  cv::GaussianBlur(depth, depth, cv::Size(gaussSize,gaussSize), sigmaX2, sigmaY2, cv::BORDER_DEFAULT);
604 
605  cv::erode(depth, depth, cv::Mat(), cv::Point(-1,-1), erode_niter, cv::BORDER_CONSTANT, cv::morphologyDefaultBorderValue());
606 
607 
608  /* Find closest valid blob */
609  double minVal, maxVal;
610  cv::Point minLoc, maxLoc;
611 
612  int fillFlags = 8 | ( 255 << 8 ) | cv::FLOODFILL_FIXED_RANGE; // flags for floodFill
613 
614  cv::Mat aux = depth.clone();
615 
616  int minBlobSize = 300;
617  int fillSize = 0;
618  int imageThreshRatioLow = 10;
619  int imageThreshRatioHigh = 20;
620  while (fillSize < minBlobSize){
621 
622  cv::minMaxLoc( aux, &minVal, &maxVal, &minLoc, &maxLoc );
623 
624  // if its too small, paint it black and search again
625  fillSize = floodFill(aux, maxLoc, 0, 0, cv::Scalar(maxVal/imageThreshRatioLow), cv::Scalar(maxVal/imageThreshRatioHigh), fillFlags);
626 
627  }
628  // paint closest valid blob white
629  cv::Mat fillMask(depth.rows+2, depth.cols+2, CV_8U);
630  fillMask.setTo(0);
631  cv::floodFill(depth, fillMask, maxLoc, 255, 0, cv::Scalar(maxVal/imageThreshRatioLow), cv::Scalar(maxVal/imageThreshRatioHigh), cv::FLOODFILL_MASK_ONLY + fillFlags);
632 
633  /* Find contours */
634  std::vector<std::vector<cv::Point > > contours;
635  std::vector<cv::Vec4i> hierarchy;
636 
637  // use aux because findContours modifies the input image
638  aux = fillMask(cv::Range(1,depth.rows+1), cv::Range(1,depth.cols+1)).clone();
639  cv::findContours( aux, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
640 
641 
642  /* If any blob is found check again that only the biggest valid blob is selected */
643  int blobI = -1;
644  double blobSizeOld = -1, blobSize = -1;
645  for( int c = 0; c < contours.size(); c++ ){
646 
647  // find the area of contour
648  blobSize = cv::contourArea(contours[c]);
649 
650  // select only the biggest valid blob
651  if( blobSize > minBlobSize && blobSize > blobSizeOld)
652  {
653  blobI = c;
654  blobSizeOld = blobSize;
655  }
656  }
657 
658  /* If any blob is found (after the double-check) */
659  if (blobI>=0)
660  {
661  /* Get the Bounding Box */
662  cv::Rect blobBox = cv::boundingRect(contours[blobI]);
663 
664  double seedValid = -1; // false
665  cv::Point2i seedAux;
666  seedAux.x = blobBox.tl().x;
667  seedAux.y = blobBox.tl().y;
668  while (seedValid < 0){ // Move diagonally through the image until a point inside the blob is found.
669  seedAux.x = seedAux.x + 4;
670  seedAux.y = seedAux.y + 1;
671 
672 
673  cv::circle(depth, seedAux, 1, cv::Scalar(0,0,0),2);
674 
675  if ((seedAux.x > blobBox.br().x ) ||(seedAux.y > blobBox.br().y )){
676  cout << "Seed could not be found"<< endl;
677  return false;
678  }
679 
680  seedValid = pointPolygonTest(contours[blobI], seedAux, false );
681  }
682 
683  seedPoint.x = seedAux.x+2; // add a small margin to be fully inside the contour
684  seedPoint.y = seedAux.y+2;
685 
686  cout << "Seed found at " << seedPoint.x << " ," << seedPoint.y << endl;
687 
688  return true;
689  }else{
690  cout << "Seed could not be determined " << endl;
691  return false;
692  }
693  }
694 
695 
696  /*******************************************************************************/
697  void clearRec()
698  {
699  //contour.clear();
700  floodPoints.clear();
701  rect = cv::Rect(1,1,0,0);
702 
703  return;
704  }
705 
706 
707  /*******************************************************************************/
708  bool respond(const Bottle &command, Bottle &reply)
709  {
710  string cmd=command.get(0).asString();
711  int ack=Vocab32::encode("ack");
712  int nack=Vocab32::encode("nack");
713 
714  if (cmd=="clear")
715  {
716  clearRec();
717  reply.addVocab32(ack);
718  return true;
719  }
720 
721  else if (cmd=="seedAuto"){
722  seedAuto = command.get(1).asBool();
723  cout << "Automatic seeding is " << seedAuto << endl;
724  reply.addVocab32(ack);
725  return true;
726  }
727 
728  else if (cmd=="saving"){
729  saving = command.get(1).asBool();
730  cout << "Saving clouds as files is " << saving << endl;
731  reply.addVocab32(ack);
732  return true;
733  }
734 
735  else if (cmd=="setFormat")
736  {
737  if (command.size()>=2){
738  string format = command.get(1).asString();
739  if ((format=="ply")||(format=="off")||(format=="none")){
740  fileFormat = format;
741  reply.addVocab32(ack);
742  reply.addString("Format set correctly");
743  } else {
744  reply.addVocab32(nack);
745  reply.addString("No valid format chosen. Choose ply/off/none");
746  }
747  }
748  }
749 
750  else if (cmd=="setFileName")
751  {
752  if (command.size()>=2){
753  savename = command.get(1).asString();
754  reply.addVocab32(ack);
755  reply.addString("File Name set correctly");
756  }
757  }
758 
759  else if (cmd=="help")
760  {
761  reply.addVocab32("many");
762  reply.addString("Available commands are:");
763  reply.addString("help - produces this help");
764  reply.addString("clear - Clears displays and saved points");
765  reply.addString("setFormat string(fileformat)- Sets the format in which the points will be saved. 'fileformat' can be 'ply', 'off' or 'none'.");
766  reply.addString("setFileName string(filename)- Sets the base name given to the files where the 3D points will be saved. ");
767  reply.addString("saving (bool) - Activates/deactivated saving each reconstructed cloud in file.");
768  reply.addString("---------- Extraction Methods -----------");
769  reply.addString("polygon - Gets pointcloud from the selected polygon on the disp image");
770  reply.addString("flood int(color_distance) int int (coords(opt))- Gets pointcloud from 2D color flood using the parameter color_distance. Optionally coords can be given by command.");
771  reply.addString("flood3d double(spatial_distance) int int (coords(opt))- gets pointcloud from 3D color flood (based on depth with parameter spatial_distance). Optionally coords can be given by command.");
772  reply.addString("seg int int (coords(opt))- gets pointcloud from an externally segmented blob. Optionally coords can be given by command.");
773  reply.addString("seedAuto (bool) - Toggles from manual (click) seed to 'automatic' one for flood3d");
774  return true;
775  }
776 
777  else if ((cmd=="polygon") || (cmd=="flood3d")|| (cmd=="flood")|| (cmd=="seg"))
778  {
779  if (portSFM.getOutputCount()==0)
780  reply.addVocab32(nack);
781  else
782  {
783  lock_guard<mutex> lg(mtx);
784  if (cmd=="polygon")
785  {
786  if (contour.size()>2)
787  {
788  polygon=true;
789  reply.addVocab32(ack);
790  }
791  else
792  reply.addVocab32(nack);
793  }
794  else if (cmd=="flood3d")
795  {
796 
797  contour.clear();
798  floodPoints.clear();
799  if (command.size()>=2){
800  spatial_distance=command.get(1).asFloat64();
801  }
802 
803 
804  if (seedAuto) // Autoseed overwrites present values of 'seed'
805  {
806  if(!getDepthSeed(seed))
807  {
808  cout << "couldnt retrieve autoseed "<< endl;
809  }
810 
811  }else if (command.size()>=4){
812  seed.x=command.get(2).asInt32();
813  seed.y=command.get(3).asInt32();
814 
815  }else if ((seed.x <0) && (seed.y<0)){
816  cout << "seed needs to be clicked" << endl;
817  }
818 
819  reply.addVocab32(ack);
820  // Remember that cv::Points are (x =column, y = row) and YARP coords are (u = row, v = column)
821  reply.addInt32(seed.x);
822  reply.addInt32(seed.y);
823  reply.addFloat64(spatial_distance);
824  flood3d=true;
825 
826  }
827  else if (cmd=="flood")
828  {
829  reply.addVocab32(ack);
830  contour.clear();
831  floodPoints.clear();
832  if (command.size()>=2){
833  color_distance=command.get(1).asInt32();
834  reply.addInt32(color_distance);
835  }
836 
837  if (command.size()>=4){
838  seed.x=command.get(2).asInt32();
839  seed.y=command.get(3).asInt32();
840  reply.addInt32(seed.x);
841  reply.addInt32(seed.y);
842  }
843  flood=true;
844 
845  }
846  else if (cmd=="seg")
847  {
848  contour.clear();
849  reply.addVocab32(ack);
850  if (command.size()>=3){
851  seed.x=command.get(1).asInt32();
852  seed.y=command.get(2).asInt32();
853  reply.addInt32(seed.x);
854  reply.addInt32(seed.y);
855  }
856  seg=true;
857 
858  }
859  }
860  }
861 
862 
863  else
864  RFModule::respond(command,reply);
865 
866  return true;
867  }
868 };
869 
870 
871 /*******************************************************************************/
872 int main(int argc,char *argv[])
873 {
874  Network yarp;
875  if (!yarp.checkNetwork())
876  {
877  yError("unable to find YARP server!");
878  return 1;
879  }
880 
881  Seg2cloudModule mod;
882  ResourceFinder rf;
883  rf.setDefaultContext("seg2cloud");
884  rf.configure(argc,argv);
885  return mod.runModule(rf);
886 }
887 
Generated by doxygen 1.9.1