34 #define BOOST_PARAMETER_MAX_ARITY 7 37 #include <visualization_msgs/Marker.h> 38 #include <pcl/surface/concave_hull.h> 39 #include <pcl/sample_consensus/method_types.h> 40 #include <pcl/sample_consensus/model_types.h> 41 #include <pcl/segmentation/sac_segmentation.h> 42 #include <pcl/filters/project_inliers.h> 43 #include <pcl/filters/extract_indices.h> 44 #include <pcl/ModelCoefficients.h> 45 #include <pcl/kdtree/kdtree.h> 46 #include <pcl/segmentation/extract_clusters.h> 47 #include <pcl/common/centroid.h> 53 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
54 DiagnosticNodelet::onInit();
55 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
56 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
58 srv_->setCallback (f);
61 *
pnh_,
"output/hint/polygon", 1);
63 *
pnh_,
"output/hint/polygon_array", 1);
65 *
pnh_,
"output/hint/inliers", 1);
67 *
pnh_,
"output/hint/coefficients", 1);
69 *
pnh_,
"output/polygon", 1);
71 *
pnh_,
"output/polygon_array", 1);
73 *
pnh_,
"output/hint_filtered_indices", 1);
75 *
pnh_,
"output/plane_filtered_indices", 1);
77 *
pnh_,
"output/density_filtered_indices", 1);
79 *
pnh_,
"output/euclidean_filtered_indices", 1);
81 *
pnh_,
"output/inliers", 1);
83 *
pnh_,
"output/coefficients", 1);
91 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
104 Config &config, uint32_t level)
126 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
127 const sensor_msgs::PointCloud2::ConstPtr& hint_cloud_msg)
131 pcl::PointCloud<pcl::PointNormal>::Ptr
132 input_cloud (
new pcl::PointCloud<pcl::PointNormal>);
133 pcl::PointCloud<pcl::PointXYZ>::Ptr
134 hint_cloud (
new pcl::PointCloud<pcl::PointXYZ>);
153 pcl::PointCloud<pcl::PointNormal>::Ptr input_cloud,
154 const std::vector<pcl::PointIndices>& cluster_indices,
157 Eigen::Vector3f center = hint_convex->centroid();
158 double min_dist = DBL_MAX;
160 for (
size_t i = 0; i < cluster_indices.size(); i++) {
161 Eigen::Vector4f center_cluster4;
162 pcl::compute3DCentroid<pcl::PointNormal>(*input_cloud,
163 cluster_indices[i].indices,
165 Eigen::Vector3f center_cluster3(center_cluster4[0], center_cluster4[1], center_cluster4[2]);
166 double dist = (center - center_cluster3).norm();
167 if (dist < min_dist) {
172 pcl::PointIndices::Ptr ret (
new pcl::PointIndices);
173 ret->indices = cluster_indices[min_index].indices;
178 const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
179 const pcl::PointIndices::Ptr indices,
180 pcl::PointIndices& output)
183 pcl::KdTreeFLANN<pcl::PointNormal> kdtree;
184 pcl::KdTreeFLANN<pcl::PointNormal>::IndicesPtr indices_ptr
185 (
new std::vector<int>);
186 *indices_ptr = indices->indices;
187 kdtree.setInputCloud(cloud, indices_ptr);
188 for (
size_t i = 0; i < indices->indices.size(); i++) {
189 int point_index = indices->indices[i];
190 std::vector<int> result_indices;
191 std::vector<float> result_distances;
193 result_indices, result_distances);
195 output.indices.push_back(point_index);
202 output.header = cloud->header;
209 const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
210 const pcl::PointIndices::Ptr indices,
212 pcl::PointIndices& output)
215 pcl::EuclideanClusterExtraction<pcl::PointNormal> ec;
217 pcl::search::KdTree<pcl::PointNormal>::Ptr tree
218 (
new pcl::search::KdTree<pcl::PointNormal>);
219 tree->setInputCloud(cloud);
220 ec.setSearchMethod(tree);
221 ec.setIndices(indices);
222 ec.setInputCloud(cloud);
224 std::vector<pcl::PointIndices> cluster_indices;
225 ec.extract(cluster_indices);
226 if (cluster_indices.size() == 0) {
230 pcl::PointIndices::Ptr filtered_indices
232 output = *filtered_indices;
237 output.header = cloud->header;
244 const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
246 pcl::PointIndices& output)
248 for (
size_t i = 0; i < cloud->points.size(); i++) {
249 pcl::PointNormal
p = cloud->points[i];
250 if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.y)) {
251 Eigen::Vector4f
v = p.getVector4fMap();
253 Eigen::Vector3f
n(p.normal_x, p.normal_y, p.normal_z);
255 output.indices.push_back(i);
260 output.header = cloud->header;
267 const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
268 const pcl::PointIndices::Ptr indices,
269 const Eigen::Vector3f& normal,
270 pcl::PointIndices& output,
271 pcl::ModelCoefficients& coefficients)
273 pcl::SACSegmentation<pcl::PointNormal> seg;
274 seg.setOptimizeCoefficients (
true);
275 seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
276 seg.setMethodType (pcl::SAC_RANSAC);
281 seg.setInputCloud(cloud);
282 seg.setIndices(indices);
283 seg.segment(output, coefficients);
284 coefficients.header = cloud->header;
285 output.header = cloud->header;
292 pcl::PointCloud<pcl::PointNormal>::Ptr input_cloud,
295 pcl::PointIndices::Ptr candidate_inliers (
new pcl::PointIndices);
296 hintFilter(input_cloud, hint_convex, *candidate_inliers);
299 pcl::PointIndices::Ptr plane_inliers (
new pcl::PointIndices);
300 pcl::ModelCoefficients::Ptr plane_coefficients(
new pcl::ModelCoefficients);
302 hint_convex->getNormal(),
303 *plane_inliers, *plane_coefficients);
304 if (plane_inliers->indices.size() <
min_size_) {
305 NODELET_ERROR(
"failed to detect by plane fitting filtering");
309 Eigen::Vector3f plane_normal(plane_coefficients->values[0], plane_coefficients->values[1], plane_coefficients->values[2]);
310 if (plane_normal.dot(Eigen::Vector3f::UnitZ()) > 0) {
312 plane_coefficients->values[0] = -plane_coefficients->values[0];
313 plane_coefficients->values[1] = -plane_coefficients->values[1];
314 plane_coefficients->values[2] = -plane_coefficients->values[2];
315 plane_coefficients->values[3] = -plane_coefficients->values[3];
318 pcl::PointIndices::Ptr euclidean_filtered_indices(
new pcl::PointIndices);
320 *euclidean_filtered_indices);
321 if (euclidean_filtered_indices->indices.size() <
min_size_) {
325 pcl::PointIndices::Ptr density_filtered_indices (
new pcl::PointIndices);
327 input_cloud, euclidean_filtered_indices, *density_filtered_indices);
329 if (density_filtered_indices->indices.size() <
min_size_) {
334 = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointNormal>(
335 input_cloud, density_filtered_indices, plane_coefficients);
338 input_cloud->header);
352 const pcl::PCLHeader&
header)
354 geometry_msgs::PolygonStamped ros_polygon;
355 ros_polygon.polygon = convex->toROSMsg();
357 jsk_recognition_msgs::PolygonArray ros_polygon_array;
359 ros_polygon_array.polygons.push_back(
361 pub_polygon_array.
publish(ros_polygon_array);
362 pub_polygon.
publish(ros_polygon);
366 pcl::PointCloud<pcl::PointXYZ>::Ptr hint_cloud,
369 pcl::PointIndices::Ptr hint_inliers (
new pcl::PointIndices);
370 pcl::ModelCoefficients::Ptr hint_coefficients(
new pcl::ModelCoefficients);
371 pcl::SACSegmentation<pcl::PointXYZ> seg;
372 seg.setOptimizeCoefficients (
true);
373 seg.setModelType (pcl::SACMODEL_PLANE);
374 seg.setMethodType (pcl::SAC_RANSAC);
377 seg.setInputCloud(hint_cloud);
378 seg.segment(*hint_inliers, *hint_coefficients);
380 convex = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZ>(
381 hint_cloud, hint_inliers, hint_coefficients);
virtual bool detectHintPlane(pcl::PointCloud< pcl::PointXYZ >::Ptr hint_cloud, jsk_recognition_utils::ConvexPolygon::Ptr &convex)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
bool enable_euclidean_filtering_
#define NODELET_ERROR(...)
virtual void detect(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const sensor_msgs::PointCloud2::ConstPtr &hint_cloud_msg)
void publish(const boost::shared_ptr< M > &message) const
double normal_filter_eps_angle_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
ros::Publisher pub_hint_filtered_indices_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::HintedPlaneDetector, nodelet::Nodelet)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual pcl::PointIndices::Ptr getBestCluster(pcl::PointCloud< pcl::PointNormal >::Ptr input_cloud, const std::vector< pcl::PointIndices > &cluster_indices, const jsk_recognition_utils::ConvexPolygon::Ptr hint_convex)
virtual void hintFilter(const pcl::PointCloud< pcl::PointNormal >::Ptr cloud, const jsk_recognition_utils::ConvexPolygon::Ptr hint_convex, pcl::PointIndices &output)
ros::Publisher pub_plane_filtered_indices_
ros::Publisher pub_hint_coefficients_
ros::Publisher pub_polygon_
virtual void publishPolygon(const jsk_recognition_utils::ConvexPolygon::Ptr convex, ros::Publisher &pub_polygon, ros::Publisher &pub_polygon_array, const pcl::PCLHeader &header)
ros::Publisher pub_hint_inliers_
ros::Publisher pub_hint_polygon_
double hint_outlier_threashold_
virtual void planeFilter(const pcl::PointCloud< pcl::PointNormal >::Ptr cloud, const pcl::PointIndices::Ptr indices, const Eigen::Vector3f &normal, pcl::PointIndices &output, pcl::ModelCoefficients &coefficients)
bool enable_density_filtering_
virtual bool detectLargerPlane(pcl::PointCloud< pcl::PointNormal >::Ptr input_cloud, jsk_recognition_utils::ConvexPolygon::Ptr hint_convex)
HintedPlaneDetectorConfig Config
virtual void unsubscribe()
ros::Publisher pub_hint_polygon_array_
virtual void euclideanFilter(const pcl::PointCloud< pcl::PointNormal >::Ptr cloud, const pcl::PointIndices::Ptr indices, const jsk_recognition_utils::ConvexPolygon::Ptr hint_convex, pcl::PointIndices &output)
virtual void configCallback(Config &config, uint32_t level)
int euclidean_clustering_filter_min_size_
virtual void densityFilter(const pcl::PointCloud< pcl::PointNormal >::Ptr cloud, const pcl::PointIndices::Ptr indices, pcl::PointIndices &output)
ros::Publisher pub_euclidean_filtered_indices_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_
#define NODELET_INFO(...)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
double outlier_threashold_
ros::Publisher pub_density_filtered_indices_
double euclidean_clustering_filter_tolerance_
bool enable_normal_filtering_
ros::Publisher pub_coefficients_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_hint_cloud_
bool enable_distance_filtering_
pcl::PointIndices PCLIndicesMsg
pcl::ModelCoefficients PCLModelCoefficientMsg
std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)
ros::Publisher pub_polygon_array_
ros::Publisher pub_inliers_