38 #include <pcl/filters/extract_indices.h> 39 #include <pcl/segmentation/extract_polygonal_prism_data.h> 42 #include <pcl/filters/project_inliers.h> 51 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
52 DiagnosticNodelet::onInit();
58 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
59 nonplane_pub_ = advertise<pcl::PointCloud<pcl::PointXYZRGB> >(*
pnh_,
"output_nonplane_cloud", 1);
68 pnh_->param(
"sensor_frame",
sensor_frame_, std::string(
"head_root"));
71 NODELET_WARN(
"'~use_sensor_frame' and '~use_coefficients' cannot be enabled at the same time." 72 " disabled '~use_coefficients'");
76 NODELET_WARN(
"'~use_coefficients' and '~use_sensor_frame' are both disabled." 77 " Normal axes of planes are estimated with PCA" 78 " and they may be flipped unintentionally.");
84 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
85 dynamic_reconfigure::Server<Config>::CallbackType
f =
87 srv_->setCallback (f);
173 else if (
magnify_ != config.maginify)
185 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
186 "MultiPlaneExtraction running");
191 DiagnosticNodelet::updateDiagnostic(stat);
195 const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons)
197 jsk_recognition_msgs::ClusterPointIndices indices;
198 indices.header = polygons->header;
199 indices.cluster_indices.resize(polygons->polygons.size());
201 boost::make_shared<jsk_recognition_msgs::ClusterPointIndices>(indices));
205 const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons)
207 jsk_recognition_msgs::ModelCoefficientsArray coeffs;
208 coeffs.header = polygons->header;
209 coeffs.coefficients.resize(polygons->polygons.size());
211 boost::make_shared<jsk_recognition_msgs::ModelCoefficientsArray>(coeffs));
215 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices,
216 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients,
217 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons)
223 indices->header.frame_id) ||
225 coefficients->header.frame_id) ||
227 polygons->header.frame_id)) {
228 std::ostringstream oss;
229 oss <<
"frame id does not match. cloud: " << input->header.frame_id
230 <<
", polygons: " << polygons->header.frame_id;
232 oss <<
", indices: " << indices->header.frame_id;
235 oss <<
", coefficients: " << coefficients->header.frame_id;
242 Eigen::Vector3f viewpoint = Eigen::Vector3f::Zero();
247 input->header.frame_id,
251 Eigen::Affine3f sensor_pose;
253 viewpoint = Eigen::Vector3f(sensor_pose.translation());
270 pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(
new pcl::PointCloud<pcl::PointXYZRGB>());
271 pcl::PointCloud<pcl::PointXYZRGB>::Ptr nonplane_cloud(
new pcl::PointCloud<pcl::PointXYZRGB>());
275 pcl::PointIndices::Ptr all_indices (
new pcl::PointIndices);
276 for (
size_t i = 0; i < indices->cluster_indices.size(); i++) {
277 std::vector<int> one_indices = indices->cluster_indices[i].indices;
278 for (
size_t j = 0; j < one_indices.size(); j++) {
279 all_indices->indices.push_back(one_indices[j]);
284 pcl::ExtractIndices<pcl::PointXYZRGB> extract_nonplane;
285 extract_nonplane.setNegative(
true);
287 extract_nonplane.setInputCloud(input_cloud);
288 extract_nonplane.setIndices(all_indices);
289 extract_nonplane.filter(*nonplane_cloud);
290 sensor_msgs::PointCloud2 ros_result;
292 ros_result.header = input->header;
296 nonplane_cloud = input_cloud;
301 std::set<int> result_set;
303 for (
size_t plane_i = 0; plane_i < polygons->polygons.size(); plane_i++) {
306 for (
size_t vec_i = 0; vec_i < 3; ++vec_i)
307 viewpoint[vec_i] = coefficients->coefficients[plane_i].values[vec_i];
309 pcl::ExtractPolygonalPrismData<pcl::PointXYZRGB> prism_extract;
310 prism_extract.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
311 pcl::PointCloud<pcl::PointXYZRGB>::Ptr hull_cloud(
new pcl::PointCloud<pcl::PointXYZRGB>());
312 geometry_msgs::Polygon the_polygon = polygons->polygons[plane_i].polygon;
313 if (the_polygon.points.size() <= 2) {
318 Eigen::Vector3f centroid(0, 0, 0);
319 for (
size_t i = 0; i < the_polygon.points.size(); i++) {
321 jsk_recognition_utils::pointFromXYZToXYZ<geometry_msgs::Point32, pcl::PointXYZRGB>(
322 the_polygon.points[i], p);
323 centroid = centroid + p.getVector3fMap();
325 centroid = centroid / the_polygon.points.size();
327 for (
size_t i = 0; i < the_polygon.points.size(); i++) {
329 jsk_recognition_utils::pointFromXYZToXYZ<geometry_msgs::Point32, pcl::PointXYZRGB>(
330 the_polygon.points[i], p);
331 Eigen::Vector3f dir = (p.getVector3fMap() - centroid).
normalized();
332 p.getVector3fMap() = dir *
magnify_ + p.getVector3fMap();
333 hull_cloud->points.push_back(p);
336 pcl::PointXYZRGB p_last;
337 jsk_recognition_utils::pointFromXYZToXYZ<geometry_msgs::Point32, pcl::PointXYZRGB>(
338 the_polygon.points[0], p_last);
339 hull_cloud->points.push_back(p_last);
341 prism_extract.setInputCloud(nonplane_cloud);
343 prism_extract.setInputPlanarHull(hull_cloud);
344 pcl::PointIndices output_indices;
345 prism_extract.segment(output_indices);
347 for (
size_t i = 0; i < output_indices.indices.size(); i++) {
348 result_set.insert(output_indices.indices[i]);
354 pcl::PointCloud<pcl::PointXYZRGB> result_cloud;
355 pcl::PointIndices::Ptr all_result_indices (
new pcl::PointIndices());
356 for (std::set<int>::iterator it = result_set.begin();
357 it != result_set.end();
359 all_result_indices->indices.push_back(*it);
362 pcl::ExtractIndices<pcl::PointXYZRGB> extract_all_indices;
364 extract_all_indices.setInputCloud(nonplane_cloud);
365 extract_all_indices.setIndices(all_result_indices);
366 extract_all_indices.filter(result_cloud);
368 sensor_msgs::PointCloud2 ros_result;
370 ros_result.header = input->header;
374 ros_indices.header = input->header;
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
void summary(unsigned char lvl, const std::string s)
tf::StampedTransform lookupTransformWithDuration(tf::TransformListener *listener, const std::string &to_frame, const std::string &from_frame, const ros::Time &stamp, ros::Duration duration)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
TFSIMD_FORCE_INLINE Vector3 normalized() const
#define NODELET_ERROR_STREAM(...)
virtual void add(double v)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
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)
#define ROS_WARN_STREAM_ONCE(args)
void add(const MConstPtr &msg)
bool isSameFrameId(const std::string &a, const std::string &b)
void add(const std::string &key, const T &val)
static tf::TransformListener * getInstance()
pcl::PointIndices PCLIndicesMsg
std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)
Connection registerCallback(const C &callback)