diff --git a/doc/jsk_pcl_ros/nodes/images/organized_statistical_outlier_removal_cluster.png b/doc/jsk_pcl_ros/nodes/images/organized_statistical_outlier_removal_cluster.png new file mode 100644 index 0000000000..e8b16d36d1 Binary files /dev/null and b/doc/jsk_pcl_ros/nodes/images/organized_statistical_outlier_removal_cluster.png differ diff --git a/doc/jsk_pcl_ros/nodes/organized_statistical_outlier_removal.md b/doc/jsk_pcl_ros/nodes/organized_statistical_outlier_removal.md index bba573a66a..5e1ee4b7d2 100644 --- a/doc/jsk_pcl_ros/nodes/organized_statistical_outlier_removal.md +++ b/doc/jsk_pcl_ros/nodes/organized_statistical_outlier_removal.md @@ -1,4 +1,5 @@ -# OrganizedStatisticalOutlierRemoval +# OrganizedStatisticalOutlierRemoval + ![](images/organized_statistical_outlier_removal.png) Organized version of statistical outlier removal. @@ -9,6 +10,15 @@ Organized version of statistical outlier removal. Input pointcloud to be removed outlier. +* `~input/cluster_indices` (`jsk_recognition_msgs/ClusterPointIndices`) + + Input cluster point indices to be removed outlier. + The clusters are used to calculate deviations. + This topic is only subscribe when `~use_cluster_point_indices` is `True`. + +### Example with `ClusterPointIndices` +![](images/organized_statistical_outlier_removal_cluster.png) + ## Publishing Topic * `~output` (`sensor_msgs/PointCloud2`) @@ -16,6 +26,13 @@ Organized version of statistical outlier removal. Output pointcloud. ## Parameter +* `~use_cluster_point_indices` (Boolean, default: `False`) + + use cluster point indices or not. + this option is designed to use after clustering (i.e. euclidean clustering), + because we can consider the distribution of each cluster with cluster point indices + information. + * `~keep_organized` (Boolean, default: `True`) keep organized point cloud or not @@ -33,8 +50,26 @@ Organized version of statistical outlier removal. std deviation multipelier for statistical outlier removal. +* `~approximate_sync` (Boolean, default: `False`) + + use approximate sync or not. + this option is valid when `~use_cluster_point_indices` is `True`. + +* `~queue_size` (Int, default: `100`) + + queue size for message filter. + this option is valid when `~use_cluster_point_indices` is `True`. + ## Sample +### Normal version + +``` +roslaunch jsk_pcl_ros sample_organized_statistical_outlier_removal.launch +``` + +### ClusterPointIndices version + ``` -roslaunch jsk_pcl_ros organized_statistical_outlier_removal.launch +roslaunch jsk_pcl_ros sample_realsense_tabletop_object_detector.launch ``` diff --git a/jsk_pcl_ros/CMakeLists.txt b/jsk_pcl_ros/CMakeLists.txt index 46833c0a7c..b4d0b1bb35 100644 --- a/jsk_pcl_ros/CMakeLists.txt +++ b/jsk_pcl_ros/CMakeLists.txt @@ -666,6 +666,7 @@ if (CATKIN_ENABLE_TESTING) add_rostest(test/test_voxel_grid_downsample.test) add_rostest(test/test_voxel_grid_large_scale.test) add_rostest(test/test_organized_statistical_outlier_removal.test) + add_rostest(test/test_realsense_tabletop_object_detector.test) if(tf2_eigen_FOUND) add_rostest(test/test_container_occupancy_detector.test) endif() diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h b/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h index 2891305ad7..90ff9a2e35 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h @@ -42,6 +42,13 @@ #include #include #include +#include +#include +#include + +#include "sensor_msgs/PointCloud2.h" +#include "jsk_recognition_msgs/ClusterPointIndices.h" + #include "jsk_pcl_ros/OrganizedStatisticalOutlierRemovalConfig.h" namespace jsk_pcl_ros @@ -50,7 +57,12 @@ namespace jsk_pcl_ros { public: typedef jsk_pcl_ros::OrganizedStatisticalOutlierRemovalConfig Config; - typedef pcl::PointXYZRGB PointT; + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::PointCloud2, + jsk_recognition_msgs::ClusterPointIndices > SyncPolicy; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::PointCloud2, + jsk_recognition_msgs::ClusterPointIndices > ApproximateSyncPolicy; OrganizedStatisticalOutlierRemoval(); protected: //////////////////////////////////////////////////////// @@ -64,15 +76,29 @@ namespace jsk_pcl_ros virtual void configCallback (Config &config, uint32_t level); - virtual void filter(const sensor_msgs::PointCloud2::ConstPtr& msg); + void filter(const pcl::PCLPointCloud2::Ptr pcl_cloud, + pcl::PointIndices::Ptr pcl_indices_filtered); + void filter(const pcl::PCLPointCloud2::Ptr pcl_cloud, + const pcl::PointIndices::Ptr pcl_indices, + pcl::PointIndices::Ptr pcl_indices_filtered); + std::vector getFilteredIndices(const pcl::PointCloud::Ptr cloud); + void filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg); + void filterCloudWithClusterPointIndices( + const sensor_msgs::PointCloud2::ConstPtr& msg, + const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cpi_msg); - virtual void updateDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper &stat); + + void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat); //////////////////////////////////////////////////////// // ROS variables //////////////////////////////////////////////////////// ros::Subscriber sub_; + message_filters::Subscriber sub_cloud_; + message_filters::Subscriber sub_cpi_; + boost::shared_ptr >sync_; + boost::shared_ptr >async_; + ros::Publisher pub_; boost::shared_ptr > srv_; boost::mutex mutex_; @@ -84,6 +110,9 @@ namespace jsk_pcl_ros //////////////////////////////////////////////////////// // Parameters //////////////////////////////////////////////////////// + bool use_cpi_; + bool use_async_; + int queue_size_; bool keep_organized_; bool negative_; double std_mul_; diff --git a/jsk_pcl_ros/sample/rviz/realsense_tabletop_object_detector.rviz b/jsk_pcl_ros/sample/rviz/realsense_tabletop_object_detector.rviz new file mode 100644 index 0000000000..122b2393a0 --- /dev/null +++ b/jsk_pcl_ros/sample/rviz/realsense_tabletop_object_detector.rviz @@ -0,0 +1,173 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 728 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /outlier_removal/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: BoundingBoxArray + Topic: /segmentation_decomposer/boxes + Unreliable: false + Value: true + alpha: 0.800000011920929 + color: 239; 41; 41 + coloring: Label + line width: 0.004999999888241291 + only edge: true + show coords: true + - Alpha: 1 + Class: jsk_rviz_plugin/PolygonArray + Color: 25; 255; 0 + Enabled: true + Name: PolygonArray + Topic: /polygon_magnifier/output + Unreliable: false + Value: true + coloring: Auto + enable lighting: true + normal length: 0.10000000149011612 + only border: true + show normal: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.026046633720398 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.5050702095031738 + Y: -0.11513220518827438 + Z: 0.2703014314174652 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.2647966146469116 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.2835805416107178 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 diff --git a/jsk_pcl_ros/sample/sample_realsense_tabletop_object_detector.launch b/jsk_pcl_ros/sample/sample_realsense_tabletop_object_detector.launch new file mode 100644 index 0000000000..d44f847d22 --- /dev/null +++ b/jsk_pcl_ros/sample/sample_realsense_tabletop_object_detector.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_pcl_ros/sample/tabletop_object_detector.launch b/jsk_pcl_ros/sample/tabletop_object_detector.launch index 1cabf3c6b6..5bb1a4365d 100644 --- a/jsk_pcl_ros/sample/tabletop_object_detector.launch +++ b/jsk_pcl_ros/sample/tabletop_object_detector.launch @@ -10,6 +10,20 @@ + + + + + + + + + + + + + + @@ -33,14 +47,30 @@ machine="$(arg machine)"> + + + + + step_x: $(arg resize_step) + step_y: $(arg resize_step) + + + + - - + + + max_curvature: 0.01 estimate_normal: true + min_size: $(arg plane_min_size) - + + + use_indices: true use_sensor_frame: true sensor_frame: $(arg sensor_frame) - min_height: 0.03 + min_height: $(arg object_min_height) + max_height: $(arg object_max_height) + use_async: $(arg approximate_sync) + max_queue_size: $(arg queue_size) + + + + + + mean_k: 30 + stddev: 0.4 + keep_organized: true + use_cluster_point_indices: true + approximate_sync: $(arg approximate_sync) + queue_size: $(arg queue_size) + + + - + + - align_boxes: true - use_pca: true + align_boxes: $(arg align_boxes) + align_boxes_with_plane: $(arg align_boxes_with_plane) + target_frame_id: $(arg target_frame_id) + use_pca: $(arg use_pca) + sort_by: $(arg sort_by) publish_clouds: false publish_tf: $(arg publish_tf) + approximate_sync: $(arg approximate_sync) + queue_size: $(arg queue_size) @@ -146,7 +204,8 @@ args="load jsk_pcl/OctreeVoxelGrid $(arg manager)" output="screen" clear_params="true" machine="$(arg machine)"> - + + point_type: xyzrgb diff --git a/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp b/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp index 5fa2aa7722..ffe84bf8bf 100644 --- a/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp +++ b/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include namespace jsk_pcl_ros @@ -51,6 +52,9 @@ namespace jsk_pcl_ros { DiagnosticNodelet::onInit(); pub_ = advertise(*pnh_, "output", 1); + pnh_->param("use_cluster_point_indices", use_cpi_, false); + pnh_->param("approximate_sync", use_async_, false); + pnh_->param("queue_size", queue_size_, 100); srv_ = boost::make_shared > (*pnh_); dynamic_reconfigure::Server::CallbackType f = @@ -62,12 +66,44 @@ namespace jsk_pcl_ros void OrganizedStatisticalOutlierRemoval::subscribe() { - sub_ = pnh_->subscribe("input", 1, &OrganizedStatisticalOutlierRemoval::filter, this); + if (use_cpi_) + { + sub_cloud_.subscribe(*pnh_, "input", 1); + sub_cpi_.subscribe(*pnh_, "input/cluster_indices", 1); + if (use_async_) + { + async_ = boost::make_shared >(queue_size_); + async_->connectInput(sub_cloud_, sub_cpi_); + async_->registerCallback( + boost::bind( + &OrganizedStatisticalOutlierRemoval::filterCloudWithClusterPointIndices, this, _1, _2)); + } + else + { + sync_ = boost::make_shared >(queue_size_); + sync_->connectInput(sub_cloud_, sub_cpi_); + sync_->registerCallback( + boost::bind( + &OrganizedStatisticalOutlierRemoval::filterCloudWithClusterPointIndices, this, _1, _2)); + } + } + else + { + sub_ = pnh_->subscribe("input", 1, &OrganizedStatisticalOutlierRemoval::filterCloud, this); + } } void OrganizedStatisticalOutlierRemoval::unsubscribe() { - sub_.shutdown(); + if (use_cpi_) + { + sub_cloud_.unsubscribe(); + sub_cpi_.unsubscribe(); + } + else + { + sub_.shutdown(); + } } void OrganizedStatisticalOutlierRemoval::configCallback(Config &config, uint32_t level) @@ -84,142 +120,220 @@ namespace jsk_pcl_ros #define pcl_isfinite(x) std::isfinite(x) #endif - void OrganizedStatisticalOutlierRemoval::filter(const sensor_msgs::PointCloud2::ConstPtr& msg) + void OrganizedStatisticalOutlierRemoval::filter( + const pcl::PCLPointCloud2::Ptr pcl_cloud, + pcl::PointIndices::Ptr pcl_indices_filtered) { - boost::mutex::scoped_lock lock(mutex_); - vital_checker_->poke(); - sensor_msgs::PointCloud2 output; + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromPCLPointCloud2(*pcl_cloud, *cloud); + pcl_indices_filtered->indices = OrganizedStatisticalOutlierRemoval::getFilteredIndices(cloud); + } - if (keep_organized_&& msg->is_dense) { - NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized."); + void OrganizedStatisticalOutlierRemoval::filter( + const pcl::PCLPointCloud2::Ptr pcl_cloud, + const pcl::PointIndices::Ptr pcl_indices, + pcl::PointIndices::Ptr pcl_indices_filtered) + { + pcl::PCLPointCloud2::Ptr pcl_cloud_ex (new pcl::PCLPointCloud2); + pcl::ExtractIndices ex; + ex.setInputCloud(pcl_cloud); + ex.setKeepOrganized(false); + ex.setNegative(false); + ex.setIndices(pcl_indices); + ex.filter(*pcl_cloud_ex); + pcl::PointCloud::Ptr cloud_ex(new pcl::PointCloud); + pcl::fromPCLPointCloud2(*pcl_cloud_ex, *cloud_ex); + + const std::vector indices_ex_filtered = OrganizedStatisticalOutlierRemoval::getFilteredIndices(cloud_ex); + std::vector indices_filtered; + indices_filtered.resize(indices_ex_filtered.size()); + for (size_t i = 0; i < indices_ex_filtered.size(); i++) + { + indices_filtered[i] = pcl_indices->indices[indices_ex_filtered[i]]; } - bool keep_organized = keep_organized_ && !msg->is_dense; + pcl_indices_filtered->indices = indices_filtered; + } + std::vector OrganizedStatisticalOutlierRemoval::getFilteredIndices( + const pcl::PointCloud::Ptr cloud) + { + std::vector indices_filtered; + indices_filtered.resize(cloud->size()); #if PCL_VERSION_COMPARE (<, 1, 9, 0) - if (keep_organized) { - // Send the input dataset to the spatial locator - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); - - // Initialize the spatial locator - pcl::search::Search::Ptr tree; - tree.reset (new pcl::search::OrganizedNeighbor ()); - tree->setInputCloud (cloud); - - // Allocate enough space to hold the results - std::vector indices (cloud->size ()); - std::vector nn_indices (mean_k_); - std::vector nn_dists (mean_k_); - std::vector distances (indices.size ()); - int valid_distances = 0; - - for (size_t i = 0; i < indices.size (); ++i) indices[i] = i; - - // Go over all the points and calculate the mean or smallest distance - for (size_t cp = 0; cp < indices.size (); ++cp) + // Initialize the spatial locator + pcl::search::Search::Ptr tree; + tree.reset (new pcl::search::OrganizedNeighbor ()); + tree->setInputCloud (cloud); + + // Allocate enough space to hold the results + std::vector indices (cloud->size()); + std::vector nn_indices (mean_k_); + std::vector nn_dists (mean_k_); + std::vector distances (indices.size ()); + int valid_distances = 0; + + for (size_t i = 0; i < indices.size (); ++i) indices[i] = i; + + // Go over all the points and calculate the mean or smallest distance + for (size_t cp = 0; cp < indices.size (); ++cp) + { + if (!pcl_isfinite (cloud->points[indices[cp]].x) || + !pcl_isfinite (cloud->points[indices[cp]].y) || + !pcl_isfinite (cloud->points[indices[cp]].z)) + { + distances[cp] = 0; + continue; + } + if (tree->nearestKSearch (indices[cp], mean_k_, nn_indices, nn_dists) == 0) { - if (!pcl_isfinite (cloud->points[indices[cp]].x) || - !pcl_isfinite (cloud->points[indices[cp]].y) || - !pcl_isfinite (cloud->points[indices[cp]].z)) - { - distances[cp] = 0; - continue; - } - if (tree->nearestKSearch (indices[cp], mean_k_, nn_indices, nn_dists) == 0) - { - distances[cp] = 0; - continue; - } - // Minimum distance (if mean_k_ == 2) or mean distance - double dist_sum = 0; - for (int j = 1; j < mean_k_; ++j) - dist_sum += sqrt (nn_dists[j]); - distances[cp] = static_cast (dist_sum / (mean_k_ - 1)); - valid_distances++; + distances[cp] = 0; + continue; } + // Minimum distance (if mean_k_ == 2) or mean distance + double dist_sum = 0; + for (int j = 1; j < mean_k_; ++j) + dist_sum += sqrt (nn_dists[j]); + distances[cp] = static_cast (dist_sum / (mean_k_ - 1)); + valid_distances++; + } + + // Estimate the mean and the standard deviation of the distance vector + double sum = 0, sq_sum = 0; + for (size_t i = 0; i < distances.size (); ++i) + { + sum += distances[i]; + sq_sum += distances[i] * distances[i]; + } + double mean = sum / static_cast(valid_distances); + double variance = (sq_sum - sum * sum / static_cast(valid_distances)) / (static_cast(valid_distances) - 1); + double stddev = sqrt (variance); + //getMeanStd (distances, mean, stddev); + + double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier - // Estimate the mean and the standard deviation of the distance vector - double sum = 0, sq_sum = 0; - for (size_t i = 0; i < distances.size (); ++i) + // Build a new cloud by neglecting outliers + for (int cp = 0; cp < static_cast (indices.size ()); ++cp) + { + bool remove_point = false; + if (negative_) + { + remove_point = (distances[cp] <= distance_threshold); + } + else { - sum += distances[i]; - sq_sum += distances[i] * distances[i]; + remove_point = (distances[cp] > distance_threshold); } - double mean = sum / static_cast(valid_distances); - double variance = (sq_sum - sum * sum / static_cast(valid_distances)) / (static_cast(valid_distances) - 1); - double stddev = sqrt (variance); - //getMeanStd (distances, mean, stddev); + if (!remove_point) + { + indices_filtered.push_back(indices[cp]); + } + } +#else + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(cloud); + sor.setMeanK(mean_k_); + sor.setStddevMulThresh(std_mul_); + sor.setNegative(negative_); + sor.setKeepOrganized (true); + sor.filter(indices_filtered); +#endif + return indices_filtered; + } + + void OrganizedStatisticalOutlierRemoval::filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg) + { + boost::mutex::scoped_lock lock(mutex_); + vital_checker_->poke(); + sensor_msgs::PointCloud2 output; - double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier + if (keep_organized_&& msg->is_dense) { + NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized."); + } + bool keep_organized = keep_organized_ && !msg->is_dense; + pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*msg, *pcl_cloud); + // filter + pcl::PointIndices::Ptr pcl_indices_filtered (new pcl::PointIndices()); + OrganizedStatisticalOutlierRemoval::filter(pcl_cloud, pcl_indices_filtered); + pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2); + pcl::ExtractIndices ex; + ex.setInputCloud(pcl_cloud); + ex.setKeepOrganized(keep_organized); + ex.setNegative(false); + ex.setIndices(pcl_indices_filtered); + ex.filter(*pcl_cloud_filtered); + pcl_conversions::fromPCL(*pcl_cloud_filtered, output); +#if PCL_VERSION_COMPARE (<, 1, 9, 0) + if (keep_organized) { // Copy the common fields output.is_bigendian = msg->is_bigendian; output.fields = msg->fields; output.point_step = msg->point_step; - output.data.resize (msg->data.size ()); - - // Build a new cloud by neglecting outliers - int nr_p = 0; - int nr_removed_p = 0; - bool remove_point = false; - for (int cp = 0; cp < static_cast (indices.size ()); ++cp) - { - if (negative_) - remove_point = (distances[cp] <= distance_threshold); - else - remove_point = (distances[cp] > distance_threshold); - if (remove_point) - { - /* Set the current point to NaN. */ - *(reinterpret_cast(&output.data[nr_p * output.point_step])+0) = std::numeric_limits::quiet_NaN(); - *(reinterpret_cast(&output.data[nr_p * output.point_step])+1) = std::numeric_limits::quiet_NaN(); - *(reinterpret_cast(&output.data[nr_p * output.point_step])+2) = std::numeric_limits::quiet_NaN(); - nr_p++; - } - else - { - memcpy (&output.data[nr_p * output.point_step], - &msg->data[indices[cp] * output.point_step], - output.point_step); - nr_p++; - } - } + output.data.resize(msg->data.size()); output.width = msg->width; output.height = msg->height; - output.row_step = output.point_step * output.width; - } - else - { - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(cloud); - sor.setMeanK (mean_k_); - sor.setStddevMulThresh (std_mul_); - sor.setNegative (negative_); - sor.filter (*cloud_filtered); - pcl::toROSMsg(*cloud_filtered, output); } +#endif output.header = msg->header; + output.row_step = output.point_step * output.width; output.is_dense = !keep_organized; pub_.publish(output); -#else - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(cloud); - sor.setMeanK (mean_k_); - sor.setStddevMulThresh (std_mul_); - sor.setNegative (negative_); - sor.setKeepOrganized (keep_organized); - sor.filter (*cloud_filtered); - pcl::toROSMsg(*cloud_filtered, output); + diagnostic_updater_->update(); + } + + void OrganizedStatisticalOutlierRemoval::filterCloudWithClusterPointIndices( + const sensor_msgs::PointCloud2::ConstPtr& msg, + const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cpi_msg) + { + boost::mutex::scoped_lock lock(mutex_); + vital_checker_->poke(); + sensor_msgs::PointCloud2 output; + + if (keep_organized_ && msg->is_dense) { + NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized."); + } + bool keep_organized = keep_organized_ && !msg->is_dense; + pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2); + pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2); + pcl::PointIndices::Ptr pcl_indices_filtered (new pcl::PointIndices()); + pcl_conversions::toPCL(*msg, *pcl_cloud); + + for (size_t i = 0; i < cpi_msg->cluster_indices.size(); i++) + { + pcl::PointIndices::Ptr pcl_cluster_indices (new pcl::PointIndices()); + pcl_conversions::toPCL(cpi_msg->cluster_indices[i], *pcl_cluster_indices); + + pcl::PointIndices::Ptr pcl_cluster_indices_filtered (new pcl::PointIndices()); + OrganizedStatisticalOutlierRemoval::filter(pcl_cloud, + pcl_cluster_indices, + pcl_cluster_indices_filtered); + pcl_indices_filtered->indices.insert(pcl_indices_filtered->indices.end(), + pcl_cluster_indices_filtered->indices.begin(), + pcl_cluster_indices_filtered->indices.end()); + } + pcl::ExtractIndices ex; + ex.setInputCloud(pcl_cloud); + ex.setKeepOrganized(keep_organized); + ex.setNegative(false); + ex.setIndices(pcl_indices_filtered); + ex.filter(*pcl_cloud_filtered); + pcl_conversions::fromPCL(*pcl_cloud_filtered, output); +#if PCL_VERSION_COMPARE (<, 1, 9, 0) + if (keep_organized) { + // Copy the common fields + output.is_bigendian = msg->is_bigendian; + output.fields = msg->fields; + output.point_step = msg->point_step; + output.data.resize(msg->data.size()); + output.width = msg->width; + output.height = msg->height; + } +#endif output.header = msg->header; + output.row_step = output.point_step * output.width; output.is_dense = !keep_organized; pub_.publish(output); -#endif diagnostic_updater_->update(); } diff --git a/jsk_pcl_ros/test/test_realsense_tabletop_object_detector.test b/jsk_pcl_ros/test/test_realsense_tabletop_object_detector.test new file mode 100644 index 0000000000..b68ed4edd5 --- /dev/null +++ b/jsk_pcl_ros/test/test_realsense_tabletop_object_detector.test @@ -0,0 +1,43 @@ + + + + + + + + + topic_0: /multi_plane_estimate/output_refined + timeout_0: 30 + topic_1: /multi_plane_estimate/output_refined_polygon + timeout_1: 30 + topic_2: /multi_plane_estimate/output_refined_coefficients + timeout_2: 30 + topic_3: /plane_extraction/output + timeout_3: 30 + topic_4: /euclidean_clustering/output + timeout_4: 30 + topic_5: /euclidean_clustering/output_throttle + timeout_5: 30 + topic_6: /outlier_removal/output + timeout_6: 30 + topic_7: /segmentation_decomposer/boxes + timeout_7: 30 + topic_8: /segmentation_decomposer/centroid_pose_array + timeout_8: 30 + topic_9: /segmentation_decomposer/cluster_indices + timeout_9: 30 + topic_10: /segmentation_decomposer/debug_output + timeout_10: 30 + topic_11: /segmentation_decomposer/label + timeout_11: 30 + topic_12: /segmentation_decomposer/mask + timeout_12: 30 + topic_13: /segmentation_decomposer/negative_indices + timeout_13: 30 + + + +