From 53697344e08b90e8a88fc55aa6d986a6ab377a5a Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Tue, 3 Jun 2014 18:36:56 +0900 Subject: [PATCH 1/7] add OcludedPlaneEstimator nodelet to estimate the ocluded planes --- jsk_pcl_ros/CMakeLists.txt | 2 + jsk_pcl_ros/catkin.cmake | 3 + jsk_pcl_ros/cfg/OcludedPlaneEstimator.cfg | 21 ++ .../jsk_pcl_ros/ocluded_plane_estimator.h | 88 ++++++++ .../include/jsk_pcl_ros/plane_rejector.h | 2 +- jsk_pcl_ros/jsk_pcl_nodelets.xml | 4 + .../organized_multi_plane_segmentation.launch | 27 +++ .../src/ocluded_plane_estimator_nodelet.cpp | 210 ++++++++++++++++++ 8 files changed, 356 insertions(+), 1 deletion(-) create mode 100755 jsk_pcl_ros/cfg/OcludedPlaneEstimator.cfg create mode 100644 jsk_pcl_ros/include/jsk_pcl_ros/ocluded_plane_estimator.h create mode 100644 jsk_pcl_ros/src/ocluded_plane_estimator_nodelet.cpp diff --git a/jsk_pcl_ros/CMakeLists.txt b/jsk_pcl_ros/CMakeLists.txt index d8655ff9d9..91d5a3b23c 100644 --- a/jsk_pcl_ros/CMakeLists.txt +++ b/jsk_pcl_ros/CMakeLists.txt @@ -96,6 +96,8 @@ jsk_pcl_nodelet(src/static_polygon_array_publisher_nodelet.cpp "jsk_pcl/StaticPolygonArrayPublisher" "static_polygon_array_publisher") jsk_pcl_nodelet(src/polygon_array_transformer_nodelet.cpp "jsk_pcl/PolygonArrayTransformer" "polygon_array_transformer_nodelet") +jsk_pcl_nodelet(src/ocluded_plane_estimator_nodelet.cpp + "jsk_pcl/OcludedPlaneEstimator" "ocluded_plane_estimator") rosbuild_add_library (jsk_pcl_ros ${jsk_pcl_nodelet_sources} # ${pcl_ros_PACKAGE_PATH}/src/pcl_ros/features/feature.cpp diff --git a/jsk_pcl_ros/catkin.cmake b/jsk_pcl_ros/catkin.cmake index b06c9323ae..5c33d73c69 100644 --- a/jsk_pcl_ros/catkin.cmake +++ b/jsk_pcl_ros/catkin.cmake @@ -38,6 +38,7 @@ generate_dynamic_reconfigure_options( cfg/MultiPlaneExtraction.cfg cfg/NormalEstimationIntegralImage.cfg cfg/PlaneRejector.cfg + cfg/OcludedPlaneEstimator.cfg ) find_package(OpenCV REQUIRED core imgproc) @@ -119,6 +120,8 @@ jsk_pcl_nodelet(src/static_polygon_array_publisher_nodelet.cpp "jsk_pcl/StaticPolygonArrayPublisher" "static_polygon_array_publisher") jsk_pcl_nodelet(src/polygon_array_transformer_nodelet.cpp "jsk_pcl/PolygonArrayTransformer" "polygon_array_transformer_nodelet") +jsk_pcl_nodelet(src/ocluded_plane_estimator_nodelet.cpp + "jsk_pcl/OcludedPlaneEstimator" "ocluded_plane_estimator") add_library(jsk_pcl_ros SHARED ${jsk_pcl_nodelet_sources}) target_link_libraries(jsk_pcl_ros ${catkin_LIBRARIES} ${pcl_ros_LIBRARIES} ${OpenCV_LIBRARIES}) diff --git a/jsk_pcl_ros/cfg/OcludedPlaneEstimator.cfg b/jsk_pcl_ros/cfg/OcludedPlaneEstimator.cfg new file mode 100755 index 0000000000..beef0999f5 --- /dev/null +++ b/jsk_pcl_ros/cfg/OcludedPlaneEstimator.cfg @@ -0,0 +1,21 @@ +#!/usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'jsk_pcl_ros' + +try: + import imp + imp.find_module(PACKAGE) + from dynamic_reconfigure.parameter_generator_catkin import *; +except: + import roslib; roslib.load_manifest(PACKAGE) + from dynamic_reconfigure.parameter_generator import *; + +from math import pi + +gen = ParameterGenerator () + +gen.add("plane_angle_threshold", double_t, 0, "angle threshold", 10.0 / 180.0 * pi, 0.0, pi) +gen.add("plane_distance_threshold", double_t, 0, "distance threshold", 0.1, 0.0, 2.0) + +exit (gen.generate (PACKAGE, "jsk_pcl_ros", "OcludedPlaneEstimator")) diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/ocluded_plane_estimator.h b/jsk_pcl_ros/include/jsk_pcl_ros/ocluded_plane_estimator.h new file mode 100644 index 0000000000..f6bf188586 --- /dev/null +++ b/jsk_pcl_ros/include/jsk_pcl_ros/ocluded_plane_estimator.h @@ -0,0 +1,88 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef JSK_PCL_ROS_OCLUDED_PLANE_ESTIMATOR_H_ +#define JSK_PCL_ROS_OCLUDED_PLANE_ESTIMATOR_H_ + +// ros +#include +#include +#include +#include +#include +#include +#include + +// pcl +#include +#include + +#include +#include +#include + +namespace jsk_pcl_ros +{ + class OcludedPlaneEstimator: public pcl_ros::PCLNodelet + { + public: + typedef message_filters::sync_policies::ExactTime< jsk_pcl_ros::PolygonArray, + jsk_pcl_ros::ModelCoefficientsArray, + jsk_pcl_ros::PolygonArray, + jsk_pcl_ros::ModelCoefficientsArray> SyncPolicy; + typedef jsk_pcl_ros::OcludedPlaneEstimatorConfig Config; + protected: + boost::mutex mutex_; + virtual void onInit(); + virtual void estimate(const jsk_pcl_ros::PolygonArray::ConstPtr& polygons, + const jsk_pcl_ros::ModelCoefficientsArray::ConstPtr& coefficients, + const jsk_pcl_ros::PolygonArray::ConstPtr& static_polygons, + const jsk_pcl_ros::ModelCoefficientsArray::ConstPtr& static_coefficients); + message_filters::Subscriber sub_polygons_; + message_filters::Subscriber sub_static_polygons_; + message_filters::Subscriber sub_coefficients_; + message_filters::Subscriber sub_static_coefficients_; + boost::shared_ptr >sync_; + ros::Publisher polygon_pub_, coefficient_pub_; + boost::shared_ptr > srv_; + virtual void configCallback(Config &config, uint32_t level); + double plane_distance_threshold_; + double plane_angle_threshold_; + private: + }; + +} + +#endif diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/plane_rejector.h b/jsk_pcl_ros/include/jsk_pcl_ros/plane_rejector.h index d23a18dfbf..12290a3a32 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/plane_rejector.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/plane_rejector.h @@ -61,7 +61,7 @@ namespace jsk_pcl_ros public: typedef message_filters::sync_policies::ExactTime< jsk_pcl_ros::PolygonArray, jsk_pcl_ros::ModelCoefficientsArray > SyncPolicy; -typedef jsk_pcl_ros::PlaneRejectorConfig Config; + typedef jsk_pcl_ros::PlaneRejectorConfig Config; protected: virtual void onInit(); virtual void reject(const jsk_pcl_ros::PolygonArray::ConstPtr& polygons, diff --git a/jsk_pcl_ros/jsk_pcl_nodelets.xml b/jsk_pcl_ros/jsk_pcl_nodelets.xml index d7cd51dabb..caa89c3ae4 100644 --- a/jsk_pcl_ros/jsk_pcl_nodelets.xml +++ b/jsk_pcl_ros/jsk_pcl_nodelets.xml @@ -1,4 +1,8 @@ + + estimate the planes taking into account occulusions + transform polygons to the specified tf frame diff --git a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch index 3f94f678b6..397d6f994d 100644 --- a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch +++ b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch @@ -2,6 +2,7 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_pcl_ros/src/ocluded_plane_estimator_nodelet.cpp b/jsk_pcl_ros/src/ocluded_plane_estimator_nodelet.cpp new file mode 100644 index 0000000000..0fbc94adb1 --- /dev/null +++ b/jsk_pcl_ros/src/ocluded_plane_estimator_nodelet.cpp @@ -0,0 +1,210 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "jsk_pcl_ros/ocluded_plane_estimator.h" +#include +#include +#include + +#if ROS_VERSION_MINIMUM(1, 10, 0) +// hydro and later +typedef pcl_msgs::PointIndices PCLIndicesMsg; +typedef pcl_msgs::ModelCoefficients PCLModelCoefficientMsg; +#else +// groovy +typedef pcl::PointIndices PCLIndicesMsg; +typedef pcl::ModelCoefficients PCLModelCoefficientMsg; +#endif + + +namespace jsk_pcl_ros +{ + void OcludedPlaneEstimator::onInit() + { + PCLNodelet::onInit(); + polygon_pub_ = pnh_->advertise("output_polygons", 1); + coefficient_pub_ = pnh_->advertise("output_coefficients", 1); + srv_ = boost::make_shared > (*pnh_); + dynamic_reconfigure::Server::CallbackType f = + boost::bind (&OcludedPlaneEstimator::configCallback, this, _1, _2); + srv_->setCallback (f); + + sync_ = boost::make_shared >(100); + sub_polygons_.subscribe(*pnh_, "input_polygons", 1); + sub_coefficients_.subscribe(*pnh_, "input_coefficients", 1); + sub_static_polygons_.subscribe(*pnh_, "input_static_polygons", 1); + sub_static_coefficients_.subscribe(*pnh_, "input_static_coefficients", 1); + + sync_->connectInput(sub_polygons_, sub_coefficients_, sub_static_polygons_, sub_static_coefficients_); + sync_->registerCallback(boost::bind(&OcludedPlaneEstimator::estimate, + this, _1, _2, _3, _4)); + } + + void OcludedPlaneEstimator::configCallback(Config &config, uint32_t level) + { + boost::mutex::scoped_lock(mutex_); + plane_distance_threshold_ = config.plane_distance_threshold; + plane_angle_threshold_ = config.plane_angle_threshold; + } + + void OcludedPlaneEstimator::estimate(const jsk_pcl_ros::PolygonArray::ConstPtr& polygons, + const jsk_pcl_ros::ModelCoefficientsArray::ConstPtr& coefficients, + const jsk_pcl_ros::PolygonArray::ConstPtr& static_polygons, + const jsk_pcl_ros::ModelCoefficientsArray::ConstPtr& static_coefficients) + { + boost::mutex::scoped_lock(mutex_); + // error check + if (polygons->polygons.size() != coefficients->coefficients.size()) { + NODELET_ERROR("the size of the input polygon array and model coefficients array is not same"); + return; + } + if (static_polygons->polygons.size() != static_coefficients->coefficients.size()) { + NODELET_ERROR("the size of the input static polygon array and static model coefficients array is not same"); + return; + } + jsk_pcl_ros::PolygonArray result_polygons; + jsk_pcl_ros::ModelCoefficientsArray result_coefficients; + result_polygons = *polygons; + result_coefficients = *coefficients; + for (size_t i = 0; i < static_polygons->polygons.size(); i++) { + // looking for the nearest polygon from the static polygon + geometry_msgs::PolygonStamped static_polygon = static_polygons->polygons[i]; + PCLModelCoefficientMsg static_coefficient = static_coefficients->coefficients[i]; + + // linear search, it may not fast enough if we need to tuckle against + // larger scene + int nearest_index = -1; + double min_angle_distance = DBL_MAX; + for (size_t j = 0; j < polygons->polygons.size(); j++) { + geometry_msgs::PolygonStamped candidate_polygon = polygons->polygons[j]; + PCLModelCoefficientMsg candidate_coefficient = coefficients->coefficients[j]; + if (candidate_polygon.header.frame_id != static_polygon.header.frame_id) { + NODELET_ERROR("frame_id of static polygon and candidate polygon are not the same one: %s and %s", + static_polygon.header.frame_id.c_str(), + candidate_polygon.header.frame_id.c_str()); + continue; + } + // first, compute the angle distance + Eigen::Vector3f a_normal(candidate_coefficient.values[0], candidate_coefficient.values[1], candidate_coefficient.values[2]); + Eigen::Vector3f b_normal(static_coefficient.values[0], static_coefficient.values[1], static_coefficient.values[2]); + double a_distance = candidate_coefficient.values[3]; + double b_distance = static_coefficient.values[3]; + if (a_normal.norm() != 1.0) { + a_distance = a_distance / a_normal.norm(); + a_normal = a_normal / a_normal.norm(); + } + if (b_normal.norm() != 1.0) { + b_distance = b_distance / b_normal.norm(); + b_normal = b_normal / b_normal.norm(); + } + if (a_normal.dot(b_normal) < 0) { + b_distance = - b_distance; + b_normal = - b_normal; + } + NODELET_DEBUG("[%f, %f, %f] - %f --- [%f, %f, %f] - %f", + a_normal[0], a_normal[1], a_normal[2], a_distance, + b_normal[0], b_normal[1], b_normal[2], b_distance); + NODELET_DEBUG("%lu - %lu distance: %f", i, j, fabs(fabs(a_distance) - fabs(b_distance))); + if (fabs(fabs(a_distance) - fabs(b_distance)) > plane_distance_threshold_) { + continue; + } + double theta = fabs(acos(a_normal.dot(b_normal))); + NODELET_DEBUG("%lu - %lu angle: %f", i, j, theta); + if (theta > M_PI / 2.0) { + theta = M_PI - theta; + } + if (theta > plane_angle_threshold_) { + continue; + } + if (min_angle_distance > theta) { + min_angle_distance = theta; + nearest_index = j; + } + } + + if (nearest_index != -1) { + // merged into -1 + NODELET_DEBUG("merging %lu into %d", i, nearest_index); + // project the points to the plane before run qhull + pcl::ProjectInliers proj; + proj.setModelType (pcl::SACMODEL_PLANE); + pcl::ModelCoefficients plane_coefficients; + plane_coefficients.values = coefficients->coefficients[nearest_index].values; + proj.setModelCoefficients (boost::make_shared(plane_coefficients)); + pcl::PointCloud cloud; + geometry_msgs::PolygonStamped nearest_polygon = result_polygons.polygons[nearest_index]; + + for (size_t j = 0; j < static_polygon.polygon.points.size(); j++) { + pcl::PointXYZ p; + p.x = static_polygon.polygon.points[j].x; + p.y = static_polygon.polygon.points[j].y; + p.z = static_polygon.polygon.points[j].z; + cloud.points.push_back(p); + } + for (size_t j = 0; j < nearest_polygon.polygon.points.size(); j++) { + pcl::PointXYZ p; + p.x = nearest_polygon.polygon.points[j].x; + p.y = nearest_polygon.polygon.points[j].y; + p.z = nearest_polygon.polygon.points[j].z; + cloud.points.push_back(p); + } + pcl::PointCloud projected_cloud; + proj.setInputCloud(cloud.makeShared()); + proj.filter(projected_cloud); + pcl::ConvexHull chull; + chull.setInputCloud(projected_cloud.makeShared()); + chull.setDimension(2); + pcl::PointCloud chull_output; + chull.reconstruct(chull_output); + // rewriting the points... + nearest_polygon.polygon.points.clear(); + for (size_t j = 0; j < chull_output.points.size(); j++) { + geometry_msgs::Point32 p; + p.x = chull_output.points[j].x; + p.y = chull_output.points[j].y; + p.z = chull_output.points[j].z; + nearest_polygon.polygon.points.push_back(p); + } + result_polygons.polygons[nearest_index] = nearest_polygon; + } + } + polygon_pub_.publish(result_polygons); + coefficient_pub_.publish(result_coefficients); + } + +} + +typedef jsk_pcl_ros::OcludedPlaneEstimator OcludedPlaneEstimator; +PLUGINLIB_DECLARE_CLASS (jsk_pcl, OcludedPlaneEstimator, OcludedPlaneEstimator, nodelet::Nodelet); From 9e8f31b95f15e48a877145ba8ac642454f8dbe4d Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Fri, 6 Jun 2014 20:29:34 +0900 Subject: [PATCH 2/7] add nodelet to detect circles based on hough transformation --- jsk_perception/CMakeLists.txt | 1 + jsk_perception/catkin.cmake | 7 +- jsk_perception/cfg/HoughCircles.cfg | 22 +++ jsk_perception/jsk_perception_nodelets.xml | 6 + .../launch/hough_circle_sample.launch | 10 ++ jsk_perception/msg/Circle2D.msg | 4 + jsk_perception/msg/Circle2DArray.msg | 2 + jsk_perception/src/hough_circles.cpp | 155 ++++++++++++++++++ 8 files changed, 205 insertions(+), 2 deletions(-) create mode 100755 jsk_perception/cfg/HoughCircles.cfg create mode 100644 jsk_perception/launch/hough_circle_sample.launch create mode 100644 jsk_perception/msg/Circle2D.msg create mode 100644 jsk_perception/msg/Circle2DArray.msg create mode 100644 jsk_perception/src/hough_circles.cpp diff --git a/jsk_perception/CMakeLists.txt b/jsk_perception/CMakeLists.txt index 14b7c8f57b..8fb61c56ba 100644 --- a/jsk_perception/CMakeLists.txt +++ b/jsk_perception/CMakeLists.txt @@ -67,6 +67,7 @@ jsk_perception_nodelet(src/edge_detector.cpp "jsk_perception/EdgeDetector" "edge jsk_perception_nodelet(src/sparse_image_encoder.cpp "jsk_perception/SparseImageEncoder" "sparse_image_encoder") jsk_perception_nodelet(src/sparse_image_decoder.cpp "jsk_perception/SparseImageDecoder" "sparse_image_decoder") jsk_perception_nodelet(src/color_histogram.cpp "jsk_perception/ColorHistogram" "color_histogram") +jsk_perception_nodelet(src/hough_circles.cpp "jsk_perception/HoughCircleDetector" "hough_circles") rosbuild_add_library (${PROJECT_NAME} ${jsk_perception_nodelet_sources} ) diff --git a/jsk_perception/catkin.cmake b/jsk_perception/catkin.cmake index 3bd24e63ca..8af019559b 100644 --- a/jsk_perception/catkin.cmake +++ b/jsk_perception/catkin.cmake @@ -7,10 +7,12 @@ find_package(Boost REQUIRED COMPONENTS filesystem system signals) # Dynamic reconfigure support generate_dynamic_reconfigure_options(cfg/camshiftdemo.cfg cfg/EdgeDetector.cfg cfg/HoughLines.cfg cfg/matchtemplate.cfg cfg/point_pose_extractor.cfg cfg/RectangleDetector.cfg - cfg/ColorHistogram.cfg) + cfg/ColorHistogram.cfg + cfg/HoughCircles.cfg) add_message_files(FILES # ClusterPointIndices.msg - PointsArray.msg RotatedRectStamped.msg LineArray.msg Rect.msg Line.msg RotatedRect.msg SparseImage.msg) + PointsArray.msg RotatedRectStamped.msg LineArray.msg Rect.msg Line.msg RotatedRect.msg SparseImage.msg + Circle2D.msg Circle2DArray.msg) add_service_files(FILES EuclideanSegment.srv SetTemplate.srv WhiteBalancePoints.srv WhiteBalance.srv) @@ -53,6 +55,7 @@ jsk_perception_nodelet(src/edge_detector.cpp "jsk_perception/EdgeDetector" "edge jsk_perception_nodelet(src/sparse_image_encoder.cpp "jsk_perception/SparseImageEncoder" "sparse_image_encoder") jsk_perception_nodelet(src/sparse_image_decoder.cpp "jsk_perception/SparseImageDecoder" "sparse_image_decoder") jsk_perception_nodelet(src/color_histogram.cpp "jsk_perception/ColorHistogram" "color_histogram") +jsk_perception_nodelet(src/hough_circles.cpp "jsk_perception/HoughCircleDetector" "hough_circles") # compiling jsk_perception library for nodelet add_library(${PROJECT_NAME} SHARED ${jsk_perception_nodelet_sources}) diff --git a/jsk_perception/cfg/HoughCircles.cfg b/jsk_perception/cfg/HoughCircles.cfg new file mode 100755 index 0000000000..9e3e6d1ad6 --- /dev/null +++ b/jsk_perception/cfg/HoughCircles.cfg @@ -0,0 +1,22 @@ +#! /usr/bin/env python + +PACKAGE='jsk_perception' +try: + import imp + imp.find_module(PACKAGE) + from dynamic_reconfigure.parameter_generator_catkin import * +except: + import roslib; roslib.load_manifest(PACKAGE) + from dynamic_reconfigure.parameter_generator import * + +gen = ParameterGenerator() + +gen.add("gaussian_blur_size", int_t, 0, "the size of gaussian blur (should be odd number)", 0, 9, 30) +gen.add("gaussian_sigma_x", double_t, 0, "sigma x of gaussian kernel", 0, 2, 10) +gen.add("gaussian_sigma_y", double_t, 0, "sigma y of gaussian kernel", 0, 2, 10) +gen.add("edge_threshold", double_t, 0, "edge threshold", 0, 200, 1000) +gen.add("voting_threshold", double_t, 0, "voting threshold", 0, 100, 1000) +gen.add("dp", int_t, 0, "dp", 0, 2, 10) +gen.add("min_circle_radius", int_t, 0, "the minimum size of the circle", 0, 0, 500) +gen.add("max_circle_radius", int_t, 0, "the maximum size of the circle", 0, 0, 2000) +exit(gen.generate(PACKAGE, "hough_cirlces", "HoughCircles")) diff --git a/jsk_perception/jsk_perception_nodelets.xml b/jsk_perception/jsk_perception_nodelets.xml index 65c8dcc544..ca0510ebf1 100644 --- a/jsk_perception/jsk_perception_nodelets.xml +++ b/jsk_perception/jsk_perception_nodelets.xml @@ -1,4 +1,10 @@ + + + nodelet to detect circles based on hough transformation + + diff --git a/jsk_perception/launch/hough_circle_sample.launch b/jsk_perception/launch/hough_circle_sample.launch new file mode 100644 index 0000000000..d1b73a1bd6 --- /dev/null +++ b/jsk_perception/launch/hough_circle_sample.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/jsk_perception/msg/Circle2D.msg b/jsk_perception/msg/Circle2D.msg new file mode 100644 index 0000000000..e77c629d8c --- /dev/null +++ b/jsk_perception/msg/Circle2D.msg @@ -0,0 +1,4 @@ +Header header +float64 radius +float64 x +float64 y diff --git a/jsk_perception/msg/Circle2DArray.msg b/jsk_perception/msg/Circle2DArray.msg new file mode 100644 index 0000000000..578b9e34e0 --- /dev/null +++ b/jsk_perception/msg/Circle2DArray.msg @@ -0,0 +1,2 @@ +Header header +Circle2D[] circles diff --git a/jsk_perception/src/hough_circles.cpp b/jsk_perception/src/hough_circles.cpp new file mode 100644 index 0000000000..d969f549b5 --- /dev/null +++ b/jsk_perception/src/hough_circles.cpp @@ -0,0 +1,155 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include +#include +#include +#include +#include +#include "jsk_perception/HoughCirclesConfig.h" + +namespace jsk_perception +{ + class HoughCircleDetector: public nodelet::Nodelet + { + public: + typedef jsk_perception::HoughCirclesConfig Config; + Config config_; + boost::shared_ptr > srv_; + ros::NodeHandle nh_, pnh_; + boost::shared_ptr it_; + ros::Publisher circle_pub_, marker_pub_; + image_transport::Subscriber image_sub_; + int subscriber_count_; + int previous_marker_num_; + + int gaussian_blur_size_; + double gaussian_sigma_x_; + double gaussian_sigma_y_; + double edge_threshold_; + double voting_threshold_; + double dp_; + int min_circle_radius_; + int max_circle_radius_; + + boost::mutex mutex_; + + void imageCallback(const sensor_msgs::Image::ConstPtr& image) + { + boost::mutex::scoped_lock(mutex_); + cv_bridge::CvImagePtr cv_ptr; + cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8); + cv::Mat bgr_image = cv_ptr->image; + cv::Mat gray_image; + cv::cvtColor(bgr_image, gray_image, CV_BGR2GRAY); + cv::GaussianBlur(gray_image, gray_image, + cv::Size(gaussian_blur_size_, gaussian_blur_size_), + gaussian_sigma_x_, gaussian_sigma_y_); + std::vector circles; + cv::HoughCircles(gray_image, circles, CV_HOUGH_GRADIENT, + dp_, gray_image.rows / 4, edge_threshold_, voting_threshold_, + min_circle_radius_, max_circle_radius_); + jsk_perception::Circle2DArray circles_msg; + circles_msg.header = image->header; + for (size_t i = 0; i < circles.size(); i++) { + jsk_perception::Circle2D circle; + circle.header = image->header; + circle.radius = circles[i][2]; + circle.x = circles[i][0]; + circle.y = circles[i][1]; + circles_msg.circles.push_back(circle); + // marker + image_view2::ImageMarker2 marker; + marker.header = image->header; + marker.type = image_view2::ImageMarker2::CIRCLE; + marker.id = i; + marker.action = image_view2::ImageMarker2::ADD; + marker.position.x = circles[i][0]; + marker.position.y = circles[i][1]; + marker.scale = circles[i][2]; + marker_pub_.publish(marker); + } + circle_pub_.publish(circles_msg); + if (circles.size() < previous_marker_num_) { + for (size_t i = circles.size(); i < previous_marker_num_; i++) { + image_view2::ImageMarker2 marker; + marker.header = image->header; + marker.id = i; + marker.action = image_view2::ImageMarker2::REMOVE; + marker_pub_.publish(marker); + } + } + previous_marker_num_ = circles.size(); + } + + void configCallback(Config &new_config, uint32_t level) + { + boost::mutex::scoped_lock(mutex_); + gaussian_blur_size_ = new_config.gaussian_blur_size; + gaussian_sigma_x_ = new_config.gaussian_sigma_x; + gaussian_sigma_y_ = new_config.gaussian_sigma_y; + edge_threshold_ = new_config.edge_threshold; + voting_threshold_ = new_config.voting_threshold; + dp_ = new_config.dp; + min_circle_radius_ = new_config.min_circle_radius; + max_circle_radius_ = new_config.max_circle_radius; + } + + virtual void onInit() + { + subscriber_count_ = 0; + previous_marker_num_ = 0; + nh_ = ros::NodeHandle(getNodeHandle(), "image"); + pnh_ = getPrivateNodeHandle(); + srv_ = boost::make_shared > (pnh_); + dynamic_reconfigure::Server::CallbackType f = + boost::bind (&HoughCircleDetector::configCallback, this, _1, _2); + srv_->setCallback (f); + it_.reset(new image_transport::ImageTransport(nh_)); + circle_pub_ = pnh_.advertise("output", 1); + marker_pub_ = pnh_.advertise("image_marker", 1); + image_sub_ = it_->subscribe("", 1, &HoughCircleDetector::imageCallback, this); + } + }; +} + +#include +typedef jsk_perception::HoughCircleDetector HoughCircleDetector; +PLUGINLIB_DECLARE_CLASS (jsk_perception, HoughCircleDetector, HoughCircleDetector, nodelet::Nodelet); From 9e0a01c6356afe00b9edf82f1ea27e055840b494 Mon Sep 17 00:00:00 2001 From: JSK User Date: Fri, 6 Jun 2014 23:01:42 +0900 Subject: [PATCH 3/7] add MACHINE and GDB argument --- .../organized_multi_plane_segmentation.launch | 60 ++++++++----------- 1 file changed, 26 insertions(+), 34 deletions(-) diff --git a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch index fc5aa2679e..6a0e974ad5 100644 --- a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch +++ b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch @@ -2,22 +2,25 @@ - - - - - - + + + + + + - @@ -25,33 +28,22 @@ - - - - - - - - - - @@ -62,6 +54,7 @@ @@ -73,6 +66,7 @@ @@ -82,6 +76,7 @@ @@ -89,16 +84,10 @@ min_size: 500 - - - - - @@ -113,6 +102,7 @@ @@ -120,6 +110,7 @@ @@ -129,6 +120,7 @@ From 7cf8f77e6edb38591d80dd33c4ea7ff80abd0503 Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Sat, 7 Jun 2014 02:20:07 +0900 Subject: [PATCH 4/7] add more rosparameters to ParticleFilterTracking --- jsk_pcl_ros/CMakeLists.txt | 1 + jsk_pcl_ros/catkin.cmake | 5 + .../jsk_pcl_ros/particle_filter_tracking.h | 5 +- .../src/particle_filter_tracking_nodelet.cpp | 118 ++++++++++++++---- 4 files changed, 106 insertions(+), 23 deletions(-) diff --git a/jsk_pcl_ros/CMakeLists.txt b/jsk_pcl_ros/CMakeLists.txt index d8655ff9d9..d57cf983d3 100644 --- a/jsk_pcl_ros/CMakeLists.txt +++ b/jsk_pcl_ros/CMakeLists.txt @@ -100,6 +100,7 @@ rosbuild_add_library (jsk_pcl_ros ${jsk_pcl_nodelet_sources} # ${pcl_ros_PACKAGE_PATH}/src/pcl_ros/features/feature.cpp ) +rosbuild_add_openmp_flags(jsk_pcl_ros) rosbuild_add_compile_flags (jsk_pcl_ros ${SSE_FLAGS}) rosbuild_link_boost (jsk_pcl_ros system) diff --git a/jsk_pcl_ros/catkin.cmake b/jsk_pcl_ros/catkin.cmake index 6f3ad7126e..f3e86938c3 100644 --- a/jsk_pcl_ros/catkin.cmake +++ b/jsk_pcl_ros/catkin.cmake @@ -19,6 +19,11 @@ endif() find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure pcl_ros nodelet message_generation genmsg ${PCL_MSGS} sensor_msgs geometry_msgs eigen_conversions tf_conversions tf2_ros tf image_transport nodelet cv_bridge) +find_package(OpenMP) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") + add_message_files(FILES PointsArray.msg ClusterPointIndices.msg Int32Stamped.msg SnapItRequest.msg PolygonArray.msg ModelCoefficientsArray.msg SlicedPointCloud.msg diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h b/jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h index 5fa272a234..c2bf957870 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h @@ -93,7 +93,6 @@ namespace jsk_pcl_ros boost::mutex mtx_; bool new_cloud_; bool track_target_set_; - double downsampling_grid_size_; std::string frame_id_; ros::Subscriber sub_; @@ -113,7 +112,9 @@ namespace jsk_pcl_ros ); virtual void renew_model_topic_cb(const sensor_msgs::PointCloud2 &pc); - + virtual double getXMLDoubleValue(XmlRpc::XmlRpcValue val); + virtual bool readVectorParameter(const std::string& param_name, + std::vector& result); private: virtual void onInit(); diff --git a/jsk_pcl_ros/src/particle_filter_tracking_nodelet.cpp b/jsk_pcl_ros/src/particle_filter_tracking_nodelet.cpp index 3155937da7..9890d27c82 100644 --- a/jsk_pcl_ros/src/particle_filter_tracking_nodelet.cpp +++ b/jsk_pcl_ros/src/particle_filter_tracking_nodelet.cpp @@ -40,47 +40,123 @@ using namespace pcl::tracking; namespace jsk_pcl_ros { + + double ParticleFilterTracking::getXMLDoubleValue(XmlRpc::XmlRpcValue val) { + switch(val.getType()) { + case XmlRpc::XmlRpcValue::TypeInt: + return (double)((int)val); + case XmlRpc::XmlRpcValue::TypeDouble: + return (double)val; + default: + return 0; + } + } + + bool ParticleFilterTracking::readVectorParameter(const std::string& param_name, + std::vector& result) + { + if (pnh_->hasParam(param_name)) { + XmlRpc::XmlRpcValue v; + pnh_->param(param_name, v, v); + if (v.getType() == XmlRpc::XmlRpcValue::TypeArray && + v.size() == result.size()) { + for (size_t i = 0; i < result.size(); i++) { + result[i] = getXMLDoubleValue(v[i]); + } + return true; + } + else { + NODELET_ERROR("%s parameter does not match the length: %lu", + param_name.c_str(), + result.size()); + return false; + } + } + else { + return false; + } + } + void ParticleFilterTracking::onInit(void){ // not implemented yet PCLNodelet::onInit(); + // read parameters + int thread_nr = omp_get_num_procs(); + pnh_->getParam("thread_nr", thread_nr); + int max_particle_num = 1000; + pnh_->getParam("max_particle_num", max_particle_num); + int particle_num = 600; + pnh_->getParam("particle_num", particle_num); + double delta = 0.99; + pnh_->getParam("delta", delta); + double epsilon = 0.2; + pnh_->getParam("epsilon", epsilon); + bool use_normal = false; + pnh_->getParam("use_normal", use_normal); + int iteration_num = 1; + pnh_->getParam("iteration_num", iteration_num); + double resample_likelihood_thr = 0.0; + pnh_->getParam("resample_likelihood_thr", resample_likelihood_thr); + + std::vector bin_size_vector(6); + if (!readVectorParameter("bin_size", bin_size_vector)) { + for (size_t i = 0; i < 6; i++) { + bin_size_vector[i] = 0.1; + } + } + ParticleXYZRPY bin_size; + bin_size.x = bin_size_vector[0]; + bin_size.y = bin_size_vector[1]; + bin_size.z = bin_size_vector[2]; + bin_size.roll = bin_size_vector[3]; + bin_size.pitch = bin_size_vector[4]; + bin_size.yaw = bin_size_vector[5]; + + std::vector default_step_covariance(6); + if (!readVectorParameter("default_step_covariance", + default_step_covariance)) { + for (size_t i = 0; i < default_step_covariance.size(); i++) { + default_step_covariance[i] = 0.015 * 0.015; + } + default_step_covariance[3] *= 40.0; + default_step_covariance[4] *= 40.0; + default_step_covariance[5] *= 40.0; + } + std::vector initial_noise_covariance = std::vector (6, 0.00001); + readVectorParameter("initial_noise_covariance", + initial_noise_covariance); + std::vector default_initial_mean = std::vector (6, 0.0); + readVectorParameter("default_initial_mean", default_initial_mean); //First the track target is not set + double octree_resolution = 0.01; + pnh_->getParam("octree_resolution", octree_resolution); track_target_set_ = false; - - int thread_nr=8; - downsampling_grid_size_=0.02; + new_cloud_ = false; target_cloud_.reset(new pcl::PointCloud()); - ParticleXYZRPY bin_size; - bin_size.x = bin_size.y = bin_size.z = bin_size.roll = bin_size.pitch = bin_size.yaw = 0.1f; - boost::shared_ptr > tracker (new KLDAdaptiveParticleFilterOMPTracker (thread_nr)); //Set all parameters for KLDAdaptiveParticleFilterTrackerOMPTracker - tracker->setMaximumParticleNum (1000); - tracker->setDelta (0.99); - tracker->setEpsilon (0.2); + + tracker->setMaximumParticleNum (max_particle_num); + tracker->setDelta (delta); + tracker->setEpsilon (epsilon); tracker->setBinSize (bin_size); //Set all parameters for ParticleFilterTracker - std::vector default_step_covariance = std::vector (6, 0.015 * 0.015); - std::vector initial_noise_covariance = std::vector (6, 0.00001); - std::vector default_initial_mean = std::vector (6, 0.0); - default_step_covariance[3] *= 40.0; - default_step_covariance[4] *= 40.0; - default_step_covariance[5] *= 40.0; tracker_ = tracker; tracker_->setTrans (Eigen::Affine3f::Identity ()); tracker_->setStepNoiseCovariance (default_step_covariance); tracker_->setInitialNoiseCovariance (initial_noise_covariance); tracker_->setInitialNoiseMean (default_initial_mean); - tracker_->setIterationNum (1); - tracker_->setParticleNum (600); - tracker_->setResampleLikelihoodThr(0.00); - tracker_->setUseNormal (false); + tracker_->setIterationNum (iteration_num); + tracker_->setParticleNum (particle_num); + tracker_->setResampleLikelihoodThr(resample_likelihood_thr); + tracker_->setUseNormal (use_normal); //Setup coherence object for tracking ApproxNearestPairPointCloudCoherence::Ptr coherence = ApproxNearestPairPointCloudCoherence::Ptr(new ApproxNearestPairPointCloudCoherence ()); @@ -89,9 +165,9 @@ namespace jsk_pcl_ros = boost::shared_ptr > (new DistanceCoherence ()); coherence->addPointCoherence (distance_coherence); - boost::shared_ptr > search (new pcl::search::Octree (0.01)); + boost::shared_ptr > search (new pcl::search::Octree (octree_resolution)); coherence->setSearchMethod (search); - coherence->setMaximumDistance (0.01); + coherence->setMaximumDistance (octree_resolution); tracker_->setCloudCoherence (coherence); From 32bbee7d4c51ee2654104eebe8f8d509a5f650bb Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Wed, 4 Jun 2014 02:38:42 +0900 Subject: [PATCH 5/7] d varaible of the normal should be transformed correctly by PolygonArrayTransfomer. fix transformation compuation to normalize d parameter --- .../jsk_pcl_ros/polygon_array_transformer.h | 2 + .../launch/hrp2jsknt_footstep_polygon.launch | 6 +- .../organized_multi_plane_segmentation.launch | 2 +- .../src/ocluded_plane_estimator_nodelet.cpp | 8 +-- .../src/polygon_array_transformer_nodelet.cpp | 55 +++++++++++++++---- ...static_polygon_array_publisher_nodelet.cpp | 2 +- 6 files changed, 55 insertions(+), 20 deletions(-) diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/polygon_array_transformer.h b/jsk_pcl_ros/include/jsk_pcl_ros/polygon_array_transformer.h index faf6d5b347..6232a3112c 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/polygon_array_transformer.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/polygon_array_transformer.h @@ -72,6 +72,8 @@ namespace jsk_pcl_ros virtual void onInit(); virtual void transform(const jsk_pcl_ros::PolygonArray::ConstPtr& polygons, const jsk_pcl_ros::ModelCoefficientsArray::ConstPtr& coefficients); + virtual void computeCoefficients(const geometry_msgs::PolygonStamped& polygon, + PCLModelCoefficientMsg& coefficient); virtual void transformModelCoefficient(const Eigen::Affine3d& transform, const PCLModelCoefficientMsg& coefficient, PCLModelCoefficientMsg& result); diff --git a/jsk_pcl_ros/launch/hrp2jsknt_footstep_polygon.launch b/jsk_pcl_ros/launch/hrp2jsknt_footstep_polygon.launch index 288579b7c7..db521c7843 100644 --- a/jsk_pcl_ros/launch/hrp2jsknt_footstep_polygon.launch +++ b/jsk_pcl_ros/launch/hrp2jsknt_footstep_polygon.launch @@ -8,9 +8,11 @@ output="screen"/> + args="load jsk_pcl/StaticPolygonArrayPublisher /$(arg + MANAGER)"> + - use_periodic: true + use_message: true frame_ids: [RLEG_LINK5, LLEG_LINK5] polygon_array: [[[0.178558, -0.077564, -0.10611],[-0.082795, -0.077564, -0.10611],[-0.082795, 0.057564, -0.10611], [0.178558, 0.057564, -0.10611]],[[0.178558, 0.077564, -0.10611],[-0.082795, 0.077564, -0.10611],[-0.082795, -0.057564, -0.10611], [0.178558, -0.057564, -0.10611]]] diff --git a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch index 397d6f994d..62e5372abc 100644 --- a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch +++ b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch @@ -158,7 +158,7 @@ - + diff --git a/jsk_pcl_ros/src/ocluded_plane_estimator_nodelet.cpp b/jsk_pcl_ros/src/ocluded_plane_estimator_nodelet.cpp index 0fbc94adb1..3b0c36bb91 100644 --- a/jsk_pcl_ros/src/ocluded_plane_estimator_nodelet.cpp +++ b/jsk_pcl_ros/src/ocluded_plane_estimator_nodelet.cpp @@ -133,15 +133,15 @@ namespace jsk_pcl_ros b_distance = - b_distance; b_normal = - b_normal; } - NODELET_DEBUG("[%f, %f, %f] - %f --- [%f, %f, %f] - %f", + NODELET_INFO("[%f, %f, %f] - %f --- [%f, %f, %f] - %f", a_normal[0], a_normal[1], a_normal[2], a_distance, b_normal[0], b_normal[1], b_normal[2], b_distance); - NODELET_DEBUG("%lu - %lu distance: %f", i, j, fabs(fabs(a_distance) - fabs(b_distance))); + NODELET_INFO("%lu - %lu distance: %f", i, j, fabs(fabs(a_distance) - fabs(b_distance))); if (fabs(fabs(a_distance) - fabs(b_distance)) > plane_distance_threshold_) { continue; } double theta = fabs(acos(a_normal.dot(b_normal))); - NODELET_DEBUG("%lu - %lu angle: %f", i, j, theta); + NODELET_INFO("%lu - %lu angle: %f", i, j, theta); if (theta > M_PI / 2.0) { theta = M_PI - theta; } @@ -156,7 +156,7 @@ namespace jsk_pcl_ros if (nearest_index != -1) { // merged into -1 - NODELET_DEBUG("merging %lu into %d", i, nearest_index); + NODELET_INFO("merging %lu into %d", i, nearest_index); // project the points to the plane before run qhull pcl::ProjectInliers proj; proj.setModelType (pcl::SACMODEL_PLANE); diff --git a/jsk_pcl_ros/src/polygon_array_transformer_nodelet.cpp b/jsk_pcl_ros/src/polygon_array_transformer_nodelet.cpp index 9da18af981..e50987e0f3 100644 --- a/jsk_pcl_ros/src/polygon_array_transformer_nodelet.cpp +++ b/jsk_pcl_ros/src/polygon_array_transformer_nodelet.cpp @@ -56,23 +56,52 @@ namespace jsk_pcl_ros sync_->connectInput(sub_polygons_, sub_coefficients_); sync_->registerCallback(boost::bind(&PolygonArrayTransformer::transform, this, _1, _2)); } + + void PolygonArrayTransformer::computeCoefficients(const geometry_msgs::PolygonStamped& polygon, + PCLModelCoefficientMsg& coefficient) + { + Eigen::Vector3d A, B, C; + A[0] = polygon.polygon.points[0].x; + A[1] = polygon.polygon.points[0].y; + A[2] = polygon.polygon.points[0].z; + B[0] = polygon.polygon.points[1].x; + B[1] = polygon.polygon.points[1].y; + B[2] = polygon.polygon.points[1].z; + C[0] = polygon.polygon.points[2].x; + C[1] = polygon.polygon.points[2].y; + C[2] = polygon.polygon.points[2].z; + Eigen::Vector3d n = (B - A).cross(C - A).normalized(); + double a = n[0]; + double b = n[1]; + double c = n[2]; + double d = -(a * A[0] + b * A[1] + c * A[2]); + coefficient.header = polygon.header; + coefficient.values.push_back(a); + coefficient.values.push_back(b); + coefficient.values.push_back(c); + coefficient.values.push_back(d); + + } void PolygonArrayTransformer::transformModelCoefficient(const Eigen::Affine3d& transform, const PCLModelCoefficientMsg& coefficient, PCLModelCoefficientMsg& result) { - Eigen::Vector4d plane_parameter; - plane_parameter[0] = coefficient.values[0]; - plane_parameter[1] = coefficient.values[1]; - plane_parameter[2] = coefficient.values[2]; - plane_parameter[3] = coefficient.values[3]; - Eigen::Vector4d transformed_plane_parameter = transform.inverse() * plane_parameter; + Eigen::Vector4d n; + n[0] = coefficient.values[0]; + n[1] = coefficient.values[1]; + n[2] = coefficient.values[2]; + n[3] = 0; + Eigen::Matrix4d m = transform.matrix(); + Eigen::Vector4d n_d = m.transpose() * n; + Eigen::Vector4d n_dd = n_d.normalized(); + double d_dd = coefficient.values[3] / n_d.norm(); result.header.stamp = coefficient.header.stamp; result.header.frame_id = frame_id_; - result.values.push_back(transformed_plane_parameter[0]); - result.values.push_back(transformed_plane_parameter[1]); - result.values.push_back(transformed_plane_parameter[2]); - result.values.push_back(transformed_plane_parameter[3]); + result.values.push_back(n_dd[0]); + result.values.push_back(n_dd[1]); + result.values.push_back(n_dd[2]); + result.values.push_back(d_dd); } void PolygonArrayTransformer::transformPolygon(const Eigen::Affine3d& transform, @@ -133,11 +162,13 @@ namespace jsk_pcl_ros Eigen::Affine3d eigen_transform; tf::transformTFToEigen(transform, eigen_transform); PCLModelCoefficientMsg transformed_coefficient; - transformModelCoefficient(eigen_transform, coefficient, transformed_coefficient); - transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient); + //transformModelCoefficient(eigen_transform, coefficient, transformed_coefficient); + geometry_msgs::PolygonStamped transformed_polygon; transformPolygon(eigen_transform, polygon, transformed_polygon); transformed_polygon_array.polygons.push_back(transformed_polygon); + computeCoefficients(transformed_polygon, transformed_coefficient); + transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient); } else { NODELET_ERROR("cannot lookup transform from %s to %s at %f", diff --git a/jsk_pcl_ros/src/static_polygon_array_publisher_nodelet.cpp b/jsk_pcl_ros/src/static_polygon_array_publisher_nodelet.cpp index fe2f636673..7cd9f07e89 100644 --- a/jsk_pcl_ros/src/static_polygon_array_publisher_nodelet.cpp +++ b/jsk_pcl_ros/src/static_polygon_array_publisher_nodelet.cpp @@ -110,7 +110,7 @@ namespace jsk_pcl_ros double a = n[0]; double b = n[1]; double c = n[2]; - double d = a * A[0] + b * A[1] + c * A[2]; + double d = -(a * A[0] + b * A[1] + c * A[2]); PCLModelCoefficientMsg coefficient; coefficient.header = polygon.header; coefficient.values.push_back(a); From 6e888550ece25e6fe5c0271dec92fac1ec137a68 Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Sun, 8 Jun 2014 17:17:44 +0900 Subject: [PATCH 6/7] add normal estimation to organized multi plane segmentation --- jsk_pcl_ros/catkin.cmake | 2 +- .../cfg/OrganizedMultiPlaneSegmentation.cfg | 10 ++ .../organized_multi_plane_segmentation.h | 24 ++- .../organized_multi_plane_segmentation.launch | 29 +--- ...nized_multi_plane_segmentation_nodelet.cpp | 148 +++++++++++++----- 5 files changed, 144 insertions(+), 69 deletions(-) diff --git a/jsk_pcl_ros/catkin.cmake b/jsk_pcl_ros/catkin.cmake index 9462c119ea..230924509d 100644 --- a/jsk_pcl_ros/catkin.cmake +++ b/jsk_pcl_ros/catkin.cmake @@ -68,7 +68,7 @@ macro(jsk_pcl_nodelet _nodelet_cpp _nodelet_class _single_nodelet_exec_name) LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) endmacro(jsk_pcl_nodelet _nodelet_cpp _nodelet_class _single_nodelet_exec_name) -add_definitions("-O2") +add_definitions("-O2 -g") # pcl_ros::Filter based class is not working... # https://github.com/ros-perception/perception_pcl/issues/9 diff --git a/jsk_pcl_ros/cfg/OrganizedMultiPlaneSegmentation.cfg b/jsk_pcl_ros/cfg/OrganizedMultiPlaneSegmentation.cfg index 0d795c2176..129bf6d763 100755 --- a/jsk_pcl_ros/cfg/OrganizedMultiPlaneSegmentation.cfg +++ b/jsk_pcl_ros/cfg/OrganizedMultiPlaneSegmentation.cfg @@ -21,6 +21,16 @@ gen.add("max_curvature", double_t, 0, "maximum curvature of organized plane segm gen.add("connect_plane_angle_threshold", double_t, 0, "plane angle threshold of connecting the planes", 0.2, 0, pi) gen.add("connect_plane_distance_threshold", double_t, 0, "plane distance threshold of connecting the planes", 0.03, 0, 1.0) gen.add("connect_distance_threshold", double_t, 0, "distance threshold of connectin the planes", 0.01, 0, 1.0) +estimation_method_enum = gen.enum([gen.const("AVERAGE_3D_GRADIENT", int_t, 0, "AVERAGE 3D GRADIENT, no curvature is available"), + gen.const("COVARIANCE_MATRIX", int_t, 1, "COVARIANCE_MATRIX, curvature is available"), + gen.const("AVERAGE_DEPTH_CHANGE", int_t, 2, "AVERAGE_DEPTH_CHANGE, no curvature is available")], + "normal estimation method") +gen.add("max_depth_change_factor", double_t, 0, "max depth change factor", 0.02, 0.0, 1.0) +gen.add("normal_smoothing_size", double_t, 0, "normal smoothing size parameter", 20.0, 0.0, 100.0) +gen.add("estimation_method", int_t, 0, "estimation method", 1, 0, 2, edit_method = estimation_method_enum) +gen.add("depth_dependent_smoothing", bool_t, 0, "use depth dependent smoothing", False) +gen.add("border_policy_ignore", bool_t, 0, "ignore border policy", True) +gen.add("publish_normal", bool_t, 0, "publish normal pointcloud", False) #gen.add("concave_alpha", double_t, 0, "alpha parameter for concave estimation", 0.1, 0, 1.0) exit (gen.generate (PACKAGE, "jsk_pcl_ros", "OrganizedMultiPlaneSegmentation")) diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/organized_multi_plane_segmentation.h b/jsk_pcl_ros/include/jsk_pcl_ros/organized_multi_plane_segmentation.h index d456210a04..f0fd87ff40 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/organized_multi_plane_segmentation.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/organized_multi_plane_segmentation.h @@ -52,9 +52,11 @@ namespace jsk_pcl_ros class OrganizedMultiPlaneSegmentation: public pcl_ros::PCLNodelet { public: + typedef pcl::PointXYZRGBA PointT; protected: ros::Publisher org_pub_, org_polygon_pub_, org_coefficients_pub_; ros::Publisher pub_, polygon_pub_, coefficients_pub_; + ros::Publisher normal_pub_; ros::Subscriber sub_; int min_size_; double concave_alpha_; @@ -64,21 +66,30 @@ namespace jsk_pcl_ros double connect_plane_angle_threshold_; double connect_plane_distance_threshold_; double connect_distance_threshold_; + int estimation_method_; + bool depth_dependent_smoothing_; + double max_depth_change_factor_; + double normal_smoothing_size_; + bool border_policy_ignore_; + bool estimate_normal_; + bool publish_normal_; typedef jsk_pcl_ros::OrganizedMultiPlaneSegmentationConfig Config; boost::shared_ptr > srv_; boost::mutex mutex_; virtual void segment(const sensor_msgs::PointCloud2::ConstPtr& msg); + virtual void estimateNormal(pcl::PointCloud::Ptr input, + pcl::PointCloud::Ptr output); virtual void configCallback (Config &config, uint32_t level); - virtual void pointCloudToPolygon(const pcl::PointCloud& input, + virtual void pointCloudToPolygon(const pcl::PointCloud& input, geometry_msgs::Polygon& polygon); virtual void pclIndicesArrayToClusterPointIndices(const std::vector& inlier_indices, const std_msgs::Header& header, jsk_pcl_ros::ClusterPointIndices& output_indices); - virtual void connectPlanesMap(const pcl::PointCloud::Ptr& input, + virtual void connectPlanesMap(const pcl::PointCloud::Ptr& input, const std::vector& model_coefficients, const std::vector& boundary_indices, std::vector >& connection_map); - virtual void buildConnectedPlanes(const pcl::PointCloud::Ptr& input, + virtual void buildConnectedPlanes(const pcl::PointCloud::Ptr& input, const std_msgs::Header& header, const std::vector& inlier_indices, const std::vector& boundary_indices, @@ -86,8 +97,11 @@ namespace jsk_pcl_ros std::vector > connection_map, std::vector& output_indices, std::vector& output_coefficients, - std::vector >& output_boundary_clouds); - + std::vector >& output_boundary_clouds); + + virtual void segmentFromNormals(pcl::PointCloud::Ptr input, + pcl::PointCloud::Ptr normal, + const std_msgs::Header& header); private: virtual void onInit(); }; diff --git a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch index fa0659025c..d389705933 100644 --- a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch +++ b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch @@ -3,12 +3,6 @@ - - - - @@ -25,28 +19,15 @@ unless="$(arg GDB)" output="screen"/> - - - - - - - - - - + - + + + estimate_normal: true + #include #include - +#include #include @@ -63,12 +63,18 @@ namespace jsk_pcl_ros void OrganizedMultiPlaneSegmentation::onInit() { PCLNodelet::onInit(); + estimate_normal_ = false; + pnh_->getParam("estimate_normal", estimate_normal_); + // publishers pub_ = pnh_->advertise("output", 1); polygon_pub_ = pnh_->advertise("output_polygon", 1); coefficients_pub_ = pnh_->advertise("output_coefficients", 1); org_pub_ = pnh_->advertise("output_nonconnected", 1); org_polygon_pub_ = pnh_->advertise("output_nonconnected_polygon", 1); org_coefficients_pub_ = pnh_->advertise("output_nonconnected_coefficients", 1); + if (estimate_normal_) { + normal_pub_ = pnh_->advertise("output_normal", 1); + } srv_ = boost::make_shared > (*pnh_); dynamic_reconfigure::Server::CallbackType f = boost::bind (&OrganizedMultiPlaneSegmentation::configCallback, this, _1, _2); @@ -87,10 +93,16 @@ namespace jsk_pcl_ros connect_plane_angle_threshold_ = config.connect_plane_angle_threshold; connect_plane_distance_threshold_ = config.connect_plane_distance_threshold; connect_distance_threshold_ = config.connect_distance_threshold; + max_depth_change_factor_ = config.max_depth_change_factor; + normal_smoothing_size_ = config.normal_smoothing_size; + depth_dependent_smoothing_ = config.depth_dependent_smoothing; + estimation_method_ = config.estimation_method; + border_policy_ignore_ = config.border_policy_ignore; + publish_normal_ = config.publish_normal; //concave_alpha_ = config.concave_alpha; } - void OrganizedMultiPlaneSegmentation::connectPlanesMap(const pcl::PointCloud::Ptr& input, + void OrganizedMultiPlaneSegmentation::connectPlanesMap(const pcl::PointCloud::Ptr& input, const std::vector& model_coefficients, const std::vector& boundary_indices, std::vector >& connection_map) @@ -99,7 +111,7 @@ namespace jsk_pcl_ros return; // do nothing } - pcl::ExtractIndices extract; + pcl::ExtractIndices extract; extract.setInputCloud(input); connection_map.resize(model_coefficients.size()); for (size_t i = 0; i < model_coefficients.size() - 1; i++) { @@ -138,7 +150,7 @@ namespace jsk_pcl_ros = boost::make_shared(boundary_indices[i]); pcl::PointIndices::Ptr b_indices = boost::make_shared(boundary_indices[j]); - pcl::PointCloud a_cloud, b_cloud; + pcl::PointCloud a_cloud, b_cloud; extract.setIndices(a_indices); extract.filter(a_cloud); extract.setIndices(b_indices); @@ -147,11 +159,11 @@ namespace jsk_pcl_ros // compute the nearest neighbor of a_cloud and b_cloud, // and check the distance between them if (a_cloud.points.size() > 0) { - pcl::KdTreeFLANN kdtree; + pcl::KdTreeFLANN kdtree; kdtree.setInputCloud(a_cloud.makeShared()); bool foundp = false; for (size_t pi = 0; pi < b_cloud.points.size(); pi++) { - pcl::PointXYZRGBNormal p = b_cloud.points[pi]; + PointT p = b_cloud.points[pi]; std::vector k_indices; std::vector k_sqr_distances; if (kdtree.radiusSearch(p, connect_distance_threshold_, k_indices, k_sqr_distances, 1) > 0) { @@ -182,7 +194,7 @@ namespace jsk_pcl_ros } } - void OrganizedMultiPlaneSegmentation::pointCloudToPolygon(const pcl::PointCloud& input, + void OrganizedMultiPlaneSegmentation::pointCloudToPolygon(const pcl::PointCloud& input, geometry_msgs::Polygon& polygon) { for (size_t i = 0; i < input.points.size(); i++) { @@ -194,7 +206,7 @@ namespace jsk_pcl_ros } } - void OrganizedMultiPlaneSegmentation::buildConnectedPlanes(const pcl::PointCloud::Ptr& input, + void OrganizedMultiPlaneSegmentation::buildConnectedPlanes(const pcl::PointCloud::Ptr& input, const std_msgs::Header& header, const std::vector& inlier_indices, const std::vector& boundary_indices, @@ -202,7 +214,7 @@ namespace jsk_pcl_ros std::vector > connection_map, std::vector& output_indices, std::vector& output_coefficients, - std::vector >& output_boundary_clouds) + std::vector >& output_boundary_clouds) { std::vector > cloud_sets; for (size_t i = 0; i < connection_map.size(); i++) { @@ -259,42 +271,40 @@ namespace jsk_pcl_ros output_coefficients.push_back(model_coefficients[(*cloud_sets[i].begin())]); // estimate concave hull - pcl::PointCloud::Ptr cloud_projected (new pcl::PointCloud()); + pcl::PointCloud::Ptr cloud_projected (new pcl::PointCloud()); pcl::PointIndices::Ptr indices_ptr = boost::make_shared(one_boundaries); - pcl::ProjectInliers proj; + pcl::ProjectInliers proj; proj.setModelType (pcl::SACMODEL_PLANE); proj.setIndices (indices_ptr); proj.setInputCloud (input); proj.setModelCoefficients (boost::make_shared(output_coefficients[i])); proj.filter (*cloud_projected); - pcl::ConvexHull chull; + pcl::ConvexHull chull; chull.setInputCloud(input); chull.setDimension(2); //chull.setAlpha(concave_alpha_); // should be parameterized chull.setInputCloud(cloud_projected); - pcl::PointCloud chull_output; + pcl::PointCloud chull_output; chull.reconstruct(chull_output); output_boundary_clouds.push_back(chull_output); } } - - void OrganizedMultiPlaneSegmentation::segment - (const sensor_msgs::PointCloud2::ConstPtr& msg) + + void OrganizedMultiPlaneSegmentation::segmentFromNormals(pcl::PointCloud::Ptr input, + pcl::PointCloud::Ptr normal, + const std_msgs::Header& header) { - boost::mutex::scoped_lock(mutex_); - pcl::PointCloud::Ptr input(new pcl::PointCloud()); - pcl::fromROSMsg(*msg, *input); - pcl::OrganizedMultiPlaneSegmentation mps; + pcl::OrganizedMultiPlaneSegmentation mps; mps.setMinInliers(min_size_); mps.setAngularThreshold(angular_threshold_); mps.setDistanceThreshold(distance_threshold_); mps.setMaximumCurvature(max_curvature_); mps.setInputCloud(input); - mps.setInputNormals(input); + mps.setInputNormals(normal); - std::vector, Eigen::aligned_allocator > > regions; + std::vector, Eigen::aligned_allocator > > regions; std::vector model_coefficients; std::vector inlier_indices; pcl::PointCloud::Ptr labels (new pcl::PointCloud()); @@ -310,21 +320,21 @@ namespace jsk_pcl_ros jsk_pcl_ros::ClusterPointIndices indices; jsk_pcl_ros::ModelCoefficientsArray coefficients_array; jsk_pcl_ros::PolygonArray polygon_array; - indices.header = msg->header; - polygon_array.header = msg->header; - coefficients_array.header = msg->header; - pclIndicesArrayToClusterPointIndices(inlier_indices, msg->header, + indices.header = header; + polygon_array.header = header; + coefficients_array.header = header; + pclIndicesArrayToClusterPointIndices(inlier_indices, header, indices); - pcl::ExtractIndices extract; + pcl::ExtractIndices extract; extract.setInputCloud(input); for (size_t i = 0; i < regions.size(); i++) { - pcl::PointCloud boundary_cloud; + pcl::PointCloud boundary_cloud; pcl::PointIndices::Ptr indices_ptr = boost::make_shared(boundary_indices[i]); extract.setIndices(indices_ptr); extract.filter(boundary_cloud); geometry_msgs::PolygonStamped polygon; pointCloudToPolygon(boundary_cloud, polygon.polygon); - polygon.header = msg->header; + polygon.header = header; polygon_array.polygons.push_back(polygon); } org_pub_.publish(indices); @@ -334,7 +344,7 @@ namespace jsk_pcl_ros for (size_t i = 0; i < model_coefficients.size(); i++) { PCLModelCoefficientMsg coefficient; coefficient.values = model_coefficients[i].values; - coefficient.header = msg->header; + coefficient.header = header; coefficients_array.coefficients.push_back(coefficient); } org_coefficients_pub_.publish(coefficients_array); @@ -349,9 +359,9 @@ namespace jsk_pcl_ros { std::vector output_indices; std::vector output_coefficients; - std::vector > output_boundary_clouds; + std::vector > output_boundary_clouds; - buildConnectedPlanes(input, msg->header, + buildConnectedPlanes(input, header, inlier_indices, boundary_indices, model_coefficients, @@ -360,32 +370,92 @@ namespace jsk_pcl_ros jsk_pcl_ros::ClusterPointIndices indices; jsk_pcl_ros::PolygonArray polygon_array; - indices.header = msg->header; - polygon_array.header = msg->header; - pclIndicesArrayToClusterPointIndices(output_indices, msg->header, + indices.header = header; + polygon_array.header = header; + pclIndicesArrayToClusterPointIndices(output_indices, header, indices); for (size_t i = 0; i < output_boundary_clouds.size(); i++) { geometry_msgs::PolygonStamped polygon; - polygon.header = msg->header; + polygon.header = header; pointCloudToPolygon(output_boundary_clouds[i], polygon.polygon); polygon_array.polygons.push_back(polygon); } pub_.publish(indices); polygon_pub_.publish(polygon_array); jsk_pcl_ros::ModelCoefficientsArray coefficients_array; - coefficients_array.header = msg->header; + coefficients_array.header = header; for (size_t i = 0; i < output_coefficients.size(); i++) { PCLModelCoefficientMsg coefficient; coefficient.values = output_coefficients[i].values; - coefficient.header = msg->header; + coefficient.header = header; coefficients_array.coefficients.push_back(coefficient); } coefficients_pub_.publish(coefficients_array); } - ros::Time after_connect_time = ros::Time::now(); NODELET_DEBUG("checking connection done, %f", (after_connect_time - before_connect_time).toSec()); + } + + void OrganizedMultiPlaneSegmentation::estimateNormal(pcl::PointCloud::Ptr input, + pcl::PointCloud::Ptr output) + { + pcl::IntegralImageNormalEstimation ne; + if (estimation_method_ == 0) { + ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); + } + else if (estimation_method_ == 1) { + ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); + } + else if (estimation_method_ == 2) { + ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE); + } + else { + NODELET_FATAL("unknown estimation method: %d", estimation_method_); + return; + } + + if (border_policy_ignore_) { + ne.setBorderPolicy(pcl::IntegralImageNormalEstimation::BORDER_POLICY_IGNORE); + } + else { + ne.setBorderPolicy(pcl::IntegralImageNormalEstimation::BORDER_POLICY_MIRROR); + } + + ne.setMaxDepthChangeFactor(max_depth_change_factor_); + ne.setNormalSmoothingSize(normal_smoothing_size_); + ne.setDepthDependentSmoothing(depth_dependent_smoothing_); + + ros::Time before = ros::Time::now(); + ne.setInputCloud(input); + ne.compute(*output); + ros::Time after = ros::Time::now(); + NODELET_INFO("normal estimation: %f (%f Hz)", (after - before).toSec(), 1 / (after - before).toSec()); + } + + void OrganizedMultiPlaneSegmentation::segment + (const sensor_msgs::PointCloud2::ConstPtr& msg) + { + boost::mutex::scoped_lock(mutex_); + // if estimate_normal_ is true, we run integral image normal estimation + // before segmenting planes + pcl::PointCloud::Ptr input(new pcl::PointCloud()); + pcl::PointCloud::Ptr normal(new pcl::PointCloud()); + pcl::fromROSMsg(*msg, *input); + if (estimate_normal_) { + estimateNormal(input, normal); + // publish normal to ros + if (publish_normal_) { + sensor_msgs::PointCloud2 normal_ros_cloud; + pcl::toROSMsg(*normal, normal_ros_cloud); + normal_ros_cloud.header = msg->header; + normal_pub_.publish(normal_ros_cloud); + } + } + else { + pcl::fromROSMsg(*msg, *normal); + } + segmentFromNormals(input, normal, msg->header); } } From 7f11cc19cf4b6efdb4f33ecfd07e0cdee4c5ba97 Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Sun, 8 Jun 2014 17:50:56 +0900 Subject: [PATCH 7/7] use pcl::PointXYZRGB rather than pcl::PointXYZRGBNormal --- .../organized_multi_plane_segmentation.launch | 14 ++++++++----- .../src/multi_plane_extraction_nodelet.cpp | 20 +++++++++---------- ...nized_multi_plane_segmentation_nodelet.cpp | 2 +- .../selected_cluster_publisher_nodelet.cpp | 8 ++++---- 4 files changed, 24 insertions(+), 20 deletions(-) diff --git a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch index d389705933..7e01e40c4e 100644 --- a/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch +++ b/jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch @@ -19,12 +19,16 @@ unless="$(arg GDB)" output="screen"/> - + + + - + estimate_normal: true @@ -34,7 +38,7 @@ machine="$(arg MACHINE)" args="load jsk_pcl/ClusterPointIndicesDecomposer $(arg MANAGER)" output="screen" clear_params="true"> - + publish_clouds: false @@ -45,7 +49,7 @@ machine="$(arg MACHINE)" args="load jsk_pcl/ClusterPointIndicesDecomposer $(arg MANAGER)" output="screen" clear_params="true"> - + publish_clouds: false @@ -57,7 +61,7 @@ machine="$(arg MACHINE)" args="load jsk_pcl/MultiPlaneExtraction $(arg MANAGER)" output="screen" clear_params="true"> - + diff --git a/jsk_pcl_ros/src/multi_plane_extraction_nodelet.cpp b/jsk_pcl_ros/src/multi_plane_extraction_nodelet.cpp index 46b90457a4..db48305212 100644 --- a/jsk_pcl_ros/src/multi_plane_extraction_nodelet.cpp +++ b/jsk_pcl_ros/src/multi_plane_extraction_nodelet.cpp @@ -61,7 +61,7 @@ namespace jsk_pcl_ros PCLNodelet::onInit(); //pub_ = pnh_->advertise("output", 1); pub_ = pnh_->advertise("output", 1); - nonplane_pub_ = pnh_->advertise >("output_nonplane_cloud", 1); + nonplane_pub_ = pnh_->advertise >("output_nonplane_cloud", 1); if (!pnh_->getParam("max_queue_size", maximum_queue_size_)) { maximum_queue_size_ = 100; } @@ -93,7 +93,7 @@ namespace jsk_pcl_ros { boost::mutex::scoped_lock(mutex_); // convert all to the pcl types - pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud()); pcl::fromROSMsg(*input, *input_cloud); // concat indices into one PointIndices @@ -105,8 +105,8 @@ namespace jsk_pcl_ros } } - pcl::PointCloud::Ptr nonplane_cloud(new pcl::PointCloud()); - pcl::ExtractIndices extract_nonplane; + pcl::PointCloud::Ptr nonplane_cloud(new pcl::PointCloud()); + pcl::ExtractIndices extract_nonplane; extract_nonplane.setNegative(true); extract_nonplane.setInputCloud(input_cloud); extract_nonplane.setIndices(all_indices); @@ -118,11 +118,11 @@ namespace jsk_pcl_ros std::set result_set; for (size_t plane_i = 0; plane_i < coefficients->coefficients.size(); plane_i++) { - pcl::ExtractPolygonalPrismData prism_extract; - pcl::PointCloud::Ptr hull_cloud(new pcl::PointCloud()); + pcl::ExtractPolygonalPrismData prism_extract; + pcl::PointCloud::Ptr hull_cloud(new pcl::PointCloud()); geometry_msgs::Polygon the_polygon = polygons->polygons[plane_i].polygon; for (size_t i = 0; i < the_polygon.points.size(); i++) { - pcl::PointXYZRGBNormal p; + pcl::PointXYZRGB p; p.x = the_polygon.points[i].x; p.y = the_polygon.points[i].y; p.z = the_polygon.points[i].z; @@ -132,7 +132,7 @@ namespace jsk_pcl_ros prism_extract.setInputCloud(nonplane_cloud); prism_extract.setHeightLimits(min_height_, max_height_); prism_extract.setInputPlanarHull(hull_cloud); - //pcl::PointCloud output; + //pcl::PointCloud output; pcl::PointIndices output_indices; prism_extract.segment(output_indices); // append output to result_cloud @@ -144,7 +144,7 @@ namespace jsk_pcl_ros // convert std::set to PCLIndicesMsg //PCLIndicesMsg output_indices; - pcl::PointCloud result_cloud; + pcl::PointCloud result_cloud; pcl::PointIndices::Ptr all_result_indices (new pcl::PointIndices()); for (std::set::iterator it = result_set.begin(); it != result_set.end(); @@ -152,7 +152,7 @@ namespace jsk_pcl_ros all_result_indices->indices.push_back(*it); } - pcl::ExtractIndices extract_all_indices; + pcl::ExtractIndices extract_all_indices; extract_all_indices.setInputCloud(nonplane_cloud); extract_all_indices.setIndices(all_result_indices); extract_all_indices.filter(result_cloud); diff --git a/jsk_pcl_ros/src/organized_multi_plane_segmentation_nodelet.cpp b/jsk_pcl_ros/src/organized_multi_plane_segmentation_nodelet.cpp index fa919e22c6..a0a3befb7e 100644 --- a/jsk_pcl_ros/src/organized_multi_plane_segmentation_nodelet.cpp +++ b/jsk_pcl_ros/src/organized_multi_plane_segmentation_nodelet.cpp @@ -430,7 +430,7 @@ namespace jsk_pcl_ros ne.setInputCloud(input); ne.compute(*output); ros::Time after = ros::Time::now(); - NODELET_INFO("normal estimation: %f (%f Hz)", (after - before).toSec(), 1 / (after - before).toSec()); + NODELET_DEBUG("normal estimation: %f (%f Hz)", (after - before).toSec(), 1 / (after - before).toSec()); } void OrganizedMultiPlaneSegmentation::segment diff --git a/jsk_pcl_ros/src/selected_cluster_publisher_nodelet.cpp b/jsk_pcl_ros/src/selected_cluster_publisher_nodelet.cpp index 7f81868e72..a2956404f7 100644 --- a/jsk_pcl_ros/src/selected_cluster_publisher_nodelet.cpp +++ b/jsk_pcl_ros/src/selected_cluster_publisher_nodelet.cpp @@ -52,7 +52,7 @@ namespace jsk_pcl_ros void SelectedClusterPublisher::onInit() { PCLNodelet::onInit(); - pub_ = pnh_->advertise >("output", 1); + pub_ = pnh_->advertise >("output", 1); sync_ = boost::make_shared >(300); // 100 is enough? sub_input_.subscribe(*pnh_, "input", 1); sub_indices_.subscribe(*pnh_, "indices", 1); @@ -71,14 +71,14 @@ namespace jsk_pcl_ros indices->cluster_indices.size()); return; } - pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud()); + pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud()); pcl::fromROSMsg(*input, *input_cloud); - pcl::ExtractIndices extract; + pcl::ExtractIndices extract; pcl::PointIndices::Ptr pcl_indices (new pcl::PointIndices); pcl_indices->indices = indices->cluster_indices[index->data].indices; extract.setInputCloud(input_cloud); extract.setIndices(pcl_indices); - pcl::PointCloud::Ptr extracted_cloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr extracted_cloud(new pcl::PointCloud()); extract.filter(*extracted_cloud); pub_.publish(extracted_cloud); }