Segmenter.cpp
Go to the documentation of this file.
1 
13 // RAIL Segmentation
15 
16 using namespace std;
17 using namespace rail::segmentation;
18 
19 //constant definitions (to use in functions with reference parameters, e.g. param())
20 const bool Segmenter::DEFAULT_DEBUG;
21 const int Segmenter::DEFAULT_MIN_CLUSTER_SIZE;
22 const int Segmenter::DEFAULT_MAX_CLUSTER_SIZE;
23 
24 Segmenter::Segmenter() : private_node_("~"), tf2_(tf_buffer_)
25 {
26  // flag for the first point cloud coming in
27  first_pc_in_ = false;
28 
29  // set defaults
30  string point_cloud_topic("/camera/depth_registered/points");
31  string zones_file(ros::package::getPath("rail_segmentation") + "/config/zones.yaml");
32 
33  // grab any parameters we need
37  private_node_.param("use_color", use_color_, false);
38  private_node_.getParam("point_cloud_topic", point_cloud_topic);
39  private_node_.getParam("zones_config", zones_file);
40 
41  // setup publishers/subscribers we need
46  segmented_objects_pub_ = private_node_.advertise<rail_manipulation_msgs::SegmentedObjectList>(
47  "segmented_objects", 1, true
48  );
49  table_pub_ = private_node_.advertise<rail_manipulation_msgs::SegmentedObject>(
50  "segmented_table", 1, true
51  );
52  markers_pub_ = private_node_.advertise<visualization_msgs::MarkerArray>("markers", 1, true);
53  table_marker_pub_ = private_node_.advertise<visualization_msgs::Marker>("table_marker", 1, true);
54  point_cloud_sub_ = node_.subscribe(point_cloud_topic, 1, &Segmenter::pointCloudCallback, this);
55  // setup a debug publisher if we need it
56  if (debug_)
57  {
58  debug_pc_pub_ = private_node_.advertise<pcl::PointCloud<pcl::PointXYZRGB> >("debug_pc", 1, true);
59  debug_img_pub_ = private_node_.advertise<sensor_msgs::Image>("debug_img", 1, true);
60  }
61 
62  // check the YAML version
63 #ifdef YAMLCPP_GT_0_5_0
64  // parse the segmentation zones
65  YAML::Node zones_config = YAML::LoadFile(zones_file);
66  for (size_t i = 0; i < zones_config.size(); i++)
67  {
68  YAML::Node cur = zones_config[i];
69  // create a zone with the frame ID information
70  SegmentationZone zone(cur["name"].as<string>(), cur["parent_frame_id"].as<string>(),
71  cur["child_frame_id"].as<string>(), cur["bounding_frame_id"].as<string>(),
72  cur["segmentation_frame_id"].as<string>());
73 
74  // check for the remove surface flag
75  if (cur["remove_surface"].IsDefined())
76  {
77  zone.setRemoveSurface(cur["remove_surface"].as<bool>());
78  }
79 
80  // check for any set limits
81  if (cur["roll_min"].IsDefined())
82  {
83  zone.setRollMin(cur["roll_min"].as<double>());
84  }
85  if (cur["roll_max"].IsDefined())
86  {
87  zone.setRollMax(cur["roll_max"].as<double>());
88  }
89  if (cur["pitch_min"].IsDefined())
90  {
91  zone.setPitchMin(cur["pitch_min"].as<double>());
92  }
93  if (cur["pitch_max"].IsDefined())
94  {
95  zone.setPitchMax(cur["pitch_max"].as<double>());
96  }
97  if (cur["yaw_min"].IsDefined())
98  {
99  zone.setYawMin(cur["yaw_min"].as<double>());
100  }
101  if (cur["yaw_max"].IsDefined())
102  {
103  zone.setYawMax(cur["yaw_max"].as<double>());
104  }
105  if (cur["x_min"].IsDefined())
106  {
107  zone.setXMin(cur["x_min"].as<double>());
108  }
109  if (cur["x_max"].IsDefined())
110  {
111  zone.setXMax(cur["x_max"].as<double>());
112  }
113  if (cur["y_min"].IsDefined())
114  {
115  zone.setYMin(cur["y_min"].as<double>());
116  }
117  if (cur["y_max"].IsDefined())
118  {
119  zone.setYMax(cur["y_max"].as<double>());
120  }
121  if (cur["z_min"].IsDefined())
122  {
123  zone.setZMin(cur["z_min"].as<double>());
124  }
125  if (cur["z_max"].IsDefined())
126  {
127  zone.setZMax(cur["z_max"].as<double>());
128  }
129 
130  zones_.push_back(zone);
131  }
132 #else
133  // parse the segmentation zones
134  ifstream fin(zones_file.c_str());
135  YAML::Parser zones_parser(fin);
136  YAML::Node zones_config;
137  zones_parser.GetNextDocument(zones_config);
138  for (size_t i = 0; i < zones_config.size(); i++)
139  {
140  // parse the required information
141  string name, parent_frame_id, child_frame_id, bounding_frame_id, segmentation_frame_id;
142  zones_config[i]["name"] >> name;
143  zones_config[i]["parent_frame_id"] >> parent_frame_id;
144  zones_config[i]["child_frame_id"] >> child_frame_id;
145  zones_config[i]["bounding_frame_id"] >> bounding_frame_id;
146  zones_config[i]["segmentation_frame_id"] >> segmentation_frame_id;
147 
148  // create a zone with the frame ID information
149  SegmentationZone zone(name, parent_frame_id, child_frame_id, bounding_frame_id, segmentation_frame_id);
150 
151  // check for the remove surface flag
152  if (zones_config[i].FindValue("remove_surface") != NULL)
153  {
154  bool remove_surface;
155  zones_config[i]["remove_surface"] >> remove_surface;
156  zone.setRemoveSurface(remove_surface);
157  }
158 
159  // check for any set limits
160  if (zones_config[i].FindValue("roll_min") != NULL)
161  {
162  double roll_min;
163  zones_config[i]["roll_min"] >> roll_min;
164  zone.setRollMin(roll_min);
165  }
166  if (zones_config[i].FindValue("roll_max") != NULL)
167  {
168  double roll_max;
169  zones_config[i]["roll_max"] >> roll_max;
170  zone.setRollMax(roll_max);
171  }
172  if (zones_config[i].FindValue("pitch_min") != NULL)
173  {
174  double pitch_min;
175  zones_config[i]["pitch_min"] >> pitch_min;
176  zone.setPitchMin(pitch_min);
177  }
178  if (zones_config[i].FindValue("pitch_max") != NULL)
179  {
180  double pitch_max;
181  zones_config[i]["pitch_max"] >> pitch_max;
182  zone.setPitchMax(pitch_max);
183  }
184  if (zones_config[i].FindValue("yaw_min") != NULL)
185  {
186  double yaw_min;
187  zones_config[i]["yaw_min"] >> yaw_min;
188  zone.setYawMin(yaw_min);
189  }
190  if (zones_config[i].FindValue("yaw_max") != NULL)
191  {
192  double yaw_max;
193  zones_config[i]["yaw_max"] >> yaw_max;
194  zone.setYawMax(yaw_max);
195  }
196  if (zones_config[i].FindValue("x_min") != NULL)
197  {
198  double x_min;
199  zones_config[i]["x_min"] >> x_min;
200  zone.setXMin(x_min);
201  }
202  if (zones_config[i].FindValue("x_max") != NULL)
203  {
204  double x_max;
205  zones_config[i]["x_max"] >> x_max;
206  zone.setXMax(x_max);
207  }
208  if (zones_config[i].FindValue("y_min") != NULL)
209  {
210  double y_min;
211  zones_config[i]["y_min"] >> y_min;
212  zone.setYMin(y_min);
213  }
214  if (zones_config[i].FindValue("y_max") != NULL)
215  {
216  double y_max;
217  zones_config[i]["y_max"] >> y_max;
218  zone.setYMax(y_max);
219  }
220  if (zones_config[i].FindValue("z_min") != NULL)
221  {
222  double z_min;
223  zones_config[i]["z_min"] >> z_min;
224  zone.setZMin(z_min);
225  }
226  if (zones_config[i].FindValue("z_max") != NULL)
227  {
228  double z_max;
229  zones_config[i]["z_max"] >> z_max;
230  zone.setZMax(z_max);
231  }
232 
233  zones_.push_back(zone);
234  }
235 #endif
236 
237  // check how many zones we have
238  if (zones_.size() > 0)
239  {
240  ROS_INFO("%d segmenation zone(s) parsed.", (int) zones_.size());
241  ROS_INFO("Segmenter Successfully Initialized");
242  okay_ = true;
243  } else
244  {
245  ROS_ERROR("No valid segmenation zones defined. Check %s.", zones_file.c_str());
246  okay_ = false;
247  }
248 }
249 
250 bool Segmenter::okay() const
251 {
252  return okay_;
253 }
254 
255 void Segmenter::pointCloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &pc)
256 {
257  // lock for the point cloud
258  boost::mutex::scoped_lock lock(pc_mutex_);
259  // simply store the latest point cloud
260  first_pc_in_ = true;
261  pc_ = pc;
262 }
263 
265 {
266  // check each zone
267  for (size_t i = 0; i < zones_.size(); i++)
268  {
269  // get the current TF information
270  geometry_msgs::TransformStamped tf = tf_buffer_.lookupTransform(zones_[i].getParentFrameID(),
271  zones_[i].getChildFrameID(), ros::Time(0));
272 
273  // convert to a Matrix3x3 to get RPY
274  tf2::Matrix3x3 mat(tf2::Quaternion(tf.transform.rotation.x, tf.transform.rotation.y, tf.transform.rotation.z,
275  tf.transform.rotation.w));
276  double roll, pitch, yaw;
277  mat.getRPY(roll, pitch, yaw);
278 
279  // check if all the bounds meet
280  if (roll >= zones_[i].getRollMin() && pitch >= zones_[i].getPitchMin() && yaw >= zones_[i].getYawMin() &&
281  roll <= zones_[i].getRollMax() && pitch <= zones_[i].getPitchMax() && yaw <= zones_[i].getYawMax())
282  {
283  return zones_[i];
284  }
285  }
286 
287  ROS_WARN("Current state not in a valid segmentation zone. Defaulting to first zone.");
288  return zones_[0];
289 }
290 
291 bool Segmenter::removeObjectCallback(rail_segmentation::RemoveObject::Request &req,
292  rail_segmentation::RemoveObject::Response &res)
293 {
294  // lock for the messages
295  boost::mutex::scoped_lock lock(msg_mutex_);
296  // check the index
297  if (req.index < object_list_.objects.size() && req.index < markers_.markers.size())
298  {
299  // remove
300  object_list_.objects.erase(object_list_.objects.begin() + req.index);
301  // set header information
302  object_list_.header.seq++;
303  object_list_.header.stamp = ros::Time::now();
304  object_list_.cleared = false;
305  // republish
307  // delete marker
308  markers_.markers[req.index].action = visualization_msgs::Marker::DELETE;
310  markers_.markers.erase(markers_.markers.begin() + req.index);
311  return true;
312  } else
313  {
314  ROS_ERROR("Attempted to remove index %d from list of size %ld.", req.index, object_list_.objects.size());
315  return false;
316  }
317 }
318 
319 bool Segmenter::clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
320 {
321  // lock for the messages
322  boost::mutex::scoped_lock lock(msg_mutex_);
323  // empty the list
324  object_list_.objects.clear();
325  object_list_.cleared = true;
326  // set header information
327  object_list_.header.seq++;
328  object_list_.header.stamp = ros::Time::now();
329  // republish
331  // delete markers
332  for (size_t i = 0; i < markers_.markers.size(); i++)
333  {
334  markers_.markers[i].action = visualization_msgs::Marker::DELETE;
335  }
337  markers_.markers.clear();
338 
339  table_marker_.action = visualization_msgs::Marker::DELETE;
341  return true;
342 }
343 
344 bool Segmenter::segmentCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
345 {
346  rail_manipulation_msgs::SegmentedObjectList objects;
347  return segmentObjects(objects);
348 }
349 
350 bool Segmenter::segmentObjectsCallback(rail_manipulation_msgs::SegmentObjects::Request &req,
351  rail_manipulation_msgs::SegmentObjects::Response &res)
352 {
353  return segmentObjects(res.segmented_objects);
354 }
355 
356 bool Segmenter::segmentObjects(rail_manipulation_msgs::SegmentedObjectList &objects)
357 {
358  // check if we have a point cloud first
359  {
360  boost::mutex::scoped_lock lock(pc_mutex_);
361  if (!first_pc_in_)
362  {
363  ROS_WARN("No point cloud received yet. Ignoring segmentation request.");
364  return false;
365  }
366  }
367 
368  // clear the objects first
369  std_srvs::Empty empty;
370  this->clearCallback(empty.request, empty.response);
371 
372  // determine the correct segmentation zone
373  const SegmentationZone &zone = this->getCurrentZone();
374  ROS_INFO("Segmenting in zone '%s'.", zone.getName().c_str());
375 
376  // transform the point cloud to the fixed frame
377  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_pc(new pcl::PointCloud<pcl::PointXYZRGB>);
378  // lock on the point cloud
379  {
380  boost::mutex::scoped_lock lock(pc_mutex_);
381  // perform the copy/transform using TF
382  pcl_ros::transformPointCloud(zone.getBoundingFrameID(), ros::Time(0), *pc_, pc_->header.frame_id,
383  *transformed_pc, tf_);
384  transformed_pc->header.frame_id = zone.getBoundingFrameID();
385  transformed_pc->header.seq = pc_->header.seq;
386  transformed_pc->header.stamp = pc_->header.stamp;
387  }
388 
389  // start with every index
390  pcl::IndicesPtr filter_indices(new vector<int>);
391  filter_indices->resize(transformed_pc->points.size());
392  for (size_t i = 0; i < transformed_pc->points.size(); i++)
393  {
394  filter_indices->at(i) = i;
395  }
396 
397  // check if we need to remove a surface
398  double z_min = zone.getZMin();
399  if (zone.getRemoveSurface())
400  {
401  table_ = this->findSurface(transformed_pc, filter_indices, zone, filter_indices);
402  double z_surface = table_.centroid.z;
403  // check the new bound for Z
404  z_min = max(zone.getZMin(), z_surface + SURFACE_REMOVAL_PADDING);
405  }
406 
407  // check bounding areas (bound the inverse of what we want since PCL will return the removed indicies)
408  pcl::ConditionOr<pcl::PointXYZRGB>::Ptr bounds(new pcl::ConditionOr<pcl::PointXYZRGB>);
409  if (z_min > -numeric_limits<double>::infinity())
410  {
411  bounds->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(
412  new pcl::FieldComparison<pcl::PointXYZRGB>("z", pcl::ComparisonOps::LE, z_min))
413  );
414  }
415  if (zone.getZMax() < numeric_limits<double>::infinity())
416  {
417  bounds->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(
418  new pcl::FieldComparison<pcl::PointXYZRGB>("z", pcl::ComparisonOps::GE, zone.getZMax()))
419  );
420  }
421  if (zone.getYMin() > -numeric_limits<double>::infinity())
422  {
423  bounds->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(
424  new pcl::FieldComparison<pcl::PointXYZRGB>("y", pcl::ComparisonOps::LE, zone.getYMin()))
425  );
426  }
427  if (zone.getYMax() < numeric_limits<double>::infinity())
428  {
429  bounds->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(
430  new pcl::FieldComparison<pcl::PointXYZRGB>("y", pcl::ComparisonOps::GE, zone.getYMax()))
431  );
432  }
433  if (zone.getXMin() > -numeric_limits<double>::infinity())
434  {
435  bounds->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(
436  new pcl::FieldComparison<pcl::PointXYZRGB>("x", pcl::ComparisonOps::LE, zone.getXMin()))
437  );
438  }
439  if (zone.getXMax() < numeric_limits<double>::infinity())
440  {
441  bounds->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(
442  new pcl::FieldComparison<pcl::PointXYZRGB>("x", pcl::ComparisonOps::GE, zone.getXMax()))
443  );
444  }
445 
446  // remove past the given bounds
447  this->inverseBound(transformed_pc, filter_indices, bounds, filter_indices);
448 
449  // publish the filtered and bounded PC pre-segmentation
450  if (debug_)
451  {
452  pcl::PointCloud<pcl::PointXYZRGB>::Ptr debug_pc(new pcl::PointCloud<pcl::PointXYZRGB>);
453  this->extract(transformed_pc, filter_indices, debug_pc);
454  debug_pc_pub_.publish(debug_pc);
455  }
456 
457  // extract clusters
458  vector<pcl::PointIndices> clusters;
459  if (use_color_)
460  this->extractClustersRGB(transformed_pc, filter_indices, clusters);
461  else
462  this->extractClustersEuclidean(transformed_pc, filter_indices, clusters);
463 
464  if (clusters.size() > 0)
465  {
466  // lock for the messages
467  boost::mutex::scoped_lock lock(msg_mutex_);
468  // check each cluster
469  for (size_t i = 0; i < clusters.size(); i++)
470  {
471  // grab the points we need
472  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
473  for (size_t j = 0; j < clusters[i].indices.size(); j++)
474  {
475  cluster->points.push_back(transformed_pc->points[clusters[i].indices[j]]);
476  }
477  cluster->width = cluster->points.size();
478  cluster->height = 1;
479  cluster->is_dense = true;
480  cluster->header.frame_id = transformed_pc->header.frame_id;
481 
482  // check if we need to transform to a different frame
483  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
484  pcl::PCLPointCloud2::Ptr converted(new pcl::PCLPointCloud2);
485  if (zone.getBoundingFrameID() != zone.getSegmentationFrameID())
486  {
487  // perform the copy/transform using TF
488  pcl_ros::transformPointCloud(zone.getSegmentationFrameID(), ros::Time(0), *cluster, cluster->header.frame_id,
489  *transformed_cluster, tf_);
490  transformed_cluster->header.frame_id = zone.getSegmentationFrameID();
491  transformed_cluster->header.seq = cluster->header.seq;
492  transformed_cluster->header.stamp = cluster->header.stamp;
493  pcl::toPCLPointCloud2(*transformed_cluster, *converted);
494  } else
495  {
496  pcl::toPCLPointCloud2(*cluster, *converted);
497  }
498 
499  // convert to a SegmentedObject message
500  rail_manipulation_msgs::SegmentedObject segmented_object;
501  segmented_object.recognized = false;
502 
503  // set the RGB image
504  segmented_object.image = this->createImage(transformed_pc, clusters[i]);
505 
506  // check if we want to publish the image
507  if (debug_)
508  {
509  debug_img_pub_.publish(segmented_object.image);
510  }
511 
512  // set the point cloud
513  pcl_conversions::fromPCL(*converted, segmented_object.point_cloud);
514  segmented_object.point_cloud.header.stamp = ros::Time::now();
515  // create a marker and set the extra fields
516  segmented_object.marker = this->createMarker(converted);
517  segmented_object.marker.id = i;
518 
519  // calculate color features
520  Eigen::Vector3f rgb, lab;
521  rgb[0] = segmented_object.marker.color.r;
522  rgb[1] = segmented_object.marker.color.g;
523  rgb[2] = segmented_object.marker.color.b;
524  lab = RGB2Lab(rgb);
525  segmented_object.rgb.resize(3);
526  segmented_object.cielab.resize(3);
527  segmented_object.rgb[0] = rgb[0];
528  segmented_object.rgb[1] = rgb[1];
529  segmented_object.rgb[2] = rgb[2];
530  segmented_object.cielab[0] = lab[0];
531  segmented_object.cielab[1] = lab[1];
532  segmented_object.cielab[2] = lab[2];
533 
534  // set the centroid
535  Eigen::Vector4f centroid;
536  if (zone.getBoundingFrameID() != zone.getSegmentationFrameID())
537  {
538  pcl::compute3DCentroid(*transformed_cluster, centroid);
539  } else
540  {
541  pcl::compute3DCentroid(*cluster, centroid);
542  }
543  segmented_object.centroid.x = centroid[0];
544  segmented_object.centroid.y = centroid[1];
545  segmented_object.centroid.z = centroid[2];
546 
547  // calculate the minimum volume bounding box (assuming the object is resting on a flat surface)
548  segmented_object.bounding_volume = BoundingVolumeCalculator::computeBoundingVolume(segmented_object.point_cloud);
549 
550  // calculate the axis-aligned bounding box
551  Eigen::Vector4f min_pt, max_pt;
552  pcl::getMinMax3D(*cluster, min_pt, max_pt);
553  segmented_object.width = max_pt[0] - min_pt[0];
554  segmented_object.depth = max_pt[1] - min_pt[1];
555  segmented_object.height = max_pt[2] - min_pt[2];
556 
557  // calculate the center
558  segmented_object.center.x = (max_pt[0] + min_pt[0]) / 2.0;
559  segmented_object.center.y = (max_pt[1] + min_pt[1]) / 2.0;
560  segmented_object.center.z = (max_pt[2] + min_pt[2]) / 2.0;
561 
562  // calculate the orientation
563  pcl::PointCloud<pcl::PointXYZRGB>::Ptr projected_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
564  // project point cloud onto the xy plane
565  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
566  coefficients->values.resize(4);
567  coefficients->values[0] = 0;
568  coefficients->values[1] = 0;
569  coefficients->values[2] = 1.0;
570  coefficients->values[3] = 0;
571  pcl::ProjectInliers<pcl::PointXYZRGB> proj;
572  proj.setModelType(pcl::SACMODEL_PLANE);
573  if (zone.getBoundingFrameID() != zone.getSegmentationFrameID())
574  {
575  proj.setInputCloud(transformed_cluster);
576  } else
577  {
578  proj.setInputCloud(cluster);
579  }
580  proj.setModelCoefficients(coefficients);
581  proj.filter(*projected_cluster);
582 
583  //calculate the Eigen vectors of the projected point cloud's covariance matrix, used to determine orientation
584  Eigen::Vector4f projected_centroid;
585  Eigen::Matrix3f covariance_matrix;
586  pcl::compute3DCentroid(*projected_cluster, projected_centroid);
587  pcl::computeCovarianceMatrixNormalized(*projected_cluster, projected_centroid, covariance_matrix);
588  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance_matrix, Eigen::ComputeEigenvectors);
589  Eigen::Matrix3f eigen_vectors = eigen_solver.eigenvectors();
590  eigen_vectors.col(2) = eigen_vectors.col(0).cross(eigen_vectors.col(1));
591  //calculate rotation from eigenvectors
592  const Eigen::Quaternionf qfinal(eigen_vectors);
593 
594  //convert orientation to a single angle on the 2D plane defined by the segmentation coordinate frame
595  tf::Quaternion tf_quat;
596  tf_quat.setValue(qfinal.x(), qfinal.y(), qfinal.z(), qfinal.w());
597  double r, p, y;
598  tf::Matrix3x3 m(tf_quat);
599  m.getRPY(r, p, y);
600  double angle = r + y;
601  while (angle < -M_PI)
602  {
603  angle += 2 * M_PI;
604  }
605  while (angle > M_PI)
606  {
607  angle -= 2 * M_PI;
608  }
609  segmented_object.orientation = tf::createQuaternionMsgFromYaw(angle);
610 
611  // add to the final list
612  objects.objects.push_back(segmented_object);
613  // add to the markers
614  markers_.markers.push_back(segmented_object.marker);
615  }
616 
617  // create the new list
618  objects.header.seq++;
619  objects.header.stamp = ros::Time::now();
620  objects.header.frame_id = zone.getSegmentationFrameID();
621  objects.cleared = false;
622 
623  // update the new list and publish it
624  object_list_ = objects;
626 
627  // publish the new marker array
629 
630  // add to the markers
631  table_marker_ = table_.marker;
632 
633  // publish the new list
635 
636  // publish the new marker array
638 
639  } else
640  {
641  ROS_WARN("No segmented objects found.");
642  }
643 
644  return true;
645 }
646 
647 rail_manipulation_msgs::SegmentedObject Segmenter::findSurface(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
648  const pcl::IndicesConstPtr &indices_in, const SegmentationZone &zone,
649  const pcl::IndicesPtr &indices_out) const
650 {
651  rail_manipulation_msgs::SegmentedObject table;
652 
653  // use a plane (SAC) segmenter
654  pcl::SACSegmentation<pcl::PointXYZRGB> plane_seg;
655  // set the segmenation parameters
656  plane_seg.setOptimizeCoefficients(true);
657  plane_seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
658  plane_seg.setAxis(Eigen::Vector3f(0, 0, 1));
659  plane_seg.setEpsAngle(SAC_EPS_ANGLE);
660  plane_seg.setMethodType(pcl::SAC_RANSAC);
661  plane_seg.setMaxIterations(SAC_MAX_ITERATIONS);
662  plane_seg.setDistanceThreshold(SAC_DISTANCE_THRESHOLD);
663 
664  // create a copy to work with
665  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_copy(new pcl::PointCloud<pcl::PointXYZRGB>(*in));
666  plane_seg.setInputCloud(pc_copy);
667  plane_seg.setIndices(indices_in);
668 
669  // Check point height -- if the plane is too low or high, extract another
670  while (true)
671  {
672  // points included in the plane (surface)
673  pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices);
674 
675  // segment the the current cloud
676  pcl::ModelCoefficients coefficients;
677  plane_seg.segment(*inliers_ptr, coefficients);
678 
679  // check if we found a surface
680  if (inliers_ptr->indices.size() == 0)
681  {
682  ROS_WARN("Could not find a surface above %fm and below %fm.", zone.getZMin(), zone.getZMax());
683  *indices_out = *indices_in;
684  table.centroid.z = -numeric_limits<double>::infinity();
685  return table;
686  }
687 
688  // remove the plane
689  pcl::PointCloud<pcl::PointXYZRGB> plane;
690  pcl::ExtractIndices<pcl::PointXYZRGB> extract(true);
691  extract.setInputCloud(pc_copy);
692  extract.setIndices(inliers_ptr);
693  extract.setNegative(false);
694  extract.filter(plane);
695  extract.setKeepOrganized(true);
696  plane_seg.setIndices(extract.getRemovedIndices());
697 
698  // check the height
699  double height = this->averageZ(plane.points);
700  if (height >= zone.getZMin() && height <= zone.getZMax())
701  {
702  ROS_INFO("Surface found at %fm.", height);
703  *indices_out = *plane_seg.getIndices();
704 
705  // check if we need to transform to a different frame
706  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_pc(new pcl::PointCloud<pcl::PointXYZRGB>);
707  pcl::PCLPointCloud2::Ptr converted(new pcl::PCLPointCloud2);
708  if (zone.getBoundingFrameID() != zone.getSegmentationFrameID())
709  {
710  // perform the copy/transform using TF
711  pcl_ros::transformPointCloud(zone.getSegmentationFrameID(), ros::Time(0), plane, plane.header.frame_id,
712  *transformed_pc, tf_);
713  transformed_pc->header.frame_id = zone.getSegmentationFrameID();
714  transformed_pc->header.seq = plane.header.seq;
715  transformed_pc->header.stamp = plane.header.stamp;
716  pcl::toPCLPointCloud2(*transformed_pc, *converted);
717  } else
718  {
719  pcl::toPCLPointCloud2(plane, *converted);
720  }
721 
722  // convert to a SegmentedObject message
723  table.recognized = false;
724 
725  // set the RGB image
726  table.image = this->createImage(pc_copy, *inliers_ptr);
727 
728  // check if we want to publish the image
729  if (debug_)
730  {
731  debug_img_pub_.publish(table.image);
732  }
733 
734  // set the point cloud
735  pcl_conversions::fromPCL(*converted, table.point_cloud);
736  table.point_cloud.header.stamp = ros::Time::now();
737  // create a marker and set the extra fields
738  table.marker = this->createMarker(converted);
739  table.marker.id = 0;
740 
741  // set the centroid
742  Eigen::Vector4f centroid;
743  if (zone.getBoundingFrameID() != zone.getSegmentationFrameID())
744  {
745  pcl::compute3DCentroid(*transformed_pc, centroid);
746  } else
747  {
748  pcl::compute3DCentroid(plane, centroid);
749  }
750  table.centroid.x = centroid[0];
751  table.centroid.y = centroid[1];
752  table.centroid.z = centroid[2];
753 
754  // calculate the bounding box
755  Eigen::Vector4f min_pt, max_pt;
756  pcl::getMinMax3D(plane, min_pt, max_pt);
757  table.width = max_pt[0] - min_pt[0];
758  table.depth = max_pt[1] - min_pt[1];
759  table.height = max_pt[2] - min_pt[2];
760 
761  // calculate the center
762  table.center.x = (max_pt[0] + min_pt[0]) / 2.0;
763  table.center.y = (max_pt[1] + min_pt[1]) / 2.0;
764  table.center.z = (max_pt[2] + min_pt[2]) / 2.0;
765 
766 
767  // calculate the orientation
768  pcl::PointCloud<pcl::PointXYZRGB>::Ptr projected_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
769  // project point cloud onto the xy plane
770  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
771  coefficients->values.resize(4);
772  coefficients->values[0] = 0;
773  coefficients->values[1] = 0;
774  coefficients->values[2] = 1.0;
775  coefficients->values[3] = 0;
776  pcl::ProjectInliers<pcl::PointXYZRGB> proj;
777  proj.setModelType(pcl::SACMODEL_PLANE);
778  if (zone.getBoundingFrameID() != zone.getSegmentationFrameID())
779  {
780  proj.setInputCloud(transformed_pc);
781  } else
782  {
783  pcl::PointCloud<pcl::PointXYZRGB>::Ptr plane_ptr(new pcl::PointCloud<pcl::PointXYZRGB>(plane));
784  proj.setInputCloud(plane_ptr);
785  }
786  proj.setModelCoefficients(coefficients);
787  proj.filter(*projected_cluster);
788 
789  //calculate the Eigen vectors of the projected point cloud's covariance matrix, used to determine orientation
790  Eigen::Vector4f projected_centroid;
791  Eigen::Matrix3f covariance_matrix;
792  pcl::compute3DCentroid(*projected_cluster, projected_centroid);
793  pcl::computeCovarianceMatrixNormalized(*projected_cluster, projected_centroid, covariance_matrix);
794  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance_matrix, Eigen::ComputeEigenvectors);
795  Eigen::Matrix3f eigen_vectors = eigen_solver.eigenvectors();
796  eigen_vectors.col(2) = eigen_vectors.col(0).cross(eigen_vectors.col(1));
797  //calculate rotation from eigenvectors
798  const Eigen::Quaternionf qfinal(eigen_vectors);
799 
800  //convert orientation to a single angle on the 2D plane defined by the segmentation coordinate frame
801  tf::Quaternion tf_quat;
802  tf_quat.setValue(qfinal.x(), qfinal.y(), qfinal.z(), qfinal.w());
803  double r, p, y;
804  tf::Matrix3x3 m(tf_quat);
805  m.getRPY(r, p, y);
806  double angle = r + y;
807  while (angle < -M_PI)
808  {
809  angle += 2 * M_PI;
810  }
811  while (angle > M_PI)
812  {
813  angle -= 2 * M_PI;
814  }
815  table.orientation = tf::createQuaternionMsgFromYaw(angle);
816 
817  return table;
818  }
819  }
820 }
821 
822 void Segmenter::extractClustersEuclidean(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
823  const pcl::IndicesConstPtr &indices_in, vector<pcl::PointIndices> &clusters) const
824 {
825  // ignore NaN and infinite values
826  pcl::IndicesPtr valid(new vector<int>);
827  for (size_t i = 0; i < indices_in->size(); i++)
828  {
829  if (pcl_isfinite(in->points[indices_in->at(i)].x) & pcl_isfinite(in->points[indices_in->at(i)].y) &
830  pcl_isfinite(in->points[indices_in->at(i)].z))
831  {
832  valid->push_back(indices_in->at(i));
833  }
834  }
835 
836  pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> seg;
837  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
838  kd_tree->setInputCloud(in);
839  seg.setClusterTolerance(CLUSTER_TOLERANCE);
840  seg.setMinClusterSize(min_cluster_size_);
841  seg.setMaxClusterSize(max_cluster_size_);
842  seg.setSearchMethod(kd_tree);
843  seg.setInputCloud(in);
844  seg.setIndices(valid);
845  seg.extract(clusters);
846 }
847 
848 void Segmenter::extractClustersRGB(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
849  const pcl::IndicesConstPtr &indices_in, vector<pcl::PointIndices> &clusters) const
850 {
851  // ignore NaN and infinite values
852  pcl::IndicesPtr valid(new vector<int>);
853  for (size_t i = 0; i < indices_in->size(); i++)
854  {
855  if (pcl_isfinite(in->points[indices_in->at(i)].x) & pcl_isfinite(in->points[indices_in->at(i)].y) &
856  pcl_isfinite(in->points[indices_in->at(i)].z))
857  {
858  valid->push_back(indices_in->at(i));
859  }
860  }
861  pcl::RegionGrowingRGB<pcl::PointXYZRGB> seg;
862  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
863  kd_tree->setInputCloud(in);
864  seg.setPointColorThreshold(POINT_COLOR_THRESHOLD);
865  seg.setRegionColorThreshold(REGION_COLOR_THRESHOLD);
866  seg.setDistanceThreshold(CLUSTER_TOLERANCE);
867  seg.setMinClusterSize(min_cluster_size_);
868  seg.setMaxClusterSize(max_cluster_size_);
869  seg.setSearchMethod(kd_tree);
870  seg.setInputCloud(in);
871  seg.setIndices(valid);
872  seg.extract(clusters);
873 }
874 
875 sensor_msgs::Image Segmenter::createImage(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
876  const pcl::PointIndices &cluster) const
877 {
878  // determine the bounds of the cluster
879  int row_min = numeric_limits<int>::max();
880  int row_max = numeric_limits<int>::min();
881  int col_min = numeric_limits<int>::max();
882  int col_max = numeric_limits<int>::min();
883 
884  for (size_t i = 0; i < cluster.indices.size(); i++)
885  {
886  // calculate row and column of this point
887  int row = cluster.indices[i] / in->width;
888  int col = cluster.indices[i] - (row * in->width);
889 
890  if (row < row_min)
891  {
892  row_min = row;
893  } else if (row > row_max)
894  {
895  row_max = row;
896  }
897  if (col < col_min)
898  {
899  col_min = col;
900  } else if (col > col_max)
901  {
902  col_max = col;
903  }
904  }
905 
906  // create a ROS image
907  sensor_msgs::Image msg;
908 
909  // set basic information
910  msg.header.frame_id = in->header.frame_id;
911  msg.header.stamp = ros::Time::now();
912  msg.width = col_max - col_min;
913  msg.height = row_max - row_min;
914  // RGB data
915  msg.step = 3 * msg.width;
916  msg.data.resize(msg.step * msg.height);
917  msg.encoding = sensor_msgs::image_encodings::BGR8;
918 
919  // extract the points
920  for (int h = 0; h < msg.height; h++)
921  {
922  for (int w = 0; w < msg.width; w++)
923  {
924  // extract RGB information
925  const pcl::PointXYZRGB &point = in->at(col_min + w, row_min + h);
926  // set RGB data
927  int index = (msg.step * h) + (w * 3);
928  msg.data[index] = point.b;
929  msg.data[index + 1] = point.g;
930  msg.data[index + 2] = point.r;
931  }
932  }
933 
934  return msg;
935 }
936 
937 visualization_msgs::Marker Segmenter::createMarker(const pcl::PCLPointCloud2::ConstPtr &pc) const
938 {
939  visualization_msgs::Marker marker;
940  // set header field
941  marker.header.frame_id = pc->header.frame_id;
942 
943  // default position
944  marker.pose.position.x = 0.0;
945  marker.pose.position.y = 0.0;
946  marker.pose.position.z = 0.0;
947  marker.pose.orientation.x = 0.0;
948  marker.pose.orientation.y = 0.0;
949  marker.pose.orientation.z = 0.0;
950  marker.pose.orientation.w = 1.0;
951 
952  // default scale
953  marker.scale.x = MARKER_SCALE;
954  marker.scale.y = MARKER_SCALE;
955  marker.scale.z = MARKER_SCALE;
956 
957  // set the type of marker and our color of choice
958  marker.type = visualization_msgs::Marker::CUBE_LIST;
959  marker.color.a = 1.0;
960 
961  // downsample point cloud for visualization
962  pcl::PCLPointCloud2 downsampled;
963  pcl::VoxelGrid<pcl::PCLPointCloud2> voxel_grid;
964  voxel_grid.setInputCloud(pc);
966  voxel_grid.filter(downsampled);
967 
968  // convert to an easy to use point cloud message
969  sensor_msgs::PointCloud2 pc2_msg;
970  pcl_conversions::fromPCL(downsampled, pc2_msg);
971  sensor_msgs::PointCloud pc_msg;
973 
974  // place in the marker message
975  marker.points.resize(pc_msg.points.size());
976  int r = 0, g = 0, b = 0;
977  for (size_t j = 0; j < pc_msg.points.size(); j++)
978  {
979  marker.points[j].x = pc_msg.points[j].x;
980  marker.points[j].y = pc_msg.points[j].y;
981  marker.points[j].z = pc_msg.points[j].z;
982 
983  // use average RGB
984  uint32_t rgb = *reinterpret_cast<int *>(&pc_msg.channels[0].values[j]);
985  r += (int) ((rgb >> 16) & 0x0000ff);
986  g += (int) ((rgb >> 8) & 0x0000ff);
987  b += (int) ((rgb) & 0x0000ff);
988  }
989 
990  // set average RGB
991  marker.color.r = ((float) r / (float) pc_msg.points.size()) / 255.0;
992  marker.color.g = ((float) g / (float) pc_msg.points.size()) / 255.0;
993  marker.color.b = ((float) b / (float) pc_msg.points.size()) / 255.0;
994  marker.color.a = 1.0;
995 
996  return marker;
997 }
998 
999 void Segmenter::extract(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
1000  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &out) const
1001 {
1002  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
1003  extract.setInputCloud(in);
1004  extract.setIndices(indices_in);
1005  extract.filter(*out);
1006 }
1007 
1008 void Segmenter::inverseBound(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
1009  const pcl::IndicesConstPtr &indices_in,
1010  const pcl::ConditionBase<pcl::PointXYZRGB>::Ptr &conditions,
1011  const pcl::IndicesPtr &indices_out) const
1012 {
1013  // use a temp point cloud to extract the indices
1014  pcl::PointCloud<pcl::PointXYZRGB> tmp;
1015  //pcl::ConditionalRemoval<pcl::PointXYZRGB> removal(conditions, true);
1016  pcl::ConditionalRemoval<pcl::PointXYZRGB> removal(true);
1017  removal.setCondition(conditions);
1018  removal.setInputCloud(in);
1019  removal.setIndices(indices_in);
1020  removal.filter(tmp);
1021  *indices_out = *removal.getRemovedIndices();
1022 }
1023 
1024 double Segmenter::averageZ(const vector<pcl::PointXYZRGB, Eigen::aligned_allocator<pcl::PointXYZRGB> > &v) const
1025 {
1026  double avg = 0.0;
1027  for (size_t i = 0; i < v.size(); i++)
1028  {
1029  avg += v[i].z;
1030  }
1031  return (avg / (double) v.size());
1032 }
1033 
1034 //convert from RGB color space to CIELAB color space, adapted from pcl/registration/gicp6d
1035 Eigen::Vector3f RGB2Lab (const Eigen::Vector3f& colorRGB)
1036 {
1037  // for sRGB -> CIEXYZ see http://www.easyrgb.com/index.php?X=MATH&H=02#text2
1038  // for CIEXYZ -> CIELAB see http://www.easyrgb.com/index.php?X=MATH&H=07#text7
1039 
1040  double R, G, B, X, Y, Z;
1041 
1042  R = colorRGB[0];
1043  G = colorRGB[1];
1044  B = colorRGB[2];
1045 
1046  // linearize sRGB values
1047  if (R > 0.04045)
1048  R = pow ( (R + 0.055) / 1.055, 2.4);
1049  else
1050  R = R / 12.92;
1051 
1052  if (G > 0.04045)
1053  G = pow ( (G + 0.055) / 1.055, 2.4);
1054  else
1055  G = G / 12.92;
1056 
1057  if (B > 0.04045)
1058  B = pow ( (B + 0.055) / 1.055, 2.4);
1059  else
1060  B = B / 12.92;
1061 
1062  // postponed:
1063  // R *= 100.0;
1064  // G *= 100.0;
1065  // B *= 100.0;
1066 
1067  // linear sRGB -> CIEXYZ
1068  X = R * 0.4124 + G * 0.3576 + B * 0.1805;
1069  Y = R * 0.2126 + G * 0.7152 + B * 0.0722;
1070  Z = R * 0.0193 + G * 0.1192 + B * 0.9505;
1071 
1072  // *= 100.0 including:
1073  X /= 0.95047; //95.047;
1074  // Y /= 1;//100.000;
1075  Z /= 1.08883; //108.883;
1076 
1077  // CIEXYZ -> CIELAB
1078  if (X > 0.008856)
1079  X = pow (X, 1.0 / 3.0);
1080  else
1081  X = 7.787 * X + 16.0 / 116.0;
1082 
1083  if (Y > 0.008856)
1084  Y = pow (Y, 1.0 / 3.0);
1085  else
1086  Y = 7.787 * Y + 16.0 / 116.0;
1087 
1088  if (Z > 0.008856)
1089  Z = pow (Z, 1.0 / 3.0);
1090  else
1091  Z = 7.787 * Z + 16.0 / 116.0;
1092 
1093  Eigen::Vector3f colorLab;
1094  colorLab[0] = static_cast<float> (116.0 * Y - 16.0);
1095  colorLab[1] = static_cast<float> (500.0 * (X - Y));
1096  colorLab[2] = static_cast<float> (200.0 * (Y - Z));
1097 
1098  return colorLab;
1099 }
const SegmentationZone & getCurrentZone() const
Determine the current zone based on the latest state of the TF tree.
Definition: Segmenter.cpp:264
static rail_manipulation_msgs::BoundingVolume computeBoundingVolume(sensor_msgs::PointCloud2 cloud)
Fit a z-axis-aligned bounding box to x-y plane principal direction of point cloud.
bool segmentObjectsCallback(rail_manipulation_msgs::SegmentObjects::Request &req, rail_manipulation_msgs::SegmentObjects::Response &res)
Callback for the main segmentation request.
Definition: Segmenter.cpp:350
static const double POINT_COLOR_THRESHOLD
Definition: Segmenter.h:122
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
ros::ServiceServer remove_object_srv_
Definition: Segmenter.h:338
void publish(const boost::shared_ptr< M > &message) const
void setYawMin(const double yaw_min)
Yaw min value mutator.
double getZMax() const
Z max value accessor.
void setZMin(const double z_min)
Z min value mutator.
visualization_msgs::Marker table_marker_
Definition: Segmenter.h:359
rail_manipulation_msgs::SegmentedObjectList object_list_
Definition: Segmenter.h:353
void extract(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &out) const
Extract a new point cloud based on the given indices.
Definition: Segmenter.cpp:999
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void setYawMax(const double yaw_max)
Yaw max value mutator.
ros::Publisher debug_img_pub_
Definition: Segmenter.h:340
static const double SAC_EPS_ANGLE
Definition: Segmenter.h:108
ros::ServiceServer segment_objects_srv_
Definition: Segmenter.h:338
bool segmentCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Callback for the main segmentation request.
Definition: Segmenter.cpp:344
static const int DEFAULT_MAX_CLUSTER_SIZE
Definition: Segmenter.h:118
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
tf::TransformListener tf_
Definition: Segmenter.h:344
pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr pc_
Definition: Segmenter.h:351
ros::NodeHandle private_node_
Definition: Segmenter.h:336
void setPitchMin(const double pitch_min)
Pitch min value mutator.
void extractClustersRGB(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, std::vector< pcl::PointIndices > &clusters) const
Find clusters in a point cloud.
Definition: Segmenter.cpp:848
static bool convertPointCloud2ToPointCloud(const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud &output)
bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Callback for the clear request.
Definition: Segmenter.cpp:319
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
const std::string & getBoundingFrameID() const
Segmentation frame ID value accessor.
rail_manipulation_msgs::SegmentedObject findSurface(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, const SegmentationZone &zone, const pcl::IndicesPtr &indices_out) const
Find and remove a surface from the given point cloud.
Definition: Segmenter.cpp:647
static const float DOWNSAMPLE_LEAF_SIZE
Definition: Segmenter.h:126
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
static const bool DEFAULT_DEBUG
Definition: Segmenter.h:106
double getYMin() const
Y min value accessor.
void setPitchMax(const double pitch_max)
Pitch max value mutator.
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
bool segmentObjects(rail_manipulation_msgs::SegmentedObjectList &objects)
Callback for the main segmentation request.
Definition: Segmenter.cpp:356
visualization_msgs::MarkerArray markers_
Definition: Segmenter.h:357
static const double CLUSTER_TOLERANCE
Definition: Segmenter.h:120
void setRollMin(const double roll_min)
Roll min value mutator.
void setYMin(const double y_min)
Y min value mutator.
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
static const double SURFACE_REMOVAL_PADDING
Definition: Segmenter.h:114
double getXMax() const
X max value accessor.
bool okay() const
A check for a valid Segmenter.
Definition: Segmenter.cpp:250
std::vector< SegmentationZone > zones_
Definition: Segmenter.h:333
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
double getZMin() const
Z min value accessor.
bool removeObjectCallback(rail_segmentation::RemoveObject::Request &req, rail_segmentation::RemoveObject::Response &res)
Callback for the remove object request.
Definition: Segmenter.cpp:291
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void pointCloudCallback(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &pc)
Callback for the point cloud topic.
Definition: Segmenter.cpp:255
void setXMax(const double x_max)
X max value mutator.
double getYMax() const
Y max value accessor.
static const int SAC_MAX_ITERATIONS
Definition: Segmenter.h:112
ros::Publisher markers_pub_
Definition: Segmenter.h:340
double averageZ(const std::vector< pcl::PointXYZRGB, Eigen::aligned_allocator< pcl::PointXYZRGB > > &v) const
Find the average Z value of the point vector.
Definition: Segmenter.cpp:1024
void setZMax(const double z_max)
Z max value mutator.
ros::Publisher segmented_objects_pub_
Definition: Segmenter.h:340
static const int DEFAULT_MIN_CLUSTER_SIZE
Definition: Segmenter.h:116
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static const double REGION_COLOR_THRESHOLD
Definition: Segmenter.h:124
ROSLIB_DECL std::string getPath(const std::string &package_name)
bool getRemoveSurface() const
Remove surface value accessor.
static const double MARKER_SCALE
Definition: Segmenter.h:128
The main segmentation node object.
ros::Publisher table_marker_pub_
Definition: Segmenter.h:340
static const double SAC_DISTANCE_THRESHOLD
Definition: Segmenter.h:110
TFSIMD_FORCE_INLINE const tfScalar & w() const
void inverseBound(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, const pcl::ConditionBase< pcl::PointXYZRGB >::Ptr &conditions, const pcl::IndicesPtr &indices_out) const
Bound a point cloud based on the inverse of a set of conditions.
Definition: Segmenter.cpp:1008
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
double getXMin() const
X min value accessor.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
bool getParam(const std::string &key, std::string &s) const
visualization_msgs::Marker createMarker(const pcl::PCLPointCloud2::ConstPtr &pc) const
Create a Marker from the given point cloud.
Definition: Segmenter.cpp:937
const std::string & getSegmentationFrameID() const
Segmentation frame ID value accessor.
ros::Subscriber point_cloud_sub_
Definition: Segmenter.h:342
ros::Publisher debug_pc_pub_
Definition: Segmenter.h:340
rail_manipulation_msgs::SegmentedObject table_
Definition: Segmenter.h:355
void setXMin(const double x_min)
X min value mutator.
static Time now()
Eigen::Vector3f RGB2Lab(const Eigen::Vector3f &colorRGB)
Definition: Segmenter.cpp:1035
void setRollMax(const double roll_max)
Roll max value mutator.
ros::ServiceServer segment_srv_
Definition: Segmenter.h:338
void setYMax(const double y_max)
Y max value mutator.
The criteria for a segmentation zone.
ros::ServiceServer clear_srv_
Definition: Segmenter.h:338
#define ROS_ERROR(...)
void extractClustersEuclidean(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, std::vector< pcl::PointIndices > &clusters) const
Find clusters in a point cloud.
Definition: Segmenter.cpp:822
void setRemoveSurface(const bool remove_surface)
Remove surface value mutator.
tf2_ros::Buffer tf_buffer_
Definition: Segmenter.h:346
const std::string & getName() const
Name value accessor.
sensor_msgs::Image createImage(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::PointIndices &cluster) const
Create a cropped image of the segmented object.
Definition: Segmenter.cpp:875


rail_segmentation
Author(s): Russell Toris , David Kent
autogenerated on Sun Jun 30 2019 19:58:20